def move(self, position, max_effort=-1):
goal = pr2c.Pr2GripperCommandGoal(
pr2c.Pr2GripperCommand(position=position, max_effort=max_effort))
self._ac.send_goal(goal)
rospy.loginfo("Sending goal to gripper and waiting for result...")
self._ac.wait_for_result()
rospy.loginfo("Gripper action returned")
if self._ac.get_state() != am.GoalStatus.SUCCEEDED:
raise ex.ActionFailedError()
def move(self, pos):
"""
Move torso to a given height
:param pos: Desired height (m)
"""
goal = pr2c.SingleJointPositionGoal(position=pos,
min_duration=rospy.Duration(2),
max_velocity=1)
rospy.loginfo("Sending torso goal and waiting for result")
self._ac.send_goal_and_wait(goal)
res = self._ac.get_result()
if self._ac.get_state() != am.GoalStatus.SUCCEEDED:
raise ex.ActionFailedError()
def move_to_posture(self, grasp, action_type, max_contact_force=-1.0):
ac_goal = om.GraspHandPostureExecutionGoal(
grasp=grasp, max_contact_force=max_contact_force)
if action_type == 'pre_grasp':
ac_goal.goal = ac_goal.PRE_GRASP
elif action_type == 'grasp':
ac_goal.goal = ac_goal.GRASP
else:
ac_goal.goal = ac_goal.RELEASE
self._grasp_posture_client.send_goal(ac_goal)
rospy.loginfo(
"Sending posture goal to gripper and waiting for result...")
self._grasp_posture_client.wait_for_result()
rospy.loginfo("Gripper posture action returned")
if self._grasp_posture_client.get_state() != am.GoalStatus.SUCCEEDED:
raise ex.ActionFailedError()
def move_to(self, x, y, theta):
"""
Moves base to a 2d pose specified in the map frame. Possible outcomes:
* The function returns successfully. In this case, the robot is guaranteed to be at the goal pose.
* An ActionFailed exception is thrown if navigation fails.
* The robot loops forever. This should only happen in situations involving malicious humans.
"""
goal = mbm.MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = '/map'
goal.target_pose.pose = _to_pose(x, y, theta)
rospy.loginfo("Sending base goal ({0}, {1}, {2}) and waiting for result".\
format(x, y, theta))
self._ac.send_goal(goal)
self._ac.wait_for_result()
if self._ac.get_state() != am.GoalStatus.SUCCEEDED:
raise ex.ActionFailedError()
def move_manipulable_pose(self, x, y, z, group='torso', try_hard=False):
"""
Moves base to 2d pose from which an object can be manipulated.
:param x: x coordinate of object
:param y: y coordinate of object
:param z: z coordinate of object
:keyword group: One of 'torso', 'head', 'left_arm', or 'right_arm'.
:raise exceptions.ActionFailedError: If the base movement failed
:raise exceptions.AllYourBasePosesAreBelongToUs: If no valid base poses could be found
"""
assert group in ['left_arm', 'right_arm', 'torso', 'head']
poses = self.get_base_poses(x, y, z, group)
if poses is None or not len(poses):
raise ex.AllYourBasePosesAreBelongToUs((x, y, z))
if not try_hard:
pose = poses[0].pose
try:
self.move_to(pose.position.x, pose.position.y,
_yaw(pose.orientation))
except ex.ActionFailedError(), e:
raise e
raise ex.AllYourBasePosesAreBelongToUs((x, y, z))
if not try_hard:
pose = poses[0].pose
try:
self.move_to(pose.position.x, pose.position.y,
_yaw(pose.orientation))
except ex.ActionFailedError(), e:
raise e
else:
poses_sorted = self._sort_poses(poses)
for i in range(len(poses_sorted)):
pose = poses_sorted[i].pose
try:
self.move_to(pose.position.x, pose.position.y,
_yaw(pose.orientation))
except ex.ActionFailedError(), e:
raise e
return True
def move_to_look(self, x, y, z, try_hard=False):
self.move_manipulable_pose(x, y, z, group='head', try_hard=try_hard)
def _sort_poses(self, poses):
poses_and_dists = [(p,
self._dist_between(self.get_current_pose_stamped(),
p)) for p in poses]
poses_and_dists.sort(key=lambda pair: pair[1])
# if len(poses_and_dists) == 0:
# return None
poses_in_dist_order = [pair[0] for pair in poses_and_dists]
return poses_in_dist_order