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