主要内容

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

此示例使用：

打开生活的脚本

这个例子展示了如何在MATLAB®中发送命令给机器人。关节位置命令通过ROS网络通过ROS动作客户端发送。这个例子也展示了如何计算一个期望的末端执行器位置的关节位置。刚体树定义了机器人的几何和关节约束条件，并结合运动学逆解得到机器人的关节位置。然后你可以把这些关节位置作为一个轨迹来命令机器人移动。

调出PR2露台模拟

在Gazebo模拟器的一个简单环境中生成PR2(只有一个桌子和一个可乐罐)。请按照开始使用凉亭和模拟乌龟机器人（ROS工具箱）推出露台PR2模拟器从Ubuntu®虚拟机桌面。

从MATLAB®连接到ROS网络

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

ipaddress ='192.168.116.161'；rosinit (ipaddress, 11311);
使用NodeURI http://192.168.116.1:64988/初始化全局节点/matlab_global_node_03004

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

在这个任务中，你发送关节轨迹到PR2臂。手臂可以通过活性氧的动作来控制。关节轨迹手动指定每个手臂，并通过单独的动作客户端作为单独的目标消息发送。

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

[rArm, rGoalMsg] = rosactionclient('r_arm_controller / connt_trajectory_action'）;waitForServer (rArm);

目标信息是trajectory_msgs / traintrajectorepoint消息。每个轨迹点应该指定关节的位置和速度。

DISP（RGOALMSG）
ROS JointTrajectory: [1×1 JointTrajectory]使用showdetails显示消息的内容
disp (rGoalMsg.Trajectory)
具有属性的ROS联合消息消息：MessageType：'traometory_msgs / trafttrajectory'标题：[1×1标题]点：[0×1 jointtrajectoryPoint] Connectnames：{0×1 Cell}使用ShowDetail来显示消息的内容

设置联合名称以匹配PR2机器人联合名称。请注意，有7个接头可以控制。要查找PR2右臂中的关节，请在虚拟机终端中键入此命令：

rosparam / 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在ARM关节轨迹中创建设定值trajectory_msgs / traintrajectorepoint消息并指定所有7个关节作为向量的位置和速度。从start作为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.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3 -0.5];tjpoint2.velocities = zeros（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.Positions = [1.0 0.2 -0.1 -1.2 1.5 -0.3 0.5];tjpoint3.velocities = zeros（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 =接收(jointSub);

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

jntPos = exampleHelperJointMsgToStruct (pr2, jntState);

可视化当前机器人配置

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

展示（PR2，JNTPOS）

ans = Axes (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。JointName}, torsoJoint.Name);torsoJoint。HomePosition = jntPos (idx) .JointPosition;torsoJoint。PositionLimits = jntPos (idx)。JointPosition + (1 e - 3, 1 e - 3);

创造inverseKinematics对象。

本土知识= inverseKinematics (“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,0.08]）* Eul2tform（[PI / 8,0，-PI]）;

定义所需的笛卡儿轨迹的关键路径点。罐子应该沿着这个轨迹运动。轨迹可以分解如下:

  • 打开夹

  • 将末端效应移至当前姿势T1因此，机器人会觉得掌握掌握

  • 移动末端执行器T1来TGRASP.（准备掌握）

  • 关闭夹具并抓住罐头

  • 移动末端执行器TGRASP.来T2(把罐子举到空中)

  • 移动末端执行器T2来T3(移动可以不动心地)

  • 移动末端执行器T3来三胞胎(下罐至台面，准备释放)

  • 打开夹持器，放开罐头

  • 移动末端执行器三胞胎来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 * TGraspToCan;当释放罐时，％终效器姿势姿势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 {};获得当前的关节状态jntState =接收(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;RAMJNTPOSWAYPOINTS = Zeros（NumwayPoints，Numel（RarmJointNames））;使用IK计算每个末端执行器姿态路径点的关节位置为了k = 1：numwaypoints jntpos = ik（endeffectorname，twaypoints（：，：，k），权重，initialGuess）;jntposwaypoints（k，:) = jntpos;initialGuess = jntPos;％从JNTPOS提取右臂关节位置RARMJOINTPOS = Zeros（大小（RarmJointNode））;为了n = 1:length(rArmJointNames) rn = rArmJointNames{n};idx = strcmp ({jntPos。JointName}, rn);rArmJointPos (n) = jntPos (idx) .JointPosition;结尾: rArmJntPosWaypoints (k) = rArmJointPos ';结尾%每个航路点对应的时间点时间点= linspace(0、3、numWaypoints);数值估计关节速度轨迹h = diff（timepoints）;h = h（1）;jnttrajectorypoints = arrayfun（@（〜）rosmessage（“trajectory_msgs / JointTrajectoryPoint”），零（1，Numwaypoints））;[〜，RAMJNTVELWAYPOINTS] =梯度（RAMJNTPOSWAYPOINTS，H）;为了m = 1:numWaypoints jntTrajectoryPoints(m)。位置= rArmJntPosWaypoints (m:);jntTrajectoryPoints (m)。速度= rArmJntVelWaypoints (m:);jntTrajectoryPoints (m)。TimeFromStart = rosduration(时间点(m));结尾％可视化Matlab（R）中的机器人运动和终端执行器轨迹持有在为了j = 1：numwaypoints show（pr2，jntposwaypoints（j，:)，“PreservePlot”、假);exampleHelperShowEndEffectorPos (TWaypoints (:,:, j));drawnow;暂停(0.1);结尾将右臂轨迹发送给机器人rGoalMsg.Trajectory.Points = jntTrajectoryPoints;sendGoalAndWait (rArm rGoalMsg);结尾

总结

将手臂恢复到起始位置。

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

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

rosshutdown
用nodeuri关闭全局节点/ matlab_global_node_03004 http://192.168.116.1:64988/

机器人系统工具箱文档

金宝app