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

本例使用：

打开生活的脚本

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

打开PR2凉亭模拟

衍生PR2在一个简单的环境(只有一个表和可乐罐)在露台模拟器。执行先从Gazebo和一个模拟的TurtleBot开始（ROS工具箱）启动露台PR2模拟器从Ubuntu桌面®虚拟机。

连接到从MATLAB®ROS网络

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

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

对于机械手创建行动客户端和发送命令

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

创建一个ROS simple action客户端来发送目标信息来移动机器人的右臂。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 joint弹道]使用showdetails来显示消息的内容
disp (rGoalMsg.Trajectory)
ROS的联合弹道信息具有如下属性:MessageType:‘trajectory_msgs/ joint弹道’Header:[1×1 Header]联合名称:{0×1 cell} Points:[0×1 JointTrajectoryPoint]使用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.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3 -0.5];tjPoint2.Velocities =零（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 -0.3 1.5 0.5];tjPoint3.Velocities =零（1,7）;tjPoint3.TimeFromStart = rosduration（2.0）;lGoalMsg.Trajectory.Points = tjPoint3;sendGoalAndWait（LARM，lGoalMsg）;

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

在本节中，您将根据所需的末端执行器(PR2右夹持器)位置计算关节的轨迹。该inverseKinematics类计算所有需要的关节位置，这些位置可以通过action客户机作为轨迹目标消息发送。一个rigidBodyTree对象用于定义机器人参数，生成配置，并在MATLAB®中可视化机器人。

执行以下步骤:

  • 从ROS网络获取PR2机器人的当前状态，并将其分配给arigidBodyTree对象在MATLAB®中与机器人一起工作。

  • 定义末端执行目标姿势。

  • 使用这些关节位置以确保适当的溶液可视化的机器人配置。

  • 使用反向运动学来计算目标末端执行器的姿态共同立场。

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

创建一个刚体树MATLAB®

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

pr2 = exampleHelperWGPR2Kinect;

获取当前的国家机器人

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

jointSub = rossubscriber (“joint_states”);

获取当前联合状态消息。

jntState =接收(jointSub);

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

jntPos = exampleHelperJointMsgToStruct (pr2, jntState);

可视化当前机器人配置

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

秀（PR2，jntPos）

属性:XLim: [-2] YLim: [-2] XScale: 'linear' YScale: 'linear' GridLineStyle: '-' Position: [0.1300 0.1100 0.7750 0.8150] Units: '归一化'显示所有属性

创建一个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;

指定目标姿态的每个部件上的公差的重量。

weights = [0.25 0.25 0.25 11 11 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，-pi]）;

定义所需的笛卡尔轨迹的关键路径点。罐子应该沿着这条轨迹移动。可以将轨迹进行如下分解:

  • 打开夹

  • 从当前姿势移动末端执行器T1使机器人会觉得舒服发起把握

  • 移动末端执行器T1来TGrasp（随时掌握）

  • 关闭夹具和抢CAN

  • 移动末端执行器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，TRUE）;继续结束如果比较字符串(motionTask {},'发布') exampleHelperSendPR2GripperCommand (“对”、0.1、1、真实);继续结束Tf = motionTask {};％获取当前的结合状态jntState =接收(jointSub);jntPos = exampleHelperJointMsgToStruct(pr2, jntState);getTransform(pr2, jntPos, endEffectorName);在关键路径点之间插入numWaypoints = 10;TWaypoints = examplehelperse3弹道(T0, Tf, numWaypoints);末端执行器姿态路径点%jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);%关节位置路点rArmJointNames = rGoalMsg.Trajectory.JointNames;rArmJntPosWaypoints =零（numWaypoints，numel（rArmJointNames））;%使用IK计算每个末端执行器位姿路径点的关节位置对于K = 1：numWaypoints jntPos = IK（endEffectorName，TWaypoints（：，：，k）时，权重，initialGuess）;jntPosWaypoints（K，:) = jntPos;initialGuess = jntPos;％提取右手臂从jntPos关节位置rArmJointPos =零（大小（rArmJointNames））;对于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（时间点）;H = H（1）;jntTrajectoryPoints = arrayfun（@（〜）rosmessage（“trajectory_msgs / JointTrajectoryPoint”），零（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，:)，“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

机器人系统工具箱文档

金宝app