Movement Planning Start End Config Problem

[yck011522]

The current way of defining the Taught (End) Config is confusing with the ending config of a planned movment. This cause a lot of confusion when a replanning is needed where trajectory and end-config needs to be reset but taught configuration and planned neighbour start/end config needs to be maintained.

I think a refactor of planning code logic may be easier. There are these existing attributes in the RoboticMovement object that can be used:

• Movement.target_frame
• Movement.target_configuration
• Movement.trajectory
• Movement.state_diff[('robot', 'c')]

When planning a Movement, we need to give both start and end config for the planner. the logic for the following should be used:

Start Config

• if the movement is the first movement, use the InitialConfig
• elif the prev_movement has a target_configuration use prev_movement.target_configuration
• elif the prev_movement has a planned trajectory, use the last point of that trajectory
• else sample IK based on of the prev_movement.target_frame

End Config

• if the movement has a target_configuration use movement.target_configuration
• elif the next_movement has a planned trajectory, use the first point of that trajectory
• else sample IK based on of the prev_movement.target_frame

When resetting a Movement to it's unplanned state, simply reset it back to the state of the org process file.

• If it has a taught config, its end config will still be respected.
• If its previosu Movement has a taught config or a planned trajectory, it will still be respected.
• If its next neighbour has planned trajectory, its first trajectory point is still respected.

We need to make sure the First and Last Trajectory Point are not missing when saved after planning in the Movement.trajectory. It should equal to the start and end config given as input to the planner.

[yck011522]

process.get_movement_start_robot_config(movement)

• if the movement is the first movement, use the InitialConfig
• elif the movement has a planned trajectory, use the first point of that movement.trajectory
• elif the prev_movement has a target_configuration use prev_movement.target_configuration
• elif the prev_movement has a planned trajectory, use the last point of that prev_movement.trajectory
• else return None

process.get_movement_end_robot_config(movement)

• if the movement has a target_configuration use movement.target_configuration
• elif the movement has a planned trajectory, use the end point of movement.trajectory
• elif the next_movement has a planned trajectory, use the first point of next_movement.trajectory
• else return None

To Be removed: set_movement_end_robot_config() set_movement_start_robot_config()

New API: set_movement_trajectory()