主要内容

使用ROS PR2手臂运动的控制行为和逆运动学

这个示例使用:

打开生活的脚本

这个例子展示了如何在MATLAB®命令发送给机器人机械手。关节位置命令发送通过活性氧ROS网络客户采取行动。这个示例还展示了如何计算联合职位所需的末端执行器的位置。刚体树定义了机器人几何和关节约束,这是使用逆运动学的机器人关节位置。你可以把这些共同的职位是命令机器人移动的轨迹。

提出PR2露台模拟

产卵PR2在简单的环境中(只有一个表和一个可乐罐)在露台模拟器。遵循步骤开始使用露台和一个模拟TurtleBot(ROS工具箱)启动露台PR2模拟器从Ubuntu桌面®虚拟机。

从MATLAB®ROS连接网络

在MATLAB在主机实例,运行以下命令初始化ROS全球节点在MATLAB和连接到ROS主虚拟机通过其IP地址ipaddress。“192.168.233.133”替换为您的虚拟机的IP地址。指定端口(如果需要)。

ipaddress =“192.168.116.161”;rosinit (ipaddress, 11311);
初始化全局节点/ matlab_global_node_03004 NodeURI http://192.168.116.1:64988

创建行动客户机器人手臂和发送命令

在这个任务中,你发送联合轨迹PR2的胳膊。武器可以通过控制ROS的行动。关节轨迹是手动为每个手臂和作为单独的发送目标指定消息通过客户单独行动。

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

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

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

disp (rGoalMsg)
ROS JointTrajectoryGoal消息与属性:MessageType:“pr2_controllers_msgs / JointTrajectoryGoal”轨迹:[1×1 JointTrajectory]使用showdetails显示消息的内容
disp (rGoalMsg.Trajectory)
ROS JointTrajectory消息与属性:MessageType:“trajectory_msgs / JointTrajectory”标题:[1×1头]指出:[0×1 JointTrajectoryPoint] JointNames:{0×1细胞}使用showdetails显示消息的内容

设置关节名称匹配的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定位点的手臂关节轨迹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。点= [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。点= tjPoint3;sendGoalAndWait (lArm lGoalMsg);

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

在本节中,您计算基于所需的关节轨迹末端执行器(PR2右爪)的位置。的inverseKinematics类计算所需的所有关节的位置,它可以发送信息通过行动轨迹目标客户。一个rigidBodyTree对象是用于定义机器人参数,生成配置,和想象中的机器人MATLAB®。

执行以下步骤:

  • 得到当前状态的PR2机器人ROS网络,并将其分配给一个rigidBodyTree对象使用MATLAB®的机器人。

  • 定义一个末端执行器的目标构成。

  • 可视化的机器人配置使用这些关节的位置,以确保一个适当的解决方案。

  • 使用逆运动学计算联合职位目标末端执行器构成。

  • 关节的轨迹位置发送到服务器命令实际的PR2机器人ROS行动。

在MATLAB®创建一个刚体树

负载的PR2机器人作为rigidBodyTree对象。该对象定义了所有的运动参数(包括联合限制)的机器人。

pr2 = exampleHelperWGPR2Kinect;

获取当前机器人的状态

创建一个用户联合州机器人。

jointSub = rossubscriber (“joint_states”);

得到当前联合状态消息。

jntState =接收(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的手臂的关节角,爪(即末端执行器)在一个理想的姿势。一个序列的末端执行器构成在一段时间内被称为轨迹。

在本例中,我们只会控制机器人的手臂。因此,我们严格限制期间torso-lift联合规划。

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.SolverParameters。所有owRandomRestart = 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([π/ 8,0,-π]);

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

  • 打开夹

  • 从目前对末端执行器的移动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 * TGraspToCan;%的末端执行器构成时释放T4 = TRelease * trvec2tform ((-0.1, 0.05, 0));

实际的运动将被指定numWaypoints联合在一个序列和发送到机器人通过ROS简单操作客户端。这些配置将选择使用inverseKinematics对象,这样最终效应构成是插值从最初对最后的姿势。

执行这个动作

指定的顺序动作。

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

执行每个任务中指定motionTask一个接一个。发送命令使用指定的辅助功能。

为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;[年代,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元素个数(rArmJointNames));%计算关节位置为每个末端执行器构成使用反向路径为k = 1: numWaypoints jntPos =本土知识(endEffectorName, TWaypoints (:,:, k)、重量、initialGuess);:jntPosWaypoints (k) = jntPos;initialGuess = jntPos;%从jntPos提取右臂关节位置rArmJointPos = 0(大小(rArmJointNames));为n = 1:长度(rArmJointNames) rn = rArmJointNames {n};idx = strcmp ({jntPos。JointName}, rn);rArmJointPos (n) = jntPos (idx) .JointPosition;结束:rArmJntPosWaypoints (k) = rArmJointPos ';结束%时间点对应于每一个路标时间点= linspace (0、3、numWaypoints);%估计关节速度轨迹数值h = diff(时间点);h = h (1);jntTrajectoryPoints = arrayfun (@ (~) rosmessage (“trajectory_msgs / JointTrajectoryPoint”),0 (numWaypoints));[~,rArmJntVelWaypoints] =梯度(rArmJntPosWaypoints h);为m = 1: numWaypoints jntTrajectoryPoints (m)。位置= rArmJntPosWaypoints (m:);jntTrajectoryPoints (m)。速度= rArmJntVelWaypoints (m:);jntTrajectoryPoints (m)。TimeFromStart = rosduration(时间点(m));结束%在MATLAB可视化机器人末端执行器运动轨迹(右)持有在为j = 1: numWaypoints显示(pr2 jntPosWaypoints (j:)“PreservePlot”、假);exampleHelperShowEndEffectorPos (TWaypoints (:,:, j));drawnow;暂停(0.1);结束%将右手臂机器人轨迹rGoalMsg.Trajectory。点= jntTrajectoryPoints;sendGoalAndWait (rArm rGoalMsg);结束

总结

移动手臂回到起始位置。

exampleHelperSendPR2GripperCommand (“r”,0.0,1)rGoalMsg.Trajectory。点= tjPoint2;sendGoal (rArm rGoalMsg);

调用rosshutdown从机器人关机ROS网络和断开。

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

机器人系统工具箱文档

金宝app