moveit/moveit_grasps

Execution?

chongyi-zheng opened this issue · 3 comments

I try to use the move group interface to execute the Cartesian paths planned by moveit_grasps. However, the move group interface excepts a trajectory of type

moveit_msgs::RobotTrajectory

instead of

std::vector<moveit::core::RobotStatePtr>

in the

grasp_candidate->segmented_cartesian_traj_

Any suggestions on how to execute the paths?

You can convert it like this.

  auto robot_traj = std::make_shared<robot_trajectory::RobotTrajectory>(robot_model, jmg);
  for (std::size_t k = 0; k < cartesian_robot_state_traj.size(); ++k)
    robot_traj->addSuffixWayPoint(cartesian_robot_state_traj[k], 0.01);
  // Retime your trajectory with TOTG or another timing parametrizer
  trajectory_processing::TimeOptimalTrajectoryGeneration totg;
  totg->computeTimeStamps(robot_traj);
  moveit_msgs::RobotTrajectory trajectory_msg;
  robot_traj->getRobotTrajectoryMsg(trajectory_msg);

Got it. Thanks a lot!

Hi,

I tried to execute a cartesian path but the robot dose not move. Here is my code. Can someone help me. thank you so much!

void traj_trans(const std::vector < moveit::core::RobotStatePtr > & cartesian_traj,
  moveit_msgs::RobotTrajectory & trajectory_msg) {
  robot_trajectory::RobotTrajectoryPtr robot_traj = std::make_shared < robot_trajectory::RobotTrajectory > (
    robot_model_, arm_jmg_);
  for (const auto & k: cartesian_traj) {
    robot_traj - > addSuffixWayPoint(k, 0.01);
  }
  // Retime your trajectory with TOTG or another timing parametrizer
  trajectory_processing::IterativeParabolicTimeParameterization totg;
  totg.computeTimeStamps( * robot_traj);
  robot_traj - > getRobotTrajectoryMsg(trajectory_msg);
}

moveit_msgs::RobotTrajectory approach_trajectory3;
traj_trans(valid_grasp_candidate - > segmented_cartesian_traj_[moveit_grasps::RETREAT], approach_trajectory3);
visual_tools_ - > publishTrajectoryPath(approach_trajectory3, seed_state, wait_for_animation);