Execution?
chongyi-zheng opened this issue · 3 comments
chongyi-zheng commented
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?
mlautman commented
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);
chongyi-zheng commented
Got it. Thanks a lot!
mydansun commented
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);