Support setting IOs on preplanned trajectories

[peterdieleman]

I have a pre-planned joint trajectory that i can succesfully execute using the SDK.

However, in addition to moving the robot, I would like to trigger an IO on the middle and at the end of a trajectory at a specific index i.

In order for this to work I currently have to split this path up into 4 parts, as the 'actions' and 'trajectory' part in execute do not work together for this scenario.

Would it be possible to attach an action that works roughly the same as the implementation in the underlying API , e.g. allow trigger of an IO on a given index on the trajectory?

Below is a snippet which does what I want, but simplifying this to a single .execute() command would be great. Note also that I currently need to provide an empty actions=[] list for trajectories.

# 1. turn off gripper 
turn_off_gripper = [
    jnt(tuple(pickup_trajectory.joint_positions[0].joints)),
    io_write(key="do10", value=False)
]               
async for motion_state in motion_group.plan_and_execute(turn_off_gripper, tcp):
    print(motion_state)

# 2. move to pickup pos
actions = []
async for motion_state in motion_group.execute(pickup_trajectory, tcp, actions):
    print(motion_state)

# 3. turn on gripper
turn_on_gripper = [
    jnt(tuple(pickup_trajectory.joint_positions[-1].joints)),
    io_write(key="do10", value=True)
]
async for motion_state in motion_group.plan_and_execute(turn_on_gripper, tcp):
    print(motion_state)      
                                  
# 4. move to drop position
actions = []
async for motion_state in motion_group.execute(drop_trajectory, tcp, actions):
    print(motion_state)

[biering]

Thanks for opening the issue. We'll need to debug into it but the goal is of course that you can have one action array in this case and execute I/Os

[stefanwagnerdev]

Since plan already is stiching actions together I propose something like that with a new joint_trajectory action:

actions = [
    jnt(tuple(pickup_trajectory.joint_positions[0].joints)),
    io_write(key="do10", value=False)
    joint_trajectory(pickup_trajectory),
    io_write(key="do10", value=True)
    joint_trajectory(drop_trajectory)
]    

trajectory_plan_combined = await motion_group.plan(
    actions,
    tcp=tcp,
    start_joint_position=(0, -np.pi / 2, np.pi / 2, 0, 0, 0),
)

[peterdieleman]

@stefanwagnerdev that would be a very nice solutions, thanks.

The only unintuitive part of it for me is that you would still be using the .plan method in the end? Or am I misunderstanding what is happening under the hood.

There are good reasons to do this, e.g. to ensure the path at least has a minimal amount of blending. But if you have a well thought out joint trajectory that already takes care of that, you probably want to ensure the path that is executed is 100% identical to the one you planned yourself.

[stefanwagnerdev]

Plan basically allows to mix the different planner approaches. So you could also do a linear movement and afterwards execute your trajectory for instance.
With plan you would get a trajectory which you could log to rerun in case you need a visualization.
In your case you could just pass anything to plan and execute.