0
$\begingroup$

Rosanswers logo

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="joint_state_publisher">
    <param name="/use_gui" value="false"/>
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>

  <node
    name="robot_state_publisher"
    pkg="robot_state_publisher"
    type="state_publisher"
  />

  <include file="$(find ur5_moveit_path_planner)/launch/moveit_rviz.launch"/>

  <!-- Run the main MoveIt! executable without trajectory execution (we do not have controllers configured by default) -->
  <include file="$(find ur5_moveit_path_planner)/launch/move_group.launch">
    <arg name="allow_trajectory_execution" value="true"/>
    <arg name="fake_execution" value="true"/>
    <arg name="info" value="true"/>
    <arg name="debug" value="false"/>
  </include>

  <!--
        including our own motion planning node 
  -->
  <arg
    name="option"
    default="1"
  />
  <node
    name="ur5"
    pkg="ur5_moveit_path_planner"
    type="ur5"
    respawn="false"
    output="screen"
    args="$(arg option)"
  />
</launch>

What happened rviz starts but there appears to be several things out of place

  • the arm shows several movements that do not correspond to the original trajectory.

  • I read on terminal, "Attempting to follow 33 waypoints for link 'ee_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame) [ INFO] [1614784138.321026214]: Computed Cartesian path with 202 points (followed 85.858586% of requested trajectory)".

  • The original trajectory that I saved, I can as well assign this to my_plan.trajectory when I am loading it from the file instead of computing FK and waypoints. But, I have only two attributes available, one is joint_names, and the other is points. I do not have header. As for the points, I saved only the positions. I need to save also the rest of the information ?

trajectory_msgs/JointTrajectory has Header header string[] joint_names JointTrajectoryPoint[] points

float64[] positions float64[] velocities float64[] accelerations float64[] effort duration time_from_start

  • My original launch file is still in place, i.e. joint_state_publisher and robot_state_publisher among other nodes. What kind of nodes I need to include in the launch file if I am reading trajectory from a file.

Please, share your comments. Does this make sense to load trajectories in this way. For current work, we have fixed scenarios, i.e., scene does not changes and obstacles are also static.

thanks,

zahid


Originally posted by zahid990170 on ROS Answers with karma: 135 on 2021-02-26

Post score: 0


Original comments

Comment by fvd on 2021-03-01:
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.

Comment by zahid990170 on 2021-03-01:
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.

$\endgroup$

2 Answers 2

0
$\begingroup$

Rosanswers logo

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.


Originally posted by fvd with karma: 2180 on 2021-03-01

This answer was NOT ACCEPTED on the original site

Post score: 1


Original comments

Comment by zahid990170 on 2021-03-02:
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 ...

Comment by fvd on 2021-03-02:
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.

Comment by zahid990170 on 2021-03-03:
thanks @fvd I will make an edit to the question and elaborate.

$\endgroup$
0
$\begingroup$

Rosanswers logo

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


Originally posted by omeranar1 with karma: 31 on 2021-03-02

This answer was NOT ACCEPTED on the original site

Post score: 1

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.