def plan_linear_path(self,
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 : YuMiTrajectory
the planned trajectory to execute
"""
# check valid state types
if not isinstance(start_state, YuMiState):
raise ValueError('Start state must be specified')
# check valid pose types
if not isinstance(start_pose, RigidTransform) or not isinstance(
goal_pose, RigidTransform):
raise ValueError(
'Start and goal poses must be specified as RigidTransformations'
)
# check valid frames
if start_pose.from_frame != 'gripper' or goal_pose.from_frame != 'gripper' or (
start_pose.to_frame != 'world'
and start_pose.to_frame != 'base'
) or (goal_pose.to_frame != 'world' and goal_pose.to_frame != 'base'):
raise ValueError(
'Start and goal poses must be from frame \'gripper\' to frame \{\'world\', \'base\'\}'
)
# set start state of planner
start_state_msg = moveit_msgs.msg.RobotState()
start_state_msg.joint_state.name = [
'yumi_joint_%d_r' % i for i in range(1, 8)
]
if self._arm == 'left_arm':
start_state_msg.joint_state.name = [
'yumi_joint_%d_l' % i for i in range(1, 8)
]
start_state_msg.joint_state.position = start_state.in_radians
self._planning_group.set_start_state(start_state_msg)
# convert start and end pose to the planner's reference frame
start_pose_hand = start_pose * ymc.T_GRIPPER_HAND
goal_pose_hand = goal_pose * ymc.T_GRIPPER_HAND
# get waypoints
pose_traj = start_pose_hand.linear_trajectory_to(
goal_pose_hand, traj_len)
waypoints = [t.pose_msg for t in pose_traj]
# plan plath
plan, fraction = self._planning_group.compute_cartesian_path(
waypoints, eef_delta, jump_thresh)
if fraction >= 0.0 and fraction < 1.0:
logging.warning('Failed to plan full path.')
return None
if fraction < 0.0:
logging.warning('Error while planning path.')
return None
# convert from moveit joint traj in radians
joint_names = plan.joint_trajectory.joint_names
joint_traj = []
for t in plan.joint_trajectory.points:
joints_and_names = zip(joint_names, t.positions)
joints_and_names.sort(key=lambda x: x[0])
joint_traj.append(
YuMiState([np.rad2deg(x[1]) for x in joints_and_names]))
return YuMiTrajectory(joint_traj)
def plan_shortest_path(self,
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 : YuMiTrajectory
the planned trajectory to execute
"""
# check valid state types
if not isinstance(start_state, YuMiState):
raise ValueError('Start state must be specified')
# check valid pose types
if not isinstance(start_pose, RigidTransform) or not isinstance(
goal_pose, RigidTransform):
raise ValueError(
'Start and goal poses must be specified as RigidTransformations'
)
# check valid frames
if start_pose.from_frame != 'gripper' or goal_pose.from_frame != 'gripper' or (
start_pose.to_frame != 'world'
and start_pose.to_frame != 'base'
) or (goal_pose.to_frame != 'world' and goal_pose.to_frame != 'base'):
raise ValueError(
'Start and goal poses must be from frame \'gripper\' to frame \{\'world\', \'base\'\}'
)
# set planning time
self.planning_time = timeout
# set start state of planner
start_state_msg = moveit_msgs.msg.RobotState()
start_state_msg.joint_state.name = [
'yumi_joint_%d_r' % i for i in range(1, 8)
]
if self._arm == 'left_arm':
start_state_msg.joint_state.name = [
'yumi_joint_%d_l' % i for i in range(1, 8)
]
start_state_msg.joint_state.position = start_state.in_radians
self._planning_group.set_start_state(start_state_msg)
# convert start and end pose to the planner's reference frame
start_pose_hand = start_pose * ymc.T_GRIPPER_HAND
goal_pose_hand = goal_pose * ymc.T_GRIPPER_HAND
# plan plath
self._planning_group.set_pose_target(goal_pose_hand.pose_msg)
plan = self._planning_group.plan()
if len(plan.joint_trajectory.points) == 0:
logging.warning('Failed to plan path.')
return None
# convert from moveit joint traj in radians
joint_names = plan.joint_trajectory.joint_names
joint_traj = []
for t in plan.joint_trajectory.points:
joints_and_names = zip(joint_names, t.positions)
joints_and_names.sort(key=lambda x: x[0])
joint_traj.append(
YuMiState([np.rad2deg(x[1]) for x in joints_and_names]))
return YuMiTrajectory(joint_traj)