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