ROS moveit 代碼

將之前一次運動的目標點設置成下一次運動的起點：

如果上一次的點是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