1 笛卡尔空间圆弧轨迹规划

之前学习过笛卡尔空间下轨迹规划API：(plan, fraction) = arm.compute_cartesian_path
返回值：
plan：规划出来的运动轨迹
fraction：描述规划成功的轨迹在给定路径点列表的覆盖率【0~1】。如果fraction小于1，说明给定的路径点列表没有办法完整规划

如果将圆弧轨迹微分为一段段的小直线段，就能通过调用上述API构建出圆弧轨迹。

#include<math.h>
#include<ros/ros.h>
#include<moveit/move_group_interface/move_group_interface.h>
#include<moveit/robot_trajectory/robot_trajectory.h>int main(int argc, char **argv)
{ros::init(argc,argv,"moveit_circle_demo");ros::AsyncSpinner spinner(1);spinner.start();moveit::planning_interface::MoveGroupInterface arm("xmate_arm");std::string end_effector_link = arm.getEndEffectorLink();std::string reference_frame = "base_link";arm.setPoseReferenceFrame(reference_frame);//当运动规划失败后，允许重新规划arm.allowReplanning(true);//设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.setGoalPositionTolerance(0.001);arm.setGoalOrientationTolerance(0.01);//设置允许的最大速度和加速度arm.setMaxAccelerationScalingFactor(0.8);arm.setMaxVelocityScalingFactor(0.8);// 控制机械臂先回到初始化位置arm.setNamedTarget("home");arm.move();sleep(1);// 设置机器人终端的目标位置geometry_msgs::Pose target_pose;//设定圆心的位置target_pose.orientation.x = 0.14858;target_pose.orientation.y = 0.73236;target_pose.orientation.z = -0.03444;target_pose.orientation.w = 0.66362;target_pose.position.x = -0.050308;target_pose.position.y = -0.039851;target_pose.position.z = 0.78264;arm.setPoseTarget(target_pose);arm.move();//首先运动到圆心的位置sleep(1);std::vector<geometry_msgs::Pose> waypoints;//将初始位姿加入路点列表waypoints.push_back(target_pose);double centerA = target_pose.position.x;double centerB = target_pose.position.z;double radius = 0.13;for(double th=0.0; th<6.28; th=th+0.01){target_pose.position.y = centerA + radius * cos(th);target_pose.position.z = centerB + radius * sin(th);waypoints.push_back(target_pose);}// 笛卡尔空间下的路径规划moveit_msgs::RobotTrajectory trajectory;const double jump_threshold = 0.0;const double eef_step = 0.01;double fraction = 0.0;int maxtries = 100;   //最大尝试规划次数int attempts = 0;     //已经尝试规划次数while(fraction < 1.0 && attempts < maxtries){fraction = arm.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);attempts++;if(attempts % 10 == 0)ROS_INFO("Still trying after %d attempts...", attempts);}if(fraction == 1){   ROS_INFO("Path computed successfully. Moving the arm.");// 生成机械臂的运动规划数据moveit::planning_interface::MoveGroupInterface::Plan plan;plan.trajectory_ = trajectory;// 执行运动arm.execute(plan);sleep(1);}else{ROS_INFO("Path planning failed with only %0.6f success after %d attempts.", fraction, maxtries);}// 控制机械臂先回到初始化位置arm.setNamedTarget("home");arm.move();sleep(1);ros::shutdown(); return 0;
}

2 轨迹重定义（修改moveit生成的轨迹数据）

在很多时候会发现 Moveit 规划出的轨迹数据并不一定满足我们的需求。比如速度、加速度、时间、位置等，这时就需要我们在 plan 和 excute 之间对这条轨迹进行重新的修改和定义，之后再执行就会变成我们想要的。

问题的关键就在于我们怎么样才能对轨迹进行修改，主要就是这个函数，函数的入口参数为：

movieit 规划后的轨迹数据 moveit::planning_interface::MoveGroupInterface::Plan &plan
一个是速度的尺度因子 double scale

想要对轨迹数据进行修改的话，就必须首先了解这个 plan 的数据结构，了解 trajectory 的这些 points 是如何保存的，这就需要查找 moveit 的官方 API 文档了。
复制moveit::planning_interface::MoveGroupInterface::Plan到浏览器，查看官方API 文档：moveit::planning_interface::MoveGroup Class Reference
点击 plan
发现 plan 有三个公共属性，复制 moveit_msgs::RobotTrajectory到浏览器，查看这个数据结构：moveit_msgs/RobotTrajectory Message
一般情况下用的是第一个，所以，进入第一个
里边包含关节的名字，还有轨迹点，每一个路经点包含每一个关节的位置、速度、加速度和时间，点进去
到此 Plan 的数据结构就非常清晰了，想要变动里边的参数就可以操作了。

void scale_trajectory_speed(moveit::planning_interface::MoveGroupInterface::Plan &plan, double scale)//引用 & 作函数参数，形参可以修饰实参
{int n_joints = plan.trajectory_.joint_trajectory.joint_names.size();//获取关节个数for(int i=0; i<plan.trajectory_.joint_trajectory.points.size(); i++)//通过for循环对plan中所有的轨迹点作一个遍历{plan.trajectory_.joint_trajectory.points[i].time_from_start *= 1/scale;//速度变慢，时间变长为 1/scale 倍for(int j=0; j<n_joints; j++)//遍历各个关节，每一个关节的速度和加速度数据都要作一个尺度的变化{plan.trajectory_.joint_trajectory.points[i].velocities[j] *= scale;//速度变化为原来的 scaleplan.trajectory_.joint_trajectory.points[i].accelerations[j] *= scale*scale;//加速度变化为原来的 scale * scale}}
}

结合画圆的案例，实现先用 moveit 规划的 plan 走一遍，再轨迹重定义（调整运行速度为原来的 scale倍）走一遍。
ROS机械臂开发：MoveIt!中的潜规则

Moveit编程——moveit 编程技巧笔记——圆弧轨迹规划+修改轨迹相关推荐

采用rrt进行机械臂轨迹规划得到轨迹的算法步骤
采用RRT进行机械臂轨迹规划得到轨迹的算法步骤如下: 定义机械臂的状态空间和操作空间,其中状态空间包括机械臂的关节角度.末端执行器的位置和姿态等信息,操作空间包括机械臂的关节空间和笛卡尔空间. 初始化 ...
机器人轨迹规划——多项式轨迹实战
机器人轨迹这一系列轨迹教程将主要包括以下内容: 点到点轨迹(P2P) 三次多项式插值轨迹五次多项式插值轨迹多项式轨迹实战梯形速度曲线轨迹双S形速度曲线轨迹多个自由度轨迹的时间同步在线轨迹 ...
无人驾驶车辆路径规划及轨迹跟踪控制学习笔记（2）
目录汇总学习笔记汇总在关键交通场景中,轨迹规划和轨迹跟踪控制是自动驾驶车辆避免碰撞的两个关键.它不仅需要系统功能,而且需要强大的实时性. 我们集成了自动驾驶汽车的轨迹规划器和跟踪控制器,通过轨 ...
遨博协作机器人ROS开发 - 机械臂复杂轨迹规划
目录一.简介二.环境版本三.学习目标四.知识储备五.任务实施六.任务拓展七.课堂小结八.课后练习一.简介大家好,欢迎关注遨博学院带来的系列技术分享文章(协作机器人ROS开发),今天 ...
时间最优轨迹规划（3-5-3次多项式）
轨迹规划是指机械臂在给定起始点和终止点之间运动,其中要保持时间和能量的双重最优,就需要对路径之间的插值点进行规划,目前比较常见的就是三次多项式.五次多项式以及B样条插值等进行轨迹规划.轨迹优化是指对路 ...
Apollo项目::轨迹规划综述::导航规划、行为规划、动作规划
轨迹规划分为不同层次:导航规划.行为规划.动作规划.这里一一展开简单来说. 1.导航规划 A*算法一直是一个经典高效的离散空间路径搜索算法,这里导航规划我们就直接等同于A*算法来讲解. ...
「运动控制」“关节空间与笛卡尔空间进行轨迹规划”研究
1.什么是轨迹规划? 轨迹规划是指根据作业任务的要求,确定轨迹参数并实时计算和生成运动轨迹. 2.轨迹规划目的它是机器人研究领域中的一个很重要的内容,是工业机器人控制的依据,所有控制的目的都是要围绕 ...
公开课精华|机器人的带约束轨迹规划
本文章总结于大疆前技术总监,目前在卡内基梅隆大学读博的杨硕博士在深蓝学院的关于机器人的带约束轨迹规划的公开课演讲内容. --------全文约5000字-------- 笔者不是机器人领域的,因此特地 ...
三自由度机械臂的轨迹规划实例
一.轨迹规划简介机械臂的轨迹规划有两种形式,一种是笛卡尔空间轨迹规划,一种是关节空间轨迹规划.笛卡尔空间轨迹规划相比较关节空间轨迹规划而言,更加直观.两种方法本质上没有差别,仅仅是变量选取稍微有些不 ...

Moveit编程——moveit 编程技巧笔记——圆弧轨迹规划+修改轨迹

moveit 编程技巧笔记——圆弧轨迹规划+修改轨迹

1 笛卡尔空间圆弧轨迹规划

2 轨迹重定义（修改moveit生成的轨迹数据）

Moveit编程——moveit 编程技巧笔记——圆弧轨迹规划+修改轨迹相关推荐

最新文章

热门文章