def drop(self, obj, table):
pos1 = [0.4, -0.7, 1.1]
rot_z = [0.7071, 0, 0, -0.7071]
rot_x = [0, 1, 0, 0]
rot = openravepy.quatMult(rot_z, rot_x).tolist()
traj1, _ = self.traj_generator.traj_from_pose(pos1, rot)
with self.env:
# saving values
orig_values = self.robot.GetDOFValues(
self.robot.GetManipulator('rightarm').GetArmIndices())
self.robot.SetDOFValues(traj1[-1],
self.robot.GetManipulator('rightarm').GetArmIndices())
pos2 = [0.0, -0.7, 1.0]
traj2, _ = self.traj_generator.traj_from_pose(pos2, rot)
# reset
self.robot.SetDOFValues(orig_values,
self.robot.GetManipulator('rightarm').GetArmIndices())
self._execute_traj(traj1.tolist() + traj2.tolist())
# open gripper
self.robot.Release(obj)
if self.use_ros:
self.pr2.rgrip.open()
# transforming the object
T = obj.GetTransform()
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
_, _, _, _, z = utils.get_object_limits(table)
T[2, 3] = z
obj.SetTransform(T)
def put_left_arm_over_tray(robot, tray):
"""Move the robot's left arm so that it's about to grasp the right edge
of the tray. Raises a generate_reaching_poses.GraspingPoseError if no
IK solution can be found.
Parameters:
robot: a Robot instance
tray: a KinBody instance
"""
manip = robot.GetManipulator('leftarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot,iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
min_x, max_x, min_y, max_y, z = utils.get_object_limits(tray)
z -= 0.06
x = min_x + (max_x - min_x)/2
y = max_y - 0.02
gripper_angle = (np.pi, 0., 0) #pointing downward
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
T = np.eye(4)
T[:3,:3] = rot_mat
T[:3,3] = [x,y,z]
sol = generate_reaching_poses.check_reachable(env, tray, manip, [T], False)
if sol is not None:
robot.SetDOFValues(sol, manip.GetArmIndices())
#opening gripper
robot.SetDOFValues([0.2], manip.GetGripperJoints())
else:
raise generate_reaching_poses.GraspingPoseError("No IK solution for tray right side")
def create_cylinder(env, table, radius, height, body_name, max_radial_distance, rand, color):
robot_pos = openravepy.poseFromMatrix(env.GetRobots()[0].GetTransform())[4:7]
min_x, max_x, min_y, max_y, table_height = utils.get_object_limits(table)
x = rand.uniform(min_x + radius, max_x - radius)
y = rand.uniform(min_y + radius, max_y - radius)
# while (x - robot_pos[0])**2 + (y - robot_pos[1])**2 > max_radial_distance:
# x = rand.uniform(min_x + radius, max_x - radius)
# y = rand.uniform(min_y + radius, max_y - radius)
z = table_height + height / 2
t = openravepy.matrixFromPose([1, 0, 0, 0, x, y, z])
cylinder = object_models.create_cylinder(env, body_name, t, [radius, height], color)
env.Add(cylinder, False)
return cylinder
def spawn_table_pr2(env, robot_dist_from_table, tabletype='rll'):
if tabletype == 'rll':
thickness = 0.2
legheight = 0.6
table = object_models.create_table(env, TABLE_NAMES[tabletype], 2.235, 0.94, thickness, 1.3, 0.6, legheight)
elif tabletype == 'small':
thickness = 0.2
legheight = 0.6
table = object_models.create_table(env, TABLE_NAMES[tabletype], 0.7 - robot_dist_from_table, 1.5, thickness, 0.2 - robot_dist_from_table, 0.2, legheight)
x = -utils.get_object_limits(table)[0] + 0.35 + robot_dist_from_table
y = 0
z = thickness / 2 + legheight
table.SetTransform(openravepy.matrixFromPose([1, 0, 0, 0, x, y, z]))
# env.AddKinBody(table)
env.AddRobot(table)
# robot = env.ReadRobotXMLFile("../models/pr2/pr2-head-kinect.xml")
robot = env.ReadRobotXMLFile("../models/pr2/pr2.zae")
env.Add(robot)
def generate_manip_above_surface(obj, num_poses = 20):
gripper_angle = (np.pi, 0., 0) #just got this from trial and test
rot_mat = openravepy.rotationMatrixFromAxisAngle(gripper_angle)
min_x, max_x, min_y, max_y, z = utils.get_object_limits(obj)
gripper_height = 0.18
z += gripper_height + 0.03
poses = []
for _ in range(num_poses):
x = np.random.uniform(min_x, max_x)
y = np.random.uniform(min_y, max_y)
T = np.eye(4)
T[:3,:3] = rot_mat
T[:3,3] = [x,y,z]
poses.append(T)
return poses
class Executor(object):
def __init__(self, robot, viewer=False):
self.robot = robot
self.env = robot.GetEnv()
self.grasping_locations_cache = {}
self.viewMode = viewer
self.tray_stack = []
self.unMovableObjects = ['table']
self.objSequenceInPlan = []
self.handled_objs = set()
v = self.robot.GetActiveDOFValues()
v[self.robot.GetJoint('torso_lift_joint').GetDOFIndex()] = 1.0
self.robot.SetDOFValues(v)
if use_ros:
self.pr2robot = PR2Robot(self.env)
self.pr2robot.robot = self.robot
self.arm_mover = PR2MoveArm()
#loading the IK models
utils.pr2_tuck_arm(robot)
robot.SetActiveManipulator('leftarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
robot.SetActiveManipulator('rightarm')
ikmodel = openravepy.databases.inversekinematics.InverseKinematicsModel(
robot, iktype=openravepy.IkParameterization.Type.Transform6D)
if not ikmodel.load():
ikmodel.autogenerate()
def getGoodBodies(self):
if not doJointInterpretation:
body_filter = lambda b: b.GetName().startswith("random") or\
b.GetName().startswith('object')
else:
futureObjects = set(self.objSequenceInPlan) - self.handled_objs
body_filter = lambda b: (b.GetName() not in futureObjects) \
and (b.GetName() not in self.unMovableObjects)
return filter(body_filter, self.env.GetBodies())
def setObjSequenceInPlan(self, objList):
self.objSequenceInPlan = objList
print
print "Will try to pick objs in the order " + repr(objList)
def clear_gp_cache(self):
self.grasping_locations_cache = {}
self.objSequenceInPlan = []
def pause(self, msg=None):
if self.viewMode:
if msg is None:
raw_input("Press return to continue")
else:
print msg
#time.sleep(0.5)
raw_input(msg + "... [press return]")
def moveto(self, _unused1, pose):
if type(pose) is str:
print "Pose ", pose, " ignored"
return
else:
print "Moving to pose "
self.robot.SetTransform(pose)
def movetowithinr1(self, _unused1, unused2):
print "Ignore me!"
def movetowithinr2(self, _unused1, unused2):
print "ignore me !"
# def movetoacrossrooms(self, _unused1, unused2, unused):
# print "ignore me !"
def movetoacrossrooms(self, _unused1, unused2):
print "ignore me !"
def grasp(self, obj_name, _unused1, _unused2):
# update openrave
if use_ros:
self.pr2robot.update_rave()
print "Grasping object ", obj_name
obj = self.env.GetKinBody(obj_name)
if obj is None:
raise ValueError("Object %s does not exist" % obj_name)
cached_value = self.grasping_locations_cache.get(obj_name, None)
if cached_value is None:
print "Object %s is not cached, looking for a value" % obj_name
try:
pose, sol, torso_angle = generate_reaching_poses.get_collision_free_grasping_pose(
self.getGoodBodies(),
self.robot,
obj,
max_trials=collision_free_grasping_samples)
self.grasping_locations_cache[
obj_name] = pose, sol, torso_angle
except generate_reaching_poses.GraspingPoseError:
e = ExecutingException("Object in collision")
e.robot = self.robot
e.object_to_grasp = obj
raise e
else:
print "Object %s already cached" % obj_name
pose, sol, torso_angle = cached_value
if OpenRavePlanning:
test_grasp_move.move_arm(robot, pose, 'r')
if use_ros:
# # object matching
detector, _ = detector_and_cluster_map
# index = cluster_map[obj_name]
# # generating grasps
# box_msg = detector.detect_bounding_box(detector.last_detection_msg.detection.clusters[index])
# desired_grasps = grasp_generator.generate_grasps(box_msg, 8)
# grasp_generator.draw_grasps(desired_grasps)
# object matching
detector.detect()
box_msgs = []
for cluster in detector.last_detection_msg.detection.clusters:
box_msgs.append(detector.detect_bounding_box(cluster))
box_msg, index = utils.find_nearest_box(obj, box_msgs)
# generating grasps
desired_grasps = grasp_generator.generate_grasps(box_msg, 16)
grasp_generator.draw_grasps(desired_grasps)
# pickup
grabber = object_pickup.Grabber()
processing_reply = detector.call_collision_map_processing(
detector.last_detection_msg)
# # reset planning scene
self.arm_mover.reset_collision_map()
self.arm_mover.update_planning_scene()
res = grabber.pickup_object(
processing_reply.graspable_objects[index],
processing_reply.collision_object_names[index],
processing_reply.collision_support_surface_name,
'right_arm',
desired_grasps=desired_grasps,
lift_desired_distance=0.3)
if res is None:
e = ExecutingException("ROS pickup failed")
e.robot = self.robot
e.object_to_grasp = obj
raise e
# update openrave
if use_ros:
self.pr2robot.update_rave()
else:
self.pause("Moving to location")
self.robot.SetTransform(pose)
self.pause("Moving arm")
self.robot.SetDOFValues(
[torso_angle], [self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
self.robot.Grab(obj)
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
if use_ros:
# Move arm to drop location
p = (0.3, -0.5, 0.3)
q = transformations.quaternion_from_euler(0, 0, -numpy.pi / 2)
res = self.arm_mover.move_right_arm(p, q, '/torso_lift_link', 30)
if not res:
e = ExecutingException("ROS putdown step 1 failed")
e.robot = self.robot
e.object_to_grasp = obj
raise e
p = (0.1, -0.8, -0.2)
q = transformations.quaternion_from_euler(0, 0, -numpy.pi / 2)
res = self.arm_mover.move_right_arm(p, q, '/torso_lift_link', 30)
if not res:
e = ExecutingException("ROS putdown step 2 failed")
e.robot = self.robot
e.object_to_grasp = obj
raise e
# Drop object
joint_mover = PR2JointMover()
joint_mover.open_right_gripper(True)
# update openrave
if use_ros:
self.pr2robot.update_rave()
else:
self.pause("Moving to location")
self.robot.SetTransform(pose)
self.pause("Moving arm")
self.robot.SetDOFValues(
[torso_angle], [self.robot.GetJointIndex('torso_lift_joint')])
self.robot.SetDOFValues(
sol,
self.robot.GetActiveManipulator().GetArmIndices())
self.robot.Release(obj)
#putting the object straight
if table_name.startswith("dest_"):
print "Putting object in the right location"
T = getattr(tray_world, table_name, None)
else:
T = obj.GetTransform()
rot_angle = (np.pi / 2., 0., 0) #got this from the model
rot_mat = openravepy.rotationMatrixFromAxisAngle(rot_angle)
T[:3, :3] = rot_mat
_, _, _, _, z = utils.get_object_limits(table)
T[2, 3] = z
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!"
raw_input("Press return to continue")
