I want to record and play a pre-planned trajectory using Move-It, My Setup:
I have tried the solution mentioned here but I get the error:
could not determine a constructor for the tag \'tag:yaml.org,2002:python/tuple\'\n
in "/home/saved_trajectories/test.yaml", line 1, column 1
Here is the YAML file that I have saved:
!!python/tuple [true, !!python/object/new:moveit_msgs.msg._RobotTrajectory.RobotTrajectory {
state: [!!python/object/new:trajectory_msgs.msg._JointTrajectory.JointTrajectory {
state: [!!python/object/new:std_msgs.msg._Header.Header {state: [0, !!python/object/new:genpy.rostime.Time {
state: [0, 0]}, world]}, [yumi_robr_joint_1, yumi_robr_joint_2, yumi_robr_joint_3,
yumi_robr_joint_4, yumi_robr_joint_5, yumi_robr_joint_6, yumi_robr_joint_7,
gripper_r_joint], [!!python/object/new:trajectory_msgs.msg._JointTrajectoryPoint.JointTrajectoryPoint {
state: [!!python/tuple [0.6070651736385813, 0.29742469343822114, -1.0670289134119273,
0.5490030687107282, 1.0947504229888345, -0.2727256386719584, 3.7517497572539567,
0.0], !!python/tuple [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], !!python/tuple [
-0.09083825158724707, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], !!python/tuple [],
!!python/object/new:genpy.rostime.Duration {state: [0, 0]}]}, !!python/object/new:trajectory_msgs.msg._JointTrajectoryPoint.JointTrajectoryPoint {
state: [!!python/tuple [0.6015542781334869, 0.35733262252205716, -1.0699803518099047,
0.5145828872943146, 1.1295083723579553, -0.24630768917984783, 3.758266111374755,
0.0006667045908980072], !!python/tuple [-0.0269508869304581, 0.2929781233347364,
-0.014433930469661484, -0.16833097572292724, 0.16998282085294777,
0.12919627473216633, 0.031868055372510196, 0.003260500952830533],
!!python/tuple [-0.09029591410123787, 0.9815902358403596, -0.04835925990839515,
-0.5639739932743149, 0.5695083145140482, 0.4328575811073439, 0.10677033367824533,
0.010923941565391214], !!python/tuple [], !!python/object/new:genpy.rostime.Duration {
state: [0, 348330660]}]}, !!python/object/new:trajectory_msgs.msg._JointTrajectoryPoint.JointTrajectoryPoint {
state: [!!python/tuple [0.5960433826283926, 0.4172405516058932, -1.072931790207882,
0.4801627058779009, 1.1642663217270761, -0.21988973968773723, 3.7647824654955535,
0.0013334091817960145], !!python/tuple [-0.04391971455822303, 0.47744312021595553,
-0.02352182723397893, -0.2743155883564974, 0.2770074688423648, 0.2105408821187911,
0.051932832455485835, 0.005313378796572893], !!python/tuple [-0.0914213742266345,
0.9938249053811138, -0.0489620160714013, -0.571003439152396, 0.5766067408984992,
0.4382527748141437, 0.10810113313160441, 0.011060099006913317],
!!python/tuple [], !!python/object/new:genpy.rostime.Duration {state: [
0, 493046135]}]}, !!python/object/new:trajectory_msgs.msg._JointTrajectoryPoint.JointTrajectoryPoint {
state: [!!python/tuple [0.5905324871232982, 0.47714848068972915, -1.0758832286058593,
0.4457425244614872, 1.1990242710961971, -0.1934717901956266, 3.7712988196163515,
0.0020001137726940216], !!python/tuple [-0.043958350827791745, 0.4778631279797113,
-0.023542519437170645, -0.27455690438337166, 0.27725115292242763,
0.21072609539621331, 0.05197851788250647, 0.005318052987595537],
!!python/tuple [0.0909207245758629, -0.988382435324015, 0.04869388603666274,
0.5678764606441491, -0.5734490771037308, -0.43585277699616626, -0.10750914033989453,
-0.010999530734426131], !!python/tuple [], !!python/object/new:genpy.rostime.Duration {
state: [0, 603798916]}]}, !!python/object/new:trajectory_msgs.msg._JointTrajectoryPoint.JointTrajectoryPoint {
state: [!!python/tuple [0.5850215916182039, 0.5370564097735652, -1.0788346670038367,
0.41132234304507354, 1.233782220465318, -0.167053840703516, 3.77781517373715,
0.002666818363592029], !!python/tuple [-0.02686357658598151, 0.29202898867588584,
-0.01438717017399073, -0.16778564912516547, 0.1694321429223079,
0.12877772927646153, 0.03176481532332174, 0.0032499382035565505],
!!python/tuple [0.09064887662683936, -0.9854272263863499, 0.04854829400460408,
0.566178541365123, -0.5717344960089998, -0.43454960124550995, -0.10718769394320854,
-0.010966642744540041], !!python/tuple [], !!python/object/new:genpy.rostime.Duration {
state: [0, 748221334]}]}, !!python/object/new:trajectory_msgs.msg._JointTrajectoryPoint.JointTrajectoryPoint {
state: [!!python/tuple [0.5795106961131096, 0.5969643388574012, -1.081786105401814,
0.37690216162865986, 1.2685401698344387, -0.1406358912114054, 3.7843315278579484,
0.003333522954490036], !!python/tuple [0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0], !!python/tuple [0.08796870275503225, -0.9562915503249647,
0.04711288880209301, 0.5494385994065332, -0.5548302836807862, -0.42170147195145546,
-0.10401852442477695, -0.010642397034729915], !!python/tuple [],
!!python/object/new:genpy.rostime.Duration {state: [1, 102187696]}]}]]},
!!python/object/new:trajectory_msgs.msg._MultiDOFJointTrajectory.MultiDOFJointTrajectory {
state: [!!python/object/new:std_msgs.msg._Header.Header {state: [0, !!python/object/new:genpy.rostime.Time {
state: [0, 0]}, '']}, [], []]}]}, 0.316400488, !!python/object/new:moveit_msgs.msg._MoveItErrorCodes.MoveItErrorCodes {
state: [1]}]
I have tried to read/write JSON instead of YAML, but I have got the following Error:
ERROR: b'error processing request: Object of type RobotTrajectory is not JSON serializable'
I have also tried the yaml.safe_load(stream) approach as mentioned here, but the problem was not solved!
Can you please tell me how I can save/load the trajectory correctly? thanks in advance.
You can use pickle instead of yaml as follows:
from moveit_commander import *
import pickle
# change 'arm' to your MoveGroup Name
group = MoveGroupCommander("arm")
## Motion Planning settings here ...
# 1. Do Motion Planning
planned_trajectory = group.plan()
# 2. Save Trajectory
with open('pickled_plan', 'wb') as fp:
pickle.dump(planned_trajectory, fp)
# 3. Load Trajectory
file_path = './pickled_plan.pickle'
with open(file_path, 'rb') as file_open:
plan = pickle.load(file_open)[1]
# 4. Execute Trajectory
group.execute(plan, wait=True)