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]")