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)
