Cartesian Path Execution

[Moises Estrada]

Hi, I'm trying out the move_group_interface_tutorial.cpp, however I'm having trouble while trying to execute the trajectory on the actual robot.

I can visualize the trajectory in RViz, but group.move() wont execute the Cartesian path.

My guess is that I have to make a Plan with the RobotTrajectory and then execute group.move(), is this true? If so can I get a hint on how to do it.

Thanks
Moises

[Sachin Chitta]

The plan contains a trajectory object so you should be able to just do

plan.trajectory_ = trajectory;

and then use group.execute. group.move() does planning and execution. group.execute(plan) does execution of a plan that you pass in. There's also group.asyncExecute(plan).

Sachin

[Moises Estrada]

Thanks, that worked!

But now I have a different problem, I have so far a planed Cartesian trajectory with 26 points and before it reaches the controller I only have 2 points (???):

[ INFO] [1390524356.038997234]: Computed Cartesian path with 24 points (followed 100.000000% of requested trajectory)
[ INFO] [1390524366.049140772]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1390524366.050585093]: RRTConnect: Starting with 1 states
[ INFO] [1390524366.066325149]: RRTConnect: Created 5 states (2 start + 3 goal)
[ INFO] [1390524366.067314312]: Solution found in 0.016909 seconds
[ INFO] [1390524366.068064059]: Path simplification took 0.000028 seconds
[ WARN] [1390524366.068971159]: Trajectory size less than n: 10, pass through
...
[ INFO] [1390524366.079232235]: Uniform sampling, resample duraction: 0.125 input traj. size: 10 output traj. size: 2

I will mention that I have a trajectory filter, however I tried disabling it and nothing has change...
Also the trajectory does get visualized in RViz.

Any ideas on what am I doing wrong?


Moises.

[Sachin Chitta]

Looks like its calling a planner after creating a cartesian path. Are you still trying to plan after doing the cartesian path?

Try using just group.execute(plan).

Sachin

[Moises Estrada]

Hi Sachin, I tried group.execute(plan) and it did work, almost, the trajectory was missing velocities and acceleration which I was able to add after using time parametrization. I still have to work on the trajectory filter though.

So my question now is: is it normal for the returned trajectory of computeCartesianPath to be without the time parametrization and without the filter?

Thanks again,
Moises

Program code that gets executed with rosrun:
int main(int argc, char **argv)
{
  ros::init(argc, argv, "move_group_interface_tutorial");
  ros::NodeHandle node_handle; 
  ros::AsyncSpinner spinner(1);
  spinner.start();
 
  moveit::planning_interface::MoveGroup group("arm_gp");
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface; 

  ros::Publisher display_publisher = node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;

  moveit::planning_interface::MoveGroup::Plan plan;
  group.setStartStateToCurrentState();

  std::vector<geometry_msgs::Pose> waypoints;

  geometry_msgs::Pose target_pose = group.getCurrentPose().pose;
  target_pose.position.x += 0.0;
  target_pose.position.y += 0.05;
  target_pose.position.z -= 0.02;
  waypoints.push_back(target_pose); 

  target_pose.position.y -= 0.0;
  waypoints.push_back(target_pose);

  target_pose.position.z -= 0.08;
  target_pose.position.y += 0.0;
  target_pose.position.x -= 0.0;
  waypoints.push_back(target_pose); 

  moveit_msgs::RobotTrajectory trajectory_msg;
  group.setPlanningTime(10.0);
 
  double fraction = group.computeCartesianPath(waypoints,
                                               0.01,  // eef_step
                                               0.0,   // jump_threshold
                                               trajectory_msg, false);
  // The trajectory needs to be modified so it will include velocities as well.
  // First to create a RobotTrajectory object
  robot_trajectory::RobotTrajectory rt(group.getCurrentState()->getRobotModel(), "arm_gp");

  // Second get a RobotTrajectory from trajectory
  rt.setRobotTrajectoryMsg(*group.getCurrentState(), trajectory_msg);
 
  // Thrid create a IterativeParabolicTimeParameterization object
  trajectory_processing::IterativeParabolicTimeParameterization iptp;
  // Fourth compute computeTimeStamps
  bool success = iptp.computeTimeStamps(rt);
  ROS_INFO("Computed time stamp %s",success?"SUCCEDED":"FAILED");
  // Get RobotTrajectory_msg from RobotTrajectory
  rt.getRobotTrajectoryMsg(trajectory_msg);
  // Check trajectory_msg for velocities not empty
  std::cout << trajectory_msg << std::endl;

  plan.trajectory_ = trajectory_msg;
  ROS_INFO("Visualizing plan 4 (cartesian path) (%.2f%% acheived)",fraction * 100.0);   
  sleep(5.0);
 
  group.execute(plan); 

  ros::shutdown(); 
  return 0;
}

[Ioan Sucan]

Moises,

Unfortunately that is a missing feature. The time parametrization should be called on Cartesian paths as well, but it is not. For now, this should be fixed in the move_group capability for generating Cartesian paths. This is still not as nice, because then you do not have this capability when you generate the paths using the C++ API in robot_state.

The main reason for not generating RobotTrajectory instances from RobotState is dependencies: a robot trajectory depends on a robot state, so we cannot have a robot state depend on robot trajectory. This is a side effect of the build layout currently used. This is getting off topic, but Dave H. had a good suggestion about building all of moveit_core into one single lib. I guess when that is done (when we branch for Indigo ?) we can fix this issue the correct way.

Ioan