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):