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+