利用ROS动作和逆运动学控制PR2手臂运动

这个例子使用了:

打开实时脚本

这个例子展示了如何在MATLAB®中向机器人机械手发送命令。联合位置命令通过ROS网络通过ROS动作客户端发送。这个例子还展示了如何计算所需的末端执行器位置的关节位置。刚体树定义了机器人的几何形状和关节约束，并将其与逆运动学结合得到机器人的关节位置。然后你可以将这些关节位置作为轨迹发送给机器人，命令其移动。

打开PR2凉亭模拟

本例使用Ubuntu®虚拟机(VM)，其中包含可下载的ROS Melodic在这里．这个例子不支持ROS Noetic，因为它依万博1manbetx赖的ROS包在ROS Melodic之前只被支持。

在Gazebo Simulator中在一个简单的环境中生成PR2(只有一个桌子和一个可乐罐)凉亭PR2模拟器桌面快捷方式。看到开始与露台和模拟乌龟机器人(ROS工具箱)有关此过程的详细信息。

从MATLAB®连接到ROS网络

在主机上的MATLAB实例中，运行以下命令在MATLAB中初始化ROS全局节点，并通过其IP地址连接到虚拟机中的ROS主服务器ipaddress．将'192.168.233.133'替换为虚拟机的IP地址。如果需要，请指定端口。

ipaddress =“192.168.116.161”；rosinit (ipaddress, 11311);

使用NodeURI http://192.168.116.1:64988/初始化全局节点/matlab_global_node_03004

为机器人手臂创建动作客户端并发送命令

在这个任务中，将关节轨迹发送到PR2臂。手臂可以通过ROS作用来控制。每个手臂的关节轨迹都是手动指定的，并通过单独的动作客户端作为单独的目标信息发送。

创建一个ROS简单动作客户端，发送移动机器人右臂的目标消息。rosactionclient(ROS工具箱)对象和目标消息返回。等待客户端连接到ROS操作服务器。

[rArm, rGoalMsg] = rosactionclient(“r_arm_controller / joint_trajectory_action”）;waitForServer (rArm);

目标消息是atrajectory_msgs / JointTrajectoryPoint消息。每个轨迹点应指定关节的位置和速度。

disp (rGoalMsg)

[1×1 JointTrajectory]使用showdetails来显示消息的内容

disp (rGoalMsg.Trajectory)

[1×1 Header] Points: [0×1 JointTrajectory point] JointNames: {0×1 cell}使用showdetails显示消息的内容

设置关节名称与PR2机器人关节名称相匹配。注意有7个关节需要控制。要查找PR2右臂上的关节，在虚拟机终端中输入以下命令:

Rosparam get /r_arm_controller/关节

rGoalMsg.Trajectory.JointNames = {“r_shoulder_pan_joint”，.．.“r_shoulder_lift_joint”，.．.“r_upper_arm_roll_joint”，.．.“r_elbow_flex_joint”，.．.“r_forearm_roll_joint”，.．.“r_wrist_flex_joint”，.．.“r_wrist_roll_joint”}；

通过创建ROS在手臂关节轨迹中创建设定值trajectory_msgs / JointTrajectoryPoint信息，并指定所有7个关节的位置和速度作为一个向量。指定从开始开始的时间作为ROS持续时间对象。

%点1tjPoint1 = rosmessage(“trajectory_msgs / JointTrajectoryPoint”）;tjPoint1。位置= 0 (1,7);tjPoint1。速度= 0 (1,7);tjPoint1。TimeFromStart = rosduration(1.0);%点2tjPoint2 = rosmessage(“trajectory_msgs / JointTrajectoryPoint”）;tjPoint2。位置= [-1.0 0.2 0.1 -1.2 -1.5 -0.3 -0.5];tjPoint2。速度= 0 (1,7);tjPoint2。TimeFromStart = rosduration(2.0);

用轨迹中的点创建一个对象数组，并将其分配给目标消息。使用操作客户端发送目标。的sendGoalAndWait(ROS工具箱)函数将阻塞执行，直到PR2臂完成执行轨迹

rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2];sendGoalAndWait (rArm rGoalMsg);

创建另一个动作客户端向左臂发送命令。指定PR2机器人的关节名称。

[lArm, lGoalMsg] = rosactionclient(“l_arm_controller / joint_trajectory_action”）;waitForServer (lArm);lGoalMsg.Trajectory.JointNames = {“l_shoulder_pan_joint”，.．.“l_shoulder_lift_joint”，.．.“l_upper_arm_roll_joint”，.．.“l_elbow_flex_joint”，.．.“l_forearm_roll_joint”，.．.“l_wrist_flex_joint”，.．.“l_wrist_roll_joint”}；

为左臂设定一个轨迹点。将其分配给目标消息并发送目标。

tjPoint3 = rosmessage(“trajectory_msgs / JointTrajectoryPoint”）;tjPoint3。位置= [1.0 0.2 -0.1 -1.2 1.5 -0.3 0.5];tjPoint3。速度= 0 (1,7);tjPoint3。TimeFromStart = rosduration(2.0);lGoalMsg.Trajectory.Points = tjPoint3;sendGoalAndWait (lArm lGoalMsg);

计算末端执行器位置的逆运动学

在本节中，您将基于所需的末端执行器(PR2右夹持器)位置计算关节轨迹。的inverseKinematics类计算所有所需的关节位置，这些位置可以通过动作客户端作为轨迹目标消息发送。一个rigidBodyTree对象用于定义机器人参数，生成配置，并在MATLAB®中可视化机器人。

请执行以下步骤:

从ROS网络获取PR2机器人的当前状态，并将其分配给rigidBodyTree对象与机器人在MATLAB®中工作。
定义一个末端执行器目标姿态。
使用这些关节位置可视化机器人配置，以确保正确的解决方案。
利用逆运动学从目标末端执行器姿态计算关节位置。
将关节位置的轨迹发送给ROS动作服务器，以命令实际的PR2机器人。

在MATLAB®中创建刚体树

将PR2机器人加载为rigidBodyTree对象。该对象定义了机器人的所有运动学参数(包括关节极限)。

pr2 = exampleHelperWGPR2Kinect;

获取当前机器人状态

创建一个订阅者，从机器人获取关节状态。

jointSub = rossubscriber(“joint_states”）;

获取当前的联合状态消息。

jntState = receive(jointSub);

将连接状态消息中的连接位置分配到配置结构的字段pr2对象的理解。

jntPos = exampleHelperJointMsgToStruct(pr2,jntState);

可视化当前机器人配置

使用显示用给定的关节位置向量来可视化机器人。这应该与机器人的当前状态相匹配。

显示(pr2 jntPos)

ans = axis (Primary) with properties: XLim: [-2 2] YLim: [-2 2] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: 'normalized'显示所有属性

创建一个inverseKinematics对象中的pr2机器人对象。逆运动学的目标是计算PR2手臂的关节角度，使夹持器(即末端执行器)处于所需的姿势。在一段时间内的末端执行器姿态序列称为轨迹。

在这个例子中，我们将只控制机器人的手臂。因此，我们在规划过程中对躯干-升降机关节进行了严格的限制。

torsoJoint = pr2.getBody(“torso_lift_link”) .Joint;idx = strcmp({jntPos. idx = strcmp。JointName}, torsoJoint.Name);torsoJoint。HomePosition = jntPos(idx).JointPosition;torsoJoint。PositionLimits = jntPos(idx)。JointPosition + [-1e-3,1e-3];

创建inverseKinematics对象。

ik = inverseKinematics(“RigidBodyTree”pr2);

禁用随机重启，确保IK解决方案一致。万博尤文图斯

ik.SolverParameters.AllowRandomRestart = false;

指定目标位姿各部件公差的权重。

Weights = [0.25 0.25 0.25 1 1 1];initialGuess = jntPos;%当前JNT pos作为初始猜测

指定与任务相关的末端执行器姿态。在这个例子中，PR2将到达桌子上的易拉罐，抓住易拉罐，把它移动到另一个位置，然后再次放下。我们将使用inverseKinematics对象来计划从一个末端执行器姿态平稳过渡到另一个末端执行器姿态的运动。

指定末端执行器的名称。

endEffectorName =“r_gripper_tool_frame”；

指定罐头的初始(当前)姿势和期望的最终姿势。

TCanInitial = trvec2tform([0.7, 0.0, 0.55]);TCanFinal = trvec2tform([0.6， -0.5, 0.55]);

定义抓取时末端执行器与易拉罐之间所需的相对变换。

TGraspToCan = trvec2tform([0,0,0.08])*eul2tform([pi/8,0，-pi]);

定义目标笛卡尔轨迹的关键路径点。罐头应该沿着这个轨迹运动。轨迹可以分解如下:

打开拉手
将末端执行器从当前姿势移动到T1这样机器人才会很舒服地开始抓
移动末端执行器T1来TGrasp(准备抓住)
合上夹持器，抓住罐头
移动末端执行器TGrasp来T2(吊罐在空中)
移动末端执行器T2来T3(移动可以水平)
移动末端执行器T3来TRelease(把罐头放低到桌子表面，准备释放)
打开夹持器，放开罐头
移动末端执行器TRelease来T4(收回手臂)

TGrasp = TCanInitial*TGraspToCan;抓取易拉罐时所需的末端执行器姿势。T1 = TGrasp*trvec2tform([0.，0，-0.1]);T2 = TGrasp*trvec2tform([0,0，-0.2]);T3 = TCanFinal*TGraspToCan*trvec2tform([0,0，-0.2]);TRelease = TCanFinal* tgrastocan;释放易拉罐时所需的末端执行器姿势。T4 = TRelease*trvec2tform([-0.1,0.05,0]);

实际的运动将由numWaypoints并通过ROS简单动作客户端发送给机器人。方法来选择这些配置inverseKinematics对象，使末端执行器位姿从初始位姿插值到最终位姿。

执行动作

指定动作的顺序。

motionTask = {“发布”， T1, TGrasp，“把握”T2, T3, TRelease，“发布”, T4};

执行中指定的每个任务motionTask一个接一个。使用指定的helper函数发送命令。

为i = 1:长度(motionTask)如果比较字符串(motionTask {},“把握”) exampleHelperSendPR2GripperCommand (“对”, 0.0, 1000,假);继续结束如果比较字符串(motionTask {},“发布”) exampleHelperSendPR2GripperCommand (“对”、0.1、1、真实);继续结束Tf = motionTask{i};获取当前关节状态jntState = receive(jointSub);jntPos = exampleHelperJointMsgToStruct(pr2, jntState);T0 = getTransform(pr2, jntPos, endEffectorName);在关键路径点之间插值numWaypoints = 10;[s, sd, sdd, tvec] = trapveltraj([0 1]， numWaypoints，“AccelTime”, 0.4);相对缓慢的上升到最高速度。TWaypoints = transformtraj(T0, Tf， [0 1]， tvec，“时间尺度”[s;sd;sdd]);末端执行器姿态路径点。jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);关节位置航路点rArmJointNames = rGoalMsg.Trajectory.JointNames;rArmJntPosWaypoints = 0 (numWaypoints, numel(rArmJointNames));使用IK计算每个末端执行器姿态路径点的关节位置为k = 1:numWaypoints jntPos = ik(eneffectorname, TWaypoints(:，:，k)， weights, initialGuess);jntPosWaypoints(k，:) = jntPos;initialGuess = jntPos;从jntPos中提取右臂关节位置rArmJointPos = 0 (size(rArmJointNames));为n = 1:length(rArmJointNames) rn = rArmJointNames{n};idx = strcmp({jntPos. idx = strcmp。JointName}, rn);rArmJointPos(n) = jntPos(idx).JointPosition;结束rArmJntPosWaypoints(k，:) = rArmJointPos';结束%每个航路点对应的时间点timePoints = linspace(0,3,numWaypoints);数值估计关节速度轨迹h = diff(时间点);H = H (1);= arrayfun(@(~) rosmessage(“trajectory_msgs / JointTrajectoryPoint”), 0 (numWaypoints));[~， rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);为m = 1:numWaypoints jntTrajectoryPoints(m)。位置= rArmJntPosWaypoints(m，:);jntTrajectoryPoints (m)。速度= rArmJntVelWaypoints(m，:);jntTrajectoryPoints (m)。TimeFromStart = rosduration(timePoints(m));结束在MATLAB(R)中可视化机器人运动和末端执行器轨迹持有在为j = 1:numWaypoints show(pr2, jntPosWaypoints(j，:)，“PreservePlot”、假);exampleHelperShowEndEffectorPos (TWaypoints (:,:, j));drawnow;暂停(0.1);结束发送右臂轨迹给机器人。rGoalMsg.Trajectory.Points = jnttrajectory . points;sendGoalAndWait (rArm rGoalMsg);结束

总结

将手臂移回起始位置。

exampleHelperSendPR2GripperCommand (“r”rGoalMsg.Trajectory.Points = tjPoint2;sendGoal (rArm rGoalMsg);

调用rosshutdown关闭ROS网络，断开与机器人的连接。

rosshutdown

使用NodeURI http://192.168.116.1:64988/关闭全局节点/matlab_global_node_03004