MATLAB轨迹规划 发给ROS中机器人实现仿真运动
现象如图所示：
0、matlab 与 ROS 通信：
https://blog.csdn.net/qq_40569926/article/details/112162871
指定matlab路径：连接三句话
pe = pyenv('Version','D:\python2.7.18\python.exe');%多个python 版本可以用此指定
% 下面四行第一次运行时使用
rosshutdown
setenv('ROS_MASTER_URI','http://192.168.93.131:11311');
% setenv('ROS_IP','202.193.75.81');
%Starting ROS MASTER
rosinit();
当matlab多个版本的时候指定python的版本很重要：
pe = pyenv('Version','D:\python2.7.18\python.exe')
rosinit
1、一个轨迹点的运动：
%% 发送目标消息来移动机器人的手臂
%rosaction list 查看
%等待客户端连接到 ros 操作服务器
[rArm, rGoalMsg] = rosactionclient('/probot_elfin/arm_joint_controller/follow_joint_trajectory');
waitForServer(rArm);
% disp(rGoalMsg.Trajectory)
disp(rGoalMsg)
%ubuntu 通过 rosparam get /probot_elfin/arm_joint_controller/joints
%查看joints的名称
%设置机器人 joints的名称
rGoalMsg.Trajectory.JointNames={'elfin_joint1', ...
'elfin_joint2', ...
'elfin_joint3', ...
'elfin_joint4',...
'elfin_joint5',...
'elfin_joint6'};
% 通过创建 rostrajectory_msgs/JointTrajectoryPoint
% 消息并指定所有7个关节的位置和速度作为矢量, 在臂关节轨迹中创建设定值。将开始时的时间指定为 ros 持续时间对象。
% Point 1
tjPoint1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint1.Positions = zeros(1,6);%六个关节位置都为0
tjPoint1.Velocities = zeros(1,6);%速度位1
tjPoint1.Accelerations=zeros(1,6);%加速度1
tjPoint1.TimeFromStart = rosduration(1.0);
% Point 2
tjPoint2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
tjPoint2.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3];%六个关节位置
tjPoint2.Velocities = zeros(1,6);
tjPoint2.Accelerations=zeros(1,6);
tjPoint2.TimeFromStart = rosduration(2.0);
rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2]; %手臂轨迹点从Point1到Point2
sendGoalAndWait(rArm,rGoalMsg); % 发送消息后右臂移动
2、acro 变换为 urdf 模型：
acro 变换为 urdf 模型 这个在Ubuntu 中进行操作：
rosrun xacro xacro elfin10.urdf.xacro > elfin10.urdf --inorder
3、导入URDF 模型后获取机器人的转态：
elfin3 = importrobot('E:\MATLAB_MIN\ROS_MATLAB\elfin_description\urdf\elfin5.urdf');
% % show(elfin3);
% % showdetails(elfin3);
% %% 获取机器人手臂的状态
% 创建一个订阅者, 从机器人中获取关节状态。
jointSub = rossubscriber('joint_states');
% % % 获取当前的联合状态消息。
jntState = receive(jointSub);
% % 将联合状态从联合状态消息分配到机器人所理解的配置结构的字段。
jntPos =JointMsgToStruct(elfin3,jntState);
% 显示状态
%% 可视化当前机器人配置(从虚拟机得到当前的机器人状态，用Show可视化)
show(elfin3,jntPos);
如图所示：
4、创建对象并定义位姿
%% 创建InverseKinematics对象：
ik = robotics.InverseKinematics('RigidBodyTree', elfin3);
%% 禁用随机重新启动以确保安全解决方案一致：
ik.SolverParameters.AllowRandomRestart = false;
%% 为目标姿势的每个组件上的公差指定权重。
weights = [1 1 1 1 1 1];
initialGuess = jntPos; % current jnt pos as initial guess
endEffectorName = 'elfin_end_link';% 末端的link
%% 指定与任务相关的末端效应器姿势。
% 没有末端执行器 忽略
% % % % 指定末端执行器的名称。
% % % 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([pi/8,0,-pi]);
%% 按照特定轨迹进行运动
% 建立运动学模型
% elfin=robot('elfin');
% 机器人初始位姿
xoy = [0.1845 0.03631 0.353 ]; %位置
rpy = [0 pi/2 0]; %姿态
home = [0.266 0 0.7015];
start_home = home;
end_home = xoy;
%定义位姿
shome_T = trvec2tform(start_home);
end_T = trvec2tform(end_home)*eul2tform(rpy);
Tf=end_T;
5、轨迹规划
%% 执行动作
% 获取当前的联合状态
jntState = receive(jointSub);
jntPos = JointMsgToStruct(elfin3,jntState);
% 获取末端的T0
T0 = getTransform(elfin3,jntPos,endEffectorName);
%在关键航路点之间进行插值的百分比
numWaypoints = 10;%插值个数
t = [0 1];
[s,sd,sdd,tvec] = trapveltraj(t,numWaypoints,'AccelTime',0.4);%相对缓慢
TWaypoints = transformtraj(T0, Tf, [0 1], tvec, 'TimeScaling', [s; sd; sdd]); % end-effector pose waypoints
% joint position waypoints
jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);
%使用IK为k = 1：numWaypoints计算每个末端执行器姿势航路点的关节位置
rArmJointNames = rGoalMsg.Trajectory.JointNames;
rArmJntPosWaypoints = zeros(numWaypoints, numel(rArmJointNames));
% Calculate joint position for each end-effector pose waypoint using IK
for k = 1:numWaypoints
jntPos = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);
jntPosWaypoints(k,:) = jntPos ;
initialGuess = jntPos ;
% Extract right arm joint positions from jntPos
rArmJointPos = zeros(size(rArmJointNames));
for n = 1:length(rArmJointNames)
rn = rArmJointNames{n};
idx = strcmp({jntPos.JointName}, rn);
rArmJointPos(n) = jntPos(idx).JointPosition;
end
rArmJntPosWaypoints(k,:) = rArmJointPos';
end
% Time points corresponding to each waypoint
timePoints = linspace(0,3,numWaypoints);
% Estimate joint velocity trajectory numerically
h = diff(timePoints); h = h(1);
[~, rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);
6、轨迹发送
%更新轨迹点
jntTrajectoryPoints = arrayfun(@(~) rosmessage('trajectory_msgs/JointTrajectoryPoint'), zeros(1,numWaypoints));
% jntTrajectoryPoints = repmat(rosmessage('trajectory_msgs/JointTrajectoryPoint'),1,numWaypoints);
A = jntTrajectoryPoints.Positions;
for m = 1:numWaypoints
jntTrajectoryPoints(m).Positions = rArmJntPosWaypoints(m,:);
jntTrajectoryPoints(m).Velocities = rArmJntVelWaypoints(m,:);
jntTrajectoryPoints(m).TimeFromStart = rosduration(timePoints(m));
end
% Visualize robot motion and end-effector trajectory in MATLAB(R)
hold on
% for j = 1:numWaypoints
% show(elfin3, jntPosWaypoints(j,:),'PreservePlot', false);
% ShowEndEffectorPos(TWaypoints(:,:,j));
% drawnow;
% pause(0.1);
% end
% Send the right arm trajectory to the robot
rGoalMsg.Trajectory.Points = jntTrajectoryPoints;
disp(rGoalMsg.Trajectory)
sendGoalAndWait(rArm, rGoalMsg);
8、完整代码：
% clear
clc
% 指定matlab路径：连接三句话
% pe = pyenv('Version','D:\python2.7.18\python.exe');
% % setenv('ROS_MASTER_URI','http://202.193.75.81:11311')
% % rosinit
% % Setting ROS_MASTER_URI
% % 下面四行第一次运行时使用
% rosshutdown
% setenv('ROS_MASTER_URI','http://192.168.93.131:11311');
% % setenv('ROS_IP','202.193.75.81');
% %Starting ROS MASTER
% rosinit();
%% 发送目标消息来移动机器人的手臂
%rosaction list 查看
%等待客户端连接到 ros 操作服务器
[rArm, rGoalMsg] = rosactionclient('/probot_elfin/arm_joint_controller/follow_joint_trajectory');
waitForServer(rArm);
% disp(rGoalMsg.Trajectory)
% disp(rGoalMsg)
%ubuntu 通过 rosparam get /probot_elfin/arm_joint_controller/joints
%查看joints的名称
%设置机器人 joints的名称
rGoalMsg.Trajectory.JointNames={'elfin_joint1', ...
'elfin_joint2', ...
'elfin_joint3', ...
'elfin_joint4',...
'elfin_joint5',...
'elfin_joint6'};
% % 通过创建 rostrajectory_msgs/JointTrajectoryPoint
% % 消息并指定所有7个关节的位置和速度作为矢量, 在臂关节轨迹中创建设定值。将开始时的时间指定为 ros 持续时间对象。
% % Point 1
% tjPoint1 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
% tjPoint1.Positions = zeros(1,6);%六个关节位置都为0
% tjPoint1.Velocities = zeros(1,6);%速度位1
% tjPoint1.Accelerations=zeros(1,6);%加速度1
% tjPoint1.TimeFromStart = rosduration(1.0);
% % Point 2
% tjPoint2 = rosmessage('trajectory_msgs/JointTrajectoryPoint');
% tjPoint2.Positions = [-1.0 0.2 0.1 -1.2 -1.5 -0.3];%六个关节位置
% tjPoint2.Velocities = zeros(1,6);
% tjPoint2.Accelerations=zeros(1,6);
% tjPoint2.TimeFromStart = rosduration(2.0);
% rGoalMsg.Trajectory.Points = [tjPoint1,tjPoint2]; %手臂轨迹点从Point1到Point2
% sendGoalAndWait(rArm,rGoalMsg); % 发送消息后右臂移动
%% acro 变换为 urdf 模型
% rosrun xacro xacro elfin10.urdf.xacro > elfin10.urdf --inorder
%% matlab 导入机器人urdf 模型
% elfin3 = importrobot('E:\MATLAB_MIN\ROS_MATLAB\elfin_description\urdf\elfin5.urdf');
% % show(elfin3);
% % showdetails(elfin3);
% %% 获取机器人手臂的状态
% 创建一个订阅者, 从机器人中获取关节状态。
jointSub = rossubscriber('joint_states');
% % % 获取当前的联合状态消息。
jntState = receive(jointSub);
% % 将联合状态从联合状态消息分配到机器人所理解的配置结构的字段。
jntPos =JointMsgToStruct(elfin3,jntState);
% 显示状态
%% 可视化当前机器人配置(从虚拟机得到当前的机器人状态，用Show可视化)
% show(elfin3,jntPos);
%% 创建robotics.InverseKinematics pr2机器人对象的逆变运动学对象。
% 逆运动学的目的是计算 pr2 臂的关节角度, 将夹持器 (即末端执行器) 置于所需的姿势。
% 在一段时间内, 一系列的末端效应器被称为轨迹。
% 因此, 在规划过程中, 我们对抬起关节设置了严格的限制。
% 没有抬起关节不需要
% torsoJoint = elfin3.getBody('elfin_link4').Joint
% idx = strcmp({jntPos.JointName}, torsoJoint.Name);
% torsoJoint.HomePosition = jntPos(idx).JointPosition;
% torsoJoint.PositionLimits = jntPos(idx).JointPosition + [-1e-3,1e-3];
%% 创建InverseKinematics对象：
ik = robotics.InverseKinematics('RigidBodyTree', elfin3);
%% 禁用随机重新启动以确保安全解决方案一致：
ik.SolverParameters.AllowRandomRestart = false;
%% 为目标姿势的每个组件上的公差指定权重。
weights = [1 1 1 1 1 1];
initialGuess = jntPos; % current jnt pos as initial guess
endEffectorName = 'elfin_end_link';% 末端的link
%% 指定与任务相关的末端效应器姿势。
% 没有末端执行器 忽略
% % % % 指定末端执行器的名称。
% % % 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([pi/8,0,-pi]);
%% 按照特定轨迹进行运动
% 建立运动学模型
% elfin=robot('elfin');
% 机器人初始位姿
xoy = [0.1845 0.03631 0.353 ]; %位置
rpy = [0 pi/2 0]; %姿态
home = [0.266 0 0.7015];
start_home = home;
end_home = xoy;
%定义位姿
shome_T = trvec2tform(start_home);
end_T = trvec2tform(end_home)*eul2tform(rpy);
Tf=end_T;
%% 执行动作
% 获取当前的联合状态
jntState = receive(jointSub);
jntPos = JointMsgToStruct(elfin3,jntState);
% 获取末端的T0
T0 = getTransform(elfin3,jntPos,endEffectorName);
%在关键航路点之间进行插值的百分比
numWaypoints = 10;%插值个数
t = [0 1];
[s,sd,sdd,tvec] = trapveltraj(t,numWaypoints,'AccelTime',0.4);%相对缓慢
TWaypoints = transformtraj(T0, Tf, [0 1], tvec, 'TimeScaling', [s; sd; sdd]); % end-effector pose waypoints
% joint position waypoints
jntPosWaypoints = repmat(initialGuess, numWaypoints, 1);
%使用IK为k = 1：numWaypoints计算每个末端执行器姿势航路点的关节位置
rArmJointNames = rGoalMsg.Trajectory.JointNames;
rArmJntPosWaypoints = zeros(numWaypoints, numel(rArmJointNames));
% Calculate joint position for each end-effector pose waypoint using IK
for k = 1:numWaypoints
jntPos = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);
jntPosWaypoints(k,:) = jntPos ;
initialGuess = jntPos ;
% Extract right arm joint positions from jntPos
rArmJointPos = zeros(size(rArmJointNames));
for n = 1:length(rArmJointNames)
rn = rArmJointNames{n};
idx = strcmp({jntPos.JointName}, rn);
rArmJointPos(n) = jntPos(idx).JointPosition;
end
rArmJntPosWaypoints(k,:) = rArmJointPos';
end
% Time points corresponding to each waypoint
timePoints = linspace(0,3,numWaypoints);
% Estimate joint velocity trajectory numerically
h = diff(timePoints); h = h(1);
[~, rArmJntVelWaypoints] = gradient(rArmJntPosWaypoints, h);
%更新轨迹点
jntTrajectoryPoints = arrayfun(@(~) rosmessage('trajectory_msgs/JointTrajectoryPoint'), zeros(1,numWaypoints));
% jntTrajectoryPoints = repmat(rosmessage('trajectory_msgs/JointTrajectoryPoint'),1,numWaypoints);
A = jntTrajectoryPoints.Positions;
for m = 1:numWaypoints
jntTrajectoryPoints(m).Positions = rArmJntPosWaypoints(m,:);
jntTrajectoryPoints(m).Velocities = rArmJntVelWaypoints(m,:);
jntTrajectoryPoints(m).TimeFromStart = rosduration(timePoints(m));
end
% Visualize robot motion and end-effector trajectory in MATLAB(R)
hold on
% for j = 1:numWaypoints
% show(elfin3, jntPosWaypoints(j,:),'PreservePlot', false);
% ShowEndEffectorPos(TWaypoints(:,:,j));
% drawnow;
% pause(0.1);
% end
% Send the right arm trajectory to the robot
rGoalMsg.Trajectory.Points = jntTrajectoryPoints;
disp(rGoalMsg.Trajectory)
sendGoalAndWait(rArm, rGoalMsg);
9、几个函数
ShowEndEffectorPos
function ShowEndEffectorPos( T )
%EXAMPLEHELPERSHOWENDEFFECTORPOS Plot end-effector position
% Copyright 2016 The MathWorks, Inc.
s = 0.005;
[X0,Y0,Z0]=sphere;
X = s*X0 + T(1,4);
Y = s*Y0 + T(2,4);
Z = s*Z0 + T(3,4);
surf(X, Y, Z, 'facecolor','r', 'linestyle','none');
end
JointMsgToStruct
function jntPos = JointMsgToStruct(robot,jntState)
% exampleHelperJointMsgToStruct The order of body names in the received
% jntState message is different from that the pr2 model in MATLAB would
% expect. This function is to provide a convenient conversion.
% Copyright 2016 The MathWorks, Inc.
jntPos = robot.homeConfiguration;
% 根据jntState.Name的实际个数改正
for i = 1:length(jntState.Name)-1
idx = strcmp({jntPos.JointName}, jntState.Name{i});
jntPos(idx).JointPosition = jntState.Position(i);
end
for i = 1:robot.NumBodies
joint = robot.Bodies{i}.Joint;
if ~strcmp(joint.Type,'fixed')
idx = strcmp({jntPos.JointName}, joint.Name);
jntPos(idx).JointPosition = max(min(jntPos(idx).JointPosition, joint.PositionLimits(2)), joint.PositionLimits(1));
end
end
end
例子：
https://download.csdn.net/download/qq_40569926/18233484
10、博客内容主要来源 mathwork 官网：
https://www.mathworks.com/help/robotics/ug/control-pr2-arm-movements-using-actions-and-ik.html#PR2ManipulationExample-5
https://blog.csdn.net/weixin_39090239/article/details/84378770
9778
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
