这个例子展示了如何在MATLAB®中发送命令给机器人。关节位置命令通过ROS网络通过ROS动作客户端发送。这个例子也展示了如何计算一个期望的末端执行器位置的关节位置。刚体树定义了机器人的几何和关节约束条件,并结合运动学逆解得到机器人的关节位置。然后你可以把这些关节位置作为一个轨迹来命令机器人移动。
在Gazebo模拟器的一个简单环境中生成PR2(只有一个桌子和一个可乐罐)。请按照开始使用凉亭和模拟乌龟机器人(ROS工具箱)启动露台PR2模拟器
从Ubuntu®虚拟机桌面。
在主机上的MATLAB实例中,运行以下命令在MATLAB中初始化ROS全局节点,并通过其IP地址连接到虚拟机中的ROS主节点ipaddress
.将'192.168.233.133'替换为虚拟机的IP地址。如果需要,请指定端口。
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 / joint_trajectory_action”);waitForServer (rArm);
目标信息是trajectory_msgs / JointTrajectoryPoint
消息。每个轨迹点应该指定关节的位置和速度。
disp (rGoalMsg)
ROS JointTrajectory: [1×1 JointTrajectory]使用showdetails显示消息的内容
disp (rGoalMsg.Trajectory)
ROS JointTrajectory message with properties: MessageType: 'trajectory /JointTrajectory' Header: [1×1 Header] Points: [0×1 JointTrajectory point] JointNames: {0×1 cell}使用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.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。position = [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.Points = tjPoint3;sendGoalAndWait (lArm lGoalMsg);
在本节中,您将根据所需的末端执行器(PR2右夹持器)位置计算关节的轨迹。的inverseKinematics
类计算所有所需的关节位置,这些位置可以通过操作客户端作为轨迹目标消息发送。一个rigidBodyTree
对象用于定义机器人参数,生成构型,并在MATLAB®中可视化机器人。
执行以下步骤:
从ROS网络获取PR2机器人的当前状态,并将其分配给arigidBodyTree
在MATLAB®中与机器人一起工作。
定义一个末端执行器目标位姿。
使用这些关节位置可视化机器人配置,以确保正确的解决方案。
使用逆运动学从目标末端执行器的姿态来计算关节位置。
将关节位置的轨迹发送给ROS动作服务器来指挥实际的PR2机器人。
加载一个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]);
定义当抓取时末端执行器与can之间的期望相对变换。
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
一个接一个。使用指定的helper函数发送命令。
为i = 1: length(运动任务)如果比较字符串(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;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: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);jttrajectory = arrayfun(@(~))“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中可视化机器人运动和末端执行器轨迹(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 http://192.168.116.1:64988/关闭全局节点/matlab_global_node_03004