def placeontray(self, unused1, obj_name, unused2, tray_name, unused4):
"""Put an item on the tray.
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if (not len(self.tray_stack) == 0 and
not tray_world.can_stack(self.tray_stack[-1], obj)):
e = ExecutingException("Incompatible objects")
e.robot = self.robot
e.object_to_grasp = obj
e.stacktop = self.tray_stack[-1]
raise e
T = tray_world.tray_putdown_pose(tray, self.tray_stack)
try:
(pose,
sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.robot,
T,
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on tray has problems!")
self.pause("Going to the tray")
self.robot.SetTransform(pose)
self.pause("Arm/Torso in position")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(sol,
self.robot.GetActiveManipulator().GetArmIndices())
print "Releasing object"
self.robot.Release(obj)
self.tray_stack.append(obj)
#putting the object straight
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
T[2,3] -= 0.05
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
print "cache is: ", self.grasping_locations_cache
raw_input("Press return to continue")
print "Back to rest"
utils.pr2_tuck_arm(self.robot)
print "The tray now has: ", self.tray_stack
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" % (obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError(
"The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3, :3] = rot_mat
T[2, 3] += 0.03
try:
(pose, sol, torso_angle
) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials=1000)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException(
"Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(
self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
def putdown(self, obj_name, table_name, _unused1):
print "Putting down object %s on %s" %(obj_name, table_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if table_name.startswith("dest_"):
#this is a fixed location!!!
T = getattr(tray_world, table_name, None)
if T is None:
raise ValueError("The location %s is unknown! check spelling?" % table_name)
T = T.copy()
#put the gripper facing down
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T[:3,:3] = rot_mat
T[2, 3] += 0.03
try:
(pose,
sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
only_reachable=False,
max_trials =1000
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on location has problems!")
else:
table = self.env.GetKinBody(table_name)
if table is None:
raise ValueError("Object %s does not exist" % table_name)
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_surface_pose(self.getGoodBodies(),
self.robot,
table,
)
except generate_reaching_poses.GraspingPoseError, e:
raise e
def placeontray(self, unused1, obj_name, unused2, tray_name, unused4):
"""Put an item on the tray.
"""
tray = self.env.GetKinBody(tray_name)
if tray is None:
raise ValueError("Object %s does not exist" % tray_name)
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
if (not len(self.tray_stack) == 0
and not tray_world.can_stack(self.tray_stack[-1], obj)):
e = ExecutingException("Incompatible objects")
e.robot = self.robot
e.object_to_grasp = obj
e.stacktop = self.tray_stack[-1]
raise e
T = tray_world.tray_putdown_pose(tray, self.tray_stack)
try:
(pose, sol,
torso_angle) = generate_reaching_poses.get_collision_free_ik_pose(
self.getGoodBodies(),
self.env,
obj,
self.robot,
T,
)
except generate_reaching_poses.GraspingPoseError:
raise ExecutingException("Putting down on tray has problems!")
self.pause("Going to the tray")
self.robot.SetTransform(pose)
self.pause("Arm/Torso in position")
self.robot.SetDOFValues([torso_angle],
[self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
print "Releasing object"
self.robot.Release(obj)
self.tray_stack.append(obj)
#putting the object straight
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
T[2, 3] -= 0.05
obj.SetTransform(T)
try:
del self.grasping_locations_cache[obj_name]
except KeyError:
print "funny, object ", obj_name, " was not in cache.. something wrong here!"
print "cache is: ", self.grasping_locations_cache
raw_input("Press return to continue")
print "Back to rest"
utils.pr2_tuck_arm(self.robot)
print "The tray now has: ", self.tray_stack
