def main():
# find objects on the table
head_point = PointStamped()
head_point.header.frame_id = '/torso_lift_link'
head_point.point.x = 0.5
head_point = None
detector = TablewareDetection(table_thickness=0.1)
det = detector.detect_objects(point_head_at=head_point)
if not det.pickup_goals:
rospy.loginfo('Nothing to pick up!')
return
# pick up the first object that we found
if len(sys.argv) > 1:
arm = sys.argv[1]
else:
arm = "right_arm"
rospy.loginfo("Trying arm: %s", arm)
pickplace = PickPlace()
pg = det.pickup_goals[0]
pg.arm_name = arm
try:
pickplace.pickup(pg)
except Exception as e:
rospy.logerr("No grasping, err: %s", e)
return
gripper = Gripper(arm)
gripper.open()
def main():
pub = rospy.Publisher('/fail_location', Marker)
pickplace = PickPlace(search_place=[])
place_pose = PoseStamped()
place_pose.header.frame_id = '/torso_lift_link'
place_pose.pose.position.x = 0.7
place_pose.pose.position.y = -0.2
place_pose.pose.position.z = -0.28
place_pose.pose.orientation.x = 0.707
place_pose.pose.orientation.w = 0.707
obj = sys.argv[1]
try:
place_goal = PlaceGoal('right_arm', [place_pose], \
collision_support_surface_name='table', \
collision_object_name=obj)
pickplace.place(place_goal)
except ManipulationError:
pass
# my_world = WorldInterface()
# fail_loc = my_world.collision_object(obj)
# rospy.loginfo("FAIL LOC="+str(fail_loc))
marker = Marker()
marker.header.frame_id = '/torso_lift_link'
marker.ns = ''
marker.id = 0
marker.type = Marker.SPHERE
marker.action = marker.ADD
marker.header.frame_id = place_pose.header.frame_id
marker.pose = place_pose.pose
marker.scale.x = 0.05
marker.scale.y = 0.05
marker.scale.z = 0.05
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 1.0
marker.color.a = 0.8
rospy.loginfo("marker msg=" + str(marker))
pub.publish(marker)
rospy.sleep(15.0)
def __init__(self, goal, mass, prims):
'''
@type goal: PoseStameped
@param goal: goal pose of the object
@type mass: float
@param mass: mass of the object
@type prims: list
@param prims: list of possible motion primitives for the given object
'''
self.goal = goal
self.mass = mass
self.prims = prims
self.my_world = world_interface.WorldInterface()
self.right_arm = arm_planner.ArmPlanner('right_arm')
self.left_arm = arm_planner.ArmPlanner('left_arm')
self.pickplace = PickPlace(search_place=[], place_on_table=False)
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, goal, prims):
'''
@type goal: PoseStameped
@param goal: goal pose of the object
@type prims: list
@param prims: list of possible motion primitives for the given object
'''
self.goal = goal
self.prims = prims
self.my_world = world_interface.WorldInterface()
self.right_arm = arm_planner.ArmPlanner('right_arm')
self.left_arm = arm_planner.ArmPlanner('left_arm')
self.my_arm_mover = arm_mover.ArmMover()
self.pickplace = PickPlace(search_place=[], place_on_table=False)
self._get_model_mesh_srv = rospy.ServiceProxy(
'/objects_database_node/get_model_mesh', GetModelMesh)
rospy.loginfo('Waiting for model mesh service')
self._get_model_mesh_srv.wait_for_service()
def main(goal, mass):
rospy.init_node("test_placing")
arm = arm_planner.ArmPlanner('right_arm')
pickplace = PickPlace(search_place=[])
obj_name = sys.argv[1]
# shape = Shape()
# shape.type = 1 # BOX
# shape.dimensions = [5,5,10]
# orien = Orientation(0.0, 0.0, 0.0)
# obj = Object(shape, mass, orien)
my_world = world_interface.WorldInterface()
objs = my_world.attached_collision_objects()
# objs = [toShape.get_obj()]
rospy.loginfo("collision objects="+str(len(objs)))
for obj in objs:
rospy.loginfo("id="+str(obj.object.id))
rospy.loginfo("name="+obj_name)
print "id="+str(obj.object.id)+" name"+str(obj_name)
if obj.object.id == obj_name:
for i in range(len(obj.object.shapes)):
orien = quat_to_orien(obj.object.poses[i].orientation)
my_obj = Object(obj.object.shapes[i], mass, orien)
#marker(my_obj.mesh_centroid())
placements = copy.deepcopy(my_obj.placements)
placements.reverse()
(height, orientation) = placements.pop()
print "height="+str(height)+" orien="+str(orientation)
pose = make_pose(goal, height, orientation)
# while not good(pose):
while not acceptable(arm, pose):
# while not acceptable(pickplace, obj_name, pose):
(height, orientation) = placements.pop()
print "height="+str(height)+" orien="+str(orientation)
pose = make_pose(goal, height, orientation)
print "pose= "+str(pose)
place(pickplace, obj_name, pose)
def __init__(self, goal, modes):
'''
@type goal: PoseStameped
@param goal: goal pose of the object
@type modes: list
@param modes: list of priors for each of the modes [tipping, sliding]
'''
self.goal = goal
self.modes = modes
self.my_world = world_interface.WorldInterface()
self.right_arm = arm_planner.ArmPlanner('right_arm')
self.left_arm = arm_planner.ArmPlanner('left_arm')
self.my_arm_mover = arm_mover.ArmMover()
self.pickplace = PickPlace(search_place=[], place_on_table=False)
self._get_model_mesh_srv = rospy.ServiceProxy(
'/objects_database_node/get_model_mesh', GetModelMesh)
rospy.loginfo('Waiting for model mesh service')
self._get_model_mesh_srv.wait_for_service()
class placer:
def __init__(self, goal, mass, prims):
'''
@type goal: PoseStameped
@param goal: goal pose of the object
@type mass: float
@param mass: mass of the object
@type prims: list
@param prims: list of possible motion primitives for the given object
'''
self.goal = goal
self.mass = mass
self.prims = prims
self.my_world = world_interface.WorldInterface()
self.right_arm = arm_planner.ArmPlanner('right_arm')
self.left_arm = arm_planner.ArmPlanner('left_arm')
self.pickplace = PickPlace(search_place=[], place_on_table=False)
def get_obj(self, name):
'''
Returns the object with the given id if it is attached in the collision map
@type name: String
@param name: name of the object in the collision map
@rtype arm_navigation_msgs.msg.attached_collision_object
@returns object if one exists in the collision map
'''
# other = self.my_world.
objs = self.my_world.attached_collision_objects()
for obj in objs:
if obj.object.id == name:
return obj
print "ERROR: OBJECT NAME DOES NOT EXIST"
return
def get_shape(self, obj):
'''
for a given object, returns its shape
@type obj: arm_navigation_msgs.msg.attached_collision_object
@param obj: detected object
@rtype arm_navigation_msgs.msg.shape
@returns the first (should be only 1) shape in an obj
'''
if len(obj.object.shapes) != 1:
print "ERROR: TOO MANY SHAPES: " + str(len(obj.object.shapes))
return obj.object.shapes[0]
def get_table_height(self):
'''
returns the height of the table in '/torso_lift_link' frame
@rtype float
@returns table height
'''
height = 0.0
objs = self.my_world.collision_objects()
for obj in objs:
print "obj id=" + str(obj.id)
if obj.id == "table":
table_pose = tf.transform_pose("/torso_lift_link",
obj.header.frame_id,
obj.poses[0])
height = table_pose.position.z + 0.01
return height
def get_gripper(self, obj, goal):
'''
finds the pose of the gripper for an object in its hand at the given goal pose
@type obj: arm_navigation_msgs.msg.attached_collision_object
@param obj: the object being held by the gripper
@type goal: PoseStamped
@param goal: goal pose for the object in torso_lift_link
@rtype PoseStamped
@returns pose of the gripper for the given object at the given goal
'''
# first get the object's current pose
obj_pose = tf.transform_pose("/torso_lift_link",
obj.object.header.frame_id,
obj.object.poses[0])
obj_pose_stamped = PoseStamped()
obj_pose_stamped.header.frame_id = "/torso_lift_link"
obj_pose_stamped.pose = obj_pose
# self.make_marker(obj_pose_stamped, namespace='obj_pose', color=(1.0,1.0,1.0,0.8))
# next get the gripper's current pose
gripper_pose_stamped = self.right_arm.get_hand_frame_pose(
frame_id="/torso_lift_link")
gripper_pose = gripper_pose_stamped.pose
# self.make_marker(obj_pose_stamped, namespace='gripper_pose', color=(1.0,0.0,0.0,0.8), mtype=Marker.ARROW)
# the position of the object when the wrist is the origin
# grasp = gripper_pose^{-1}*obj_pose
grasp = gt.inverse_transform_pose(obj_pose, gripper_pose)
#we need the position of the gripper when the object is at goal
#gripper_goal = goal*grasp^{-1}
origin = Pose()
origin.orientation.w = 1
#this is grasp^{-1}
grasp_inv = gt.inverse_transform_pose(origin, grasp)
#this is goal*grasp_inv
gripper_goal = gt.transform_pose(grasp_inv, goal.pose)
gripper_ps = PoseStamped()
gripper_ps.header.frame_id = goal.header.frame_id
gripper_ps.pose = gripper_goal
return gripper_ps
def move(self, arm, pose, collisions):
'''
@type pose: PoseStamped
@param pose: desired pose for the arm
@rtype boolean
@returns whether the move was successful
'''
# goal = PoseStamped()
# goal.header.frame_id = '/torso_lift_link'
# goal.pose.position.x = 0.7
# goal.pose.position.y = 0.2
# goal.pose.position.z = -0.28
# goal.pose.orientation.x = 0.707
# goal.pose.orientation.y = 0.0
# goal.pose.orientation.z = 0.0
# goal.pose.orientation.w = 0.707
# print "move pose="+str(goal)
my_arm_mover = arm_mover.ArmMover()
arm_name = arm #'left_arm'
handle = my_arm_mover.move_to_goal(arm_name,
pose,
ordered_collisions=collisions)
return handle.reached_goal()
def release(self, arm):
my_gripper = gripper.Gripper(arm)
my_gripper.open()
def make_marker(self, pose, namespace="my_namespace", mtype=Marker.SPHERE, \
scale=(0.05,0.05,0.05), color=(1.0,0.0,1.0,0.8)):
pub = rospy.Publisher('doodie', Marker)
count = 0
while count < 10:
marker = Marker()
marker.header.frame_id = pose.header.frame_id #'/torso_lift_link'
marker.ns = namespace
marker.id = 0
marker.type = mtype
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.r = color[0] #1.0
marker.color.g = color[1] #0.0
marker.color.b = color[2] #1.0
marker.color.a = color[3] #0.8
# print "marker pose="+str(pose)
#rospy.loginfo("MARKER msg="+str(marker))
pub.publish(marker)
rospy.sleep(0.1)
count = count + 1
def block(self, obs, i, dist, shape, goal):
'''
for a given degree of freedom, will find the best placement for the obstacle to block
that direction. moves the hand to that pose.
@type obs: arm_navigation_msgs.msg.collision_object
@param obs: obstacle (box representation of the other gripper)
@type i: int
@param i: index of the of the degree of freedom (x_trans,y_trans,z_trans,x_rot,y_rot,z_rot)
in the frame of the object
@type dist: float
@param dist: dist in the specified degree of freedom the obj will travel
@type shape: arm_navigation_msgs.msg.shape
@param shape: shape of obj
@type goal: PoseStamped
@param goal: goal pose of the object
@type PoseStamped
@returns the pose of the gripper positioned to block the given the degree of freedom
'''
# print "obs dim="+str(obs.dimensions)
buf = 0.02 #0.1#.2#0.05
x = goal.pose.position.x
y = goal.pose.position.y
z = goal.pose.position.z
x_displacement = shape.dimensions[0] / 2 + obs.dimensions[0] / 2 + buf
y_displacement = shape.dimensions[1] / 2 + obs.dimensions[1] / 2 + buf
z_displacement = shape.dimensions[2] / 2 + obs.dimensions[2] / 2 + buf
#print "shape dims="+str(shape.dimensions[1]/2)+" obs dims="+str(obs.dimensions[1]/2)
if i == 0 or i == 3: # x axis
if dist > 0:
x = x + x_displacement
else:
x = x - x_displacement
if i == 1 or i == 4: # y axis
if dist > 0:
y = y + y_displacement
else:
y = y - y_displacement
if i == 2 or i == 5: # z axis
if dist > 0:
z = z + z_displacement
else:
z = z - z_displacement
pose = PoseStamped()
pose.header.frame_id = "/torso_lift_link"
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z + 0.1
pose.pose.orientation.w = 0.707
pose.pose.orientation.x = 0.707
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0 #-0.707
self.make_marker(pose,
namespace="block",
mtype=Marker.ARROW,
scale=(0.1, 0.1, 0.05),
color=(0.0, 1.0, 0.0, 0.8))
success = self.move("left_arm", pose, None)
print "MOVE SUCCESS=" + str(success)
if not success:
print "MOVE FAILED"
return success
# return pose
def reachable(self, pose):
'''
determines whether the a pose is has an ik solution
@type pose:
@param pose:
@rtype boolean
@returns whether the pose is valid
'''
ik_sol = self.right_arm.get_ik(pose)
# rospy.loginfo("ERROR CODE="+str(ik_sol.error_code.val))
if ik_sol.error_code.val != 1:
# print "false. not reachable"
return False
else:
print "true. reachable"
return True
def place(self, arm, obj_name, pose):
'''
places the given object with the given arm in the give pose.
@type arm: String
@param arm: 'left_arm' or 'right_arm'. arm to place
@type obj_name: String
@param obj_name: name of the object being placed from the collision map
@type pose: PoseStamped
@param pose: pose to place the object
@rtype: boolean
@returns whether the place was successful
'''
try:
if arm == "right_arm":
place_goal = PlaceGoal('right_arm', [pose],
collision_support_surface_name='all',
collision_object_name=obj_name)
else:
place_goal = PlaceGoal('left_arm', [pose],
collision_support_surface_name='table',
collision_object_name=obj_name)
print "ABOUT TO PLACE"
doodie = self.pickplace.place(place_goal)
print "doodie=" + str(doodie)
except ManipulationError:
rospy.loginfo("POSE FAILED")
return False
return True
def make_pose(self, pose, h, orientation):
'''
@type pose: PoseStamped
@param pose: original pose
@type h: float
@param h: additional height added in the z direction
@type orientation: list
@param orientation: orientation to be added to pose
@rtype PoseStamped
@returns a pose modified by h and orientation
'''
pose = PoseStamped()
pose.header.frame_id = goal.header.frame_id
pose.pose.position.x = goal.pose.position.x
pose.pose.position.y = goal.pose.position.y
pose.pose.position.z = goal.pose.position.z + h
pose.pose.orientation = gt.multiply_quaternions(
goal.pose.orientation, orientation)
# pose.pose.orientation.x = #goal.pose.orientation.x + orientation.x#orientation[1]
# pose.pose.orientation.y = #goal.pose.orientation.y + orientation.y#orientation[2]
# pose.pose.orientation.z = #goal.pose.orientation.z + orientation.z#orientation[3]
# pose.pose.orientation.w = #goal.pose.orientation.w + orientation.w#orientation[0]
return pose
def get_best_release(self, mobj, obj, goal):
'''
finds the best release for the given object at the given goal and returns
the pose of the gripper for that release.
@type mobj: Mobject
@param mobj: Mobject of the object to be placed
@type obj: arm_navigation_msgs.msg.collision_object
@param obj: obj to be placed
@type goal: PoseStamped
@param goal: goal pose for the object
@rtype PoseStamped
@returns pose of the gripper for the object at the goal
'''
# height = 0.15
# orientation = Quaternion()
# orientation.w = 1.0
# pose = self.make_pose(goal, height, orientation)
# gripper = self.get_gripper(obj, pose)
# orien = Orientation(0.0,0.0,0.0)
placements = mobj.object.best_placements(goal)
placements.reverse()
(height, orientation) = placements.pop()
pose = self.make_pose(goal, height, orientation)
# print "REACHABLE="+str(reachable(arm, pose))
print "start height=" + str(height) + " orien=" + str(orientation)
gripper = self.get_gripper(obj, pose)
while not self.reachable(gripper):
if len(placements) == 0:
print 'SHIT SNACKS'
print "F**K height=" + str(height) + " orien=" + str(
orientation)
break
# print "len(placements)="+str(len(placements))
(height, orientation) = placements.pop()
# print "height="+str(height)+" orien="+str(orientation)
pose = self.make_pose(goal, height, orientation)
gripper = self.get_gripper(obj, pose)
print "height=" + str(height) + " orien=" + str(orientation)
self.make_marker(pose,
namespace='farts',
mtype=Marker.ARROW,
color=(0.0, 0.0, 1.0, 0.8))
self.make_marker(gripper,
namespace="poopie",
mtype=Marker.ARROW,
scale=(0.1, 0.1, 0.05))
# print "pose="+str(pose)
# print "gripper="+str(gripper)
return (pose, gripper)
def make_gripper_obs(self):
obs = Shape()
obs.type = 1
obs.dimensions = [0.10, 0.10, 0.10]
return obs
def run(self): #(goal, mass, prims, obs):
search_start = time.time()
obj_name = sys.argv[1]
table_height = self.get_table_height()
self.goal.pose.position.z = table_height + 0.01
print "goal=" + str(self.goal)
self.make_marker(self.goal,
"goal_pose",
mtype=Marker.ARROW,
color=[0.0, 1.0, 0.0, 0.8])
obj = self.get_obj(obj_name)
shape = self.get_shape(obj)
obs = self.make_gripper_obs()
mobj = Mobject(mass, shape, prims)
pose = PoseStamped()
pose.header.frame_id = "/torso_lift_link"
pose.pose.position.x = goal.pose.position.x
pose.pose.position.y = goal.pose.position.y
pose.pose.position.z = goal.pose.position.z + 0.02 #.1
pose.pose.orientation = goal.pose.orientation
vels = [0.0, 0.0, 0.0]
# self.place('right_arm', obj_name, pose)
print "DIMS=" + str(shape.dimensions)
(release_pose, wrist_release) = self.get_best_release(mobj, obj, goal)
# release_pose = pose
self.make_marker(release_pose, namespace="release_pose")
print "release_pose=" + str(release_pose)
search_end = time.time()
# block_start = time.time()
# dof = mobj.find_dof(goal, release_pose, vels)
# print "DOF="+str(dof.degrees)
# success = False
# order = dof.index_order()
# while not success: # not passive placing
# if len(order) != 0.0:
# next = order.pop(0)
# print "NEXT="+str(next)
# success = self.block(obs, next, dof[next], shape, goal)
# print "SUCCESS="+str(success)
# else:
# print "BLOCK FAILED"
# break
# block_end = time.time()
print "waiting for place:"
# raw_input()
ignore = OrderedCollisionOperations()
gripper = CollisionOperation()
gripper.object1 = CollisionOperation.COLLISION_SET_ALL #"l_end_effector"
gripper.object2 = CollisionOperation.COLLISION_SET_ALL
gripper.operation = CollisionOperation.DISABLE
wrist = CollisionOperation()
wrist.object1 = "l_wrist_roll_link"
wrist.object2 = CollisionOperation.COLLISION_SET_ALL
wrist.operation = CollisionOperation.DISABLE
obj = CollisionOperation()
obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
obj.object2 = "table" #CollisionOperation.COLLISION_SET_ALL
obj.operation = CollisionOperation.DISABLE
ignore.collision_operations = [gripper, wrist, obj]
self.move("right_arm", wrist_release, ignore)
# self.release("right_arm")
# self.place('right_arm', obj_name, release_pose)
search = search_end - search_start
block = 0.0 #block_end - block_start
return (search, block)
class placer:
def __init__(self, goal, prims):
'''
@type goal: PoseStameped
@param goal: goal pose of the object
@type prims: list
@param prims: list of possible motion primitives for the given object
'''
self.goal = goal
self.prims = prims
self.my_world = world_interface.WorldInterface()
self.right_arm = arm_planner.ArmPlanner('right_arm')
self.left_arm = arm_planner.ArmPlanner('left_arm')
self.my_arm_mover = arm_mover.ArmMover()
self.pickplace = PickPlace(search_place=[], place_on_table=False)
self._get_model_mesh_srv = rospy.ServiceProxy(
'/objects_database_node/get_model_mesh', GetModelMesh)
rospy.loginfo('Waiting for model mesh service')
self._get_model_mesh_srv.wait_for_service()
def get_obj(self, name):
'''
Returns the object with the given id if it is attached in the collision map
@type name: String
@param name: name of the object in the collision map
@rtype arm_navigation_msgs.msg.attached_collision_object
@returns object if one exists in the collision map
'''
objs = self.my_world.attached_collision_objects()
for obj in objs:
if obj.object.id == name:
return obj
rospy.loginfo("ERROR: OBJECT NAME DOES NOT EXIST")
return
def get_shape(self, obj):
'''
for a given object, returns its shape
@type obj: arm_navigation_msgs.msg.attached_collision_object
@param obj: detected object
@rtype arm_navigation_msgs.msg.shape
@returns the first (should be only 1) shape in an obj
'''
if len(obj.object.shapes) != 1:
rospy.loginfo("ERROR: TOO MANY SHAPES: %s",
str(len(obj.object.shapes)))
return obj.object.shapes[0]
def get_table_height(self):
'''
returns the height of the table in '/torso_lift_link' frame
@rtype float
@returns table height
'''
height = 0.0
objs = self.my_world.collision_objects()
for obj in objs:
if obj.id == "table":
table_pose = tf.transform_pose("/torso_lift_link", \
obj.header.frame_id, \
obj.poses[0])
height = table_pose.position.z + 0.01
return height
def get_gripper(self, obj, goal):
'''
finds the pose of the gripper for an object in its hand at the given goal pose
@type obj: arm_navigation_msgs.msg.attached_collision_object
@param obj: the object being held by the gripper
@type goal: PoseStamped
@param goal: goal pose for the object in torso_lift_link
@rtype PoseStamped
@returns pose of the gripper for the given object at the given goal
'''
# first get the object's current pose
obj_pose = tf.transform_pose("/torso_lift_link", obj.object.header.frame_id, \
obj.object.poses[0])
obj_pose_stamped = PoseStamped()
obj_pose_stamped.header.frame_id = "/torso_lift_link"
obj_pose_stamped.pose = obj_pose
# self.make_marker(obj_pose_stamped, namespace='obj_pose', color=(1.0,1.0,1.0,0.8))
# next get the gripper's current pose
gripper_pose_stamped = self.right_arm.get_hand_frame_pose(\
frame_id="/torso_lift_link")
gripper_pose = gripper_pose_stamped.pose
# self.make_marker(obj_pose_stamped, namespace='gripper_pose', color=(1.0,0.0,0.0,0.8), mtype=Marker.ARROW)
# the position of the object when the wrist is the origin
# grasp = gripper_pose^{-1}*obj_pose
grasp = gt.inverse_transform_pose(obj_pose, gripper_pose)
#we need the position of the gripper when the object is at goal
#gripper_goal = goal*grasp^{-1}
origin = Pose()
origin.orientation.w = 1
#this is grasp^{-1}
grasp_inv = gt.inverse_transform_pose(origin, grasp)
#this is goal*grasp_inv
gripper_goal = gt.transform_pose(grasp_inv, goal.pose)
gripper_ps = PoseStamped()
gripper_ps.header.frame_id = goal.header.frame_id
gripper_ps.pose = gripper_goal
return gripper_ps
def move(self, arm, pose, collisions, try_hard):
'''
@type pose: PoseStamped
@param pose: desired pose for the arm
@rtype boolean
@returns whether the move was successful
'''
arm_name = arm #'left_arm'
handle = self.my_arm_mover.move_to_goal(arm_name, pose, \
ordered_collisions=collisions, \
try_hard=try_hard)
rospy.loginfo("MOVE SUCCESS=%s", str(handle.reached_goal()))
return handle.reached_goal()
def release(self, arm, pose, collisions, obj_type):
'''
Releases the object: opens gripper and pulls it back out of the way.
@type arm: String
@param arm: 'left_arm' or 'right_arm'
@type pose: PoseStamped
@param pose: pose the obj is released from
@type collisions: OrderedCollisionOperations
@param collisions: list of collisions to ignore in move
'''
# open gripper
#raw_input()
rospy.loginfo("Releasing the object.")
my_gripper = gripper.Gripper(arm)
my_gripper.open()
#raw_input()
# pull hand back
rospy.loginfo("Pulling the gripper out of the way.")
back = PoseStamped()
back.header.frame_id = "torso_lift_link"
if obj_type == "box":
back.pose.position.x = pose.pose.position.x
back.pose.position.y = pose.pose.position.y - 0.15
else:
back.pose.position.x = pose.pose.position.x - 0.15
back.pose.position.y = pose.pose.position.y
back.pose.position.z = pose.pose.position.z
back.pose.orientation = pose.pose.orientation
self.make_marker(back, namespace="pull_out", mtype=Marker.ARROW, \
scale=(0.1,0.1,0.05), color=(1.0,1.0,0.0,0.8))
if arm == 'right_arm':
joint_trajectory = self.right_arm.plan_interpolated_ik(\
back, collision_aware=False, consistent_angle=2.0*np.pi)
else:
joint_trajectory = self.left_arm.plan_interpolated_ik(\
back, collision_aware=False, consistent_angle=2.0*np.pi)
self.my_arm_mover.execute_joint_trajectory(arm, joint_trajectory)
def make_marker(self, pose, namespace="my_namespace", mtype=Marker.SPHERE, \
scale=(0.05,0.05,0.05), color=(1.0,0.0,1.0,0.8)):
'''
Publishes a marker in rviz.
@type pose: PoseStamped
@param pose: pose of the marker
@type namespace: String
@param namespace: name of the marker in rviz
@type mtype: int
@param mtype: type of marker
@type scale: array
@param scale: (x,y,z) size of the marker
@type color: array
@param color: [r,g,b,a] color of the marker
'''
pub = rospy.Publisher('robust_placing', Marker)
count = 0
while count < 10:
marker = Marker()
marker.header.frame_id = pose.header.frame_id #'/torso_lift_link'
marker.ns = namespace
marker.id = 0
marker.type = mtype
marker.action = Marker.ADD
marker.pose = pose.pose
marker.scale.x = scale[0]
marker.scale.y = scale[1]
marker.scale.z = scale[2]
marker.color.r = color[0]
marker.color.g = color[1]
marker.color.b = color[2]
marker.color.a = color[3]
pub.publish(marker)
rospy.sleep(0.1)
count = count + 1
def block(self, obs, i, dist, shape, goal):
'''
for a given degree of freedom, will find the best placement for the obstacle to block
that direction. moves the hand to that pose.
@type obs: arm_navigation_msgs.msg.collision_object
@param obs: obstacle (box representation of the other gripper)
@type i: int
@param i: index of the of the degree of freedom (x_trans,y_trans,z_trans,x_rot,y_rot,z_rot)
in the frame of the object
@type dist: float
@param dist: dist in the specified degree of freedom the obj will travel
@type shape: arm_navigation_msgs.msg.shape
@param shape: shape of obj
@type goal: PoseStamped
@param goal: goal pose of the object
@type boolean
@returns the success of the block move
'''
buf = 0.02 #0.1#.2#0.05
x = goal.pose.position.x
y = goal.pose.position.y
z = goal.pose.position.z
if shape.type == Shape.MESH:
bb = gt.bounding_box_corners(shape)
dims = [(bb[7][0] - bb[0][0]), (bb[7][1] - bb[0][1]),
(bb[7][2] - bb[0][2])]
x_displacement = dims[0] / 2 + obs.dimensions[0] / 2 + buf
y_displacement = dims[1] / 2 + obs.dimensions[1] / 2 + buf
z_displacement = dims[2] / 2 + obs.dimensions[2] / 2 + buf
else:
x_displacement = shape.dimensions[0] / 2 + obs.dimensions[
0] / 2 + buf
y_displacement = shape.dimensions[1] / 2 + obs.dimensions[
1] / 2 + buf
z_displacement = shape.dimensions[2] / 2 + obs.dimensions[
2] / 2 + buf
#print "shape dims="+str(shape.dimensions[1]/2)+" obs dims="+str(obs.dimensions[1]/2)
pose = PoseStamped()
pose.header.frame_id = "/torso_lift_link"
pose.pose.orientation.w = 0.707
pose.pose.orientation.x = 0.707
pose.pose.orientation.y = 0.0
pose.pose.orientation.z = 0.0 #-0.707
if i == 0 or i == 3: # x axis
if dist == 2.0: # stick
pose.pose.orientation.w = 0.707
pose.pose.orientation.x = 0.0
pose.pose.orientation.y = 0.707
pose.pose.orientation.z = 0.0
x = x - x_displacement + .02
y = y + 0.09
z = z + 0.18
elif dist > 0: # tip
x = x + x_displacement
else:
x = x - x_displacement
if i == 1 or i == 4: # y axis
if dist > 0:
y = y + y_displacement
else:
y = y - y_displacement
if i == 2 or i == 5: # z axis
if dist > 0:
z = z + z_displacement
else:
z = z - z_displacement
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z + 0.02
self.make_marker(pose,namespace="block", mtype=Marker.ARROW, scale=(0.1,0.1,0.05), \
color=(0.0,1.0,0.0,0.8))
success = self.move("left_arm", pose, None, False)
rospy.loginfo("MOVE SUCCESS=%s", str(success))
return success
def reachable(self, pose, collisions):
'''
determines whether the a pose is has an ik solution
@type pose:
@param pose:
@rtype boolean
@returns whether the pose is valid
'''
ik_sol = self.right_arm.get_ik(pose, ordered_collisions=collisions)
#rospy.loginfo("ERROR CODE=%s"+str(ik_sol.error_code.val))
if ik_sol.error_code.val != 1:
return False
else:
return True
def place(self, arm, obj_name, pose):
'''
places the given object with the given arm in the give pose.
@type arm: String
@param arm: 'left_arm' or 'right_arm'. arm to place
@type obj_name: String
@param obj_name: name of the object being placed from the collision map
@type pose: PoseStamped
@param pose: pose to place the object
@rtype: boolean
@returns whether the place was successful
'''
try:
if arm == "right_arm":
place_goal = PlaceGoal('right_arm', [pose], \
collision_support_surface_name='all', \
collision_object_name=obj_name)
else:
place_goal = PlaceGoal('left_arm', [pose], \
collision_support_surface_name='table', \
collision_object_name=obj_name)
place_result = self.pickplace.place(place_goal)
rospy.loginfo("Place result=%s", str(place_result))
except ManipulationError:
rospy.loginfo("POSE FAILED")
return False
return True
def make_pose(self, pose, h, orientation):
'''
@type pose: PoseStamped
@param pose: original pose
@type h: float
@param h: additional height added in the z direction
@type orientation: list
@param orientation: orientation to be added to pose
@rtype PoseStamped
@returns a pose modified by h and orientation
'''
pose = PoseStamped()
pose.header.frame_id = self.goal.header.frame_id
pose.pose.position.x = self.goal.pose.position.x
pose.pose.position.y = self.goal.pose.position.y
pose.pose.position.z = self.goal.pose.position.z + h
pose.pose.orientation = gt.multiply_quaternions(
self.goal.pose.orientation, orientation)
return pose
def get_best_release(self, mobj, obj, goal, collisions):
'''
finds the best release for the given object at the given goal and returns
the pose of the gripper for that release.
@type mobj: Mobject
@param mobj: Mobject of the object to be placed
@type obj: arm_navigation_msgs.msg.collision_object
@param obj: obj to be placed
@type goal: PoseStamped
@param goal: goal pose for the object
@rtype PoseStamped
@returns pose of the gripper for the object at the goal
'''
placements = mobj.object.best_placements(goal)
placements.reverse()
(height, orientation) = placements.pop()
pose = self.make_pose(goal, height, orientation)
gripper = self.get_gripper(obj, pose)
while not self.reachable(gripper, collisions):
if len(placements) == 0:
rospy.loginfo("ERROR: No reachable place pose.")
break
(height, orientation) = placements.pop()
pose = self.make_pose(goal, height, orientation)
gripper = self.get_gripper(obj, pose)
self.make_marker(pose, namespace='place_pose', mtype=Marker.ARROW, \
color=(0.0,0.0,1.0,0.8))
self.make_marker(gripper, namespace="place_gripper_pose", mtype=Marker.ARROW, \
scale=(0.1,0.1,0.05))
return (pose, gripper)
def make_gripper_obs(self):
'''
Creates a box obstacle from the dimensions of the gripper.
'''
obs = Shape()
obs.type = 1
obs.dimensions = [0.10, 0.10, 0.10]
return obs
def make_plate(self, obj_name, plate):
'''
Converts the given obj in the collision map into a plate mesh in the database
@type obj_name: String
@param obj_name: name of the obj in the collision map ("graspable_object_****")
@type plate: int
@param plate: either the big (18980) or small (18979) plate
'''
mesh = self._get_model_mesh_srv(plate)
co = self.my_world.attached_collision_object(obj_name)
co.object.shapes[0] = mesh.mesh
co.object.poses[0].orientation.x = 0.707
co.object.poses[0].orientation.y = 0
co.object.poses[0].orientation.z = 0
co.object.poses[0].orientation.w = 0.707
self.my_world.add_attached_object(co)
def get_collisions(self):
'''
creates list of collisions to be ignored.
@rtype OrderedCollisionOperations
@returns list of collisions to be ignored
'''
ignore = OrderedCollisionOperations()
gripper = CollisionOperation()
gripper.object1 = "l_end_effector"
gripper.object2 = CollisionOperation.COLLISION_SET_ALL
gripper.operation = CollisionOperation.DISABLE
wrist = CollisionOperation()
wrist.object1 = "l_wrist_roll_link"
wrist.object2 = CollisionOperation.COLLISION_SET_ALL
wrist.operation = CollisionOperation.DISABLE
obj = CollisionOperation()
obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
obj.object2 = "table"
obj.operation = CollisionOperation.DISABLE
map_obj = CollisionOperation()
map_obj.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
map_obj.object2 = "collision_map"
map_obj.operation = CollisionOperation.DISABLE
lfing = CollisionOperation()
lfing.object1 = "r_gripper_r_finger_tip_link"
lfing.object2 = "table"
lfing.operation = CollisionOperation.DISABLE
rfingtip = CollisionOperation()
rfingtip.object1 = "r_gripper_l_finger_tip_link"
rfingtip.object2 = "table"
rfingtip.operation = CollisionOperation.DISABLE
rfing = CollisionOperation()
rfing.object1 = "r_gripper_l_finger_link"
rfing.object2 = "table"
rfing.operation = CollisionOperation.DISABLE
forearm = CollisionOperation()
forearm.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
forearm.object2 = "l_forearm_link"
forearm.operation = CollisionOperation.DISABLE
flex = CollisionOperation()
flex.object1 = CollisionOperation.COLLISION_SET_ATTACHED_OBJECTS
flex.object2 = "l_wrist_flex_link"
flex.operation = CollisionOperation.DISABLE
ops = []
op = CollisionOperation()
op.operation = op.DISABLE
op.object1 = op.COLLISION_SET_ATTACHED_OBJECTS
for l in self.my_world.hands['left_arm'].hand_links:
op.object2 = l
ops.append(copy.deepcopy(op))
ignore.collision_operations = [
gripper, wrist, obj, map_obj, lfing, rfing, rfingtip, forearm, flex
] + ops
return ignore
def setup(self, obj_type, obj_name):
'''
@type obj_type: String
@param obj_type: type of place, "dropping","passive","robust"
@type obj_name: String
@param obj_name: type of object, "paper_plate","plastic_plate","box"
'''
# self.my_world.reset_collider_node() # takes a long time
table_height = self.get_table_height()
self.goal.pose.position.z = table_height + 0.1 #+ 0.095 # height of the gripper
rospy.loginfo("Place goal=%s", str(self.goal))
self.make_marker(self.goal,
"goal_pose",
mtype=Marker.ARROW,
color=[0.0, 1.0, 0.0, 0.8])
if obj_type == "paper_plate":
self.make_plate(obj_name, 18979) # small plate
elif obj_type == "plastic_plate":
self.make_plate(obj_name, 18980) # big plate
obj = self.get_obj(obj_name)
shape = self.get_shape(obj)
mobj = Mobject(shape, self.prims)
obs = self.make_gripper_obs()
return (obj, obs, mobj, shape)
def drop(self, obj, obj_type, drop_height=0.1):
'''
@type obj: physics.Object
@param obj: object being dropped
@type drop_height: float
@param drop_height: height to drop the object from (above the table)
'''
pose = PoseStamped()
pose.header.frame_id = "/torso_lift_link"
pose.pose.position.x = self.goal.pose.position.x
pose.pose.position.y = self.goal.pose.position.y
pose.pose.position.z = self.goal.pose.position.z + drop_height #.1#.05# + 0.02 #.1
pose.pose.orientation = self.goal.pose.orientation
ignore = self.get_collisions()
gripper = self.get_gripper(obj, pose)
success = self.move("right_arm", gripper, ignore, True)
self.release("right_arm", gripper, ignore, obj_type)
def passive(self, obj, mobj, obj_type):
'''
@type obj: physics.Object
@param obj: object being placed
@type mobj: mobject.Mobject
@param mobj: object being placed
@rtype boolean
@returns success of passive place
'''
ignore = self.get_collisions()
(release_pose,
wrist_release) = self.get_best_release(mobj, obj, self.goal, ignore)
self.make_marker(release_pose, namespace="release_pose")
print "release_pose=" + str(release_pose)
success = self.move("right_arm", wrist_release, ignore, True)
self.release("right_arm", wrist_release, ignore, obj_type)
return success
def robust(self, obj, mobj, obs, obj_type, shape):
'''
@type obj: physics.Object
@param obj: object being placed
@type mobj: mobject.Mobject
@param obj: object being placed
@type obs: Shape
@param obs: the gripper as an obstacle
@rtype boolean
@returns succes of the robust place
'''
ignore = self.get_collisions()
(release_pose,
wrist_release) = self.get_best_release(mobj, obj, self.goal, ignore)
self.make_marker(release_pose, namespace="release_pose")
print "release_pose=" + str(release_pose)
vels = [0.0, 0.0, 0.0]
dof = mobj.find_dof(self.goal, release_pose, vels)
print "DOF=" + str(dof.degrees)
success = False
order = dof.index_order()
while not success: # not passive placing
if len(order) != 0.0:
next = order.pop(0)
#print "NEXT="+str(next)
success = self.block(obs, next, dof[next], shape, self.goal)
rospy.loginfo("Block success=%s", str(success))
else:
rospy.loginfo("Block failed.")
break
success = self.move("right_arm", wrist_release, ignore, True)
self.release("right_arm", wrist_release, ignore, obj_type)
return success
def run(self, action, obj_type, obj_name):
'''
@type action: String
@param action: type of place, "dropping","passive","robust"
@type obj_type: String
@param obj_type: type of object, "paper_plate","plastic_plate","box"
@type obj_name: String
@param obj_name: name of the object in the collision map, "graspable_object_****"
@rtype float
@returns time for execution
'''
(obj, obs, mobj, shape) = self.setup(obj_type, obj_name)
if action == 'dropping':
self.drop(obj, obj_type)
return time.time()
elif action == "passive":
success = self.passive(obj, mobj, obj_type)
return time.time()
elif action == "robust":
success = self.robust(obj, mobj, obs, obj_type, shape)
return time.time()
else:
rospy.loginfo("ERROR: %s not a valid action", str(action))
return time.time()
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 main():
rospy.init_node("demo_berkeley", anonymous=True)
sm = smach.StateMachine(outcomes=["success",
"failure"])
base_mover = base.Base()
detector = TablewareDetection()
pickplace = PickPlace()
pickplace.min_open_gripper_pos = 0.0014
rospy.loginfo("Waiting for find_cluster_bounding_box2 service")
find_box = rospy.ServiceProxy("/find_cluster_bounding_box", FindClusterBoundingBox)
world = two_arms_states.WorldState(find_box)
interface = WorldInterface()
tasks = Tasks()
placer_r = SensingPlacer('right_arm')
placer_l = SensingPlacer('left_arm')
head = Head()
rospy.loginfo("Creating State Machine")
inspector = StateInspector(two_arms_states)
inspector.world = world
inspector.tasks = tasks
inspector.base_mover = base_mover
inspector.interface = interface
inspector.detector = detector
inspector.placer_r = placer_r
inspector.placer_l = placer_l
inspector.pickplace = pickplace
inspector.head = head
inspector.find_box = find_box
with sm:
smach.StateMachine.add("reset",
inspector(two_arms_states.Resetter),
transitions={"success":"move_arms_side"})
smach.StateMachine.add("move_arms_side",
inspector(two_arms_states.MoveArmsToSide),
transitions={"success":"spinning_table_pickup"})
spinning_sm = create_spinning_table_clear_sm(inspector)
smach.StateMachine.add("spinning_table_pickup", spinning_sm,
transitions = {"success":"success",
"failure":"failure"}
)
# clean_table = create_clean_table_sm(inspector)
# smach.StateMachine.add("clean_table", clean_table,
# transitions = {"success":"success",
# "failure":"failure"
# }
# )
# setup_table = create_setup_table_sm(inspector)
# smach.StateMachine.add("setup_table", setup_table,
# transitions = {"success":"success",
# "failure":"failure"
# }
# )
sis = smach_ros.IntrospectionServer('sushi_sm', sm, '/SM_ROOT')
sis.start()
outcome = sm.execute()
rospy.loginfo("Outcome: %s", outcome)
sis.stop()
