MATLAB轨迹规划 发给ROS中机器人实现仿真运动
2024-05-21 08:45:44
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);% 获取末端的T0T0 = 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 waypointsjntPosWaypoints = 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 IKfor k = 1:numWaypointsjntPos = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);jntPosWaypoints(k,:) = jntPos ;initialGuess = jntPos ;% Extract right arm joint positions from jntPosrArmJointPos = 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 waypointtimePoints = linspace(0,3,numWaypoints); % Estimate joint velocity trajectory numericallyh = 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:numWaypointsjntTrajectoryPoints(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 robotrGoalMsg.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);% 获取末端的T0T0 = 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 waypointsjntPosWaypoints = 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 IKfor k = 1:numWaypointsjntPos = ik(endEffectorName, TWaypoints(:,:,k), weights, initialGuess);jntPosWaypoints(k,:) = jntPos ;initialGuess = jntPos ;% Extract right arm joint positions from jntPosrArmJointPos = 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 waypointtimePoints = linspace(0,3,numWaypoints); % Estimate joint velocity trajectory numericallyh = 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:numWaypointsjntTrajectoryPoints(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 robotrGoalMsg.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)-1idx = strcmp({jntPos.JointName}, jntState.Name{i});jntPos(idx).JointPosition = jntState.Position(i);
endfor i = 1:robot.NumBodiesjoint = 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
MATLAB轨迹规划 发给ROS中机器人实现仿真运动相关推荐
- 【Matlab路径规划】A_star算法机器人栅格地图路径规划【含源码 116期】
一.代码运行视频(哔哩哔哩) [Matlab路径规划]A_star算法机器人栅格地图路径规划[含源码 116期] 二.matlab版本及参考文献 1 matlab版本 2014a 2 参考文献 [1] ...
- 弯曲圆波导matlab_基于MATLAB编程的圆形波导中能流密度分布图仿真
基于 MATLAB 编程的圆形波导中能流密度分布图仿真 王永龙 ; 夏昌龙 ; 刘朋 [期刊名称] <临沂大学学报> [年 ( 卷 ), 期] 2008(030)003 [摘要] 基于 M ...
- 【机械臂轨迹规划】足式机器人足端摆线轨迹曲线
系列文章目录 提示:这里可以添加系列文章的所有文章的目录,目录需要自己手动添加 TODO:写完再整理 文章目录 系列文章目录 前言 一.摆线函数原理 二.摆线函数缺点 总结 参考资料 前言 本文先对摆 ...
- 【ROS】—— 机器人导航(仿真)—导航原理(十七)
文章目录 前言 1. 导航模块简介 1.1 全局地图 1.2 自身定位 1.3 路径规划 1.4 运动控制 1.5 环境感知 2. 导航之坐标系 前言
- 【ROS】—— 机器人导航(仿真)—导航实现(十八)[重要][重要][重要]
文章目录 前言 准备条件 1. 导航实现01_SLAM建图 1.1 gmapping简介 1.2 gmapping节点说明 1.3 gmapping使用 1.3.1 编写gmapping节点相关lau ...
- matlab求系统根轨迹代码_广州数控GSK RH06六轴焊接机器人的轨迹规划
技术参数 运动范围 任务点位置 问题:如实现上图中的 运动轨迹(以基坐标系为参照),请给出相关的轨迹规划算法及其实现方式(结合运动学及动力学方程进行相关的轨迹点选取及工程实现),其中各个点的坐标为A= ...
- 机器人轨迹规划（熊友伦）
机器人轨迹规划(熊友伦) http://blog.csdn.net/jyc1228/article/details/3991881 http://blog.csdn.net/wx545644217/a ...
- 六自由度机器人（机械臂）运动学建模及运动规划系列（四）——轨迹规划
本篇目录 一.轨迹规划概述 二.关节空间规划 1. 点对点规划 2. 多节点规划 示例程序 三.笛卡尔空间规划 1. 速度规划 2. 位置规划 3. 姿态插补 4. 基于几何解法的一种简化位置规划方法 ...
- 六轴机器人轨迹规划（直线轨迹规划，弧线轨迹规划）——C#实现+ABB为例（规划直接下发离线程序运动）
机器人直线插补算法,弧线插补算法+离线编程转换(空间直线插补规划.空间弧线插补规划.离线编程.ABB二次开发.六轴机器人控制.C#) 一,通过对空间点的插补,形成空间点轨迹 1.插补算法原理简述: ( ...
最新文章
- 0基础学怎么学习python
- 自动化测试框架的隐藏小细节，你都知道么
- 以前写的一点东西，放上来吧。否则就扔掉了
- Linux /etc/init.d目录
- 在.net 2.0中使用了masterpager 重写WebForm_OnSubmit()
- Python学习笔记：Day 7 编写MVC
- 你面对以希望为名的绝望微笑
- win10微软图标点击无反应_win10点击任务栏没反应？这样操作就可以了
- LeetCode 1864. 构成交替字符串需要的最小交换次数
- 【转】关于PHP的header(P3P: CP=CURa……)
- matlab设计一个简单图像直方图均衡的GUI程序
- java 反射 接口_Java 怎么通过反射获取并实现这个类里面的接口，并且实现接口中的方法...
- 三、用python实现平稳时间序列的建模
- Smart3D系列教程3之 《论照片三维重建中Smart3D几个工作模块的功能意义》
- Centos7/8 Oracle11g R2 图形化部署
- 彻底删除VMware ！！！
- 兰州理工大学计算机考研好考吗,兰州理工大学考研难吗
- 利用ISA防火墙实现安全快速上网
- 微信小程序实现封装处理
- laravel5.4中orm中的wherein与多条件查询