def __init__(self):
try:
isInit
except:
isInit = True
CLIENT = pybullet.connect(pybullet.GUI)
print("client", CLIENT)
pb.setAdditionalSearchPath(
pybullet_data.getDataPath()) #used by loadURDF
plane = pybullet.loadURDF("plane.urdf")
print("plane ID", plane)
#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)
self.table = pb.loadURDF("table/table.urdf")
print("table ID", self.table)
self.plate = pb.loadURDF("dish/plate.urdf")
print("plate ID", self.plate)
self.rgripper = RGripper()
self.lgripper = LGripper()
#self.robot_model = skrobot.models.urdf.RobotModelFromURDF(urdf_file=skrobot.data.pr2_urdfpath())
self.robot_model = PR2()
interface = PybulletRobotInterface(self.robot_model)
self.try_num = 100 #Simulator loop times
self.frames = [] #Movie buffer
pb.resetDebugVisualizerCamera(2.0, 90, -0.0, (0.0, 0.0, 1.0))
# For pybullet getCameraImage argument
self.viewMatrix = pb.computeViewMatrix(cameraEyePosition=[5, 5, 30],
cameraTargetPosition=[3, 3, 3],
cameraUpVector=[3, 1, 3])
self.projectionMatrix = pb.computeProjectionMatrixFOV(fov=45.0,
aspect=1.0,
nearVal=0.1,
farVal=3.1)
isInit
except:
isInit = True
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)