def main(): sim_manager = SimulationManager() client = sim_manager.launchSimulation(gui=True) # client_direct_1 = sim_manager.launchSimulation(gui=False) pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) pybullet.loadURDF('plane.urdf') duck = pybullet.loadURDF('duck_vhacd.urdf', basePosition=[0, 0, 0], globalScaling=5.0, physicsClientId=client) # r2d2 = pybullet.loadURDF( # "r2d2.urdf", # basePosition = [-2, 0, 0], # globalScaling = 5.0, # physicsClientId = client_direct_1) nao = sim_manager.spawnNao(client, translation=[2, 0, 0], quaternion=pybullet.getQuaternionFromEuler( [0, 0, 3])) pepper = sim_manager.spawnPepper( client, translation=[0, -2, 0], quaternion=pybullet.getQuaternionFromEuler([0, 0, 1.5])) romeo = sim_manager.spawnRomeo(client, translation=[0, 2, 0], quaternion=pybullet.getQuaternionFromEuler( [0, 0, -1.5])) nao.goToPosture('StandInit', 1) pepper.goToPosture('StandInit', 1) romeo.goToPosture('StandInit', 1) nao.subscribeCamera(NaoVirtual.ID_CAMERA_TOP) pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP) romeo.subscribeCamera(RomeoVirtual.ID_CAMERA_DEPTH) nao.setAngles('HeadPitch', 0.25, 1) while True: nao_cam = nao.getCameraFrame() cv2.imshow('Nao Cam', nao_cam) pepper_cam = pepper.getCameraFrame() cv2.imshow('Pepper Cam', pepper_cam) romeo_cam = romeo.getCameraFrame() cv2.imshow('Romeo Cam', romeo_cam) cv2.waitKey(1) pass
def test_spawn_romeo(self): """ Test the @spawnRomeo method """ manager = SimulationManager() client = manager.launchSimulation(gui=False) romeo = manager.spawnRomeo(client, translation=[1, 1, 4], quaternion=[0, 0, 0.4, 1], spawn_ground_plane=True) self.assertIsInstance(romeo, RomeoVirtual) self.assertNotEqual(len(romeo.joint_dict.keys()), 0) manager.stopSimulation(client)
def test_remove_romeo(self): """ Test the @removeRomeo method """ manager = SimulationManager() client = manager.launchSimulation(gui=False) romeo = manager.spawnRomeo(client, spawn_ground_plane=True) manager.removeRomeo(romeo) with self.assertRaises(pybullet.error): pybullet.getBodyInfo(romeo.getRobotModel()) manager.stopSimulation(client)
if __name__ == "__main__": simulation_manager = SimulationManager() if (sys.version_info > (3, 0)): rob = input("Which robot should be spawned? (pepper/nao/romeo): ") else: rob = raw_input("Which robot should be spawned? (pepper/nao/romeo): ") client = simulation_manager.launchSimulation(gui=True) if rob.lower() == "nao": robot = simulation_manager.spawnNao(client, spawn_ground_plane=True) elif rob.lower() == "pepper": robot = simulation_manager.spawnPepper(client, spawn_ground_plane=True) elif rob.lower() == "romeo": robot = simulation_manager.spawnRomeo(client, spawn_ground_plane=True) else: print("You have to specify a robot, pepper, nao or romeo.") simulation_manager.stopSimulation(client) sys.exit(1) # Subscribe to the IMU of the robot with a default frequency robot.subscribeImu() robot.unsubscribeImu() # Get the IMU of the robot as an Imu object imu = robot.getImu() print("Type of the robot IMU: " + str(type(imu))) # Subscribe to the IMU, and define a specific frequency robot.subscribeImu(frequency=100) # Or imu.setFrequency(100)
class RomeoEnv(gym.Env): """docstring for RomeoEnv""" def __init__(self): super(RomeoEnv, self).__init__() self.simulation_manager = SimulationManager() self.client = self.simulation_manager.launchSimulation(gui=True) self.simulation_manager.setLightPosition(self.client, [0, 0, 100]) self.robot = self.simulation_manager.spawnRomeo( self.client, spawn_ground_plane=True) time.sleep(1.0) self.joint_names = [] self.lower_limits = [] self.upper_limits = [] self.init_angles = [] for name, joint in self.robot.joint_dict.items(): if "Finger" not in name and "Thumb" not in name: self.joint_names.append(name) self.lower_limits.append(joint.getLowerLimit()) self.upper_limits.append(joint.getUpperLimit()) self.init_angles.append(self.robot.getAnglesPosition(name)) self.action_space = spaces.Box(np.array(self.lower_limits), np.array(self.upper_limits)) self.observation_space = spaces.Box( np.array([-1] * len(self.joint_names)), np.array([1] * len(self.joint_names))) def step(self, actions): if isinstance(actions, np.ndarray): actions = actions.tolist() # set joint angles self.robot.setAngles(self.joint_names, actions, 1.0) # get observations observation = { 'position': self.robot.getPosition(), 'anglesPosition': self.robot.getAnglesPosition(self.joint_names), 'anglesVelocity': self.robot.getAnglesVelocity(self.joint_names), # TODO: add more observations } # TODO: design your reward reward = 0 done = False info = {} return observation, reward, done, info def reset(self): self.simulation_manager.removeNao(self.robot) self.robot = self.simulation_manager.spawnNao(self.client, spawn_ground_plane=True) time.sleep(1.0) def render(self, mode='human'): view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[0.5, 0, 0.5], distance=.7, yaw=90, pitch=0, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(960) / 720, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage(width=960, height=720, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(px, dtype=np.uint8) rgb_array = np.reshape(rgb_array, (720, 960, 4)) rgb_array = rgb_array[:, :, :3] return rgb_array def close(self): p.disconnect()