def _reset(self): if (self.physicsClientId < 0): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False self.physicsClientId = 0 else: self.ownsPhysicsClient = True self.physicsClientId = p.connect(p.SHARED_MEMORY) if (self.physicsClientId < 0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) else: self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) if self.scene is None: self.scene = self.create_single_player_scene() if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart() self.robot.scene = self.scene self.frame = 0 self.done = 0 self.reward = 0 dump = 0 s = self.robot.reset() self.potential = self.robot.calc_potential() return s
def _reset(self): if (self.physicsClientId<0): conInfo = p.getConnectionInfo() if (conInfo['isConnected']): self.ownsPhysicsClient = False self.physicsClientId = 0 else: self.ownsPhysicsClient = True self.physicsClientId = p.connect(p.SHARED_MEMORY) if (self.physicsClientId<0): if (self.isRender): self.physicsClientId = p.connect(p.GUI) else: self.physicsClientId = p.connect(p.DIRECT) p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) if self.scene is None: self.scene = self.create_single_player_scene() if not self.scene.multiplayer and self.ownsPhysicsClient: self.scene.episode_restart() self.robot.scene = self.scene self.frame = 0 self.done = 0 self.reward = 0 dump = 0 s = self.robot.reset() self.potential = self.robot.calc_potential() return s
def init(self, mode='direct'): """Initializes the connection to Bullet. :param mode: Mode of the connection. Options: gui | direct :type mode: str """ while True: if pb.getConnectionInfo(self.__client_id)['isConnected'] > 0: self.__client_id += 1 else: break self.physicsClient = pb.connect({ 'gui': pb.GUI, 'direct': pb.DIRECT }[mode], self.__client_id) #or p.DIRECT for non-graphical version pb.setGravity(*self.gravity, physicsClientId=self.__client_id) pb.setTimeStep(self.time_step, physicsClientId=self.__client_id)
def plot_test_set(): # plot test data angles and positions for (i, point) in enumerate(test_data[:10]): bb = BusyBox.bb_from_result(point) if p.getConnectionInfo()['isConnected']: p.disconnect() setup_pybullet.setup_env(bb, False, False) mech = bb._mechanisms[0] true_policy = policies.generate_policy(mech, False) pos = (true_policy.rigid_position[0], true_policy.rigid_position[2]) pos_ax.plot(*pos, 'm.') pos_ax.annotate(i, pos) if i == len(test_data): angle_ax.plot(true_policy.pitch, 2.0, 'k.', label='test data') else: angle_ax.plot(true_policy.pitch, 2.0, 'k.') angle_ax.annotate(i, (true_policy.pitch, 2.0))
parser.initializeFromBulletBody(mb, physicsClientId=org) parser.saveUrdf("test.urdf") if (1): print("\ncreatingMultiBody...................n") obUid = parser.createMultiBody(physicsClientId=gui) parser2 = UrdfEditor() print("\n###########################################\n") parser2.initializeFromBulletBody(obUid, physicsClientId=gui) parser2.saveUrdf("test2.urdf") for i in range(p.getNumJoints(obUid, physicsClientId=gui)): p.setJointMotorControl2(obUid, i, p.VELOCITY_CONTROL, targetVelocity=0, force=1, physicsClientId=gui) print(p.getJointInfo(obUid, i, physicsClientId=gui)) parser = 0 p.setRealTimeSimulation(1, physicsClientId=gui) while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]): p.stepSimulation(physicsClientId=gui) time.sleep(0.01)
parser = urdfEditor.UrdfEditor() parser.initializeFromBulletBody(mb,physicsClientId=org) parser.saveUrdf("test.urdf") if (1): print("\ncreatingMultiBody...................n") obUid = parser.createMultiBody(physicsClientId=gui) parser2 = urdfEditor.UrdfEditor() print("\n###########################################\n") parser2.initializeFromBulletBody(obUid,physicsClientId=gui) parser2.saveUrdf("test2.urdf") for i in range (p.getNumJoints(obUid, physicsClientId=gui)): p.setJointMotorControl2(obUid,i,p.VELOCITY_CONTROL,targetVelocity=0,force=1,physicsClientId=gui) print(p.getJointInfo(obUid,i,physicsClientId=gui)) parser=0 p.setRealTimeSimulation(1,physicsClientId=gui) while (p.getConnectionInfo(physicsClientId=gui)["isConnected"]): p.stepSimulation(physicsClientId=gui) time.sleep(0.01)
def get_connection(): return p.getConnectionInfo()['connectionMethod']
def isconnected(self): return p.getConnectionInfo(self.cid)['isConnected']
def setup_env(bb, viz, debug, show_im=False): # disconnect if already connected (may want to change viz from False to True) if p.getConnectionInfo()['isConnected']: p.disconnect() if not viz: client = p.connect(p.DIRECT) else: client = p.connect(p.GUI) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) p.configureDebugVisualizer(p.COV_ENABLE_MOUSE_PICKING, 0) p.setAdditionalSearchPath(pybullet_data.getDataPath()) p.setRealTimeSimulation(0) p.resetDebugVisualizerCamera(cameraDistance=.15, cameraYaw=180, cameraPitch=0, cameraTargetPosition=(0., 0., bb.height / 2)) plane_id = p.loadURDF("plane.urdf") model = p.loadURDF(bb.file_name, [0., -.3, 0.]) bb.set_mechanism_ids(model) #p.setGravity(0, 0, -10) maxForce = 0 mode = p.VELOCITY_CONTROL bb.set_joint_control_mode(mode, maxForce) # enable torque sensor if bb._mechanisms[0].mechanism_type == 'Door': p.enableJointForceTorqueSensor(bb._bb_id, bb._mechanisms[0]._door_base_id, True) # can change resolution and shadows with this call view_matrix = p.computeViewMatrixFromYawPitchRoll( distance=0.4, yaw=180, pitch=0, roll=0, upAxisIndex=2, cameraTargetPosition=(0., 0., bb.height / 2)) aspect = 205. / 154. nearPlane = 0.01 farPlane = 100 fov = 60 projection_matrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane) image_data_pybullet = p.getCameraImage(205, 154, shadow=0, renderer=p.ER_TINY_RENDERER, viewMatrix=view_matrix, lightDiffuseCoeff=0, lightSpecularCoeff=0, projectionMatrix=projection_matrix) image_data = util.ImageData(*image_data_pybullet[:3]) w, h, im = image_data_pybullet[:3] np_im = np.array(im, dtype=np.uint8).reshape(h, w, 4)[:, :, 0:3] np_im = np_im[7:125, 44:160, :] w, h, im = np_im.shape[1], np_im.shape[0], np_im.flatten().tolist() image_data = util.ImageData(w, h, im) # Display the cropped image. if show_im: np_im = np.array(im, dtype=np.uint8).reshape(h, w, 3) plt.imshow(np_im) plt.show() p.stepSimulation() gripper = Gripper(bb._mechanisms[0]) return image_data, gripper
def get_connection(client=None): return p.getConnectionInfo( physicsClientId=get_client(client))['connectionMethod']
def is_connected(): return p.getConnectionInfo(physicsClientId=CLIENT)['isConnected']
# Check Env properties # Test World properties # Check is_sim flag assert env.world.is_sim # Check observation space type assert isinstance(env.observation_space, spaces.Box) # Check action space type assert isinstance(env.action_space, spaces.Box) # Check use visualizer flag set. assert env.world.use_visualizer # Pybullet specific checks # Check pybullet connection. assert isinstance(env._physics_id, int) assert pb.getConnectionInfo(env._physics_id) == { 'connectionMethod': 1, 'isConnected': 1 } # Check object loading assert env.has_objects assert isinstance(env.object_interfaces, dict) assert '013_apple' in env.object_interfaces # BulletObjectInterface Tests # Attributes assert isinstance(env.object_interfaces['013_apple'].obj_id, int) assert env.object_interfaces['013_apple'].physics_id == env._physics_id # Object States
for link in self.urdfLinks: self.writeLink(file, link) for joint in self.urdfJoints: self.writeJoint(file, joint) file.write("</robot>\n") file.close() def __del__(self): pass parser = UrdfEditor() parser.initializeFromBulletBody(door) parser.saveUrdf("test.urdf") parser = 0 p.setRealTimeSimulation(1) print("numJoints:", p.getNumJoints(door)) print("base name:", p.getBodyInfo(door)) for i in range(p.getNumJoints(door)): print("jointInfo(", i, "):", p.getJointInfo(door, i)) print("linkState(", i, "):", p.getLinkState(door, i)) while (p.getConnectionInfo()["isConnected"]): time.sleep(0.01)
def viz_train_test_data(train_data, test_data): import matplotlib.pyplot as plt from gen.generator_busybox import BusyBox n_inter_per_bb = 100 n_bbs = int(len(train_data) / n_inter_per_bb) training_dataset_size = n_bbs #10 plt.ion() angle_fig, angle_ax = plt.subplots() pos_fig, pos_ax = plt.subplots() def plot_test_set(): # plot test data angles and positions for (i, point) in enumerate(test_data[:10]): bb = BusyBox.bb_from_result(point) if p.getConnectionInfo()['isConnected']: p.disconnect() setup_pybullet.setup_env(bb, False, False) mech = bb._mechanisms[0] true_policy = policies.generate_policy(mech, False) pos = (true_policy.rigid_position[0], true_policy.rigid_position[2]) pos_ax.plot(*pos, 'm.') pos_ax.annotate(i, pos) if i == len(test_data): angle_ax.plot(true_policy.pitch, 2.0, 'k.', label='test data') else: angle_ax.plot(true_policy.pitch, 2.0, 'k.') angle_ax.annotate(i, (true_policy.pitch, 2.0)) dataset_sizes = list( range(training_dataset_size, n_bbs + 1, training_dataset_size)) pitches = [] # plot training data angles and positions for n in range(1, n_bbs + 1): point = train_data[(n - 1) * n_inter_per_bb] if p.getConnectionInfo()['isConnected']: p.disconnect() bb = BusyBox.bb_from_result(point) setup_pybullet.setup_env(bb, False, False) mech = bb._mechanisms[0] true_policy = policies.generate_policy(mech, False) pitches += [true_policy.pitch] pos = (true_policy.rigid_position[0], true_policy.rigid_position[2]) pos_ax.plot(*pos, 'c.') if n in dataset_sizes: angle_ax.clear() pos_ax.set_title('Positions of Training and Test Data, n_bbs=' + str(n)) pos_fig.savefig('dataset_imgs/poss_n_bbs' + str(n)) angle_ax.set_title('Historgram of Training Data Angle, n_bbs=' + str(n)) angle_ax.hist(pitches, 30) plot_test_set() #angle_fig.savefig('dataset_imgs/pitches_n_bbs'+str(n)) plt.show() input('enter to close') angle_ax.set_title('Historgram of Training Data Angle, n_bbs=' + str(n)) angle_ax.hist(pitches, 30) plot_test_set() #angle_fig.savefig('dataset_imgs/pitches_n_bbs'+str(n)) plt.show() input('enter to close')
# Check Env properties # Test World properties # Check is_sim flag assert env.world.is_sim # Check observation space type assert isinstance(env.observation_space, spaces.Box) # Check action space type assert isinstance(env.action_space, spaces.Box) # Check use visualizer flag set. assert env.world.use_visualizer # Pybullet specific checks # Check pybullet connection. assert isinstance(env._physics_id, int) assert pb.getConnectionInfo(env._physics_id) == {'connectionMethod': 1, 'isConnected': 1} # Check object loading assert env.world.has_object assert isinstance(env.object_interfaces, dict) assert '013_apple' in env.object_interfaces # BulletObjectInterface Tests # Attributes assert isinstance(env.object_interfaces['013_apple'].obj_id, int) assert env.object_interfaces['013_apple'].physics_id == env._physics_id # Object States assert len(env.object_interfaces['013_apple'].pose) == 7 assert len(env.object_interfaces['013_apple'].position) == 3 assert len(env.object_interfaces['013_apple'].orientation) == 4
def isconnected(self): """ :return: pybullet is alive """ return p.getConnectionInfo(self.cid)['isConnected']
def run(self, runtime, goal_position=[]): if (not goal_position == []) and (not self.goal_position == goal_position): # if self.goalBodyID is not None: # pybullet.removeBody(self.goalBodyID, physicsClientId=self.physicsClient) self.goalBodyID = pybullet.createMultiBody( baseMass=0.0, baseInertialFramePosition=[0, 0, 0], baseVisualShapeIndex=self.visualGoalShapeId, basePosition=goal_position, useMaximalCoordinates=True, physicsClientId=self.physicsClient) # print ("Goal information: ", self.goalBodyID) self.__base_collision = False self.__heightExceed = False self.goal_position = goal_position assert not self.__controller == None, "Controller not set" self.__states = [self.getState()] #load with init state self.__rotations = [self.getRotation()] self.__commands = [] old_time = 0 first = True self.__command = object controlStep = 0 self.flip = False for i in range(int(runtime / self.__simStep)): if i * self.__simStep - old_time > self.__controlStep or first: state = self.getState() self.__command = self.__controller.nextCommand(i * self.__simStep) if not first: self.__states.append(state) self.__rotations.append(self.getRotation()) self.__commands.append(self.__command) first = False old_time = i * self.__simStep controlStep += 1 self.set_commands(self.__command) pybullet.stepSimulation(physicsClientId=self.physicsClient) if pybullet.getConnectionInfo( self.physicsClient)['connectionMethod'] == pybullet.GUI: time.sleep(self.__simStep / float(self.__vspeed)) # Flipfing behavior if self.getZOrientation() < 0.0: self.flip = True #Base floor collision if len( pybullet.getContactPoints( self.__planeId, self.__hexapodId, -1, -1, physicsClientId=self.physicsClient)) > 0: self.__base_collision = True #Jumping behavior when CM crosses 2.2 if self.getState()[2] > 2.2: self.__heightExceed = True