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 plan_joints_collision_free(self, joint_state, starting_state=None, ordered_collisions=None, timeout=15.0, bounds=None, planner_id='', ntries=3): ''' **Deprecated.** Use plan_collision_free. ''' starting_state = self.get_closest_state_in_limits(robot_state=starting_state) goal = conv.joint_state_to_motion_plan_request(joint_state, self.hand.hand_frame, self.arm_name, starting_state, timeout=timeout, bounds=bounds, planner_id=planner_id) goal_state = tt.set_joint_state_in_robot_state(joint_state, copy.deepcopy(starting_state)) rospy.loginfo('Position of wrist is\n'+str(self._transformer.get_transform ('l_wrist_roll_link', self._psi.robot_frame, goal_state))) rospy.loginfo('Position of fingertip is\n'+str(self._transformer.get_transform('l_gripper_r_finger_tip_link', self._psi.robot_frame, goal_state))) rospy.loginfo('Collision objects are\n') cos = self._psi.collision_objects() for co in cos: if 'table' not in co.id: rospy.loginfo(str(co)) rospy.loginfo('Attached collision objects are\n') aos = self._psi.attached_collision_objects() for ao in aos: rospy.loginfo(ao) return self.plan_collision_free(goal, ordered_collisions=ordered_collisions, ntries=ntries)
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 plan_joints_collision_free(self, joint_state, starting_state=None, ordered_collisions=None, timeout=15.0, bounds=None, planner_id='', ntries=3): ''' **Deprecated.** Use plan_collision_free. ''' starting_state = self.get_closest_state_in_limits( robot_state=starting_state) goal = conv.joint_state_to_motion_plan_request(joint_state, self.hand.hand_frame, self.arm_name, starting_state, timeout=timeout, bounds=bounds, planner_id=planner_id) goal_state = tt.set_joint_state_in_robot_state( joint_state, copy.deepcopy(starting_state)) rospy.loginfo('Position of wrist is\n' + str( self._transformer.get_transform( 'l_wrist_roll_link', self._psi.robot_frame, goal_state))) rospy.loginfo('Position of fingertip is\n' + str( self._transformer.get_transform('l_gripper_r_finger_tip_link', self ._psi.robot_frame, goal_state))) rospy.loginfo('Collision objects are\n') cos = self._psi.collision_objects() for co in cos: if 'table' not in co.id: rospy.loginfo(str(co)) rospy.loginfo('Attached collision objects are\n') aos = self._psi.attached_collision_objects() for ao in aos: rospy.loginfo(ao) return self.plan_collision_free(goal, ordered_collisions=ordered_collisions, ntries=ntries)