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 _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)
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
import rospy
from geometry_msgs.msg import PoseStamped
import tf
from pr2_python.controller_manager_client import ControllerManagerClient
from pr2_python.cartesian_controller_interface import CartesianControllerInterface
arm_name = "right_arm"
goal_frame = "/cartesian_goal_frame"
cartesian_controller = "%s_cart" % arm_name[0]
# initialize
rospy.init_node("test_cartesian_tracking", anonymous=True)
controller_manager = ControllerManagerClient()
controller_manager.switch_controllers([cartesian_controller])
cci = CartesianControllerInterface(arm_name, "cci")
# tell cartesian control interface to follow tracking frame
goal_pose = PoseStamped()
goal_pose.pose.position.z = 0.2
goal_pose.pose.orientation.w = 1.0
goal_pose.header.frame_id = goal_frame
goal_pose.header.stamp = rospy.Time(0)
cci.set_desired_pose(goal_pose, 0.02)
# loop, broadcasting the transform to the tracking frame
br = tf.TransformBroadcaster()
r = rospy.Rate(100)
while not rospy.is_shutdown():
t = rospy.Time.now().to_sec()
x = 0.65 + 0.1 * np.sin(t)