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

这个示例使用:

打开生活的脚本

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

启动PR2 Gazebo仿真

在一个简单的环境中生成PR2(只有一个表格和一个可乐罐)在Gazebo模拟器。按照步骤开始与Gazebo和一个模拟的龟甲机器人(ROS工具箱)来启动露台PR2模拟器从Ubuntu®虚拟机桌面。

从MATLAB®连接到ROS网络

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

ipaddress =“192.168.233.133”;rosinit (ipaddress, 11311);
使用NodeURI http://192.168.233.1:57169/初始化全局节点/matlab_global_node_59258

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

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

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

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

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

disp (rGoalMsg)
轨迹:[1×1个联名轨迹]使用showdetails来显示信息的内容
disp (rGoalMsg.Trajectory)
属性为:MessageType: ' tory_msgs/ joint弹道'报头:[1×1报头]联名名称:{0×1细胞}点:[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 duration对象。

% 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);

使用轨迹中的点创建一个对象数组，并将其分配给目标消息。使用action客户机发送目标。的sendGoalAndWait函数将阻止执行，直到PR2 arm完成执行轨迹

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类计算所有需要的关节位置，这些位置可以通过action客户端作为轨迹目标消息发送。一个rigidBodyTree对象用于定义机器人参数，生成配置，并在MATLAB®中可视化机器人。

执行以下步骤:

  • 从ROS网络中获取PR2机器人的当前状态，并将其赋值给arigidBodyTree目的在MATLAB®中使用机器人。

  • 定义末端执行器目标姿态。

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

  • 利用逆向运动学从目标末端执行器姿态计算关节位置。

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

在MATLAB®中创建一个刚体树

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

pr2 = exampleHelperWGPR2Kinect;

获取当前机器人状态

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

jointSub = rossubscriber (“joint_states”);

获取当前联合状态消息。

jntState =接收(jointSub);

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

jntPos = exampleHelperJointMsgToStruct (pr2, jntState);

可视化当前的机器人配置

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

显示(pr2 jntPos)

ans =坐标轴(主轴)的属性:XLim: [-2] YLim: [-2] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position:[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 + (1 e - 3, 1 e - 3);

创建inverseKinematics对象。

本土知识= inverseKinematics (“RigidBodyTree”pr2);

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

ik.SolverParameters。所有owRandomRestart = false;

为目标姿态的每个部分的公差指定权重。

权重= [0.25 0.25 0.25 1 1 1];initialGuess = jntPos;%目前的jnt pos作为初始猜测

指定与任务相关的末端执行器姿势。在这个例子中，PR2会拿起桌子上的罐子，抓住罐子，把它移到一个不同的位置，然后再把它放下。我们将使用inverseKinematics目的:规划从一个末端执行器姿态平稳过渡到另一个末端执行器姿态的运动。

指定末端执行器的名称。

endEffectorName =“r_gripper_tool_frame”;

指定can的初始(当前)姿态和期望的最终姿态。

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 = T3 * trvec2tform ([-0.1, 0,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;TWaypoints = examplehelperse3弹道(T0, Tf, numWaypoints);末端执行器姿态路径点jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);%关节位置路径点rArmJointNames = rGoalMsg.Trajectory.JointNames;rArmJntPosWaypoints = 0 (numWaypoints, numel(rArmJointNames));使用IK计算每个末端执行器位姿路径点的关节位置为k = 1:numWaypoints jntPos = ik(endEffectorName, TWaypoints(:，:，k)， weights, 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);jnt弹道ypoints = arrayfun(@(~) rosmessage(“trajectory_msgs / JointTrajectoryPoint”),0 (numWaypoints));[~， rArmJntVelWaypoints] =坡度(rArmJntPosWaypoints, h);为m = 1: jnt弹道点的个数(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。点= jntTrajectoryPoints;sendGoalAndWait (rArm rGoalMsg);结束

总结

将手臂移回起始位置。

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

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

rosshutdown
使用NodeURI http://192.168.233.1:57169/关闭全局节点/matlab_global_node_59258
机器人系统工具箱文档
万博1manbetx

尝试MATLAB、Sim万博1manbetxulink和其他产品s manbetx 845

得到审判现在