def reset(self): table_pos = np.array([0.0, 0.0, 0.0]) utils.set_point(self.table, table_pos) utils.set_zrot(self.table, pi*0.5) table_x = np.random.rand()-0.5 table_y = np.random.rand()-0.5 utils.set_point(self.plate, [table_x, table_y, 0.63]) plate_pos = utils.get_point(self.plate) #Get target obj center position self.rgripper.set_basepose(np.array([0, 0.25, 0.78]) + np.array([plate_pos[0], plate_pos[1], 0]), [-1.54, 0.5, -1.57]) self.rgripper.set_state([0, 0, 0]) self.rgripper.set_angle(self.rgripper.gripper, 0) self.lgripper.set_basepose(np.array([0, -0.24, 0.78]) + np.array([plate_pos[0], plate_pos[1], 0]), [1.54, 0.65, 1.57]) self.lgripper.set_state([0, 0, 0]) self.lgripper.set_angle(self.lgripper.gripper, 0) self.rgripper.set_gripper_width(0.5, force=True) self.lgripper.set_gripper_width(0.5, force=True) """ Currently, If plate is on the right side, grasping tactics is rotational grasping. If plate is on the left side, grasping tactics is pushing and grasping. """ if(plate_pos[1] < 0): tactics = 0 else: tactics = 1 for i in range(100): pb.stepSimulation() return tactics
def move_cup(self, new_loc, new_euler=None, duration=0.7, teleport=False, force=1000): if new_euler is None: new_orn = p.getBasePositionAndOrientation(self.base_world.cupID)[1] else: new_orn = p.getQuaternionFromEuler(new_euler) if teleport: set_point(self.base_world.cupID, new_loc) for bead in self.base_world.droplets: #original_loc loc = p.getBasePositionAndOrientation(bead)[0] new_loc = np.array(loc) + np.array(new_loc) set_point(bead, new_loc) p.changeConstraint(self.cid, new_loc, new_orn, maxForce=force) simulate_for_duration(duration)
CLIENT = pybullet.connect(pybullet.GUI) pb.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF plane = pybullet.loadURDF("plane.urdf") pb.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0, physicsClientId=CLIENT) pb.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0, physicsClientId=CLIENT) pb.setGravity(0, 0, -10.0) table = pb.loadURDF("table/table.urdf", physicsClientId=CLIENT) dish = pb.loadURDF("dish/dish.urdf", physicsClientId=CLIENT) rgripper = RGripper() table_pos = np.array([0.0, 0.0, 0.0]) utils.set_point(table, table_pos) utils.set_zrot(table, pi * 0.5) utils.set_point(dish, [0.0, 0.0, 0.63]) rgripper.set_basepose([0, 0.15, 0.86], [-1.54, 1.2, -1.57]) rgripper.set_gripper_width(0.5, force=True) time.sleep(0.5) rgripper.set_gripper_width(0.0) rgripper.set_state([0.0, 0.0, -0.2]) rgripper.set_state([-0.2, 0.5, 0.0]) time.sleep(0.5) for i in range(300): pb.stepSimulation(physicsClientId=CLIENT) time.sleep(0.005)
pb.setAdditionalSearchPath(pybullet_data.getDataPath()) #used by loadURDF plane = pybullet.loadURDF("plane.urdf") pb.configureDebugVisualizer(pybullet.COV_ENABLE_GUI, 0, physicsClientId=CLIENT) pb.configureDebugVisualizer(pybullet.COV_ENABLE_TINY_RENDERER, 0, physicsClientId=CLIENT) pb.setGravity(0, 0, -10.0) table = pb.loadURDF("table/table.urdf", physicsClientId=CLIENT) plate = pb.loadURDF("dish/plate.urdf", physicsClientId=CLIENT) rgripper = RGripper() lgripper = LGripper() table_pos = np.array([0.0, 0.0, 0.0]) utils.set_point(table, table_pos) utils.set_zrot(table, pi * 0.5) utils.set_point(plate, [0.0, 0.0, 0.63]) rgripper.set_basepose([0, 0.25, 0.78], [-1.54, 0.6, -1.57]) lgripper.set_basepose([0, -0.23, 0.77], [1.54, 0.65, 1.57]) rgripper.set_gripper_width(0.5, force=True) lgripper.set_gripper_width(0.5, force=True) time.sleep(7) rgripper.set_gripper_width(0.0) rgripper.set_state([-0.2, 0.5, 0.0]) lgripper.set_state([0, 0, 0]) rgripper.set_pose([-1.54, 0.8, -1.57]) for i in range(100):