def test_gravity(self):
"""
Test the @setGravity and @getGravity methods
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
gravity = manager.getGravity(client)
self.assertIsInstance(gravity, list)
self.assertEqual(len(gravity), 3)
# Test with an invalid client
gravity = manager.getGravity(-1)
self.assertIsNone(gravity)
gravities = [[0.0, 0.0, -9.81], [0.0, 0.0, 9.81], [1.0, 3.0, 0.0],
[-1.0, 3.5, 2.0]]
for gravity in gravities:
manager.setGravity(client, gravity)
value = manager.getGravity(client)
self.assertIsInstance(value, list)
for i in range(len(gravity)):
self.assertEqual(value[i], gravity[i])
# Test with an invalid client
with self.assertRaises(pybullet.error):
manager.setGravity(-1, gravities[0])
manager.stopSimulation(client)
def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False, use_shared_memory=True)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False,
use_shared_memory=True,
auto_step=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False, auto_step=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
# Ensure that stopping the simulation one more time won't raise an
# error
try:
manager.stopSimulation(client)
self.assertTrue(True)
except Exception:
self.assertTrue(False)
def test_camera_removal(self):
"""
Ensure that the camera processes are stopped when removing a robot,
stopping or resetting a simulation
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
pepper = manager.spawnPepper(client, spawn_ground_plane=True)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
camera = pepper.getCamera(handle)
self.assertTrue(camera.isActive())
manager.removePepper(pepper)
self.assertFalse(camera.isActive())
pepper = manager.spawnPepper(client, spawn_ground_plane=True)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
camera = pepper.getCamera(handle)
self.assertTrue(camera.isActive())
manager.resetSimulation(client)
self.assertFalse(camera.isActive())
pepper = manager.spawnPepper(client, spawn_ground_plane=True)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
camera = pepper.getCamera(handle)
self.assertTrue(camera.isActive())
manager.stopSimulation(client)
self.assertFalse(camera.isActive())
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("duck_vhacd.urdf",
basePosition=[1, 0, 0.5],
globalScaling=10.0,
physicsClientId=client)
pepper.showLaser(True)
pepper.subscribeLaser()
pepper.goToPosture("Stand", 0.6)
try:
while True:
laser_list = pepper.getRightLaserValue()
laser_list.extend(pepper.getFrontLaserValue())
laser_list.extend(pepper.getLeftLaserValue())
if all(laser == 3.0 for laser in laser_list):
print("Nothing detected")
else:
print("Detected")
pass
time.sleep(0.1)
except KeyboardInterrupt:
pass
finally:
simulation_manager.stopSimulation(client)
def test_reset_simulation(self):
"""
Test the @resetSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.resetSimulation(client)
manager.stopSimulation(client)
def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
def test_set_light_position(self):
"""
Test the @setLightPosition method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.setLightPosition(client, [10, 20, 2])
manager.setLightPosition(client, [0, 0, 10])
manager.setLightPosition(client, [-5, -20, 50])
manager.stopSimulation(client)
def test_spawn_pepper(self):
"""
Test the @spawnPepper method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
pepper = manager.spawnPepper(client,
translation=[0, 0, 0],
quaternion=[0, 0, 0, 1],
spawn_ground_plane=True)
self.assertIsInstance(pepper, PepperVirtual)
self.assertNotEqual(len(pepper.joint_dict.keys()), 0)
manager.stopSimulation(client)
def test_spawn_nao(self):
"""
Test the @spawnNao method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
nao = manager.spawnNao(client,
translation=[2, 4, 1],
quaternion=[0, 0.1, 0, 1],
spawn_ground_plane=True)
self.assertIsInstance(nao, NaoVirtual)
self.assertNotEqual(len(nao.joint_dict.keys()), 0)
manager.stopSimulation(client)
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_nao(self):
"""
Test the @removeNao method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
nao = manager.spawnNao(client, spawn_ground_plane=True)
manager.removeNao(nao)
with self.assertRaises(pybullet.error):
pybullet.getBodyInfo(nao.getRobotModel())
manager.stopSimulation(client)
def test_stop_simulation(self):
"""
Test the @stopSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.stopSimulation(client)
with self.assertRaises(pybullet.error):
pybullet.stepSimulation(physicsClientId=client)
# Try stopping an invalid simulation instance
try:
manager.stopSimulation(-1)
self.assertTrue(True)
except Exception:
self.assertTrue(False)
def test_set_light_position(self):
"""
Test the @setLightPosition method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.setLightPosition(client, [10, 20, 2])
manager.setLightPosition(client, [0, 0, 10])
manager.setLightPosition(client, [-5, -20, 50])
with self.assertRaises(pybullet.error):
manager.setLightPosition(client, "not a list")
with self.assertRaises(pybullet.error):
manager.setLightPosition(client,
[1, 2, 3, "wrong amount of elements"])
manager.stopSimulation(client)
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.goToPosture("Crouch", 0.6)
time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
pepper.goToPosture("StandZero", 0.6)
time.sleep(1)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM)
try:
while True:
img = pepper.getCameraFrame(handle)
cv2.imshow("bottom camera", img)
cv2.waitKey(1)
except KeyboardInterrupt:
simulation_manager.stopSimulation(client)
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.goToPosture("Crouch", 0.6)
time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
pepper.goToPosture("StandZero", 0.6)
time.sleep(1)
# Subscribe to the top RGB camera in QVGA (default value, specified here
# for more clarity) and 15fps
handle_top = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP,
resolution=Camera.K_QVGA,
fps=15.0)
# Same process for the bottom camera
handle_bottom = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM,
resolution=Camera.K_QVGA,
fps=15.0)
try:
while True:
img_top = pepper.getCameraFrame(handle_top)
img_bottom = pepper.getCameraFrame(handle_bottom)
cv2.imshow("top camera", img_top)
cv2.imshow("bottom camera", img_bottom)
cv2.waitKey(1)
except KeyboardInterrupt:
pass
finally:
# No need to manually unsubscribe from the cameras when calling
# stopSimulation, the method will automatically unsubscribe from the
# cameras
simulation_manager.stopSimulation(client)
def test_step_simulation(self):
"""
Test the @stepSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False, auto_step=False)
manager.stepSimulation(client)
with self.assertRaises(pybullet.error):
manager.stepSimulation(client + 1)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False,
use_shared_memory=True,
auto_step=False)
manager.stepSimulation(client)
with self.assertRaises(pybullet.error):
manager.stepSimulation(client + 1)
manager.stopSimulation(client)
class VirtualPepperMotionControll:
def __init__(self):
# self.head_sub = rospy.Subscriber('/pepper_dcm/Head_controller/command', JointTrajectory, self.callback)
# self.left_arm_sub = rospy.Subscriber('/pepper_dcm/LeftArm_controller/command', JointTrajectory, self.callback)
# self.right_arm_sub = rospy.Subscriber('/pepper_dcm/RightArm_controller/command', JointTrajectory, self.callback)
# self.left_hand_sub = rospy.Subscriber('/pepper_dcm/LeftHand_controller/command', JointTrajectory, self.callback)
# self.right_hand_sub = rospy.Subscriber('/pepper_dcm/RightHand_controller/command', JointTrajectory, self.callback)
self.simulation_manager = SimulationManager()
self.client_id = self.simulation_manager.launchSimulation(gui=True)
self.pepper = self.simulation_manager.spawnPepper(
self.client_id, spawn_ground_plane=True)
self.wrap = PepperRosWrapper()
self.wrap.launchWrapper(self.pepper, "/naoqi_driver")
#self.pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
# def callback(self, msg):
# print(msg.joint_names)
# print(list(msg.points[0].positions))
# self.pepper.setAngles(msg.joint_names, list(msg.points[0].positions))
def __del__(self):
self.wrap.stopWrapper()
self.simulation_manager.stopSimulation(self.client_id)
class RobotEnv(gym.Env):
def __init__(self, gui=False):
self.r_kinematic_chain = [
# "KneePitch",
# "HipPitch",
# "HipRoll",
"RShoulderPitch",
"RShoulderRoll",
"RElbowRoll",
"RElbowYaw",
"RWristYaw"]
self.initial_stand = [
1.207,
-0.129,
1.194,
1.581,
1.632]
self.gui = gui
self.joints_initial_pose = list()
# Passed to True at the end of an episode
self.episode_start_time = None
self.episode_over = False
self.episode_failed = False
self.episode_reward = 0.0
self.episode_number = 0
self.episode_steps = 0
self.simulation_manager = SimulationManager()
# self.initial_bucket_pose = [0.65, -0.2, 0.0]
self.projectile_radius = 0.03
self._setupScene()
lower_limits = list()
upper_limits = list()
# Bucket footprint in base_footprint and r_gripper 6D in base_footprint
# (in reality in odom, but the robot won't move and is on the odom
# frame): (x_b, y_b, x, y, z, rx, ry, rz)
# lower_limits.extend([-10, -10, -10, -10, 0, -7, -7, -7])
# upper_limits.extend([10, 10, 10, 10, 10, 7, 7, 7])
lower_limits.extend([-10, -10])
upper_limits.extend([10, 10])
# Add the joint positions to the limits
lower_limits.extend([self.pepper.joint_dict[joint].getLowerLimit() for
joint in self.r_kinematic_chain])
upper_limits.extend([self.pepper.joint_dict[joint].getUpperLimit() for
joint in self.r_kinematic_chain])
# Add gripper position to the limits
lower_limits.extend([-2, -2, 0, -1, -1, -1, -1])
upper_limits.extend([2, 2, 3, 1, 1, 1, 1])
self.observation_space = spaces.Box(
low=np.array(lower_limits),
high=np.array(upper_limits))
# Define the action space
velocity_limits = [
self.pepper.joint_dict[joint].getMaxVelocity() for
joint in self.r_kinematic_chain]
velocity_limits.extend([
-self.pepper.joint_dict[joint].getMaxVelocity() for
joint in self.r_kinematic_chain])
normalized_limits = self.normalize(velocity_limits)
self.max_velocities = normalized_limits[:len(self.r_kinematic_chain)]
self.min_velocities = normalized_limits[len(self.r_kinematic_chain):]
self.action_space = spaces.Box(
low=np.array(self.min_velocities),
high=np.array(self.max_velocities))
def step(self, action):
"""
Parameters
----------
action :
Returns
-------
ob, reward, episode_over, info : tuple
ob (object) :
an environment-specific object representing your observation of
the environment.
reward (float) :
amount of reward achieved by the previous action. The scale
varies between environments, but the goal is always to increase
your total reward.
episode_over (bool) :
whether it's time to reset the environment again. Most (but not
all) tasks are divided up into well-defined episodes, and done
being True indicates the episode has terminated. (For example,
perhaps the pole tipped too far, or you lost your last life.)
info (dict) :
diagnostic information useful for debugging. It can sometimes
be useful for learning (for example, it might contain the raw
probabilities behind the environment's last state change).
However, official evaluations of your agent are not allowed to
use this for learning.
"""
try:
action = list(action)
assert len(action) == len(self.action_space.high.tolist())
except AssertionError:
print("Incorrect action")
return None, None, None, None
self.episode_steps += 1
np.clip(action, self.min_velocities, self.max_velocities)
self._setVelocities(self.r_kinematic_chain, action)
obs, reward = self._getState()
return obs, reward, self.episode_over, {}
def reset(self):
"""
Resets the environment for a new episode
"""
self.episode_over = False
self.episode_failed = False
self.episode_reward = 0.0
self.episode_steps = 0
self._resetScene()
# Reset the start time for the current episode
self.episode_start_time = time.time()
# Fill and return the observation
return self._getObservation()
def render(self, mode='human', close=False):
pass
def _setVelocities(self, angles, normalized_velocities):
"""
Sets velocities on the robot joints
"""
for angle, velocity in zip(angles, normalized_velocities):
# Unnormalize the velocity
velocity *= self.pepper.joint_dict[angle].getMaxVelocity()
position = self.pepper.getAnglesPosition(angle)
lower_limit = self.pepper.joint_dict[angle].getLowerLimit()
upper_limit = self.pepper.joint_dict[angle].getUpperLimit()
if position <= lower_limit and velocity < 0.0:
velocity = 0.0
self.episode_failed = True
elif position >= upper_limit and velocity > 0.0:
velocity = 0.0
self.episode_failed = True
pybullet.setJointMotorControl2(
self.pepper.robot_model,
self.pepper.joint_dict[angle].getIndex(),
pybullet.VELOCITY_CONTROL,
targetVelocity=velocity,
force=self.pepper.joint_dict[angle].getMaxEffort())
def _getBucketPosition(self):
"""
Returns the position of the target bucket in the world
"""
# Get the position of the bucket (goal) in the world
bucket_pose, bucket_qrot = pybullet.getBasePositionAndOrientation(
self.bucket)
return bucket_pose, bucket_qrot
def _getProjectilePosition(self):
"""
Returns the position of the projectile in the world
"""
# Get the position of the projectile in the world
project_pose, project_qrot = pybullet.getBasePositionAndOrientation(
self.projectile)
return project_pose, project_qrot
def _getLinkPosition(self, link_name):
"""
Returns the position of the link in the world frame
"""
link_state = pybullet.getLinkState(
self.pepper.robot_model,
self.pepper.link_dict[link_name].getIndex())
return link_state[0], link_state[1]
def _computeProjectileSpawnPose(self):
"""
Returns the ideal position for the projectile (in the robot's hand)
"""
r_wrist_pose, _ = self._getLinkPosition("r_wrist")
return [
r_wrist_pose[0] + 0.04,
r_wrist_pose[1] - 0.01,
r_wrist_pose[2] + 0.064]
def _computeBucketSpawnPose(self):
"""
Returns a spawning pose for the targeted bin
"""
# Ranges in polar coordinates
radius_range = [0.4, 0.65]
angle_range = [-1.2, 0.4]
radius = np.random.uniform(low=radius_range[0], high=radius_range[1])
angle = np.random.uniform(low=angle_range[0], high=angle_range[1])
# return [radius * np.cos(angle), radius * np.sin(angle), 0.0]
return [0.6, -0.22, 0.0]
def normalize(self, values, range_min=-1.0, range_max=1.0):
"""
Normalizes values (list) according to a specific range
"""
zero_bound = [x - min(values) for x in values]
range_bound = [
x * (range_max - range_min) / (max(zero_bound) - min(zero_bound))
for x in zero_bound]
return [x - max(range_bound) + range_max for x in range_bound]
def _getState(self, convergence_norm=0.15):
"""
Gets the observation and computes the current reward. Will also
determine if the episode is over or not, by filling the episode_over
boolean. When the euclidian distance between the wrist link and the
cube is inferior to the one defined by the convergence criteria, the
episode is stopped
"""
reward = 0.0
# Get position of the object and gripper pose in the odom frame
projectile_pose, _ = self._getProjectilePosition()
bucket_pose, _ = self._getBucketPosition()
# Check if there is a self collision on the r_wrist, if so stop the
# episode. Else, check if the ball is below 0.049 (height of the
# trash's floor)
if self.pepper.isSelfColliding("r_wrist") or\
self.pepper.isSelfColliding("RForeArm"):
self.episode_over = True
self.episode_failed = True
reward += -1
elif projectile_pose[2] <= 0.049:
self.episode_over = True
elif (time.time() - self.episode_start_time) > 2.5:
self.episode_over = True
self.episode_failed = True
reward += -1
# Fill the observation
obs = self._getObservation()
# Compute the reward
bucket_footprint = [bucket_pose[0], bucket_pose[1], 0.0]
projectile_footprint = [projectile_pose[0], projectile_pose[1], 0.0]
previous_footprint = [
self.prev_projectile_pose[0],
self.prev_projectile_pose[1],
0.0]
prev_to_target =\
np.array(bucket_footprint) - np.array(previous_footprint)
current_to_target =\
np.array(bucket_footprint) - np.array(projectile_footprint)
# test
# init_to_current = np.array(projectile_footprint) - np.array([
# self.initial_projectile_pose[0],
# self.initial_projectile_pose[1],
# 0.0])
norm_to_target = np.linalg.norm(current_to_target)
reward += np.linalg.norm(prev_to_target) - norm_to_target
# If the episode is over, check the position of the floor projection
# of the projectile, to know wether if it's in the trash or not
if self.episode_over:
if norm_to_target <= 0.115 and not self.episode_failed:
reward += 2.0
else:
self.episode_failed = True
initial_proj_footprint = [
self.initial_projectile_pose[0],
self.initial_projectile_pose[1],
0.0]
initial_norm = np.linalg.norm(
np.array(initial_proj_footprint) - np.array(bucket_footprint))
# Test replace norm
reward += initial_norm - norm_to_target
# Test velocity vector reward
# K = 500
# if (norm_to_target <= 0.115 and projectile_pose[2] <= 0.32):
# reward += 1/K
# elif np.linalg.norm(init_to_current) != 0:
# ref_cos = np.cos(np.arctan2(0.3, norm_to_target))
# center_cos =\
# np.dot(current_to_target, init_to_current) /\
# (norm_to_target * np.linalg.norm(init_to_current))
# if center_cos > ref_cos:
# reward += 1/K
# else:
# reward += center_cos/K
# else:
# reward += -1/K
# Add the reward to the episode reward
self.episode_reward += reward
# Update the previous projectile pose
self.prev_projectile_pose = projectile_pose
if self.episode_over:
self.episode_number += 1
self._printEpisodeSummary()
return obs, reward
def _getObservation(self):
"""
Returns the observation
Returns:
obs - the list containing the observations
"""
# Get position of the projectile and bucket in the odom frame
# projectile_pose, _ = self._getProjectilePosition()
bucket_pose, _ = self._getBucketPosition()
# Get the position of the r_gripper in the odom frame (base footprint
# is on the origin of the odom frame in the xp)
gripper_pose, gripper_rot = self._getLinkPosition("r_gripper")
# Fill and return the observation
return [pose for pose in bucket_pose[:2]] +\
self.pepper.getAnglesPosition(self.r_kinematic_chain) +\
[pose for pose in gripper_pose] +\
[rot for rot in gripper_rot]
def _setupScene(self):
"""
Setup a scene environment within the simulation
"""
self.client = self.simulation_manager.launchSimulation(gui=self.gui)
self.pepper = self.simulation_manager.spawnPepper(
self.client,
spawn_ground_plane=True)
self.pepper.goToPosture("Stand", 1.0)
self.pepper.setAngles("RHand", 0.7, 1.0)
self.pepper.setAngles(
self.r_kinematic_chain,
self.initial_stand,
1.0)
time.sleep(1.0)
self.joints_initial_pose = self.pepper.getAnglesPosition(
self.pepper.joint_dict.keys())
pybullet.setAdditionalSearchPath("environment")
self.bucket = pybullet.loadURDF(
"trash.urdf",
self._computeBucketSpawnPose(),
flags=pybullet.URDF_USE_MATERIAL_COLORS_FROM_MTL)
# The initial pose of the projectile
self.initial_projectile_pose = self._computeProjectileSpawnPose()
self.projectile = pybullet.loadURDF(
"ball.urdf",
self.initial_projectile_pose)
time.sleep(0.2)
# Get position of the projectile in the odom frame
self.prev_projectile_pose, _ = self._getProjectilePosition()
def _resetScene(self):
"""
Resets the scene for a new scenario
"""
pybullet.resetBasePositionAndOrientation(
self.pepper.robot_model,
posObj=[0.0, 0.0, 0.0],
ornObj=[0.0, 0.0, 0.0, 1.0],
physicsClientId=self.client)
self._hardResetJointState()
# The initial pose of the projectile
self.initial_projectile_pose = self._computeProjectileSpawnPose()
pybullet.resetBasePositionAndOrientation(
self.projectile,
posObj=self.initial_projectile_pose,
ornObj=[0.0, 0.0, 0.0, 1.0],
physicsClientId=self.client)
pybullet.resetBasePositionAndOrientation(
self.bucket,
posObj=self._computeBucketSpawnPose(),
ornObj=[0.0, 0.0, 0.0, 1.0],
physicsClientId=self.client)
time.sleep(0.2)
# Get position of the object and gripper pose in the odom frame
self.prev_projectile_pose, _ = self._getProjectilePosition()
def _hardResetJointState(self):
"""
Performs a hard reset on the joints of the robot, avoiding the robot to
get stuck in a position
"""
for joint, position in\
zip(self.pepper.joint_dict.keys(), self.joints_initial_pose):
pybullet.setJointMotorControl2(
self.pepper.robot_model,
self.pepper.joint_dict[joint].getIndex(),
pybullet.VELOCITY_CONTROL,
targetVelocity=0.0)
pybullet.resetJointState(
self.pepper.robot_model,
self.pepper.joint_dict[joint].getIndex(),
position)
def _printEpisodeSummary(self, info_dict={}):
"""
Prints a summary for an episode
Parameters:
info_dict - Dictionnary containing extra data to be displayed
"""
if self.episode_failed:
episode_status = "FAILURE"
else:
episode_status = "SUCCESS"
print("#---------Episode-Summary---------#")
print("Episode number: " + str(self.episode_number))
print("Episode's number of steps: " + str(self.episode_steps))
print("Episode status: " + episode_status)
print("Episode reward: " + str(self.episode_reward))
for key, value in info_dict.items():
print(key + ": " + str(value))
def _termination(self):
"""
Terminates the environment
"""
self.simulation_manager.stopSimulation(self.client)
class Simulation(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.manager = SimulationManager()
self.client_id = self.manager.launchSimulation(gui=True)
p.connect(p.DIRECT)
self.robot = Robot(self.manager, self.client_id)
self.createScene()
self.initUI()
def initUI(self):
self.joint_parameters = list()
for name, joint in self.robot.pepper.joint_dict.items():
if "Finger" not in name and "Thumb" not in name:
self.joint_parameters.append(
(
p.addUserDebugParameter(
name,
joint.getLowerLimit(),
joint.getUpperLimit(),
self.robot.pepper.getAnglesPosition(name)
),
name
)
)
def createScene(self):
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("duck_vhacd.urdf", basePosition=[random.randrange(-3,3),random.randrange(-3,3),0.5], globalScaling=10, )
for i in range(24):
self.createSphere(0.2, random.choice([-5, -4, -3, -2, -1, 1, 2, 4, 5]), random.choice([-5,-4, -3,-2,-1,1,2,3,4,5]), 0.3,
color=(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255)))
def createWall(self, w, h, d, x, y, z):
wall_visual = p.createVisualShape(p.GEOM_BOX, halfExtents=[w, h, d])
wall_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[w, h, d])
wall_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=wall_collision, baseVisualShapeIndex=wall_visual, basePosition=[x, y, z])
def createSphere(self, radius, x, y, z, color=(255, 0, 0)):
sphere_visual = p.createVisualShape(p.GEOM_SPHERE, rgbaColor=[color[0]/255, color[1]/255, color[2]/255, 1], radius=radius)
sphere_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=radius)
sphere_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=sphere_collision,baseVisualShapeIndex=sphere_visual, basePosition=[x, y, z])
def createCube(self, width, x, y, color=(255, 0, 0)):
cube_visual = p.createVisualShape(p.GEOM_BOX, rgbaColor=[color[0]/255, color[1]/255, color[2]/255, 1], halfExtents=[0.5, 0.5, 0.5])
cube_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[width, width, width])
cube_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=cube_collision, baseVisualShapeIndex=cube_visual, basePosition=[x,y,width/2])
def signal_handler(self, signal, frame):
#print("CTRL-C detected")
for t in self.robot.threads:
t.kill()
self.robot.kill()
self.manager.stopSimulation(self.client_id)
sys.exit(0)
def run(self):
self.robot.start()
"""
#Used to follow the robot position but lower performances
while True:
p.resetDebugVisualizerCamera(cameraDistance= p.getDebugVisualizerCamera(self.client_id)[10],
cameraYaw= p.getDebugVisualizerCamera(self.client_id)[8],
cameraPitch= p.getDebugVisualizerCamera(self.client_id)[9],
cameraTargetPosition= [self.robot.pepper.getPosition()[0], self.robot.pepper.getPosition()[1], 0.5])
"""
"""
#Used to find new posture for PiLDIM in case we add more.
for joint_parameter in self.joint_parameters:
self.robot.pepper.setAngles(
joint_parameter[1],
p.readUserDebugParameter(joint_parameter[0]), 1.0
)
"""
def main():
nb_clients = 10
iterations = 10
if iterations < nb_clients:
nb_clients = iterations
task_list = list()
iterations_per_task = int(iterations / nb_clients)
simulation_manager = SimulationManager()
client_list = list()
pepper_list = list()
keys = list()
collision_links = list()
gui = False
for i in range(nb_clients):
client = simulation_manager.launchSimulation(gui=gui)
pepper = simulation_manager.spawnPepper(client,
spawn_ground_plane=True)
if gui:
gui = False
client_list.append(client)
pepper_list.append(pepper)
collision_links = ['r_wrist', 'LBicep', 'l_wrist', 'RBicep', 'Pelvis']
for key, value in pepper_list[0].joint_dict.items():
if "Finger" in key or "Thumb" in key or "Hand" in key or "Head" in key:
continue
else:
keys.append(key)
for i in range(nb_clients):
task_list.append(
Task(pepper_list[i], keys, collision_links, iterations_per_task))
mean_error = [0] * len(keys)
for task in task_list:
task.start()
print("Task " + str(task.pepper.getPhysicsClientId()) + " started")
for task in task_list:
task.join()
print("Task " + str(task.pepper.getPhysicsClientId()) +
" finished after " + str(iterations_per_task) + " iteration(s)")
temp_error = [sum(x) for x in zip(mean_error, task.getResult())]
mean_error = list(temp_error)
for client in client_list:
simulation_manager.stopSimulation(client)
# Normalize the error given the iteration number
normalized_mean_error = [x / iterations for x in mean_error]
plt.bar(range(len(normalized_mean_error)), normalized_mean_error)
plt.xticks(range(len(normalized_mean_error)),
tuple(keys),
rotation='vertical',
horizontalalignment='left')
plt.show()
class Engine:
def __init__(self, gui=False):
self._simulation_manager = SimulationManager()
self._client_id = self._simulation_manager.launchSimulation(gui=gui)
self._agent = self._simulation_manager.spawnPepper(
self._client_id, spawn_ground_plane=True)
self._laser_handle = self._agent.subscribeLaser()
self._camera_handle = self._agent.subscribeCamera(
PepperVirtual.ID_CAMERA_BOTTOM)
def genClassData(self, cube_rho, cube_teta, cube_rot, cubeRGB, cubeSize):
cube = Cube((np.cos(cube_teta) * cube_rho,
np.sin(cube_teta) * cube_rho, cubeSize / 2),
(cube_rot, 0, 0), cubeRGB, cubeSize)
data = self.screenshot()
cube.remove()
return (*data, np.array(cubeSize, dtype=np.float16))
def screenshot(self, skip_frame=True):
# Skipping a frame before saving
img = Camera._getCameraFromHandle(self._camera_handle).frame
if skip_frame:
for i in range(2):
tmp = img
while img is tmp:
time.sleep(0.01)
img = Camera._getCameraFromHandle(
self._camera_handle).frame
img = img.copy()
shape = (len(img), len(img[0]))
ratio = 25 / 100
img = cv2.resize(img, (int(shape[1] * ratio), int(shape[0] * ratio)),
interpolation=cv2.INTER_AREA)
sensors = self._agent.getFrontLaserValue()
return (np.array(img,
dtype=np.uint8), np.array(sensors, dtype=np.float16))
def genDataset(self,
output_path,
size,
seed=None,
debug=False,
fixMissingSensors=True):
r = random.Random(seed)
img = []
sensors = []
class_id, cubeRGB = Colors.next_random_colors(size)
cubeSize = []
i = 1
time_start = time.time()
last_step = 0.0
av_speed = 1.0
sensors_fail_count = 0
while i <= size:
av_speed = 0.90 * av_speed + 0.1 / (time.time() - last_step)
last_step = time.time()
eta = (time.time() - time_start) / i * (size - i)
print(
f"\rGenerating data... [{'='*int(20*i/size)}>{' '*int(20-20*i/size)}] {i}/{size}, "
f"ETA {int(eta/60)}min{int(eta%60)}, "
f"Current speed is {round(av_speed,2)} per second, "
f"Sensors fail rate is {round(sensors_fail_count/i*100,2)}%",
end='')
line = self.genClassData(
r.random() + 0.5, # CubeRho
r.random() * np.pi / 3 - np.pi / 6, # CubeTeta
r.random() * 2 * np.pi, # CubeRotZ
cubeRGB[i - 1], # RGB
r.random() * 1.5 + 0.5)
if fixMissingSensors:
sensors_ok = False
for j in line[1]:
if j != 3.0:
sensors_ok = True
break
if not sensors_ok:
sensors_fail_count += 1
continue
if debug:
plt.imshow(line[0], interpolation="bilinear")
plt.show()
i += 1
img += [line[0]]
sensors += [line[1]]
cubeSize += [line[2]]
os.makedirs(os.path.dirname(output_path), exist_ok=True)
np.savez(output_path,
img=img,
sensors=sensors,
cubeRGB=cubeRGB,
cubeSize=cubeSize,
class_id=class_id,
labels=[color.name for color in Colors])
def stop(self):
self._agent.unsubscribeLaser()
self._agent.unsubscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM)
self._simulation_manager.stopSimulation(self._client_id)
baseOrientation=p.getQuaternionFromEuler(
(0, 0, math.pi)))
time.sleep(3) #waiting for pepper and all to be initialized
#path = pybullet_data.getDataPath() + '/'
#table = p.loadURDF(path+'table/table.urdf', basePosition=[1,0,0],
# globalScaling=1, baseOrientation=p.getQuaternionFromEuler((0,0,math.pi)))
try:
#objs = genereObjs()
#turnToObj(pepper,cam,'table')
#guideToTable(pepper)
#turnToObj(pepper,cam,'chair')
#bringBackChair(pepper,cam)
#simulation_manager.stopSimulation(client_id)
if DEMO_CAM_HUMAN: #demo caméra Humain
while True:
pass
if DEMO_TURN_TO_OBJ: #turnToObj
objs = genereObjs()
turnToObj(pepper, cam, 'chair')
if DEMO_BRING_BACK_CHAIR: #bringBackChair
bringBackChair(pepper, cam)
while True:
pass
simulation_manager.stopSimulation(client_id)
except KeyboardInterrupt:
simulation_manager.stopSimulation(client_id)
PepperLaserTest,
PepperCameraTest,
PepperPostureTest,
PepperSelfCollisionTest]
physics_client = simulation_manager.launchSimulation(gui=False)
for test_class in test_classes:
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadMJCF("mjcf/ground_plane.xml")
test_results.append(
test_runner.run(test_loader.loadTestsFromTestCase(test_class)))
simulation_manager.resetSimulation(physics_client)
simulation_manager.stopSimulation(physics_client)
print("------------------------------------------------------------------")
for i in range(len(test_results)):
test_count = test_results[i].testsRun
test_failures = test_results[i].failures
test_errors = test_results[i].errors
test_passed = test_count - len(test_failures)
if len(test_failures) != 0:
has_failed = True
print(test_classes[i].__name__ + "[" + str(test_count) + " tests]:")
print(str(test_passed) + " [\033[1m\033[92mpassed\033[0m]")
print(str(len(test_failures)) + " [\033[1m\033[91mfailed\033[0m]")
