def main():
obj_type = sys.argv[1]
my_world = WorldInterface()
# print "quat="+str(gt.euler_to_quaternion(math.pi/2,math.pi/2,0))
if obj_type == "plate" or obj_type == "paper_plate" or obj_type == "plastic_plate":
obj = create_plate(my_world)
elif obj_type == "box":
obj = create_box(my_world)
my_world.attach_object_to_gripper('left_arm', obj)
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 pickplace_main():
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
wi = WorldInterface()
psi = get_planning_scene_interface()
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal = DARRTGoal()
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.lift_distance = 0.3
goal.pickup_goal.arm_name = 'right_arm'
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -1.3
place_pose_stamped.pose.position.y = 0.7
if 'graspable' in goal.pickup_goal.collision_object_name:
place_pose_stamped.pose.position.z = 0.8
else:
place_pose_stamped.pose.position.z = 0.75
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'serving_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.object_sampling_fraction = 0.3
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 30
goal.tries = 10
goal.goal_bias = 0.2
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('Restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
def main():
rospy.init_node("touring", anonymous=True)
sm = smach.StateMachine(outcomes=["success",
"failure"])
base_mover = base.Base()
detector = TablewareDetection()
pickplace = PickPlace()
pickplace.min_open_gripper_pos = 0.0014
world = two_arms_states.WorldState()
interface = WorldInterface()
tasks = Tasks()
placer_r = SensingPlacer('right_arm')
placer_l = SensingPlacer('left_arm')
interface.reset()
psi = get_planning_scene_interface()
psi.reset()
tasks.move_arms_to_side()
rospy.loginfo("Creating State Machine")
with sm:
detect = create_detect_sm(world, base_mover, detector)
smach.StateMachine.add("detect", detect,
transitions = {"success":"clean_table",
"failure":"failure"}
)
clean_table = clean_table_sm(world, tasks, base_mover, placer_l, placer_r, interface)
smach.StateMachine.add("clean_table", clean_table,
transitions = {"success":"setup_table",
"failure":"failure"
}
)
setup_table = setup_table_sm(world, tasks, base_mover, placer_l, placer_r, interface)
smach.StateMachine.add("setup_table", setup_table,
transitions = {"success":"success",
"failure":"failure"
}
)
outcome = sm.execute()
rospy.loginfo("Outcome: %s", outcome)
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!')
def __init__(self,
world,
screenshot,
fake_walls=False,
box_plate=False,
real=False):
self.world = world
self.screenshot = screenshot
self.wi = WorldInterface()
self.psi = get_planning_scene_interface()
self.pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped)
self.arms = ArmTasks()
self.torso = Torso()
self.gripper = {
'right_arm': Gripper('right_arm'),
'left_arm': Gripper('left_arm')
}
self.box_plate = box_plate
self.real = real
self.fake_walls = fake_walls
def time_plate_main():
rospy.loginfo('Reading file')
[result, goal, collision_objects, robot_state] = pickle.load(open(sys.argv[1], 'r'))
rospy.loginfo('Original starting state was\n'+str(robot_state))
markers = vt.robot_marker(robot_state, ns='robot_starting_state', a = 0.5)
pub = rospy.Publisher('/darrt_planning/robot_state_markers', MarkerArray)
for i in range(10):
rospy.loginfo('Publishing starting state!')
pub.publish(markers)
rospy.sleep(0.1)
wi = WorldInterface()
wi.reset(repopulate=False)
psi = get_planning_scene_interface()
psi.reset()
plate = None
for co in collision_objects:
wi.add_object(co)
if (co.id == goal.pickup_goal.collision_object_name):
plate = co
psi.reset()
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
#the old pickup goal may have used base link to generate grasps
#i don't know what we do with things for which we need a point cloud
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name=goal.pickup_goal.collision_object_name,
collision_support_surface_name=goal.pickup_goal.collision_support_surface_name,
desired_grasps=plate_grasps(ops, ops.header.frame_id))
rospy.loginfo('Teleop to initial state. Ready to go?')
raw_input()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.execute = False
goal.planning_time = 200
goal.tries = 10000
goal.debug_level = 1
goal.do_pause = False
goal.goal_bias = 0.2
# goal.place_goal.place_locations[0].pose.position.x = 1
# goal.place_goal.place_locations[0].pose.position.y = 1
# goal.place_goal.place_locations[0].pose.position.z = 0.86
average_time = 0.0
rospy.loginfo('First place goal is\n'+str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: '+sys.argv[1]+', restart after '+str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error '+str(result.error_code)+'. We had '+str(i)+
' successful trials with an average time of '+str(average_time/float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL '+str(i)+': '+str(result.planning_time))
rospy.loginfo(str(NTRIALS)+' averaged '+str(average_time/float(NTRIALS))+' seconds to solve')
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 main():
[result, goal, collision_objects,
robot_state] = pickle.load(open(sys.argv[1], 'r'))
wi = WorldInterface()
#set the planning scene up correctly
wi.reset(repopulate=False)
for co in collision_objects:
#for running easy version
if ('table' in co.id or co.id == 'plate'):
wi.add_object(co)
psi = get_planning_scene_interface()
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
average_time = 0
goal.planning_time = 30
goal.tries = 10000
goal.debug_level = 0
goal.do_pause = False
goal.goal_bias = 0.2
#i think that this does not work
#goal.robot_state = robot_state
goal.execute = False
rospy.loginfo('First place goal is\n' +
str(goal.place_goal.place_locations[0]))
rospy.loginfo('File: ' + sys.argv[1] + ', restart after ' +
str(goal.planning_time))
for i in range(NTRIALS):
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Something went wrong! Error ' +
str(result.error_code) + '. We had ' + str(i) +
' successful trials with an average time of ' +
str(average_time / float(i)))
return
average_time += result.planning_time
rospy.loginfo('TRIAL ' + str(i) + ': ' + str(result.planning_time))
rospy.loginfo(
str(NTRIALS) + ' averaged ' + str(average_time / float(NTRIALS)) +
' seconds to solve')
def plate_main():
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
rospy.loginfo('Waiting for action')
rospy.loginfo('Doing detection')
wi = WorldInterface()
psi = get_planning_scene_interface()
detector = TablewareDetection()
plate = wi.collision_object('plate')
good_detection = False
goal = DARRTGoal()
if plate:
rospy.loginfo('Use last detection?')
good_detection = (raw_input() == 'y')
ops = PoseStamped()
ops.header = copy.deepcopy(plate.header)
ops.pose = copy.deepcopy(plate.poses[0])
goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id),
object_pose_stamped=ops, collision_object_name='plate',
collision_support_surface_name='serving_table')
while not good_detection:
det = detector.detect_objects(add_table_to_map=False,
add_objects_as_mesh=False, table_name='serving_table')
goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi)
psi.reset()
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
rospy.loginfo('Good detection?')
good_detection = (raw_input() == 'y')
if not goal.pickup_goal:
rospy.loginfo('Nothing to pick up!')
return
add_map_tables(wi)
psi.reset()
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
client.wait_for_server()
goal.pickup_goal.arm_name = 'right_arm'
goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped,
goal.pickup_goal.target.reference_frame_id)
goal.pickup_goal.lift.desired_distance = 0.3
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
# place_pose_stamped.pose.position.x = 1.3
# place_pose_stamped.pose.position.y = -0.3
# place_pose_stamped.pose.position.z = 1.01
place_pose_stamped.pose.position.x = 0.0
place_pose_stamped.pose.position.y = 0.0
place_pose_stamped.pose.position.z = 0.76
place_pose_stamped.pose.orientation.w = 1.0
# place_pose_stamped.pose.orientation.x = 0.1
# place_pose_stamped.pose.orientation.y = 0.1
# place_pose_stamped.pose.orientation.w = np.sqrt(0.98)
#place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped)
#place_pose_stamped.pose.position.x -= 0.2
#place_pose_stamped.pose.position.y -= 0.2
#place_pose_stamped.pose.position.z += 0.2
goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name = 'dirty_table',
collision_object_name = goal.pickup_goal.collision_object_name)
goal.place_goal.approach.direction.header.frame_id = wi.world_frame
goal.place_goal.approach.direction.vector.x = np.sqrt(0.18)
goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18)
goal.place_goal.approach.direction.vector.z = -0.8
goal.place_goal.approach.desired_distance = 0.2
goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT]
goal.min_grasp_distance_from_surface = 0.19
goal.object_sampling_fraction = 0.7
goal.retreat_distance = 0.3
goal.debug_level = 2
goal.do_pause = False
goal.execute = False
goal.planning_time = 6000
goal.tries = 1
goal.goal_bias = 0.2
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
result = client.get_result()
marray = MarkerArray()
if result.error_code == result.SUCCESS:
#pickle everything!! great excitement
filename = 'trajectory_and_objects.pck'
rospy.loginfo('Pickling to '+filename)
#the last bit allows us to recreate the planning
pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb'))
rospy.loginfo('Successful write')
for t in result.primitive_trajectories:
marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers
for (i, m) in enumerate(marray.markers):
m.id = i
(m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1)
while not rospy.is_shutdown():
pub.publish(marray)
rospy.sleep(0.1)
class Executor:
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!')
def execute_trajectories(self, result):
rospy.loginfo('There are ' + str(len(result.primitive_names)) +
' segments')
for (i, t) in enumerate(result.primitive_types):
rospy.loginfo('Executing trajectory ' + str(i) + ' of type ' + t +
' and length ' + str(
len(result.primitive_trajectories[i].
joint_trajectory.points)))
#print 'Trajectory is\n', result.primitive_trajectories[i]
splits = t.split('-')
if splits[0] == 'BaseTransit':
self.execute_base_trajectory(result.primitive_trajectories[i])
continue
if splits[0] == 'BaseWarp':
self.arm_tasks.move_arm_to_side('right_arm')
self.arm_tasks.move_arm_to_side('left_arm')
self.execute_warp_trajectory(result.primitive_trajectories[i])
continue
if splits[0] == 'ArmTransit':
self.execute_arm_trajectory(splits[1],
result.primitive_trajectories[i])
continue
#if splits[0] == 'Approach':
#gripper.open()
if splits[0] == 'Pickup':
try:
self.grippers[splits[1]].close()
except ActionFailedError:
pass
self.wi.attach_object_to_gripper(splits[1], splits[2])
#so the trajectory filter doesn't complain
ops = OrderedCollisionOperations()
op = CollisionOperation()
op.operation = op.DISABLE
op.object2 = splits[2]
for t in self.wi.hands[splits[1]].touch_links:
op.object1 = t
ops.collision_operations.append(copy.deepcopy(op))
self.psi.add_ordered_collisions(ops)
self.execute_straight_line_arm_trajectory(
splits[1], result.primitive_trajectories[i])
if splits[0] == 'Place':
self.grippers[splits[1]].open()
self.wi.detach_and_add_back_attached_object(
splits[1], splits[2])
self.psi.reset()
def execute_warp_trajectory(self, trajectory):
#figure out the last point on the trajectory
tp = trajectory.multi_dof_joint_trajectory.points[-1]
(phi, theta, psi) = gt.quaternion_to_euler(tp.poses[0].orientation)
self.base.move_to(tp.poses[0].position.x, tp.poses[0].position.y, psi)
def execute_base_trajectory(self, trajectory):
goal = BaseTrajectoryGoal()
#be a little slower and more precise
goal.linear_velocity = 0.2
goal.angular_velocity = np.pi / 8.0
goal.linear_error = 0.01
goal.angular_error = 0.01
joint = -1
for (i, n) in enumerate(
trajectory.multi_dof_joint_trajectory.joint_names):
if n == 'world_joint' or n == '/world_joint':
goal.world_frame = trajectory.multi_dof_joint_trajectory.frame_ids[
i]
goal.robot_frame = trajectory.multi_dof_joint_trajectory.child_frame_ids[
i]
joint = i
break
if joint < 0:
raise ActionFailedError('No world joint found in base trajectory')
for p in trajectory.multi_dof_joint_trajectory.points:
(phi, theta,
psi) = gt.quaternion_to_euler(p.poses[joint].orientation)
goal.trajectory.append(
Pose2D(x=p.poses[joint].position.x,
y=p.poses[joint].position.y,
theta=psi))
self.base_action.send_goal_and_wait(goal)
def execute_arm_trajectory(self, arm_name, trajectory):
arm_trajectory = self.arm_planners[arm_name].joint_trajectory(
trajectory.joint_trajectory)
arm_trajectory.header.stamp = rospy.Time.now()
#try:
# arm_trajectory = self.arm_planners[arm_name].filter_trajectory(arm_trajectory)
#except ArmNavError, e:
#rospy.logwarn('Trajectory filter failed with error '+str(e)+'. Executing anyway')
arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory,
time_per_pt=0.35)
self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)
def execute_straight_line_arm_trajectory(self, arm_name, trajectory):
arm_trajectory = self.arm_planners[arm_name].joint_trajectory(
trajectory.joint_trajectory)
arm_trajectory.header.stamp = rospy.Time.now()
arm_trajectory = tt.convert_path_to_trajectory(arm_trajectory,
time_per_pt=0.4)
self.arm_mover.execute_joint_trajectory(arm_name, arm_trajectory)
def main():
#Find an object to pick up
psi = get_planning_scene_interface()
psi.reset()
rospy.loginfo('Doing detection')
detector = TablewareDetection()
det = detector.detect_objects(add_objects_as_mesh=False)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
psi.reset()
#DARRT action client
client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction)
rospy.loginfo('Waiting for DARRT action')
client.wait_for_server()
rospy.loginfo('Found DARRT action')
#DARRTGoal
goal = DARRTGoal()
#pickup goal (from the tabletop detection)
goal.pickup_goal = det.pickup_goals[0]
goal.pickup_goal.arm_name = 'right_arm'
#world interface to tell us whether we're using the
#map or odom_combined frame
wi = WorldInterface()
#place goal (move far away)
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = wi.world_frame
place_pose_stamped.pose.position.x = -2
place_pose_stamped.pose.position.y = 0
place_pose_stamped.pose.position.z = 0.85
place_pose_stamped.pose.orientation.w = 1.0
goal.place_goal = PlaceGoal(
goal.pickup_goal.arm_name, [place_pose_stamped],
collision_support_surface_name=det.table_name,
collision_object_name=goal.pickup_goal.collision_object_name)
goal.primitives = [goal.PICK, goal.PLACE, goal.BASE_TRANSIT]
goal.planning_time = 30
goal.tries = 3
goal.debug_level = 1
goal.object_sampling_fraction = 0.5
#Send the goal to the action
rospy.loginfo('Sending goal')
client.send_goal_and_wait(goal)
rospy.loginfo('Returned')
#Check the result
result = client.get_result()
if result.error_code != result.SUCCESS:
rospy.logerr('Planning failed. Error code: ' + str(result.error_code))
return
rospy.loginfo('Planning succeeded!')
#at this point the planning is done.
#now we execute and visualize the plan
#visualize trajectory
pub = rospy.Publisher('darrt_trajectory', MarkerArray)
marray = get_trajectory_markers(result.primitive_trajectories)
for i in range(10):
pub.publish(marray)
rospy.sleep(0.05)
#Executor is a Python class for executing DARRT plans
executor = Executor()
#execute trajectory
rospy.loginfo('Press enter to execute')
raw_input()
executor.execute_trajectories(result)
rospy.loginfo('Successfully executed!')
#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_arm_navigation_python')
import rospy
from MotionPlan import *
from MoveArm import *
from MoveHand import *
from tf.transformations import *
from copy import deepcopy
rospy.init_node("test")
from pr2_python.world_interface import WorldInterface
wi = WorldInterface()
grasp_pose = PoseStamped()
#grasp_pose.header.frame_id = "base_link"
grasp_pose.header.frame_id = "odom_combined"
grasp_pose.header.stamp = rospy.Time()
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x,o.y,o.z,o.w = quaternion_from_euler(-1.581, -0.019, 2.379)
lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
class SimSetup:
def __init__(self,
world,
screenshot,
fake_walls=False,
box_plate=False,
real=False):
self.world = world
self.screenshot = screenshot
self.wi = WorldInterface()
self.psi = get_planning_scene_interface()
self.pose_pub = rospy.Publisher('/initialpose',
PoseWithCovarianceStamped)
self.arms = ArmTasks()
self.torso = Torso()
self.gripper = {
'right_arm': Gripper('right_arm'),
'left_arm': Gripper('left_arm')
}
self.box_plate = box_plate
self.real = real
self.fake_walls = fake_walls
def setup(self, arm):
self.wi.reset(repopulate=False)
self.set_robot_position()
goal = DARRTGoal()
goal.support_surfaces = self.add_tables()
if self.fake_walls:
self.add_walls()
#all the primitives
arm_transit = Primitive(name='ArmTransit', parameters=[arm])
base_transit = Primitive(name='BaseTransit', parameters=['pr2_base'])
approach = Primitive(name="Approach", parameters=[arm])
retreat = Primitive(name="Retreat", parameters=[arm])
rigid_transfer = Primitive(name="RigidTransfer", parameters=[arm])
push = Primitive(name="Push", parameters=[arm])
pickup = Primitive(name="Pickup", parameters=[arm])
uspat = Primitive(name="UseSpatula",
parameters=[arm],
numeric_parameters=[0.22, 0.2, 0.2])
strans = Primitive(name="SpatulaTransfer", parameters=[arm])
if self.world < 0:
obj = self.add_little_object()
spatula, grasps = self.add_spatula(arm)
block = self.add_block()
if self.world < -8:
self.add_barrier()
goal.objects = [obj, spatula, block]
rigid_transfer.parameters.append(spatula.collision_object.id)
push.parameters += [
obj.collision_object.id, arm[0] + '_end_effector'
]
uspat.parameters += [
spatula.collision_object.id, obj.collision_object.id,
block.collision_object.id
]
uspat.grasps = grasps
lift = Vector3(z=1.0)
uspat.directional_parameters.append(lift)
strans.parameters += [
spatula.collision_object.id, obj.collision_object.id
]
goal.primitives = [
arm_transit, base_transit, approach, retreat, rigid_transfer,
push, uspat, strans
]
goal.goals = [
Goal(obj.collision_object.id, self.get_goal_poses(), False)
]
self.psi.reset()
return goal
else:
rospy.loginfo('Old world set up is broken!')
return None
#this no longer works right
if self.world < 14:
obj = self.add_plate()
else:
obj = self.add_box()
if self.world > 7:
self.add_barrier()
self.psi.reset()
if self.world < 6 or self.world > 7:
ss_surface = ss_names[2]
#elif self.world == 12:
# ss_surface = ss_names[1]
else:
ss_surface = ss_names[0]
return World(obj, ss_surface, ss_names[1])
def set_robot_position(self):
#right now worlds 6 and above don't move the robot
if self.world > 15:
return
ps = PoseWithCovarianceStamped()
ps.pose.covariance = [0] * 36
ps.header.frame_id = '/map'
if abs(self.world) <= 2:
#easier start pose
ps.pose.pose.position.x = 2.7
ps.pose.pose.position.y = -2.5
ps.pose.pose.orientation.w = 1
elif self.world < 0 or abs(self.world) < 6 or abs(self.world) == 14:
ps.pose.pose.position.x = -1
ps.pose.pose.position.y = 0
ps.pose.pose.orientation.w = 1
elif abs(self.world) == 12:
ps.pose.pose.position.x = 0
ps.pose.pose.position.y = 1.5
ps.pose.pose.orientation.w = 1
else:
ps.pose.pose.position.x = 1
ps.pose.pose.position.y = 1
ps.pose.pose.orientation.z = -1.0 / np.sqrt(2.0)
ps.pose.pose.orientation.w = 1.0 / np.sqrt(2.0)
if not self.real:
self.pose_pub.publish(ps)
self.gripper['right_arm'].open()
self.torso.move(0.3)
self.arms.move_arm_to_side('left_arm')
if abs(self.world) < 4:
rospy.loginfo('Moving right arm to up position')
self.arms._arm_mover.move_to_goal(
'right_arm',
self.arms._arm_planners['right_arm'].
joint_positions_to_joint_state(
self.arms.side_joint_trajectory['right_arm'][1]),
try_hard=True)
else:
self.arms.move_arm_to_side('right_arm')
def add_tables(self):
if self.screenshot:
return ['', '', '']
table = CollisionObject()
table.header.frame_id = self.wi.world_frame
table.operation.operation = table.operation.ADD
shape = Shape()
shape.type = shape.MESH
#center table
shape.vertices = [
Point(x=-0.2, y=-0.4, z=CENTER_TABLE_HEIGHT),
Point(x=0.97, y=-0.6, z=CENTER_TABLE_HEIGHT),
Point(x=1.0, y=0.25, z=CENTER_TABLE_HEIGHT),
Point(x=-0.25, y=0.3, z=CENTER_TABLE_HEIGHT)
]
shape.triangles = [0, 1, 2, 2, 3, 0]
pose = Pose()
pose.orientation.w = 1.0
poseb = copy.deepcopy(pose)
poseb.position.z = -0.02
poset = copy.deepcopy(pose)
poset.position.z = 0.02
table.shapes.append(shape)
table.shapes.append(shape)
table.shapes.append(shape)
table.poses.append(poset)
table.poses.append(pose)
table.poses.append(poseb)
table.id = 'center_table'
self.wi.add_object(table)
#table near the door
table.id = 'door_table'
shape.vertices = [
Point(x=-2.4, y=1.84, z=DOOR_TABLE_HEIGHT),
Point(x=-1.15, y=1.75, z=DOOR_TABLE_HEIGHT),
Point(x=-1.15, y=2.5, z=DOOR_TABLE_HEIGHT),
Point(x=-2.4, y=2.5, z=DOOR_TABLE_HEIGHT)
]
self.wi.add_object(table)
#table in far corner
table.id = 'far_corner'
shape.vertices = [
Point(x=3, y=-2.7, z=FAR_TABLE_HEIGHT),
Point(x=2.4, y=-3.8, z=FAR_TABLE_HEIGHT),
Point(x=3.2, y=-4.3, z=FAR_TABLE_HEIGHT),
Point(x=3.8, y=-3.2, z=FAR_TABLE_HEIGHT)
]
self.wi.add_object(table)
if self.fake_walls:
#these are the table feet
foot = CollisionObject()
foot.header.frame_id = self.wi.world_frame
foot.operation.operation = foot.operation.ADD
foot.id = "far_corner_foot"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.5, FAR_TABLE_HEIGHT / 2.0]
pose = Pose()
pose.position.x = 3
pose.position.y = -3.4
pose.position.z = shape.dimensions[2] / 2.0
angle = 0.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
foot.shapes.append(shape)
foot.poses.append(pose)
self.wi.add_object(foot)
foot.id = "center_table_foot1"
shape.dimensions = [0.1, 0.75, 0.3]
pose.position.x = 0.9
pose.position.y = -0.1
pose.position.z = shape.dimensions[2] / 2.0
angle = 0
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
self.wi.add_object(foot)
foot.id = "center_table_foot2"
pose.position.x = -0.2
self.wi.add_object(foot)
foot.id = "door_table_foot"
pose.position.x = -1.25
pose.position.y = 2.1
self.wi.add_object(foot)
return ['center_table', 'door_table', 'far_corner']
def add_walls(self):
window_wall = CollisionObject()
window_wall.header.frame_id = self.wi.world_frame
window_wall.operation.operation = window_wall.operation.ADD
window_wall.id = "window_wall"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [8, 0.1, 2.5]
pose = Pose()
pose.position.x = 0
pose.position.y = -2.63
pose.position.z = shape.dimensions[2] / 2.0 - 0.1
angle = np.pi / 6.25
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.shapes.append(shape)
window_wall.poses.append(pose)
self.wi.add_object(window_wall)
shape.dimensions[0] = shape.dimensions[1]
shape.dimensions[1] = 5
pose.position.x = -2.45
pose.position.y = 1
angle = 0.05
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "door_wall"
self.wi.add_object(window_wall)
shape.dimensions[1] = shape.dimensions[0]
shape.dimensions[0] = 8
pose.position.x = 0
pose.position.y = 2.3
angle = 0.1
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "cabinet_wall"
self.wi.add_object(window_wall)
shape.dimensions[0] = shape.dimensions[1]
shape.dimensions[1] = 3
pose.position.x = 4
pose.position.y = -3
angle = 0.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "banner_wall"
self.wi.add_object(window_wall)
shape.dimensions[1] = 4
shape.dimensions[2] = 1
pose.position.x = 3.25
pose.position.y = 0.65
pose.position.z = shape.dimensions[2] / 2.0 - 0.05
angle = 0
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "computer_wall"
self.wi.add_object(window_wall)
shape.dimensions[1] = shape.dimensions[0]
shape.dimensions[0] = 1.8
pose.position.x = 3.9
pose.position.y = -1.6
angle = np.pi / 5.5
pose.orientation.z = np.cos(angle / 2.0)
pose.orientation.w = np.sin(angle / 2.0)
window_wall.id = "nook_wall"
self.wi.add_object(window_wall)
def add_little_object(self):
little_obj = CollisionObject()
little_obj.header.frame_id = self.wi.world_frame
little_obj.operation.operation = little_obj.operation.ADD
little_obj.id = "little_obj"
shape = Shape()
shape.type = shape.CYLINDER
shape.dimensions = [0.05, 0.02]
pose = Pose()
pose.orientation.w = 1.0
little_obj.shapes.append(shape)
little_obj.poses.append(pose)
little_obj_p = copy.deepcopy(little_obj)
little_obj_p.poses[0].position.x = 3.1
little_obj_p.poses[0].position.y = -3.2
little_obj_p.poses[
0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[1] / 2.0
self.wi.add_object(little_obj_p)
return ObjectType(type="RoundObject",
collision_object=little_obj,
parameters=['far_corner'])
def add_block(self):
block = CollisionObject()
block.header.frame_id = self.wi.world_frame
block.operation.operation = block.operation.ADD
block.id = "block"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.1, 0.13]
pose = Pose()
pose.orientation.w = 1.0
block.shapes.append(shape)
block.poses.append(pose)
block_p = copy.deepcopy(block)
block_p.poses[0].position.x = 2.9
block_p.poses[0].position.y = -3.5
block_p.poses[
0].position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
self.wi.add_object(block_p)
return ObjectType(type="FixedObject",
collision_object=block,
parameters=['far_table'])
def add_spatula(self, arm):
spatula = CollisionObject()
spatula.id = "spatula"
spatula.header.frame_id = self.wi.world_frame
spatula.operation.operation = spatula.operation.ADD
paddle = Shape()
handle = Shape()
paddle.type = paddle.BOX
paddle.dimensions = [0.11, 0.12, 0.005]
handle.type = handle.CYLINDER
handle.dimensions = [0.02, 0.24]
paddle_pose = Pose()
handle_pose = Pose()
paddle_pose.position.y = paddle.dimensions[1] / 2.0
paddle_pose.orientation.w = 1.0
angle = np.pi / 5.0
handle_pose.position.y = -1.0 * handle.dimensions[1] / 2.0 * np.sin(
np.pi / 2.0 - angle)
handle_pose.position.z = handle.dimensions[1] / 2.0 * np.cos(
np.pi / 2.0 - angle)
handle_pose.orientation.x = np.sin((np.pi / 2.0 - angle) / 2.0)
handle_pose.orientation.w = np.cos((np.pi / 2.0 - angle) / 2.0)
spatula.shapes = [paddle, handle]
spatula.poses = [paddle_pose, handle_pose]
#this is the grasp transformation
pos_on_handle = handle.dimensions[1] - 0.1
inv_grasp = Transform()
grasp = RigidGrasp()
#really should be calculating this...
inv_grasp.translation.y = GRIPPER_LENGTH
inv_grasp.translation.z = pos_on_handle / 2.0
#flip 90 degrees
inv_grasp.rotation.z = np.sin(-1.0 * np.pi / 4.0)
inv_grasp.rotation.w = np.cos(-1.0 * np.pi / 4.0)
g = gt.transform_pose(transform_to_pose(inv_grasp), handle_pose)
origin = Pose()
origin.orientation.w = 1.0
grasp.transform = pose_to_transform(
gt.inverse_transform_pose(origin, g))
grasp.touch_links = [arm[0] + '_end_effector']
grasp.attach_link = arm[0] + '_gripper_r_finger_tip_link'
grasp.min_approach_distance = 0
grasp.desired_approach_distance = 0.15
grasp.min_distance_from_surface = -1
spat_p = copy.deepcopy(spatula)
wtrans = Pose()
wtrans.orientation.x = np.sin(angle / 2.0)
wtrans.orientation.w = np.cos(angle / 2.0)
if self.world == -1:
wtrans.position.x = 3
wtrans.position.y = -2.8
wtrans.position.z = FAR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['far_corner']
elif self.world == -2 or self.world == -7 or self.world == -9 or self.world == -5:
wtrans.position.x = -1.7
wtrans.position.y = 2
wtrans.position.z = DOOR_TABLE_HEIGHT + 0.02 + handle.dimensions[0]
ss = ['door_table']
else:
wtrans.position.x = 0.6
wtrans.position.y = -0.3
wtrans.position.z = CENTER_TABLE_HEIGHT + 0.02 + handle.dimensions[
0]
ss = ['center_table']
if self.world == -4 or self.world == -5:
wtrans.position.y = 0
rot = Quaternion()
rot.z = np.sin(np.pi / 2.0)
rot.w = np.cos(np.pi / 2.0)
wtrans.orientation = gt.multiply_quaternions(
rot, wtrans.orientation)
if self.world == -5:
wtrans.position.x = 0
for i in range(len(spat_p.poses)):
spat_p.poses[i] = gt.transform_pose(spat_p.poses[i], wtrans)
self.wi.add_object(spat_p)
return ObjectType(type="SpatulaObject",
collision_object=spatula,
parameters=ss,
numeric_parameters=paddle.dimensions +
handle.dimensions + [angle]), [grasp]
def add_plate(self):
plate = CollisionObject()
plate.header.frame_id = self.wi.world_frame
plate.operation.operation = plate.operation.ADD
plate.id = "plate"
shape = Shape()
shape.type = shape.CYLINDER
shape.dimensions = [0.15, 0.04]
if self.box_plate:
shape.type = shape.BOX
shape.dimensions = [0.3, 0.3, 0.04]
pose = Pose()
if self.world < 6 or self.world == 8 or self.world == 9 or self.world == 12:
pose.position.x = 3.1
pose.position.y = -3.2
pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[
1] / 2.0
elif self.world == 6:
pose.position.x = 0.6 #0.894692#0.5
pose.position.y = -0.3 #-0.468198#-0.2
pose.position.z = CENTER_TABLE_HEIGHT + shape.dimensions[1] / 2.0
# elif self.world == 12:
# pose.position.x = -1.3
# pose.position.y = 2
# pose.position.z = DOOR_TABLE_HEIGHT + 0.02 + shape.dimensions[1]/2.0
else:
pose.position.x = 0.5 #0.894692#0.5
pose.position.y = 0.1
pose.position.z = CENTER_TABLE_HEIGHT + 0.02 + shape.dimensions[
1] / 2.0
pose.orientation.w = 1.0
plate.shapes.append(shape)
plate.poses.append(pose)
self.wi.add_object(plate)
return plate
def add_box(self):
box = CollisionObject()
box.header.frame_id = self.wi.world_frame
box.operation.operation = box.operation.ADD
box.id = "box"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 0.05, 0.15]
pose = Pose()
pose.position.x = 3.1
pose.position.y = -3.2
pose.position.z = FAR_TABLE_HEIGHT + 0.02 + shape.dimensions[2] / 2.0
pose.orientation.w = 1.0
box.shapes.append(shape)
box.poses.append(pose)
self.wi.add_object(box)
return box
def add_barrier(self):
box = CollisionObject()
box.header.frame_id = self.wi.world_frame
box.operation.operation = box.operation.ADD
box.id = "barrier1"
shape = Shape()
shape.type = shape.BOX
shape.dimensions = [0.1, 2, 1]
box.shapes.append(shape)
pose = Pose()
pose.position.x = 1
pose.position.y = -1.5
pose.position.z = 0
pose.orientation.w = 1.0
box.poses.append(pose)
self.wi.add_object(box)
# if self.world != 12:
# box.id = "barrier2"
# box.shapes[0].dimensions = [1, 0.1, 1]
# box.poses[0].position.x = 1.5
# box.poses[0].position.y = 0
# self.wi.add_object(box)
def get_goal_poses(self):
place_pose_stamped = PoseStamped()
place_pose_stamped.header.frame_id = self.wi.world_frame
if abs(self.world) < 0 or (self.world == -5 or abs(self.world) % 2 == 0
and abs(self.world) != 12
and abs(self.world) != 4):
#easiest goal
x = 1
y = -2
z = 0.85
else:
#slightly harder goal
x = -1
y = 1
z = 0.85
if abs(self.world) == 12:
x = 0
y = -1
z - 0.85
if abs(self.world) == 14:
x = 0
y = 1
z = 0.85
place_pose_stamped.pose.position.x = x
place_pose_stamped.pose.position.y = y
place_pose_stamped.pose.position.z = z
place_pose_stamped.pose.orientation.w = 1.0
return [place_pose_stamped]
def main():
wi = WorldInterface()
wi.reset()
psi = get_planning_scene_interface()
psi.reset()
rospy.sleep(2.0)
def main():
rospy.init_node("touring", anonymous=True)
sm = smach.StateMachine(outcomes=["success",
"failure"])
base_mover = base.Base()
detector = TablewareDetection()
pickplace = PickPlace()
pickplace.min_open_gripper_pos = 0.0014
poses = basic_states.poses
world = basic_states.WorldState()
interface = WorldInterface()
tasks = Tasks()
placer = SensingPlacer('right_arm')
interface.reset()
psi = get_planning_scene_interface()
psi.reset()
tasks.move_arms_to_side()
rospy.loginfo("Creating State Machine")
nav_states = {}
for name, pos in poses.iteritems():
nav_states[name] = basic_states.NavigateTo(
world, base_mover, pos[0], pos[1], pos[2])
detect_state = basic_states.Detect(world, detector)
top_pos_place_down = (-2.15, .5, .98)
place_down_top = basic_states.PlaceDown(world, placer,
top_pos_place_down)
T = {"failure":"failure"}
with sm:
T["success"] = "detect"
T["failure"] = "failure"
smach.StateMachine.add("move_shelf", nav_states["shelf"],
transitions=T.copy())
T["success"] = "choose_bowl"
T["failure"] = "failure"
smach.StateMachine.add("detect", detect_state,
transitions=T.copy()
)
T["success"] = "move_to_pickable"
T["failure"] = "failure"
smach.StateMachine.add("choose_bowl",
basic_states.ChooseItem(world, "bowl"),
transitions=T.copy()
)
T["success"] = "pickup"
T["failure"] = "pickup"
smach.StateMachine.add("move_to_pickable",
basic_states.MoveToPickable(world, base_mover ),
transitions=T.copy()
)
T["success"] = "move_arms_to_side"
T["failure"] = "move_arms_to_side_after_pickup"
smach.StateMachine.add("pickup",
basic_states.PickUp(world, pickplace, interface),
transitions=T.copy()
)
T["success"] = "detect"
T["failure"] = "failure"
smach.StateMachine.add("move_arms_to_side_after_pickup",
basic_states.MoveArmsToSide(world, tasks),
transitions=T.copy()
)
T["success"] = "move_top_table"
T["failure"] = "failure"
smach.StateMachine.add("move_arms_to_side",
basic_states.MoveArmsToSide(world, tasks),
transitions=T.copy()
)
T["success"] = "approach_top_table"
T["failure"] = "failure"
smach.StateMachine.add("move_top_table",
nav_states["table_top_edge"],
transitions=T.copy())
T["success"] = "place_down_top"
T["failure"] = "failure"
smach.StateMachine.add("approach_top_table",
basic_states.MoveToReachable(world, base_mover, top_pos_place_down),
transitions=T.copy())
T["success"] = "success"
T["failure"] = "failure"
smach.StateMachine.add("place_down_top",
place_down_top,
transitions=T.copy())
outcome = sm.execute()
rospy.loginfo("Outcome: %s", outcome)
def execute(self):
WorldInterface().detach_and_add_back_attached_object(
self.arm, self.object_id)
return MotionHandleDummy()
def execute(self):
WorldInterface().attach_object_to_gripper(self.arm, self.object_id)
return MotionHandleDummy()
def __init__(self, allow_vertical_tables=False, vertical_threshold=DEFAULT_VERTICAL_THRESHOLD,
table_search_resolution=DEFAULT_TABLE_SEARCH_RESOLUTION,
table_search_max=DEFAULT_TABLE_SEARCH_MAX, table_thickness=DEFAULT_TABLE_THICKNESS,
tableware_labels=None, object_padding=DEFAULT_OBJECT_PADDING):
'''
Constructor for TablewareDetection.
**Args:**
*allow_vertical_tables (boolean):* If True will do segmentation and return even if a vertical table is
found.
*vertical_threshold (double):* The dot product of the normal to the table plane and the z axis must be
greater than this for the table to be considered horizontal.
*table_search_resolution (double):* If detect_objects is initially given a point for the head to look at,
it will search along the world x axis near that point until it founds a horizontal table. This is the
resolution in meters of that search. Resolution cannot be smaller than a centimeter.
*table_search_max (double):* The maximum distance in meters from the point the search should go. It will
go this far in both directions before failing.
*table_thickness (double):* If the table is added to the map, it will be added with this thickness in
meters. This is helpful for placing as it will occlude points that the robot sees below the table.
*tableware_labels ([string]):* A set of database tags that count as "tableware". If objects have
multiple tags, they are considered tableware if anyone of these tags matches.
*object_padding (double):* If objects are added to the map, this is the padding in centimeters that they
will be added with. Padding is used only for replacing voxels in the collision map. We recommend setting
this high so that during pick there is no collision between the object and the voxels where it used to be.
'''
self.allow_vertical_tables = allow_vertical_tables
self.vertical_threshold = vertical_threshold
self.table_search_resolution = table_search_resolution
self.table_search_max = table_search_max
self.table_thickness = table_thickness
self.tableware_labels = tableware_labels
if not tableware_labels:
self.tableware_labels = copy.copy(DEFAULT_TABLEWARE_LABELS)
self.object_padding = object_padding
self._wi = WorldInterface()
self._head = Head()
self._detect_srv = rospy.ServiceProxy(OBJECT_DETECTION_SERVICE, TabletopDetection)
rospy.loginfo('Waiting for object detection service')
self._detect_srv.wait_for_service()
self._bounding_box_srv = rospy.ServiceProxy(FIND_CLUSTER_BOUNDING_BOX_SERVICE, FindClusterBoundingBox)
rospy.loginfo('Waiting for bounding box service to fill pickup goal.')
self._bounding_box_srv.wait_for_service()
rospy.loginfo('Waiting for object segmentation service')
self.segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE,TabletopSegmentation)
rospy.loginfo('Waiting for object recognition service')
self.recognition_service = rospy.ServiceProxy(RECOGNITION_SERVICE,TabletopObjectRecognition)
self._get_model_mesh_srv = rospy.ServiceProxy(GET_MODEL_MESH_SERVICE, GetModelMesh)
self.get_model_description = rospy.ServiceProxy(GET_MODEL_DESCRIPTION_SERVICE, GetModelDescription)
try:
rospy.loginfo('Waiting for get model mesh service')
self._get_model_mesh_srv.wait_for_service(5.0)
rospy.loginfo('Waiting for get model description service')
self.get_model_description.wait_for_service(5.0)
except rospy.ROSException:
rospy.logwarn('Did not find database services. Likely will not be able to return recognition results')
self._get_model_mesh_srv = None
self.get_model_description = None
self._object_id = 0
rospy.loginfo('Ready to do object detections!')
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
def execute(self, target=PoseStamped(), blocking=True):
ah = ActionHandle("pick_up", "dummy", "", blocking)
rospy.loginfo("Picking up object...")
#ah = self.actions.grasp_object(target, blocking)
wi = WorldInterface()
wi.reset_attached_objects()
wi.reset_collision_objects()
# add table
table_extent = (2.0, 2.0, 1.0)
table_pose = conversions.create_pose_stamped([ -0.5 - table_extent[0]/2.0, 0 ,table_extent[2]/2.0 ,0,0,0,1], 'base_link')
wi.add_collision_box(table_pose, table_extent, "table")
mp = MotionPlan()
mp += CallFunction(sss.move, 'torso','front')
#mp += MoveArm('arm',['pregrasp'])
mp += CallFunction(sss.move, 'sdh','cylopen', False)
# OpenIssues:
# - where to place the sdh_grasp_link to grasp the object located at target?
# - there is no orientation in the target? -> hardcoded taken from pregrasp
grasp_pose = PoseStamped()
grasp_pose = deepcopy(target)
grasp_pose.header.stamp = rospy.Time.now()
grasp_pose.pose.orientation.x = 0.220
grasp_pose.pose.orientation.y = 0.670
grasp_pose.pose.orientation.z = -0.663
grasp_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[grasp_pose,['sdh_grasp_link']], seed = 'pregrasp')
mp += CallFunction(sss.move, 'sdh','cylclosed', True)
#ah = self.actions.lift_object(target, blocking)
lift_pose = PoseStamped()
lift_pose = deepcopy(target)
lift_pose.header.stamp = rospy.Time.now()
lift_pose.pose.position.z += 0.08
lift_pose.pose.orientation.x = 0.220
lift_pose.pose.orientation.y = 0.670
lift_pose.pose.orientation.z = -0.663
lift_pose.pose.orientation.w = -0.253
mp += MoveArm('arm',[lift_pose,['sdh_grasp_link']])
#ah = self.actions.retrieve_object(target, blocking)
#mp += MoveArm('arm',['pregrasp'])
mp += MoveArm('arm',['wavein'])
planning_res = mp.plan(2)
print planning_res
if planning_res.success:
for e in mp.execute():
exec_res = e.wait()
print exec_res
if not exec_res.success:
#rospy.logerr("Execution of MotionExecutable %s failed", e.name)
ah.set_failed(4)
break
ah.set_succeeded()
else:
rospy.logerr("Planning failed")
ah.set_failed(4)
return ah
#!/usr/bin/env python
import roslib
roslib.load_manifest('cob_arm_navigation_python')
import rospy
from cob_arm_navigation_python.MotionPlan import *
from cob_arm_navigation_python.MoveArm import *
from cob_arm_navigation_python.MoveHand import *
from tf.transformations import *
from copy import deepcopy
rospy.init_node("test")
from pr2_python.world_interface import WorldInterface
wi = WorldInterface()
grasp_pose = PoseStamped()
#grasp_pose.header.frame_id = "base_link"
grasp_pose.header.frame_id = "odom_combined"
grasp_pose.header.stamp = rospy.Time()
grasp_pose.pose = wi.collision_object('milk').poses[0]
p = grasp_pose.pose.position
#p.x, p.y, p.z = [-0.8,-0.2,0.9]
p.x += 0.02
p.y += 0.02
p.z += 0.02
o = grasp_pose.pose.orientation
o.x, o.y, o.z, o.w = quaternion_from_euler(-1.581, -0.019, 2.379)
lift_pose = deepcopy(grasp_pose)
p = lift_pose.pose.position
p.x += 0.0
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 TablewareDetection:
'''
Calls tabletop detection allowing the user a lot of (optional) control over when and how objects are added
to the collision map. The usual use of this class is to instantiate an instance and then use detect_objects
to find objects on a table in front of the robot:
detector = TablewareDetection()
res = detector.detect_objects([optional arguments])
This class is a wrapper around the tabletop_object_detector, which assumes that the robot is standing in
front of a table on which objects are positioned at least 3cm apart. It first locates the principle
horizontal plane in the image and then segments clusters above that plane based on Euclidean distance.
It runs those clusters through the object recognition pipeline and returns them in the form of pickup goals.
The attributes of this class can all be changed by passing in different defaults although the default settings
should work well in most cases. However, most of the parameters pertinent to detection must be changed
service side. Look at the tabletop_object_detector package for that.
Many of the parameters in this class are for working with "vertical tables".
The detector finds the "principle plane" in the image. Unfortunately, this plane may not be horizontal.
In general, it is best if any detection that does not return a table that is approximately perpendicular to
the z axis in the world is considered as failed. If a point for the head to look at is passed to
detect_objects, the robot will move its head trying to find a horizontal table.
**Attributes:**
**allow_vertical_tables (boolean):** If True will do segmentation and return even if a vertical table is found.
**vertical_threshold (double):** The dot product of the normal to the table plane and the z axis must be greater
than this for the table to be considered horizontal.
**table_search_resolution (double):** If detect_objects is initially given a point for the head to look at, it
will search along the world x axis near that point until it founds a horizontal table. This is the
resolution in meters of that search. Resolution cannot be smaller than a centimeter.
**table_search_max (double):** The maximum distance in meters from the point the search should go. It will go
this far in both directions before failing.
**table_thickness (double):** If the table is added to the map, it will be added with this thickness in meters.
This is helpful for placing as it will occlude points that the robot sees below the table.
**tableware_labels ([string]):** A set of database tags that count as "tableware". If objects have
multiple tags, they are considered tableware if anyone of these tags matches.
**object_padding (double):** If objects are added to the map, this is the padding in centimeters that they will
be added with. Padding is used only for replacing voxels in the collision map. We recommend setting
this high so that during pick there is no collision between the object and the voxels where it used to be.
**get_model_description (rospy.ServiceProxy):** Service for getting the description of a model from the model
ID. See the household_database_msgs documentation for more information.
'''
def __init__(self, allow_vertical_tables=False, vertical_threshold=DEFAULT_VERTICAL_THRESHOLD,
table_search_resolution=DEFAULT_TABLE_SEARCH_RESOLUTION,
table_search_max=DEFAULT_TABLE_SEARCH_MAX, table_thickness=DEFAULT_TABLE_THICKNESS,
tableware_labels=None, object_padding=DEFAULT_OBJECT_PADDING):
'''
Constructor for TablewareDetection.
**Args:**
*allow_vertical_tables (boolean):* If True will do segmentation and return even if a vertical table is
found.
*vertical_threshold (double):* The dot product of the normal to the table plane and the z axis must be
greater than this for the table to be considered horizontal.
*table_search_resolution (double):* If detect_objects is initially given a point for the head to look at,
it will search along the world x axis near that point until it founds a horizontal table. This is the
resolution in meters of that search. Resolution cannot be smaller than a centimeter.
*table_search_max (double):* The maximum distance in meters from the point the search should go. It will
go this far in both directions before failing.
*table_thickness (double):* If the table is added to the map, it will be added with this thickness in
meters. This is helpful for placing as it will occlude points that the robot sees below the table.
*tableware_labels ([string]):* A set of database tags that count as "tableware". If objects have
multiple tags, they are considered tableware if anyone of these tags matches.
*object_padding (double):* If objects are added to the map, this is the padding in centimeters that they
will be added with. Padding is used only for replacing voxels in the collision map. We recommend setting
this high so that during pick there is no collision between the object and the voxels where it used to be.
'''
self.allow_vertical_tables = allow_vertical_tables
self.vertical_threshold = vertical_threshold
self.table_search_resolution = table_search_resolution
self.table_search_max = table_search_max
self.table_thickness = table_thickness
self.tableware_labels = tableware_labels
if not tableware_labels:
self.tableware_labels = copy.copy(DEFAULT_TABLEWARE_LABELS)
self.object_padding = object_padding
self._wi = WorldInterface()
self._head = Head()
self._detect_srv = rospy.ServiceProxy(OBJECT_DETECTION_SERVICE, TabletopDetection)
rospy.loginfo('Waiting for object detection service')
self._detect_srv.wait_for_service()
self._bounding_box_srv = rospy.ServiceProxy(FIND_CLUSTER_BOUNDING_BOX_SERVICE, FindClusterBoundingBox)
rospy.loginfo('Waiting for bounding box service to fill pickup goal.')
self._bounding_box_srv.wait_for_service()
rospy.loginfo('Waiting for object segmentation service')
self.segmentation_service = rospy.ServiceProxy(SEGMENTATION_SERVICE,TabletopSegmentation)
rospy.loginfo('Waiting for object recognition service')
self.recognition_service = rospy.ServiceProxy(RECOGNITION_SERVICE,TabletopObjectRecognition)
self._get_model_mesh_srv = rospy.ServiceProxy(GET_MODEL_MESH_SERVICE, GetModelMesh)
self.get_model_description = rospy.ServiceProxy(GET_MODEL_DESCRIPTION_SERVICE, GetModelDescription)
try:
rospy.loginfo('Waiting for get model mesh service')
self._get_model_mesh_srv.wait_for_service(5.0)
rospy.loginfo('Waiting for get model description service')
self.get_model_description.wait_for_service(5.0)
except rospy.ROSException:
rospy.logwarn('Did not find database services. Likely will not be able to return recognition results')
self._get_model_mesh_srv = None
self.get_model_description = None
self._object_id = 0
rospy.loginfo('Ready to do object detections!')
def detect_objects(self, add_objects_to_map='all', add_table_to_map=True,
add_objects_as_mesh='all', reset_collision_objects=True,
table_name='', point_head_at=None, reset_collider=True,
labels_callback=None):
'''
This is the function you should call to get detections of objects in the scene. By default, it will
give you back a TablewareResult, remove all collision objects (although not attached objects) currently in
the collision map and add the objects detected and the table to the map. This returns first pickup goals for
objects recognized as tableware, then for objects recognized but not as tableware and then for objects not
recognized at all. In order to use the pickup goals returned, you must fill in the arm_name field.
The object labels returned in pickup goal reflect the recognition result. Labels are:
For tableware: One of the keys in the tableware_labels list.
For recognized objects that are not tableware: The first tag in the database
For unrecognized objects: graspable
**Args:**
*add_objects_to_map (string or False):* This argument controls whether and which objects are added to the
collision map during detection. Passing 'all' will add all detected objects to map, 'recognized'
will add only recognized objects to map, 'tableware' will add only objects recognized as tableware,
and False or 'None' or 'none' will add nothing to the map.
*add_table_to_map (boolean):* If true, this will add the detected table to the map. If you do not already
have a representation of the table in the map and you wish to do pick or place it is a good idea to
have the detection add it so there is a defined region of the map in which collision checking for the
object can be disabled. If the table is added to the map, the collision_support_surface_id field of
the pickup goals will be set to its collision id. Tables are usually added as meshes although
if only the bounding box for the table is returned they will be added as boxes.
*add_objects_as_mesh ('all', False, or [string]):* This argument controls which objects are added
to the map as mesh rather than as a bounding box. 'all' will add all recognized objects as mesh while
False will add no recognized objects as mesh. For finer control, you can pass a list of labels that
you want to add as mesh.
*reset_collision_objects (boolean):* If true, this will remove all collision objects from the map before
adding new ones. The detection does no correlation with what is currently in the map so if you have
done a detection on this table before, you will get repeat objects added to the map if you do not
clear them first.
*table_name (string):* The name of the table on which the objects are sitting. If passed in, this will be
added to the pickup goals to avoid extraneous collisions caused by the object contacting the table
(and will also be the table_name in the result). If the table is added to the collision
map and this is passed in, the table with have this name in the collision map. If the table is added
to the collision map and this is not passed in, it will have the name 'current_table', which is
returned by the TablewareResult.
*point_head_at (geometry_msgs.msg.PointStamped):* A point at which the head should look for the detection.
If passed in, the robot will first look at this point, but if it cannot find a table or finds a
vertical table, it will search along the x axis for a table. For this reason, it is always a good
idea to pass in this argument. If left at None, the detection will not move the head.
*reset_collider (boolean):* True if you want the collider to be reset before detection. This will not
wait for repopulate before the detection but will delay the return from the detection until after the
repopulate is done if the detection is quick.
*labels_callback (function: string f(pr2_tasks.pickplace_definitions.PickupGoal,
tabletop_object_detector.msgs.Table, string)):* After do_detection is finished assigning a label to a
cluster, this function (if not None) will be called to do more post-processing on the cluster. For
example, this function is used to identify utensils by checking the height and pose of any
unidentified cluster. It should return the label of the cluster.
**Returns:**
A TablewareDetectionResult. The pickup goals (see pickplace_definitions.py) in this result have all of the
detection information, including recognized models and point cloud information, in the GraspableObject
field. The pose at which the object was detected and the label are also returned as fields in the
pickup goals.
**Raises:**
**exceptions.DetectionError:** if a table is not found or only vertical tables cannot be found. If only
vertical tables were found the error code will be OTHER_ERROR.
'''
if reset_collision_objects:
self._wi.reset_collision_objects()
if reset_collider:
#this function takes so long that we'll have repopulated by the end of it
self._wi.reset_collider_node(repopulate=False)
reset_time = rospy.Time.now()
collision_objects = self._wi.collision_objects()
attached_objects = self._wi.attached_collision_objects()
used_names = []
for o in collision_objects:
used_names.append(o.id)
for ao in attached_objects:
used_names.append(ao.object.id)
res = self._do_detection(point_head_at)
if add_table_to_map:
table_name = self._add_table_to_map(res.detection.table, table_name)
#we want to return first the tableware objects we might want to pick up
tableware_pgs = []
#recognized means we recognized them but don't think
#they are anything we should pick up
recognized_pgs = []
#graspable means we could pick them up but we don't
#know what they are
graspable_pgs = []
for c in range(len(res.detection.clusters)):
#Compute the bounding box
try:
bres = self._bounding_box_srv(res.detection.clusters[c])
except rospy.ServiceException, e:
rospy.logerr('Unable to call cluster bounding box service. Exception was '+str(e)+
'. Ignoring cluster.')
continue
if bres.error_code != bres.SUCCESS:
rospy.logerr('Cluster bounding box service returned error '+str(bres.error_code)+
'. Ignoring cluster')
continue
go = GraspableObject()
go.reference_frame_id = res.detection.clusters[c].header.frame_id
go.cluster = res.detection.clusters[c]
pg = PickupGoal('', go, object_pose_stamped=bres.pose,
collision_support_surface_name=table_name, allow_gripper_support_collision=True)
co = CollisionObject()
label = 'graspable'
if self.get_model_description and self.get_mesh_from_database and\
len(res.detection.models[c].model_list) > 0 and\
res.detection.models[c].model_list[0].confidence < 0.005:
#we recognized this - figure out what it is
model_id = res.detection.models[c].model_list[0].model_id
go.potential_models = res.detection.models[c].model_list
rospy.logdebug('Potential models are: '+ str([m.model_id for m in res.detection.models[c].model_list]))
try:
descr = self.get_model_description(model_id)
if descr.return_code.code != descr.return_code.SUCCESS:
rospy.logwarn('Get model description returned error '+
str(descr.return_code.code))
for t in descr.tags:
if t in self.tableware_labels:
label = t
break
if label == 'graspable' and descr.tags:
label = descr.tags[0]
rospy.logdebug('Name of model is '+descr.name)
except rospy.ServiceException, e:
rospy.logerr('Call to get description threw exception '+str(e))
shape = None
if add_objects_as_mesh and (add_objects_as_mesh == 'all' or label in add_objects_as_mesh):
try:
shape = self.get_mesh_from_database(res.detection.models[c].model_list[0].model_id)
if label == 'graspable': label = 'recognized'
co.header = res.detection.models[c].model_list[0].pose.header
co.shapes.append(shape)
co.poses.append(res.detection.models[c].model_list[0].pose.pose)
pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id,
res.detection.models[c].model_list[0].pose)
except DetectionError:
shape = None
if not shape:
co = self._get_bounding_box_collision_object(bres)
pg.object_pose_stamped = tl.transform_pose_stamped(pg.target.reference_frame_id, bres.pose)
else:
def main():
obj = sys.argv[1]
my_world = WorldInterface()
my_world.attach_object_to_gripper('left_arm', obj)