How to save and replay a trajectory generated with moveit

Question

Hi all,

I am using moveit's move_group_interface in C++ to generate a trajectory between two NamedTargets for Youbot. Everything works fine including visualization in RViz. However, on repeated execution, the trajectories look slightly different for the same set of targets. So, my question is: Is there any way one can save a trajectory so that it can be reused later on? I have tried saving the "plan.trajectory_", but it doesn't seem to work. Details of current setup is given below.

Robot: Youbot
Planner: OMPL/RRTkConfigDefault
Interface: moveit/move_group_interface in C++
Logic/Sequence Used:

1. Set Start State to Current State
   
2. Set a Named Target
   
3. Visualize the plan in Rviz
   
4. Execute the plan
   

Thanks

---

Originally posted by ani on ROS Answers with karma: 11 on 2017-09-20

Post score: 1

Answer 1

You can write a script to save the generated plan. When you command a target position in MoveIt!, you can access the trajectory using moveit::planning_interface::MoveGroup::Plan (API). You can save this trajectory as a YAML file and read it next time you are executing the motion, use this Plan for execution. Unfortunately, there is no available function which performs this action.

---

Originally posted by pk with karma: 76 on 2018-01-02

This answer was ACCEPTED on the original site

Post score: 2

---

Original comments

Comment by NYs on 2018-01-03:
in case you're wondering how to save a yaml file https://stackoverflow.com/questions/12470665/how-can-i-write-data-in-yaml-format-in-a-file

Comment by pranavb104 on 2019-01-21:
Is there a reference for dumping yaml in c++?

Answer 2

Got this working for python on UR5 in Gazebo

import yaml
import os
    
plan = group.plan()
        
file_path = os.path.join(os.path.expanduser('~'), 'saved_trajectories', 'plan.yaml')
with open(file_path, 'w') as file_save:
    yaml.dump(plan, file_save, default_flow_style=True)
        
with open(file_path, 'r') as file_open:
    loaded_plan = yaml.load(file_open)
        
group.execute(loaded_plan)

---

Originally posted by NYs with karma: 11 on 2018-01-04

This answer was NOT ACCEPTED on the original site

Post score: 1

---

Original comments

Comment by pranavb104 on 2019-01-21:
Is there a reference for dumping yaml in c++?

Comment by Diliban on 2021-01-08:
Is there a way to save the trajectory using RViz?