ROS moveit 代码

最新推荐文章于 2026-07-31 14:25:58 发布

原创最新推荐文章于 2026-07-31 14:25:58 发布 · 945 阅读

4 ·

本内容遵循CC 4.0 BY-SA版权协议

收录于

c++

本文介绍了如何使用ROS MoveIt!库进行轨迹规划，包括将上一个运动目标点设置为下一个运动的起点，无论是从Pose还是Joint角度。同时，详细讲解了如何调整轨迹点的速度和加速度，并展示如何将多个轨迹平滑地连接在一起，确保轨迹的首尾相连。

将之前一次运动的目标点设置成下一次运动的起点：

如果上一次的点是Pose:

robot_state::RobotState start_state(*group.getCurrentState());

const robot_state::JointModelGroup *joint_model_group =

                start_state.getJointModelGroup(group.getName());

 start_state.setFromIK(joint_model_group, target_pose); //target_pose就是上次运动的目标点

如果上一次的点是Joint:

joint_model_group = start_state->getJointModelGroup(group.getName());

start_state->setJointGroupPositions(joint_model_group,group_variable_values);

group.setStartState(*start_state); //group_variable_values 就是上次运动的目标点

修改规划好的点位的速度和加速度：

   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++) {

          plan.trajectory_.joint_trajectory.points[i].time_from_start *= 1/scale;

          for(int j=0;j<n_joints;j++){

               plan.trajectory_.joint_trajectory.points[i].velocities[j] *=scale;

               plan.trajectory_.joint_trajectory.points[i].accelerations[j] *=scale*scale;

          }

      }

   }

将多条轨迹联合成一条轨迹,但轨迹应该首尾相连，即下一条的起始点是上一条的终点：

     #include <moveit/robot_trajectory/robot_trajectory.h>

         #include <moveit/trajectory_processing/iterative_time_parameterization.h>

         //假设有三条，分别是plan1,plan2,plan3



         //连接三条轨迹

         moveit_msgs::RobotTrajectory trajectory;

        

         trajectory.joint_trajectory.joint_names = plan1.trajectory_.joint_trajectory.joint_names;

         trajectory.joint_trajectory.points = plan1.trajectory_.joint_trajectory.points;

        

         //连接第二条

         for(size_t i=1;i<plan2.trajectory_.joint_trajectory.points.size();i++)

         {

             trajectory.joint_trajectory.points.push_back(plan2.trajectory_.joint_trajectory.points[i]);

         }

        

         //连接第三条

         for(size_t s=1;s<plan3.trajectory_.joint_trajectory.points.size();s++)

         {

             trajectory.joint_trajectory.points.push_back(plan3.trajectory_.joint_trajectory.points[s]);

         }

         //重新规划

         robot_trajectory::RobotTrajectory rt(group->getCurrentState()->getRobotModel(),"arm");

         rt.setRobotTrajectoryMsg(*group->getCurrentState(),trajectory);

         trajectory_processing::IterativeParabolicTimeParameterization iptp;

         iptp.computeTimeStamps(rt,1,1);//后面两个参数分别是，速度的比例和加速度的比例，设为1即不变

        

         rt.getRobotTrajectoryMsg(trajectory);

         moveit::planning_interface::MoveGroupInterface::Plan joinedPlan.trajectory_=trajectory;   //生成新的规划joinedPlan