主要内容

使用ROS动作和反向运动学控制PR2手臂移动

此示例使用：

打开实时脚本

此示例显示了如何在MATLAB®中向机器人操作器发送命令。关节位置命令通过ROS网络通过ROS action客户端发送。此示例还显示了如何计算所需末端执行器位置的关节位置。刚体树定义了机器人几何体和关节约束，与反向kinema一起使用通过tics获取机器人关节位置。然后，您可以将这些关节位置作为轨迹发送，以命令机器人移动。

打开PR2露台模拟

在Gazebo模拟器中，在简单的环境（只有一张桌子和一个可乐罐）中生成PR2开始使用露台和模拟的高跷机器人（ROS工具箱）启动露台PR2模拟器从Ubuntu®虚拟机桌面。

从MATLAB®连接到ROS网络

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

IP地址='192.168.116.161'；罗西尼特（IP地址，11311）；
使用NodeURI初始化全局节点/matlab_全局_节点_03004http://192.168.116.1:64988/

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

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

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

[rArm，rGoalMsg]=rosactionclient(‘r_手臂_控制器/关节_轨迹_动作’); waitForServer（rArm）；

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

副秘书长（rGoalMsg）
ROS JointTrajectoryGoal消息，其属性为：MessageType:'pr2_controllers_msgs/JointTrajectoryGoal'轨迹：[1×1 JointTrajectory]使用showdetails显示消息的内容
显示（rGoalMsg.轨迹）
具有以下属性的ROS JointTrajectory消息：MessageType:'Traction_msgs/JointTrajectory'标头：[1×1标头]点：[0×1 JointTrajectoryPoint]JointNames:{0×1 cell}使用showdetails显示消息的内容

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

rosparam get/r_手臂控制器/关节
rGoalMsg.TRACTION.JointNames={“r_肩_盘_关节”,...‘r_肩部_提升_关节’,...‘r_上臂_滚动_关节’,...‘r_弯头_挠性_接头’,...“前臂旋转关节”,...“r_腕关节\柔性关节”,...“r_腕关节\u滚关节”};

通过创建ROS在手臂关节轨迹中创建设定点轨迹\u msgs/JointTrajectoryPoint消息并将所有7个关节的位置和速度指定为向量。将从开始算起的时间指定为ROS duration对象。

%第1点tjPoint1=rosmessage(“轨迹/连接轨迹点”)tjPoint1.位置=零（1,7）；tjPoint1.速度=零（1,7）；tjPoint1.TimeFromStart=持续时间（1.0）；%第2点tjPoint2=rosmessage(“轨迹/连接轨迹点”)tjPoint2.位置=[-1.0.20.1-1.2-1.5-0.3-0.5]；tjPoint2.速度=零（1,7）；tjPoint2.TimeFromStart=持续时间（2.0）；

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

rGoalMsg.traction.Points=[tjPoint1，tjPoint2]；sendGoalAndWait（rArm，rGoalMsg）；

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

[lArm，lGoalMsg]=rosactionclient('左手臂\控制器/关节\轨迹\动作')；waitForServer（lArm）；lGoalMsg.traction.JointNames={“左肩关节”,...‘l_肩_举_关节’,...“l_上臂_滚动_关节”,...“l_弯头_伸缩接头”,...“前臂旋转关节”,...“l_腕关节_伸缩关节”,...“l_手腕_滚动_关节”};

设置左臂的轨迹点。将其指定给目标消息并发送目标。

tjPoint3=rosmessage(“轨迹/连接轨迹点”)tjPoint3.位置=[1.0.2-0.1-1.2 1.5-0.3 0.5]；tjPoint3.速度=零（1,7）；tjPoint3.TimeFromStart=rosduration（2.0）；lGoalMsg.Traction.Points=tjPoint3；sendGoalAndWait（lArm，lGoalMsg）；

计算末端效应器位置的反向运动学

在本节中，您将根据所需的末端执行器（PR2右夹持器）位置计算关节的轨迹逆线性类计算所有必需的关节位置，这些位置可以作为轨迹目标消息通过操作客户端发送。a刚体树对象用于定义机器人参数、生成配置以及在MATLAB®中可视化机器人。

执行以下步骤：

  • 从ROS网络获取PR2机器人的当前状态，并将其分配给刚体树对象在MATLAB®中使用机器人。

  • 定义末端效应器目标姿势。

  • 使用这些关节位置可视化机器人配置，以确保正确的解决方案。

  • 使用反向运动学从目标末端效应器姿势计算关节位置。

  • 将关节位置的轨迹发送至ROS动作服务器，以命令实际的PR2机器人。

在MATLAB®中创建刚体树

将PR2机器人加载为刚体树对象此对象定义机器人的所有运动学参数（包括关节限制）。

pr2=示例HelperWgpr2Kinect；

获取当前机器人状态

创建订阅服务器以从机器人获取关节状态。

Jointsubscriber=rossubscriber(“联合国”);

获取当前关节状态消息。

jntState=接收（jointSub）；

将关节状态消息中的关节位置指定给pr2对象可以理解。

jntPos=exampleHelperJointMsgToStruct（pr2，jntState）；

可视化当前机器人配置

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

显示（pr2，JNTPO）

ans=轴（主）和属性：XLim:[-2]YLim:[-2]XScale:'linear'YScale:'linear'网格线样式：'-'位置：[0.1300 0.1100 0.7750 0.8150]单位：'normalized'显示所有属性

创建一个逆线性来自pr2机器人对象。反向运动学的目标是计算PR2手臂的关节角度，该手臂将夹持器（即末端执行器）置于所需姿势。一段时间内末端执行器姿势的序列称为轨迹。

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

torsoJoint=pr2.getBody(“躯干提升连杆”).Joint；idx=strcmp（{jntPos.JointName}，torsoJoint.Name）；torsoJoint.HomePosition=jntPos（idx）.JointPosition；torsoJoint.PositionLimits=jntPos（idx）.JointPosition+[-1e-3,1e-3]；

创建逆线性对象

ik=逆线性(“刚体树”，pr2）；

禁用随机重新启动以确保一致的IK解决方案。金宝搏官方网站

ik.SolverParameters.AllowRandomRestart=false；

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

权重=[0.25 0.25 0.25 1]；initialGuess=jntPos；%当前jnt pos作为初始猜测

指定与任务相关的末端效应器姿势。在本例中，PR2将触及桌上的罐，抓住罐，将其移动到其他位置，然后再次将其放下。我们将使用逆线性对象来规划从一个末端效应器姿势平滑过渡到另一个末端效应器姿势的运动。

指定末端效应器的名称。

endEffectorName=“r_夹具_工具_框架”;

指定can的初始（当前）姿势和所需的最终姿势。

TCanInitial=trvec2tform（[0.7,0.0,0.55]）；tcanifinal=trvec2tform（[0.6，-0.5,0.55]）；

抓取时，定义末端效应器和can之间所需的相对变换。

TGraspToCan=trvec2tform（[0,0,0.08]）*eul2tform（[pi/8,0，-pi]）；

定义所需笛卡尔轨迹的关键航路点。罐子应该沿着这个轨迹移动。轨迹可以分解如下：

  • 打开夹持器

  • 将末端效应器从当前姿势移动到当前姿势T1这样机器人在开始抓取时会感到舒适

  • 从中移动末端效应器T1到TGrasp（随时准备抓住）

  • 关闭夹持器并抓取罐

  • 从中移动末端效应器TGrasp到T2（将罐吊在空中）

  • 从中移动末端效应器T2到T3（可以水平移动）

  • 从中移动末端效应器T3到特雷利斯（将罐降至工作台表面并准备释放）

  • 打开夹持器，放开罐头

  • 从中移动末端效应器特雷利斯到T4（回缩臂）

TGrasp=TCanInitial*TGraspToCan；%抓取can时所需的末端效应器姿势T1=TGrasp*trvec2tform（[0,0，-0.1]）；T2=TGrasp*trvec2tform（[0,0，-0.2]）；T3=Tcapfinal*TGraspToCan*trvec2tform（[0,0，-0.2]）；TRelease=Tcapfinal*TGraspToCan；%释放can时所需的末端效应器姿势T4=TRelease*trvec2tform（[-0.1,0.05,0]）；

实际运动将由numWaypoints按顺序确定关节位置，并通过ROS简单动作客户端发送给机器人。这些配置将使用逆线性对象，使末端效应器姿势从初始姿势插值到最终姿势。

执行动议

指定运动的顺序。

motionTask={“释放”，T1，TGrasp，“抓住”，T2，T3，TRelease，“释放”，T4}；

执行中指定的每个任务运动任务一个接一个。使用指定的助手函数发送命令。

对于i=1：长度（运动任务）如果strcmp（motionTask{i}，“抓住”)示例HelperSendPR2Gripper命令(“对”，0.01000，假）；持续终止如果strcmp（motionTask{i}，“释放”)示例HelperSendPR2Gripper命令(“对”，0.1，-1，对）；持续终止Tf=运动任务{i}；%获取当前关节状态jntState=receive（jointSub）；jntPos=exampleHelperJointMsgToStruct（pr2，jntState）；T0=getTransform（pr2，jntPos，endEffectorName）；%关键航路点之间的插值numWaypoints=10；[s、sd、sdd、tvec]=trapveltraj（[01]，numWaypoints，“加速时间”, 0.4);%相对缓慢的上升到最高速度TWaypoints=transformtraj（T0，Tf，[01]，tvec，“时间尺度”[s；sd；sdd]）；%末端效应器姿态航路点jntPosWaypoints=repmat（初始猜测，numWaypoints，1）；%联合位置航路点rArmJointNames=rGoalMsg.traction.JointNames；rArmJntPosWaypoints=零（numWaypoints，numMel（rArmJointNames））；%使用IK计算每个末端效应器姿势航路点的关节位置对于k=1:numWaypoints jntPos=ik（endEffectorName，TWaypoints（：，：，k），权重，初始猜测）；jntPosWaypoints（k，：）=jntPos；initialGuess=jntPos；%从jntPos提取右臂关节位置rArmJointPos=零（大小（RARMJOINTNAME））；对于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=差异（时间点）；h=h（1）；jntTrajectoryPoints=arrayfun（@（~）rosmessage(“轨迹/连接轨迹点”)，零（1，numWaypoints））；[~，rArmJntVelWaypoints]=梯度（rArmJntPosWaypoints，h）；对于m=1:numWaypoints jntTrajectoryPoints（m）。位置=rArmJntPosWaypoints（m，：）；jntTrajectoryPoints（m）.速度=rArmJntVelWaypoints（m，：）；jntTrajectoryPoints（m）.TimeFromStart=rosduration（时间点（m））；终止%在MATLAB（R）中可视化机器人运动和末端执行器轨迹持有在…上对于j=1:numWaypoints显示（pr2，jntPosWaypoints（j，：），“保护区”，false）；例如HelpershowendeEffectorPos（两个点（：，：，j））；drawnow；pause（0.1）；终止%将右臂轨迹发送给机器人rGoalMsg.traction.Points=jntTrajectoryPoints；sendGoalAndWait（rArm、rGoalMsg）；终止

收尾

将手臂移回起始位置。

示例HelperSendPR2Gripper命令(“r”，0.0，-1）rGoalMsg.TRACTION.Points=tjPoint2；sendGoal（rArm、rGoalMsg）；

呼叫罗斯赫顿关闭ROS网络并断开与机器人的连接。

罗斯赫顿
使用NodeURI关闭全局节点/matlab_全局节点_03004http://192.168.116.1:64988/

机器人系统工具箱文档

金宝app