def __init__(self): self.arm_planner = ArmPlanner('right_arm') self.arm_mover = ArmMover() self.base_plan_service = rospy.ServiceProxy('/move_base/make_plan', GetPlan) self.psi = get_planning_scene_interface() self.viz_pub = rospy.Publisher('lower_bound', MarkerArray) self.pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped)
def _initialize(self): '''Connect up all services and action clients. ''' self._world_interface = WorldInterface() self._controller_manager = ControllerManagerClient() if self._arm_name in ['right_arm', 'left_arm']: self._group_name = self._arm_name self._planner = ArmPlanner(self._arm_name) self._hand_description = HandDescription(self._arm_name) arm_abbr = self._arm_name[0] self._joint_controller = '%s_arm_controller' % arm_abbr self._cartesian_controller = '%s_cart' % arm_abbr self._move_arm_client = actionlib.SimpleActionClient( 'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction) self._wait_for_action_server(self._move_arm_client) jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) self._cart_interface = CartesianControllerInterface(self._arm_name) elif self._arm_name == 'both': self._joint_controller = 'two_arm_controller' jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action' self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) else: raise ValueError('Invalid arm name for worker: %s' % self._arm_name)
def __init__(self): self._state_lock = threading.Lock() # to avoid synchronization problems when running left and right arm movements # at the same time, we start separate clients for each arm and for both arms, and for # control and queries self._workers = {} for arm_name in ['right_arm', 'left_arm', 'both']: rospy.loginfo('Initializing ArmMoverWorker for arm: %s' % arm_name) worker = ArmMoverWorker(arm_name) self._workers[arm_name] = worker worker.start() # these just used for queries in the main thread self._planners = { 'right_arm': ArmPlanner('right_arm'), 'left_arm': ArmPlanner('left_arm') }
def _initialize(self): '''Connect up all services and action clients. ''' self._world_interface = WorldInterface() self._controller_manager = ControllerManagerClient() if self._arm_name in ['right_arm', 'left_arm']: self._group_name = self._arm_name self._planner = ArmPlanner(self._arm_name) self._hand_description = HandDescription(self._arm_name) arm_abbr = self._arm_name[0] self._joint_controller = '%s_arm_controller' % arm_abbr self._cartesian_controller = '%s_cart' % arm_abbr self._move_arm_client = actionlib.SimpleActionClient( 'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction) self._wait_for_action_server(self._move_arm_client) #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction) #self._wait_for_action_server(self._joint_trajectory_client) jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr self._joint_trajectory_client = actionlib.SimpleActionClient( jt_action_name, FollowJointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) self._cart_interface = CartesianControllerInterface(self._arm_name) elif self._arm_name == 'both': self._joint_controller = 'two_arm_controller' #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action' #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction) jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory' self._joint_trajectory_client = actionlib.SimpleActionClient( jt_two_arm_action_name, FollowJointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) else: raise ValueError('Invalid arm name for worker: %s' % self._arm_name)
def __init__(self): arms = ['right_arm', 'left_arm'] self.grippers = {} self.arm_planners = {} for arm in arms: self.grippers[arm] = Gripper(arm) self.arm_planners[arm] = ArmPlanner(arm) self.arm_mover = ArmMover() self.arm_tasks = ArmTasks() self.base = Base() self.wi = WorldInterface() self.psi = get_planning_scene_interface() self.base_action = SimpleActionClient('base_trajectory_action', BaseTrajectoryAction) rospy.loginfo('Waiting for base trajectory action') self.base_action.wait_for_server() rospy.loginfo('Found base trajectory action') rospy.loginfo('DARRT trajectory executor successfully initialized!')
class ArmMoverWorker(threading.Thread): def __init__(self, arm_name): '''Worker class that can move or query the arm. By making several of these, the arm mover class can do multiple movements/queries at once. ''' self._arm_name = arm_name threading.Thread.__init__(self) self.daemon = True self._current_handle = None self._task_cv = threading.Condition() def assign_task(self, handle): ''' Assign a task to this worker. **Args:** handle (MovementHandle): Handle to the movement being assigned. **Raises:** ArmNavError if already doing another task. ''' self._task_cv.acquire() try: if self._current_handle is not None: raise ArmNavError('Movement requested while other movement still runnning!') self._current_handle = handle self._task_cv.notify() finally: self._task_cv.release() def run(self): '''Loop and wait to be assigned a task. ''' self._initialize() while not rospy.is_shutdown(): self._task_cv.acquire() while (self._current_handle is None) and (not rospy.is_shutdown()): self._task_cv.wait(0.01) self._task_cv.release() if rospy.is_shutdown(): break try: rospy.logdebug('ArmMoverWorker starting task: %s(%s)' % ( str(self._current_handle.task_func), str(self._current_handle.task_args))) self._current_handle.task_func(self, *self._current_handle.task_args) except Exception as e: self._current_handle._add_error(e) self._task_cv.acquire() self._current_handle._set_in_progress(False) self._current_handle = None self._task_cv.release() def _initialize(self): '''Connect up all services and action clients. ''' self._world_interface = WorldInterface() self._controller_manager = ControllerManagerClient() if self._arm_name in ['right_arm', 'left_arm']: self._group_name = self._arm_name self._planner = ArmPlanner(self._arm_name) self._hand_description = HandDescription(self._arm_name) arm_abbr = self._arm_name[0] self._joint_controller = '%s_arm_controller' % arm_abbr self._cartesian_controller = '%s_cart' % arm_abbr self._move_arm_client = actionlib.SimpleActionClient( 'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction) self._wait_for_action_server(self._move_arm_client) #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction) #self._wait_for_action_server(self._joint_trajectory_client) jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, FollowJointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) self._cart_interface = CartesianControllerInterface(self._arm_name) elif self._arm_name == 'both': self._joint_controller = 'two_arm_controller' #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action' #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction) jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory' self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, FollowJointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) else: raise ValueError('Invalid arm name for worker: %s' % self._arm_name) def _wait_for_action_server(self, action_client, max_wait=10., wait_increment=0.1): ''' Wait for the action server corresponding to this action client to be ready. **Args:** **action_client (actionlib.SimpleActionClient):** client for an action *max_wait (float):* Total number of seconds to wait before failing *wait_increment (float):* Number or seconds to wait between checks to rospy.is_shutdown() **Raises:** **exceptions.ActionFailedError:** if max_wait seconds elapses without server being ready. ''' for ii in range(int(round(max_wait / wait_increment))): if rospy.is_shutdown(): raise ActionFailedError('Could not connect to action server (rospy shutdown requested)') if action_client.wait_for_server(rospy.Duration(wait_increment)): return raise ActionFailedError('Could not connect to action server (timeout exceeeded)') def _move_to_goal(self, goal, try_hard=False, collision_aware_goal=True, planner_timeout=5., ordered_collisions=None, bounds=None, planner_id='', cartesian_timeout=5.0): ''' Move the specified arm to the given goal. This function should only get called indirectly by calling move_arm_to_goal. ''' try: reached_goal = False # check which kind of goal we were given if type(goal) == PoseStamped: goal_is_pose = True elif type(goal) == JointState: goal_is_pose = False else: raise ArmNavError('Invalid goal type %s' % str(type(goal))) rospy.loginfo('Attempting to use move arm to get to goal') try: self._move_to_goal_using_move_arm( goal, planner_timeout, ordered_collisions, bounds, planner_id) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) rospy.loginfo('Move arm failed: %s' % str(e)) if (not reached_goal) and try_hard: rospy.loginfo('Attempting to move directly to goal') try: self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=True) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) rospy.loginfo('Collision aware IK failed: %s' % str(e)) if (not reached_goal) and try_hard and (not collision_aware_goal) and goal_is_pose: rospy.loginfo('Attempting to move directly to goal, ignoring collisions') try: self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=False) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) rospy.loginfo('Non-collision aware IK failed: %s' % str(e)) if (not reached_goal) and try_hard and (not collision_aware_goal) and goal_is_pose: rospy.loginfo('Attempting to use cartesian controller to move towards goal') try: self._move_to_goal_using_cartesian_control(goal, cartesian_timeout, bounds) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) finally: self._current_handle._set_reached_goal(reached_goal) def _move_into_joint_limits(self): ''' Moves the arm into joint limits if it is outside of them. This cannot be a collision free move but it is almost always very very short. **Raises:** **exceptions.ArmNavError:** if IK fails **rospy.ServiceException:** if there is a problem calling the IK service **ValueError:** if the goal type is wrong ''' joint_state = self._planner.get_closest_joint_state_in_limits() self._move_to_goal_directly(joint_state, trajectory_time=0.5) self._current_handle._set_reached_goal(True) def _move_out_of_collision(self, move_mag=0.3, num_tries=100): ''' Tries to find a small movement that will take the arm out of collision. **Args:** *move_mag (float):* Max magnitude in radians of movement for each joint. *num_tries (int):* Number of random joint angles to try before giving up. **Returns:** succeeded (boolean): True if arm was sucessfully moved out of collision. ''' req = GetStateValidityRequest() req.robot_state = self.world_interface.get_robot_state() req.check_collisions = True req.check_path_constraints = False req.check_joint_limits = False req.group_name = self._arm_name res = self._planner.get_state_validity_service(req) if res.error_code.val == ArmNavErrorCodes.SUCCESS: rospy.logdebug('Current state not in collision') return False joint_state = self._planner.arm_joint_state() current_joint_position = np.array(joint_state.position) for ii in range(num_tries): joint_position = current_joint_position + np.random.uniform( -move_mag, move_mag, (len(joint_state.position),)) joint_state.position = list(joint_position) trajectory_tools.set_joint_state_in_robot_state(joint_state, req.robot_state) res = self._planner.get_state_validity_service(req) in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS) rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision))) if not in_collision: self._move_to_goal_directly(joint_state, None, None, collision_aware=False) self._current_handle._set_reached_goal(True) self._current_handle._set_reached_goal(False) def _call_action(self, action_client, goal): ''' Call an action and wait for it to complete. **Returns:** Result of action. **Raises:** **exceptions.ArmNavError:** if action fails. ''' action_client.send_goal(goal) gs = actionlib_msgs.msg.GoalStatus() r = rospy.Rate(100) while True: if self._current_handle._get_cancel_requested(): raise ActionFailedError('Preempted (cancel requested)') state = action_client.get_state() if state in [gs.PENDING, gs.ACTIVE, gs.PREEMPTING, gs.RECALLING]: # action is still going pass elif state in [gs.PREEMPTED, gs.REJECTED, gs.RECALLED, gs.LOST]: raise ArmNavError('Action call failed (%d)!' % (state,)) elif state in [gs.SUCCEEDED, gs.ABORTED]: return action_client.get_result() r.sleep() def _move_to_goal_directly(self, goal, planner_timeout=5.0, bounds=None, collision_aware=True, trajectory_time=5.0): ''' Move directly to the goal. No planning, just interpolated joint positions. If goal is a PoseStamped, does IK to find joint positions to put the end effector in that pose. Then executes a trajectory where the only point is the goal joint positions. Note: planner_timeout, collision_aware and bounds only apply to the IK, and so are not used when the goal is already a JointState **Raises:** **exceptions.ArmNavError:** if IK fails **rospy.ServiceException:** if there is a problem calling the IK service **ValueError:** if the goal type is wrong ''' if type(goal) == JointState: joint_state = goal elif type(goal) == PoseStamped: ik_res = self._planner.get_ik(goal, collision_aware=collision_aware, starting_state=None, seed_state=None, timeout=planner_timeout) if not ik_res.error_code.val == ArmNavErrorCodes.SUCCESS: raise ArmNavError('Unable to get IK for pose', ik_res.error_code) joint_state = ik_res.solution.joint_state else: raise ValueError('Invalid goal type: %s' % str(type(goal))) trajectory = JointTrajectory() trajectory.joint_names = self._planner.joint_names jtp = JointTrajectoryPoint() jtp.positions = joint_state.position jtp.time_from_start = rospy.Duration(trajectory_time) trajectory.points.append(jtp) self._execute_joint_trajectory(trajectory) # should actually check this... self._current_handle._set_reached_goal(True) def _move_to_goal_using_move_arm(self, goal, planner_timeout, ordered_collisions, bounds, planner_id=''): ''' Try using the MoveArm action to get to the goal. ''' self._controller_manager.switch_controllers(start_controllers=[self._joint_controller]) current_state = self._world_interface.get_robot_state() link_name = self._hand_description.hand_frame if type(goal) == JointState: mp_request = conversions.joint_state_to_motion_plan_request( goal, link_name, self._group_name, current_state, timeout=planner_timeout, bounds=bounds, planner_id=planner_id) elif type(goal) == PoseStamped: mp_request = conversions.pose_stamped_to_motion_plan_request( goal, link_name, self._group_name, starting_state=current_state, timeout=planner_timeout, bounds=bounds, planner_id=planner_id) else: raise ValueError('Invalid goal type %s' % str(type(goal))) ma_goal = arm_navigation_msgs.msg.MoveArmGoal() ma_goal.motion_plan_request = mp_request if ordered_collisions: ma_goal.operations = ordered_collisions ma_goal.planner_service_name = DEFAULT_PLANNER_SERVICE_NAME # send goal to move arm res = self._call_action(self._move_arm_client, ma_goal) if res == None: raise ArmNavError('MoveArm failed without setting result') elif not res.error_code.val == ArmNavErrorCodes.SUCCESS: raise ArmNavError('MoveArm failed', res.error_code) else: self._current_handle._set_reached_goal(True) def _move_to_goal_using_cartesian_control(self, goal, timeout, bounds): if type(goal) == PoseStamped: pose_stamped = goal else: raise ValueError('Invalid goal type for cartesian control: %s' % str(type(goal))) self._controller_manager.switch_controllers(start_controllers=[self._cartesian_controller]) self._cart_interface.set_desired_pose(pose_stamped) start_time = time.time() r = rospy.Rate(100) try: print 'Current handle' print self._current_handle._get_cancel_requested() while not self._current_handle._get_cancel_requested(): print 'Inside while loop' # ignores bounds right now and uses defaults... fixme if self._cart_interface.reached_desired_pose(): self._current_handle._set_reached_goal(True) return if (time.time() - start_time) > timeout: raise ArmNavError('Cartesian control move time out', ArmNavErrorCodes(ArmNavErrorCodes.TIMED_OUT)) r.sleep() finally: self._cart_interface.cancel_desired_pose() def _execute_joint_trajectory(self, trajectory): ''' Executes the given trajectory, switching controllers first if needed. **Args:** **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute. ''' self._controller_manager.switch_controllers(start_controllers=[self._joint_controller]) #goal = JointTrajectoryGoal() goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory jt_res = self._call_action(self._joint_trajectory_client, goal) # should actually check this self._current_handle._set_reached_goal(True) return jt_res def _execute_two_arm_trajectory(self, trajectory): ''' Executes the given trajectory, switching controllers first if needed. **Args:** **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute. ''' self._controller_manager.switch_controllers(start_controllers=[self._joint_controller]) #goal = JointTrajectoryGoal() goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory jt_res = self._call_action(self._joint_trajectory_client, goal) # should actually check this self._current_handle._set_reached_goal(True) return jt_res
class ArmMoverWorker(threading.Thread): def __init__(self, arm_name): '''Worker class that can move or query the arm. By making several of these, the arm mover class can do multiple movements/queries at once. ''' self._arm_name = arm_name threading.Thread.__init__(self) self.daemon = True self._current_handle = None self._task_cv = threading.Condition() def assign_task(self, handle): ''' Assign a task to this worker. **Args:** handle (MovementHandle): Handle to the movement being assigned. **Raises:** ArmNavError if already doing another task. ''' self._task_cv.acquire() try: if self._current_handle is not None: raise ArmNavError( 'Movement requested while other movement still runnning!') self._current_handle = handle self._task_cv.notify() finally: self._task_cv.release() def run(self): '''Loop and wait to be assigned a task. ''' self._initialize() while not rospy.is_shutdown(): self._task_cv.acquire() while (self._current_handle is None) and (not rospy.is_shutdown()): self._task_cv.wait(0.01) self._task_cv.release() if rospy.is_shutdown(): break try: rospy.logdebug('ArmMoverWorker starting task: %s(%s)' % (str(self._current_handle.task_func), str(self._current_handle.task_args))) self._current_handle.task_func(self, *self._current_handle.task_args) except Exception as e: self._current_handle._add_error(e) self._task_cv.acquire() self._current_handle._set_in_progress(False) self._current_handle = None self._task_cv.release() def _initialize(self): '''Connect up all services and action clients. ''' self._world_interface = WorldInterface() self._controller_manager = ControllerManagerClient() if self._arm_name in ['right_arm', 'left_arm']: self._group_name = self._arm_name self._planner = ArmPlanner(self._arm_name) self._hand_description = HandDescription(self._arm_name) arm_abbr = self._arm_name[0] self._joint_controller = '%s_arm_controller' % arm_abbr self._cartesian_controller = '%s_cart' % arm_abbr self._move_arm_client = actionlib.SimpleActionClient( 'move_%s' % self._arm_name, arm_navigation_msgs.msg.MoveArmAction) self._wait_for_action_server(self._move_arm_client) #jt_action_name = '/%s_arm_controller/joint_trajectory_action' % arm_abbr #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_action_name, JointTrajectoryAction) #self._wait_for_action_server(self._joint_trajectory_client) jt_action_name = '/%s_arm_controller/follow_joint_trajectory' % arm_abbr self._joint_trajectory_client = actionlib.SimpleActionClient( jt_action_name, FollowJointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) self._cart_interface = CartesianControllerInterface(self._arm_name) elif self._arm_name == 'both': self._joint_controller = 'two_arm_controller' #jt_two_arm_action_name = '/two_arm_controller/joint_trajectory_action' #self._joint_trajectory_client = actionlib.SimpleActionClient(jt_two_arm_action_name, JointTrajectoryAction) jt_two_arm_action_name = '/two_arm_controller/follow_joint_trajectory' self._joint_trajectory_client = actionlib.SimpleActionClient( jt_two_arm_action_name, FollowJointTrajectoryAction) self._wait_for_action_server(self._joint_trajectory_client) else: raise ValueError('Invalid arm name for worker: %s' % self._arm_name) def _wait_for_action_server(self, action_client, max_wait=10., wait_increment=0.1): ''' Wait for the action server corresponding to this action client to be ready. **Args:** **action_client (actionlib.SimpleActionClient):** client for an action *max_wait (float):* Total number of seconds to wait before failing *wait_increment (float):* Number or seconds to wait between checks to rospy.is_shutdown() **Raises:** **exceptions.ActionFailedError:** if max_wait seconds elapses without server being ready. ''' for ii in range(int(round(max_wait / wait_increment))): if rospy.is_shutdown(): raise ActionFailedError( 'Could not connect to action server (rospy shutdown requested)' ) if action_client.wait_for_server(rospy.Duration(wait_increment)): return raise ActionFailedError( 'Could not connect to action server (timeout exceeeded)') def _move_to_goal(self, goal, try_hard=False, collision_aware_goal=True, planner_timeout=5., ordered_collisions=None, bounds=None, planner_id='', cartesian_timeout=5.0): ''' Move the specified arm to the given goal. This function should only get called indirectly by calling move_arm_to_goal. ''' try: reached_goal = False # check which kind of goal we were given if type(goal) == PoseStamped: goal_is_pose = True elif type(goal) == JointState: goal_is_pose = False else: raise ArmNavError('Invalid goal type %s' % str(type(goal))) rospy.loginfo('Attempting to use move arm to get to goal') try: self._move_to_goal_using_move_arm(goal, planner_timeout, ordered_collisions, bounds, planner_id) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) rospy.loginfo('Move arm failed: %s' % str(e)) if (not reached_goal) and try_hard: rospy.loginfo('Attempting to move directly to goal') try: self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=True) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) rospy.loginfo('Collision aware IK failed: %s' % str(e)) if (not reached_goal) and try_hard and ( not collision_aware_goal) and goal_is_pose: rospy.loginfo( 'Attempting to move directly to goal, ignoring collisions') try: self._move_to_goal_directly(goal, planner_timeout, bounds, collision_aware=False) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) rospy.loginfo('Non-collision aware IK failed: %s' % str(e)) if (not reached_goal) and try_hard and ( not collision_aware_goal) and goal_is_pose: rospy.loginfo( 'Attempting to use cartesian controller to move towards goal' ) try: self._move_to_goal_using_cartesian_control( goal, cartesian_timeout, bounds) reached_goal = True except ArmNavError as e: self._current_handle._add_error(e) finally: self._current_handle._set_reached_goal(reached_goal) def _move_into_joint_limits(self): ''' Moves the arm into joint limits if it is outside of them. This cannot be a collision free move but it is almost always very very short. **Raises:** **exceptions.ArmNavError:** if IK fails **rospy.ServiceException:** if there is a problem calling the IK service **ValueError:** if the goal type is wrong ''' joint_state = self._planner.get_closest_joint_state_in_limits() self._move_to_goal_directly(joint_state, trajectory_time=0.5) self._current_handle._set_reached_goal(True) def _move_out_of_collision(self, move_mag=0.3, num_tries=100): ''' Tries to find a small movement that will take the arm out of collision. **Args:** *move_mag (float):* Max magnitude in radians of movement for each joint. *num_tries (int):* Number of random joint angles to try before giving up. **Returns:** succeeded (boolean): True if arm was sucessfully moved out of collision. ''' req = GetStateValidityRequest() req.robot_state = self.world_interface.get_robot_state() req.check_collisions = True req.check_path_constraints = False req.check_joint_limits = False req.group_name = self._arm_name res = self._planner.get_state_validity_service(req) if res.error_code.val == ArmNavErrorCodes.SUCCESS: rospy.logdebug('Current state not in collision') return False joint_state = self._planner.arm_joint_state() current_joint_position = np.array(joint_state.position) for ii in range(num_tries): joint_position = current_joint_position + np.random.uniform( -move_mag, move_mag, (len(joint_state.position), )) joint_state.position = list(joint_position) trajectory_tools.set_joint_state_in_robot_state( joint_state, req.robot_state) res = self._planner.get_state_validity_service(req) in_collision = (res.error_code.val != ArmNavErrorCodes.SUCCESS) rospy.logdebug('%s in collision: %s' % (str(joint_position), str(in_collision))) if not in_collision: self._move_to_goal_directly(joint_state, None, None, collision_aware=False) self._current_handle._set_reached_goal(True) self._current_handle._set_reached_goal(False) def _call_action(self, action_client, goal): ''' Call an action and wait for it to complete. **Returns:** Result of action. **Raises:** **exceptions.ArmNavError:** if action fails. ''' action_client.send_goal(goal) gs = actionlib_msgs.msg.GoalStatus() r = rospy.Rate(100) while True: if self._current_handle._get_cancel_requested(): raise ActionFailedError('Preempted (cancel requested)') state = action_client.get_state() if state in [gs.PENDING, gs.ACTIVE, gs.PREEMPTING, gs.RECALLING]: # action is still going pass elif state in [gs.PREEMPTED, gs.REJECTED, gs.RECALLED, gs.LOST]: raise ArmNavError('Action call failed (%d)!' % (state, )) elif state in [gs.SUCCEEDED, gs.ABORTED]: return action_client.get_result() r.sleep() def _move_to_goal_directly(self, goal, planner_timeout=5.0, bounds=None, collision_aware=True, trajectory_time=5.0): ''' Move directly to the goal. No planning, just interpolated joint positions. If goal is a PoseStamped, does IK to find joint positions to put the end effector in that pose. Then executes a trajectory where the only point is the goal joint positions. Note: planner_timeout, collision_aware and bounds only apply to the IK, and so are not used when the goal is already a JointState **Raises:** **exceptions.ArmNavError:** if IK fails **rospy.ServiceException:** if there is a problem calling the IK service **ValueError:** if the goal type is wrong ''' if type(goal) == JointState: joint_state = goal elif type(goal) == PoseStamped: ik_res = self._planner.get_ik(goal, collision_aware=collision_aware, starting_state=None, seed_state=None, timeout=planner_timeout) if not ik_res.error_code.val == ArmNavErrorCodes.SUCCESS: raise ArmNavError('Unable to get IK for pose', ik_res.error_code) joint_state = ik_res.solution.joint_state else: raise ValueError('Invalid goal type: %s' % str(type(goal))) trajectory = JointTrajectory() trajectory.joint_names = self._planner.joint_names jtp = JointTrajectoryPoint() jtp.positions = joint_state.position jtp.time_from_start = rospy.Duration(trajectory_time) trajectory.points.append(jtp) self._execute_joint_trajectory(trajectory) # should actually check this... self._current_handle._set_reached_goal(True) def _move_to_goal_using_move_arm(self, goal, planner_timeout, ordered_collisions, bounds, planner_id=''): ''' Try using the MoveArm action to get to the goal. ''' self._controller_manager.switch_controllers( start_controllers=[self._joint_controller]) current_state = self._world_interface.get_robot_state() link_name = self._hand_description.hand_frame if type(goal) == JointState: mp_request = conversions.joint_state_to_motion_plan_request( goal, link_name, self._group_name, current_state, timeout=planner_timeout, bounds=bounds, planner_id=planner_id) elif type(goal) == PoseStamped: mp_request = conversions.pose_stamped_to_motion_plan_request( goal, link_name, self._group_name, starting_state=current_state, timeout=planner_timeout, bounds=bounds, planner_id=planner_id) else: raise ValueError('Invalid goal type %s' % str(type(goal))) ma_goal = arm_navigation_msgs.msg.MoveArmGoal() ma_goal.motion_plan_request = mp_request if ordered_collisions: ma_goal.operations = ordered_collisions ma_goal.planner_service_name = DEFAULT_PLANNER_SERVICE_NAME # send goal to move arm res = self._call_action(self._move_arm_client, ma_goal) if res == None: raise ArmNavError('MoveArm failed without setting result') elif not res.error_code.val == ArmNavErrorCodes.SUCCESS: raise ArmNavError('MoveArm failed', res.error_code) else: self._current_handle._set_reached_goal(True) def _move_to_goal_using_cartesian_control(self, goal, timeout, bounds): if type(goal) == PoseStamped: pose_stamped = goal else: raise ValueError('Invalid goal type for cartesian control: %s' % str(type(goal))) self._controller_manager.switch_controllers( start_controllers=[self._cartesian_controller]) self._cart_interface.set_desired_pose(pose_stamped) start_time = time.time() r = rospy.Rate(100) try: print 'Current handle' print self._current_handle._get_cancel_requested() while not self._current_handle._get_cancel_requested(): print 'Inside while loop' # ignores bounds right now and uses defaults... fixme if self._cart_interface.reached_desired_pose(): self._current_handle._set_reached_goal(True) return if (time.time() - start_time) > timeout: raise ArmNavError( 'Cartesian control move time out', ArmNavErrorCodes(ArmNavErrorCodes.TIMED_OUT)) r.sleep() finally: self._cart_interface.cancel_desired_pose() def _execute_joint_trajectory(self, trajectory): ''' Executes the given trajectory, switching controllers first if needed. **Args:** **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute. ''' self._controller_manager.switch_controllers( start_controllers=[self._joint_controller]) #goal = JointTrajectoryGoal() goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory jt_res = self._call_action(self._joint_trajectory_client, goal) # should actually check this self._current_handle._set_reached_goal(True) return jt_res def _execute_two_arm_trajectory(self, trajectory): ''' Executes the given trajectory, switching controllers first if needed. **Args:** **trajectory (trajectory_msgs.msg.JointTrajectory):** Trajectory to execute. ''' self._controller_manager.switch_controllers( start_controllers=[self._joint_controller]) #goal = JointTrajectoryGoal() goal = FollowJointTrajectoryGoal() goal.trajectory = trajectory jt_res = self._call_action(self._joint_trajectory_client, goal) # should actually check this self._current_handle._set_reached_goal(True) return jt_res
class Planner: def __init__(self): self.arm_planner = ArmPlanner('right_arm') self.arm_mover = ArmMover() self.base_plan_service = rospy.ServiceProxy('/move_base/make_plan', GetPlan) self.psi = get_planning_scene_interface() self.viz_pub = rospy.Publisher('lower_bound', MarkerArray) self.pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped) #self.base = Base() def publish(self, markers): for i in range(50): self.viz_pub.publish(markers) rospy.sleep(0.01) def plan_base(self, goal): #find the current base pose from the planning scene robot_state = self.psi.get_robot_state() starting_state = PoseStamped() starting_state.header.frame_id = robot_state.multi_dof_joint_state.frame_ids[ 0] starting_state.pose = robot_state.multi_dof_joint_state.poses[0] goal_robot_state = copy.deepcopy(robot_state) goal_robot_state.multi_dof_joint_state.poses[0] = goal.pose start_markers = vt.robot_marker(robot_state, ns='base_starting_state') goal_markers = vt.robot_marker(goal_robot_state, ns='base_goal_state', g=1, b=0) self.publish(start_markers) self.publish(goal_markers) #plan good_plan = False while not good_plan: try: start_time = time.time() self.base_plan_service(start=starting_state, goal=goal) end_time = time.time() good_plan = True except: rospy.loginfo( 'Having trouble probably with starting state. Sleeping and trying again' ) rospy.sleep(0.2) #it's apparently better just to do the trajectory #than fool with the planning scene goal_cov = PoseWithCovarianceStamped() goal_cov.header = goal.header goal_cov.pose.pose = goal.pose self.pose_pub.publish(goal_cov) self.psi.reset() # self.base.move_to(goal.pose.position.x, goal.pose.position.y, # 2.0*np.arcsin(goal.pose.orientation.z)) return (end_time - start_time) def plan_arm(self, goal, ordered_colls=None, interpolated=False): time = 0 robot_state = self.psi.get_robot_state() ending_robot_state = copy.deepcopy(robot_state) start_markers = vt.robot_marker(robot_state, ns='arm_starting_state') self.publish(start_markers) marray = MarkerArray() marker = vt.marker_at(goal, ns="arm_goal_pose") marray.markers.append(marker) self.publish(marray) if not interpolated: traj, time = self.arm_planner.plan_collision_free( goal, ordered_collisions=ordered_colls, runtime=True) else: goal_bl = self.psi.transform_pose_stamped('base_link', goal) marray = MarkerArray() marker = vt.marker_at(goal_bl, ns="arm_goal_pose_base_link") marray.markers.append(marker) self.publish(marray) traj, time = self.arm_planner.plan_interpolated_ik( goal_bl, ordered_collisions=ordered_colls, consistent_angle=2.0 * np.pi, runtime=True) joint_state = tt.joint_trajectory_point_to_joint_state( traj.points[-1], traj.joint_names) ending_robot_state = tt.set_joint_state_in_robot_state( joint_state, ending_robot_state) goal_markers = vt.robot_marker(ending_robot_state, ns='arm_goal_state', g=1, b=0) self.publish(goal_markers) #do the trajectory # traj.points = [traj.points[0], traj.points[1], traj.points[-1]] # traj.header.stamp = rospy.Time.now() # traj.points[-1].time_from_start = 3 #rospy.loginfo('Trajectory =\n'+str(traj)) self.arm_mover.execute_joint_trajectory('right_arm', traj) #self.psi.set_robot_state(ending_robot_state) return time def plan_arm_interpolated(self, goal, ordered_colls=None): return self.plan_arm(goal, ordered_colls=ordered_colls, interpolated=True)
def __init__(self, arm_name): self.arm_name = arm_name self.mover = ArmMover(arm_name) self.planner = ArmPlanner(arm_name) self.gripper = Gripper(arm_name) self.base = base.Base()
class SensingPlacer: def __init__(self, arm_name): self.arm_name = arm_name self.mover = ArmMover(arm_name) self.planner = ArmPlanner(arm_name) self.gripper = Gripper(arm_name) self.base = base.Base() #self.gripper.close() def place(self, x, y, z): self.base.move_manipulable_pose(x, y, z, try_hard = True, group=self.arm_name) world_pose = Pose() world_pose.position.x = x world_pose.position.y = y world_pose.position.z = z world_pose.orientation.x = 0.0 world_pose.orientation.y = 0.0 world_pose.orientation.z = 0.0 world_pose.orientation.w = 1.0 new_pose = transform_pose('base_footprint', 'map', world_pose) hand_pose = self.planner.get_hand_frame_pose() #print hand_pose #print new_pose hand_pose.pose.position.x = new_pose.position.x hand_pose.pose.position.y = new_pose.position.y hand_pose.pose.position.z = new_pose.position.z + 0.1 self if self.arm_name == 'right_arm': joint_position = [-1.254, -0.325, -1.064, -1.525, 0.109, -1.185, 0.758] else: joint_position = [1.191, -0.295, 0.874, -1.610, 0.048, -1.069, -.988] joint_state = self.mover.get_joint_state() joint_state.position = joint_position self.mover.move_to_goal(joint_state) self.mover.move_to_goal_directly(hand_pose,collision_aware=False) #print "********" #print self.mover.get_exceptions() #print "********" still_in_free_space = True hand_pose = self.planner.get_hand_frame_pose() while still_in_free_space: goal_z = hand_pose.pose.position.z - 0.03 hand_pose.pose.position.z = goal_z self.mover.move_to_goal_directly(hand_pose,collision_aware=False) hand_pose = self.planner.get_hand_frame_pose() if abs(hand_pose.pose.position.z - goal_z) > 0.01: still_in_free_space = False #print self.mover.reached_goal() #if not self.mover.reached_goal(): # still_in_free_space = False self.gripper.open() if self.arm_name == 'right_arm': gripper_name = 'r_gripper_palm_link' else: gripper_name = 'l_gripper_palm_link' gripper_pose = transform_pose_stamped(gripper_name, hand_pose) gripper_pose.pose.position.x -= 0.20 hand_pose = transform_pose_stamped(hand_pose.header.frame_id, gripper_pose) self.mover.move_to_goal_directly(hand_pose, collision_aware=False) self.mover.move_to_goal(joint_state)