Ask Your Question
0

how to save and read / load a trajectory

asked 2021-02-26 11:01:01 -0500

zahid990170 gravatar image

updated 2021-03-03 09:11:42 -0500

Hi,

I want to capture a trajectory in one run of the program saving it to a file, and try to load the same trajectory the next time. So, actually skipping the move_group.setJointValueTarget(jointVals) or move_group.setPoseTarget(sample_pose) on next execution and trying to read the trajectory points directly from the file.

Why we are looking to this option The real robotic arm is available for tests only occasionally for short times. Since planners are random sampling based planners, the planners might compute a different path to the goal point during different runs of the program. At certain times, we noticed that a certain trajectory found and played in rviz, took a different path when executed on the real robotic arm. Since, workspace of the robotic arm have some physical constraints in place, it was not possible to carry out the deviated trajectory execution. An example, a movement went through the negative z-axis before reaching the goal pose. The real robotic arm workspace is constrained that way. Hence, the robotic arm had to be emergency stopped using the red button on the panel.

Below is the code that I use to save the trajectory, where my_plan is the successfully computed plan to the goal pose/joint-space goal.

moveit_msgs::RobotTrajectory path_points;
  trajectory_msgs::JointTrajectory jt;
  std::stringstream ss;
  size_t size_trajectory;

  path_points = my_plan.trajectory_;
  jt = path_points.joint_trajectory;
  size_trajectory = jt.points.size();

  for (unsigned i=0; i<size_trajectory; i++)
  {
          ss << "point_index: " << i << std::endl
             << "positions: "
             << "[" << jt.points[i].positions[0]
             << "," << jt.points[i].positions[1]
             << "," << jt.points[i].positions[2]
             << "," << jt.points[i].positions[3]
             << "," << jt.points[i].positions[4]
             << "," << jt.points[i].positions[5]
             << "]" << endl;
  }

My file contains entries of the following form (vector of joint-angles in radians).

point_index: 3
positions: [-0.286826,-0.120969,0.155815,-0.0343112,-0.139144,0.295076]

Next, I read the above file. I parse it and create a std::vector<std::vector<double>> traj_matrix where each row is a joint-configuration vector. And, the total number of rows in this matrix is the total number of points in the original trajectory i.e. size_trajectory. Given each row of this matrix, I calculate the corresponding end-effector pose (forward kinematics). I can save all these poses as waypoints to compute a cartesian path through these points. Following the tutorial on kinematics, I write the following code:

void SimpleMotionPlanning::computeFK()
{
   std::vector<double> joint_values;
   geometry_msgs::Pose a_pose;

  robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
  robot_model::RobotModelPtr kinematic_model = robot_model_loader.getModel();
  robot_state::RobotStatePtr kinematic_state(new robot_state::RobotState(kinematic_model));
  joint_model_group = kinematic_model->getJointModelGroup(PLANNING_GROUP);

  for (int i = 0; i < this->traj_matrix.size(); i++)
  {
    kinematic_state->setJointGroupPositions(joint_model_group, traj_matrix[i]);
    kinematic_state->enforceBounds();
    tf::poseEigenToMsg(kinematic_state->getGlobalLinkTransform("ee_link"), a_pose);
    this->waypoints.push_back(a_pose);
  }

}

Here is the launch file

<launch>
  <include file="$(find ur5_moveit_path_planner)/launch/planning_context.launch" >
    <arg name="load_robot_description" value="true" />
  </include>

  <!--
    first try, in which, we do not have a robot connected. so, we publish fake joint states
  -->
<node
    name="joint_state_publisher"
    pkg="joint_state_publisher"
    type ...
(more)
edit retag flag offensive close merge delete

Comments

I can't make a connection between the title "How to save/read/load a trajectory" and the question at the end of the post "Is there a direct way to convert Eigen::Affine3d to geometry_msgs::Pose". Please limit your posts to one question as much as possible and provide background for that question. Otherwise users will not be able to find your question if they have the same problem.

In the meantime, conversions between Eigen and geometry_msgs are defined here.

fvd gravatar image fvd  ( 2021-03-01 09:28:20 -0500 )edit

thank you @fvd for the pointer. I will check it indeed. Actually, I wanted to know if my process concerning saving and loading the trajectory (i.e., different representations on save and loading), was correct, besides other question in the end.

zahid990170 gravatar image zahid990170  ( 2021-03-01 11:28:28 -0500 )edit

2 Answers

Sort by » oldest newest most voted
1

answered 2021-03-01 22:16:55 -0500

fvd gravatar image

Your code using the Kinematics API to calculate the end effector position will work, and you can convert between the Eigen and geometry_msg types using these convenience functions, but you asked if your process is "correct", and I can't answer that, since we don't know what exactly you are aiming to do. As mentioned in your previous post, this site works better if you try to keep questions focused.

Note that while your way of calculating the end-effector position (via forward kinematics) is valid, the move_group also advertises a compute_fk service you can use. There is also this question regarding saving/writing message to/from a file in C++. In Python, you can easily save messages using YAML input/output.

edit flag offensive delete link more

Comments

thanks @fvd, I will look into these ... my purpose was to capture a trajectory in one run of the program, and try to load the same trajectory the next time. So, actually skipping the move_group.setJointValueTarget(jointVals); or move_group.setPoseTarget(sample_pose); on next execution and trying to read the trajectory points, then compute FK, get a set of way points and plan a cartesian path through these.

 fraction = move_group.computeCartesianPath(this->waypoints,
                                                0.01,  // eef_step
                                                0.0,   // jump_threshold
                                               trajectory);
  my_plan.trajectory_ = trajectory;
  move_group.execute(my_plan);

It did not work ...

zahid990170 gravatar image zahid990170  ( 2021-03-02 08:41:16 -0500 )edit

You don't say why or how it did not work, but you shouldn't have to calculate a cartesian path through the trajectory – the points are already close enough for the robot to execute the trajectory. Just make sure (!) that the robot starts in the same joint configuration as the trajectory, or your robot will attempt to move there at max speed and be dangerous.

fvd gravatar image fvd  ( 2021-03-02 18:49:16 -0500 )edit

thanks @fvd I will make an edit to the question and elaborate.

zahid990170 gravatar image zahid990170  ( 2021-03-03 05:31:26 -0500 )edit
1

answered 2021-03-01 23:22:51 -0500

omeranar1 gravatar image

As an alternaive; you don't need "ros" after taking and saving angles, just send angles to the motors with "time"

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2021-02-26 11:01:01 -0500

Seen: 66 times

Last updated: Mar 03