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

这个例子使用了:

打开实时脚本

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

打开PR2凉亭模拟

本例使用一个Ubuntu®虚拟机(VM)，其中包含可下载的ROS Melodic在这里．这个例子不支持ROS Noetic，因为它依金宝app赖于ROS包，而ROS包只支持ROS Melodic。

在凉亭模拟器的一个简单的环境中(只有一张桌子和一个可乐罐)通过启动凉亭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)

ROS JointTrajectory message with properties: MessageType: 'pr2_controllers_msgs/JointTrajectory ' Trajectory: [1×1 JointTrajectory]使用showdetails显示消息的内容

disp (rGoalMsg.Trajectory)

带有属性的ROS JointTrajectory消息:MessageType: 'trajectory - msgs/JointTrajectory' Header: [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机器人的当前状态，并将其分配给arigidBodyTree对象在MATLAB®中与机器人一起工作。
定义一个末端执行器目标姿态。
使用这些关节位置可视化机器人配置，以确保正确的解决方案。
利用逆运动学从目标末端执行器姿态计算关节位置。
将关节位置轨迹发送给ROS动作服务器，以命令实际的PR2机器人。

创建一个刚体树在MATLAB®

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

pr2 = exampleHelperWGPR2Kinect;

获取当前机器人状态

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

jointSub = rossubscriber(“joint_states”）;

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

jntState = receive(jointSub);

将联合状态消息中的联合位置分配给配置结构的字段pr2对象的理解。

jntPos = exampleHelperJointMsgToStruct(pr2,jntState);

可视化当前的机器人配置

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

显示(pr2 jntPos)

ans =轴(主要)与属性:XLim: [-2 2] YLim: [-2 2] XScale: '线性' YScale: '线性' GridLineStyle: '-'位置:[0.1300 0.1100 0.7750 0.8150]单位:'归一化'显示所有属性

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

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

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

创建inverseKinematics对象。

ik =逆运动学(“RigidBodyTree”pr2);

禁止随机重启，确保IK方案一致。金宝搏官方网站

ik.SolverParameters.AllowRandomRestart = false;

为目标姿势的每个组件指定公差的权重。

权重= [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,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]);TCanFinal*TGraspToCan;释放易拉罐时所需的末端执行器姿态T4 = trvec2tform([-0.1,0.05,0]);

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

执行动作

指定动作的顺序。

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

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

为i = 1: length(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, nummel (rArmJointNames));使用IK计算每个末端执行器姿态路点的关节位置为k = 1:numWaypoints jntPos = ik(endEffectorName, TWaypoints(:，:，k)， weights, initialGuess);jntPosWaypoints(k，:) = jntPos;initialGuess = jntPos;从jntPos中提取右臂关节位置rArmJointPos = 0(大小(rArmJointNames));为n = 1:length(rArmJointNames) rn = rArmJointNames{n};idx = strcmp({jntPos。JointName}, rn);rArmJointPos(n) = jntPos(idx).JointPosition;结束rArmJntPosWaypoints(k，:) = rArmJointPos';结束%每个路点对应的时间点timePoints = linspace(0,3,numWaypoints);数值估计关节速度轨迹h = diff(时间点);H = H (1);jntTrajectoryPoints = 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