Client-Side Motion Planning

By default, motion planning occurs on the YuMi controller. However, to avoid obstacles the YuMi can plan motions using the MoveIt! library on the client side. This module is experimental.

YuMiMotionPlanner

class yumipy.YuMiMotionPlanner(arm='left', planner='RRTstar', goal_pos_tol=0.001, planning_time=0.1, eef_delta=0.01, jump_thresh=0.0)

Client-side motion planner for the ABB YuMi, based on MoveIt!

set_planner(planner)

Sets the motion planner for the YuMi

plan_linear_path(start_state, start_pose, goal_pose, traj_len=10, eef_delta=0.01, jump_thresh=0.0)

Plans a linear trajectory between the start and goal pose from the initial joint configuration. The poses should be specified in the frame of reference of the end effector tip. Start state must correspond to the start pose - right now this is up to the user.

Parameters:
  • start_state (YuMiState) – initial state of the arm
  • start_pose (RigidTransform) – initial pose of end effector tip
  • goal_pose (RigidTransform) – goal pose of end effector tip
  • traj_len (int) – number of waypoints to use in planning
  • eef_delta (float) – distance for interpolation of the final planner path in cartesian space
  • jump_thresh (float) – the threshold for jumping between poses
Returns:

traj – the planned trajectory to execute

Return type:

YuMiTrajectory

plan_shortest_path(start_state, start_pose, goal_pose, timeout=0.1)

Plans the shortest path in joint space between the start and goal pose from the initial joint configuration. The poses should be specified in the frame of reference of the end effector tip. Start state must correspond to the start pose - right now this is up to the user.

Parameters:
  • start_state (YuMiState) – initial state of the arm
  • start_pose (RigidTransform) – initial pose of end effector tip
  • goal_pose (RigidTransform) – goal pose of end effector tip
  • timeout (float) – planner timeout (shorthand for setting new planning time then planning)
Returns:

traj – the planned trajectory to execute

Return type:

YuMiTrajectory