def plan_interpolated_ik(self, pose_stamped, starting_pose=None, ordered_collisions=None, bounds=None, starting_state=None, min_acceptable_distance=None, collision_aware=True, reverse=False, resolution=0.005, nsteps=0, consistent_angle=np.pi / 6.0, steps_before_abort=0, collision_check_resolution=2, max_joint_vels=None, max_joint_accs=None): ''' Plans a path that follows a straight line in Cartesian space. This function is useful for tasks like grasping where it is necessary to ensure that the gripper moves in a straight line relative to the world. The trajectory returned from this function is safe to execute. This function may alter the planning scene during planning, but returns it to its initial state when finished. There are a lot of possible arguments to this function but in general the defaults work well. **Args:** **pose_stamped (geomety_msgs.msg.PoseStamped):** The ending pose for the hand frame defined in hand (see hand_description.py) *starting_pose (geometry_msgs.msg.PoseStamped):* The starting pose of the hand frame. If None, this will use the current pose of the hand frame in the starting state *ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):* Any additional collision operations besides those in the planning scene interface you want to use during planning. *bounds ([double]):* Acceptable errors for the goal position as (x, y, z, roll, pitch, yaw). If nothing is passed in, uses the defaults defined in conversions.py *starting_state (arm_navigation_msgs.msg.RobotState):* The state of the robot at the start of the plan if reverse is False and at the end of the plan if reverse is True. If you pass in a starting_pose that does not match the starting state, the planner will use the starting_pose not the stating_state. If you pass in a starting state and no starting pose, the planner will use the hand frame pose in the starting state. If you pass in no starting state, but you do pass in a starting_pose, the planner will solve for a collision free IK solution for the starting state. If you pass in no starting state or starting pose, the planner will use the current robot state in the planning scene interface. If reverse is False, the starting state will be appended to the front of the trajectory. *min_acceptable_distance (double):* If the planner finds a path of at least this distance (in meters), it is a success. This must be greater than zero; to disable, set to None. *collision_aware (boolean):* Set to False if you want no collision checking to be done *reverse (boolean):* Set to True if you want the planner to start planning at pose_stamped and try to plan towards starting_pose. The trajectory returned will still end at pose_stamped. *resolution (double):* The resolution in centimeters between points on the trajectory in Carteisan space. Will only be used if nsteps=0. *nsteps (int):* The number of steps you want on the trajectory. If nsteps is set, resolution will be ignored. *consistent_angle (double):* If any joint angle between two points on this trajectory changes by more than this amount, the planning will fail. *steps_before_abort (int):* The number of invalid steps (no IK solution etc) allowed before the planning fails. *collision_check_resolution (int):* Collisions will be checked every collision_check_resolution steps *max_joint_vels ([double]):* Maximum allowed joint velocities. If not passed in, will be set to 0.1 for all joints. *max_joint_accs ([double]):* Maximum allowed joint accelerations. If not passed in, only velocity constraints will be used. **Returns:** A trajectory_msgs.msg.JointTrajectory that is safe to execute in which the hand frame moves in a straight line in Cartesian space. **Raises:** **exceptions.ArmNavError:** if no plan can be found. **rospy.ServiceException:** if there is a problem with the call to the planning or parameter service ''' #prior_state = self._psi.get_robot_state() self._psi.add_ordered_collisions(ordered_collisions) #if starting_state: #starting_state = self.get_closest_state_in_limits(robot_state=starting_state) #self._psi.set_robot_state(starting_state) if not starting_pose: starting_pose = self.get_hand_frame_pose( robot_state=starting_state, frame_id=pose_stamped.header.frame_id) else: starting_pose = self._psi.transform_pose_stamped( pose_stamped.header.frame_id, starting_pose) if starting_state: #check that it matches if not reverse: chk_pose = starting_pose else: chk_pose = pose_stamped starting_fk = self.get_hand_frame_pose( robot_state=starting_state, frame_id=chk_pose.header.frame_id) if not gt.near(starting_fk.pose, chk_pose.pose): rospy.logwarn( 'Input starting state does not match starting pose. ' + 'Solving for an IK solution instead') rospy.logdebug('Starting FK is\n' + str(starting_fk) + '\nCheck pose is\n' + str(chk_pose)) rospy.logdebug( 'Euclidean distance is: ' + str( gt.euclidean_distance(starting_fk.pose.position, chk_pose.pose.position)) + ', angular distance is: ' + str( gt.quaternion_distance(starting_fk.pose.orientation, chk_pose.pose.orientation))) starting_state = None if not starting_state: if reverse: ik_sol = self.get_ik(pose_stamped, collision_aware=collision_aware) else: ik_sol = self.get_ik(starting_pose, collision_aware=collision_aware) if ik_sol.error_code.val != ik_sol.error_code.SUCCESS: rospy.logerr( 'Starting pose for interpolated IK had IK error ' + str(ik_sol.error_code.val)) raise ArmNavError( 'Starting pose for interpolated IK had no IK solution', error_code=ik_sol.error_code) starting_state = ik_sol.solution #self._psi.set_robot_state(starting_state) rospy.logdebug('Planning interpolated IK from\n' + str(starting_pose) + '\nto\n' + str(pose_stamped)) init_state = RobotState() init_state.joint_state = starting_state.joint_state init_state.multi_dof_joint_state.frame_ids.append( starting_pose.header.frame_id) init_state.multi_dof_joint_state.child_frame_ids.append( self.hand.hand_frame) init_state.multi_dof_joint_state.poses.append(starting_pose.pose) goal = conv.pose_stamped_to_motion_plan_request(pose_stamped, self.hand.hand_frame, self.arm_name, init_state, bounds=bounds) dist = gt.euclidean_distance(pose_stamped.pose.position, starting_pose.pose.position) if nsteps == 0: if resolution == 0: rospy.logwarn( 'Resolution and steps were both zero in interpolated IK. ' + 'Using default resolution of 0.005') resolution = 0.005 nsteps = int(dist / resolution) res = dist / nsteps req = SetInterpolatedIKMotionPlanParamsRequest() req.num_steps = nsteps req.consistent_angle = consistent_angle req.collision_check_resolution = collision_check_resolution req.steps_before_abort = steps_before_abort req.collision_aware = collision_aware req.start_from_end = reverse if max_joint_vels: req.max_joint_vels = max_joint_vels if max_joint_accs: req.max_joint_accs = max_joint_accs self._interpolated_ik_parameter_service(req) rospy.loginfo( 'Calling interpolated ik motion planning service. Expecting ' + str(nsteps) + ' steps') rospy.logdebug('Sending goal\n' + str(goal)) ik_resp = self._interpolated_ik_planning_service(goal) self._psi.remove_ordered_collisions(ordered_collisions) #self._psi.set_robot_state(prior_state) traj = ik_resp.trajectory.joint_trajectory first_index = 0 rospy.logdebug('Trajectory error codes are ' + str([e.val for e in ik_resp.trajectory_error_codes])) if reverse: for first_index in range(len(ik_resp.trajectory_error_codes)): e = ik_resp.trajectory_error_codes[first_index] if e.val == e.SUCCESS: break last_index = 0 e = ArmNavigationErrorCodes() e.val = e.SUCCESS for last_index in range(first_index, len(ik_resp.trajectory_error_codes) + 1): if last_index == len(ik_resp.trajectory_error_codes): #the whole trajectory works break e = ik_resp.trajectory_error_codes[last_index] if e.val != e.SUCCESS: rospy.logerr('Interpolated IK failed with error ' + str(e.val) + ' on step ' + str(last_index) + ' after distance ' + str((last_index + 1 - first_index) * res)) last_index -= 1 break rospy.logdebug('First index = ' + str(first_index) + ', last index = ' + str(last_index)) distance = (last_index - first_index) * res traj.points = traj.points[first_index:max(0, last_index)] rospy.loginfo('Interpolated IK returned trajectory with ' + str(len(traj.points)) + ' points') if e.val != e.SUCCESS and (not min_acceptable_distance or distance < min_acceptable_distance): raise ArmNavError( 'Interpolated IK failed after ' + str(last_index - first_index) + ' steps.', error_code=e, trajectory_error_codes=ik_resp.trajectory_error_codes, trajectory=traj) if not reverse or not traj.points: return tt.add_state_to_front_of_joint_trajectory( self.arm_joint_state(robot_state=starting_state), traj) return traj
def plan_interpolated_ik(self, pose_stamped, starting_pose=None, ordered_collisions=None, bounds = None, starting_state=None, min_acceptable_distance=None, collision_aware=True, reverse=False, resolution=0.005, nsteps=0, consistent_angle=np.pi/6.0, steps_before_abort=0, collision_check_resolution=2, max_joint_vels=None, max_joint_accs=None): ''' Plans a path that follows a straight line in Cartesian space. This function is useful for tasks like grasping where it is necessary to ensure that the gripper moves in a straight line relative to the world. The trajectory returned from this function is safe to execute. This function may alter the planning scene during planning, but returns it to its initial state when finished. There are a lot of possible arguments to this function but in general the defaults work well. **Args:** **pose_stamped (geomety_msgs.msg.PoseStamped):** The ending pose for the hand frame defined in hand (see hand_description.py) *starting_pose (geometry_msgs.msg.PoseStamped):* The starting pose of the hand frame. If None, this will use the current pose of the hand frame in the starting state *ordered_collisions (arm_navigation_msgs.msg.OrderedCollisionOperations):* Any additional collision operations besides those in the planning scene interface you want to use during planning. *bounds ([double]):* Acceptable errors for the goal position as (x, y, z, roll, pitch, yaw). If nothing is passed in, uses the defaults defined in conversions.py *starting_state (arm_navigation_msgs.msg.RobotState):* The state of the robot at the start of the plan if reverse is False and at the end of the plan if reverse is True. If you pass in a starting_pose that does not match the starting state, the planner will use the starting_pose not the stating_state. If you pass in a starting state and no starting pose, the planner will use the hand frame pose in the starting state. If you pass in no starting state, but you do pass in a starting_pose, the planner will solve for a collision free IK solution for the starting state. If you pass in no starting state or starting pose, the planner will use the current robot state in the planning scene interface. If reverse is False, the starting state will be appended to the front of the trajectory. *min_acceptable_distance (double):* If the planner finds a path of at least this distance (in meters), it is a success. This must be greater than zero; to disable, set to None. *collision_aware (boolean):* Set to False if you want no collision checking to be done *reverse (boolean):* Set to True if you want the planner to start planning at pose_stamped and try to plan towards starting_pose. The trajectory returned will still end at pose_stamped. *resolution (double):* The resolution in centimeters between points on the trajectory in Carteisan space. Will only be used if nsteps=0. *nsteps (int):* The number of steps you want on the trajectory. If nsteps is set, resolution will be ignored. *consistent_angle (double):* If any joint angle between two points on this trajectory changes by more than this amount, the planning will fail. *steps_before_abort (int):* The number of invalid steps (no IK solution etc) allowed before the planning fails. *collision_check_resolution (int):* Collisions will be checked every collision_check_resolution steps *max_joint_vels ([double]):* Maximum allowed joint velocities. If not passed in, will be set to 0.1 for all joints. *max_joint_accs ([double]):* Maximum allowed joint accelerations. If not passed in, only velocity constraints will be used. **Returns:** A trajectory_msgs.msg.JointTrajectory that is safe to execute in which the hand frame moves in a straight line in Cartesian space. **Raises:** **exceptions.ArmNavError:** if no plan can be found. **rospy.ServiceException:** if there is a problem with the call to the planning or parameter service ''' #prior_state = self._psi.get_robot_state() self._psi.add_ordered_collisions(ordered_collisions) #if starting_state: #starting_state = self.get_closest_state_in_limits(robot_state=starting_state) #self._psi.set_robot_state(starting_state) if not starting_pose: starting_pose = self.get_hand_frame_pose(robot_state=starting_state, frame_id=pose_stamped.header.frame_id) else: starting_pose = self._psi.transform_pose_stamped(pose_stamped.header.frame_id, starting_pose) if starting_state: #check that it matches if not reverse: chk_pose = starting_pose else: chk_pose = pose_stamped starting_fk = self.get_hand_frame_pose(robot_state=starting_state, frame_id=chk_pose.header.frame_id) if not gt.near(starting_fk.pose, chk_pose.pose): rospy.logwarn('Input starting state does not match starting pose. '+ 'Solving for an IK solution instead') rospy.logdebug('Starting FK is\n'+str(starting_fk)+'\nCheck pose is\n'+str(chk_pose)) rospy.logdebug('Euclidean distance is: '+ str(gt.euclidean_distance(starting_fk.pose.position, chk_pose.pose.position))+ ', angular distance is: '+ str(gt.quaternion_distance(starting_fk.pose.orientation, chk_pose.pose.orientation))) starting_state = None if not starting_state: if reverse: ik_sol = self.get_ik(pose_stamped, collision_aware=collision_aware) else: ik_sol = self.get_ik(starting_pose, collision_aware=collision_aware) if ik_sol.error_code.val != ik_sol.error_code.SUCCESS: rospy.logerr('Starting pose for interpolated IK had IK error '+str(ik_sol.error_code.val)) raise ArmNavError('Starting pose for interpolated IK had no IK solution', error_code = ik_sol.error_code) starting_state = ik_sol.solution #self._psi.set_robot_state(starting_state) rospy.logdebug('Planning interpolated IK from\n'+str(starting_pose)+'\nto\n'+str(pose_stamped)) init_state = RobotState() init_state.joint_state = starting_state.joint_state init_state.multi_dof_joint_state.frame_ids.append(starting_pose.header.frame_id) init_state.multi_dof_joint_state.child_frame_ids.append(self.hand.hand_frame) init_state.multi_dof_joint_state.poses.append(starting_pose.pose) goal = conv.pose_stamped_to_motion_plan_request(pose_stamped, self.hand.hand_frame, self.arm_name, init_state, bounds=bounds) dist = gt.euclidean_distance(pose_stamped.pose.position, starting_pose.pose.position) if nsteps == 0: if resolution == 0: rospy.logwarn('Resolution and steps were both zero in interpolated IK. '+ 'Using default resolution of 0.005') resolution = 0.005 nsteps = int(dist/resolution) res = dist/nsteps req = SetInterpolatedIKMotionPlanParamsRequest() req.num_steps = nsteps req.consistent_angle = consistent_angle req.collision_check_resolution = collision_check_resolution req.steps_before_abort = steps_before_abort req.collision_aware = collision_aware req.start_from_end = reverse if max_joint_vels: req.max_joint_vels = max_joint_vels if max_joint_accs: req.max_joint_accs = max_joint_accs self._interpolated_ik_parameter_service(req) rospy.loginfo('Calling interpolated ik motion planning service. Expecting '+str(nsteps)+' steps') rospy.logdebug('Sending goal\n'+str(goal)) ik_resp = self._interpolated_ik_planning_service(goal) self._psi.remove_ordered_collisions(ordered_collisions) #self._psi.set_robot_state(prior_state) traj = ik_resp.trajectory.joint_trajectory first_index = 0 rospy.logdebug('Trajectory error codes are '+str([e.val for e in ik_resp.trajectory_error_codes])) if reverse: for first_index in range(len(ik_resp.trajectory_error_codes)): e = ik_resp.trajectory_error_codes[first_index] if e.val == e.SUCCESS: break last_index = 0 e = ArmNavigationErrorCodes() e.val = e.SUCCESS for last_index in range(first_index,len(ik_resp.trajectory_error_codes)+1): if last_index == len(ik_resp.trajectory_error_codes): #the whole trajectory works break e = ik_resp.trajectory_error_codes[last_index] if e.val != e.SUCCESS: rospy.logerr('Interpolated IK failed with error '+str(e.val)+' on step ' +str(last_index)+ ' after distance '+ str((last_index+1-first_index)*res)) last_index -= 1 break rospy.logdebug('First index = '+str(first_index)+', last index = '+str(last_index)) distance = (last_index-first_index)*res traj.points = traj.points[first_index:max(0,last_index)] rospy.loginfo('Interpolated IK returned trajectory with '+str(len(traj.points))+' points') if e.val != e.SUCCESS and (not min_acceptable_distance or distance < min_acceptable_distance): raise ArmNavError('Interpolated IK failed after '+str(last_index-first_index)+' steps.', error_code=e, trajectory_error_codes = ik_resp.trajectory_error_codes, trajectory=traj) if not reverse or not traj.points: return tt.add_state_to_front_of_joint_trajectory(self.arm_joint_state(robot_state=starting_state), traj) return traj