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)
