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_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)
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 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, -1, 0.5], globalScaling=10.0, physicsClientId=client) pepper.showLaser(True) pepper.subscribeLaser() pepper.goToPosture("Stand", 0.6) while True: laser_list = pepper.getRightLaserValue() laser_list.extend(pepper.getFrontLaserValue()) laser_list.extend(pepper.getLeftLaserValue()) if all(laser == 5.6 for laser in laser_list): print("Nothing detected") else: print("Detected") pass
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())
class Simulation: """ Classe gérant la simu """ def __init__(self): self.sim = SimulationManager() self.client = self.sim.launchSimulation() self.robot = Robot(self.sim, self.client) self.createScene() self.robot.start() def createScene(self): pb.setAdditionalSearchPath(pd.getDataPath()) pb.loadURDF("duck_vhacd.urdf", basePosition=[4, 2, 0], globalScaling=5) pb.loadURDF("sphere2red.urdf", basePosition=[-3, 0.05, 1], globalScaling=1) time.sleep(5) def getPepper(self): return self.pepper def getClient(self): return self.client def getSimulationManager(self): return self.simulationManager
def generate_world_2(self): simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) pybullet.setAdditionalSearchPath( "/home/tp/Projet/g7_dupuis_dez_flachard") StartPosTable = [5.2, 1, 0.2] StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0]) PlaneId = pybullet.loadURDF("table.urdf", StartPosTable, StartOrientationTable) StartPosLego = [2, 0, 0.1] PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego, StartOrientationTable) StartPosBox1 = [3.55, 1.5, -0.3] StartPosBox2 = [3.1, 2, -0.3] StartOrientationbox2 = pybullet.getQuaternionFromEuler([0, 0, 1.57]) Box1 = pybullet.loadURDF("box.urdf", StartPosBox1, StartOrientationTable) Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2, StartOrientationbox2) pepper = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True) pepper.angular_velocity = 1.0 pepper.goToPosture("Stand", 0.6) pepper.setAngles("HeadPitch", 0.27, 0.27) return pepper
def main(): global my_model global canMove # global attribute to replicate a state machine canMove = True CURR_DIR = os.getcwd() my_model = tf.keras.models.load_model(CURR_DIR + '\saved_model\model_3000') simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) pepper = simulation_manager.spawnPepper(client_id, translation=[-2, 0, 0], spawn_ground_plane=True) pepper2 = simulation_manager.spawnPepper(client_id, translation=[0, 0, 0], quaternion=[0, 0, -4, 0], spawn_ground_plane=True) ObjectSpawner() #cam = Camera(pepper) mover = AleaMove(pepper2, 'StandZero') pepClone = CloningBehavior(pepper) #cam.start() mover.start() pepClone.start() #cam.join() mover.join() pepClone.join()
def main(): image_name = 'image.png' simulation_manager = SimulationManager() client = simulation_manager.launchSimulation(gui=True) pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True) pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM) #pepper.goToPosture("Crouch", 0.6) #time.sleep(1) pepper.goToPosture("Stand", 0.6) time.sleep(1) #pepper.goToPosture("StandZero", 0.6) #time.sleep(1) flag_pic = 0 #flag to launch the image recognition while True: img = pepper.getCameraFrame() #capture camera frame cv2.imshow("bottom camera", img) #show the capture cv2.waitKey(1) flag_pic = flag_pic + 1 #debug flag if flag_pic == 10: print('photo taken') #debug flag_pic = 0 cv2.imwrite(image_name, img) #write the image in the current directory result = Algo_detect(image_name, score) #COCO detection
def main(): # qi App Session qiApp = qi.Application(sys.argv) # Bullet Simulator simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) pepperSim = simulation_manager.spawnPepper(client_id, translation=[0, 0, 0], quaternion=[0, 0, 0, 1], spawn_ground_plane=True) # wrap qi App Session with Simulated Pepper wrap = NaoqibulletWrapper.NaoqibulletWrapper( qiApp, pepperSim) # /!\ keep wrap instance to keep thread pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) pybullet.loadURDF("table.urdf", basePosition=[-2, 0, 0], globalScaling=1, physicsClientId=client_id) pybullet.loadURDF("totem.urdf", basePosition=[-1.5, 0, 1], globalScaling=0.7, physicsClientId=client_id) pybullet.loadURDF("totempomme.urdf", basePosition=[-1.5, -0.25, 1], globalScaling=0.7, physicsClientId=client_id) pybullet.loadURDF("totemwine.urdf", basePosition=[-1.5, 0.25, 1], globalScaling=0.7, physicsClientId=client_id) qiSession = qiApp.session motionService = qiSession.service("ALMotion") #add function here init_cam(pepperSim) motionService.setAngles(["LShoulderPitch", "RShoulderPitch"], [1, 1], [1, 1]) #take photo turn(motionService, pi) take_picture(pepperSim) #go to the object move_right_obj(motionService) #go to the box move_to_Blue(motionService) # block until stop is called. qiApp.run() # close nicely wrap.close()
def test_reset_simulation(self): """ Test the @resetSimulation method """ manager = SimulationManager() client = manager.launchSimulation(gui=False) manager.resetSimulation(client) 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 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 generate_world_1(self): simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) pybullet.setAdditionalSearchPath( "/home/tp/Projet/g7_dupuis_dez_flachard") StartPosTable = [4, 0, 0.2] StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0]) PlaneId = pybullet.loadURDF("table.urdf", StartPosTable, StartOrientationTable) StartPosLego = [2, 0, 0.1] PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego, StartOrientationTable) StartPosTot1 = [3.7, 0, 0.75] StartPosTot2 = [3.7, 0.4, 0.75] StartPosTot3 = [3.7, -0.4, 0.75] StartPosBox1 = [-1, 1, -0.3] StartPosBox2 = [-1, -1, -0.3] StartPosBox3 = [-1, 0, -0.3] Box1 = pybullet.loadURDF("box.urdf", StartPosBox1, StartOrientationTable) Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2, StartOrientationTable) Box3 = pybullet.loadURDF("box3.urdf", StartPosBox3, StartOrientationTable) Choix = [StartPosTot1, StartPosTot2, StartPosTot3] Positions = [] while (len(Choix) > 0): pos = choice(Choix) Positions.append(pos) Choix.remove(pos) Tot1_Id = pybullet.loadURDF("duck.urdf", Positions[0], StartOrientationTable) Tot2_Id = pybullet.loadURDF("eye.urdf", Positions[1], StartOrientationTable) Tot3_Id = pybullet.loadURDF("objet3.urdf", Positions[2], StartOrientationTable) pepper = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True) pepper.angular_velocity = 1.0 pepper.goToPosture("Stand", 0.6) pepper.setAngles("HeadPitch", 0.27, 0.27) return pepper
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_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)
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 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) pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM) while True: img = pepper.getCameraFrame() cv2.imshow("bottom camera", img) cv2.waitKey(1)
def main(session): global asked_object global asked_rangement # Create the simulation with the Pepper simulation_manager = SimulationManager() client = simulation_manager.launchSimulation(gui=True) pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True) # Original positions of the objects (give random positions) liste = [-0.2, 0, 0.2] gene_positions(liste) # Initialization of the decor and the robot init_decor(client) init_posture(pepper) # The user asks an object and a rangement [liste_asked_rangement, liste_asked_object] = user_request(session) # Step 1 : Detect the 3 objects and their position pepper.moveTo(1.9, 0, 0) # Move to the position to take the picture get_image(pepper) dico_objets = analyse_image(client) for i in range(len(liste_asked_rangement)): numero_obj = dico_objets[ liste_asked_object[i]] # Give the position of the object asked # Step 2 : Take the object asked grab_object(pepper, numero_obj) # Step 3 : Store the object in the right place if (liste_asked_rangement[i] == "table"): drop_night(pepper) elif (liste_asked_rangement[i] == "fauteuil"): drop_chair(pepper) elif (liste_asked_rangement[i] == "carton"): drop_carton(pepper) # Step 4 : DAB dab(pepper) while True: pass
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) # 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)
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)
from qibullet import PepperVirtual import time import random from CardScanner import cardsearch # abreviates the name so it is easier to use . can also try "FROM CardScanner import cardsearch,x,y because x and y are global variables" Robot = "pepper" for x in range( 2 ): # creates two random variables to get pepper to point at a random location random_x = random.randint(-3, 0) #gridwidth max 5 random_y = random.randint(0, 4) #gridheight max 4 #if __name__ == "__main__": simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) #Spawn PEPPER Robot = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True) #Pointing def Point_at_Match(): Reset() Robot.setAngles( "RShoulderRoll", (-1 * (2.2 / 20) * int(cardsearch()[1])), 0.1 ) ## rolls shoulder in x direction should, come after movement in y direction time.sleep(1.0) ## pepper.setAngles("RShoulderRoll", 0.0, 0.1) Robot.setAngles("RShoulderPitch", ((2.2 / 20) * int(cardsearch()[2])), 0.1) time.sleep(1.0)
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 PepperReachCamEnv(gym.Env): metadata = {"render.modes": ["human"]} def __init__( self, gui=False, sim_steps_per_action=10, max_motion_speed=0.3, dense=True, depth_camera=False, top_camera=False, ): self._sim_steps = sim_steps_per_action self._max_speeds = [max_motion_speed] * len(CONTROLLABLE_JOINTS) self._gui = gui self._dense = dense self._depth_camera = depth_camera self._top_camera = top_camera self._setup_scene() self._goal = self._sample_goal() obs = self._get_observation() self.action_space = spaces.Box(-2.0857, 2.0857, shape=(len(CONTROLLABLE_JOINTS), ), dtype="float32") obs_spaces = dict( camera_bottom=spaces.Box( 0, 255, shape=obs["camera_bottom"].shape, dtype=obs["camera_bottom"].dtype, ), joints_state=spaces.Box( -np.inf, np.inf, shape=obs["joints_state"].shape, dtype=obs["joints_state"].dtype, ), ) if self._top_camera: obs_spaces["camera_top"] = (spaces.Box( 0, 255, shape=obs["camera_top"].shape, dtype=obs["camera_top"].dtype, ), ) if self._depth_camera: obs_spaces["camera_depth"] = spaces.Box( 0, 65_535, shape=obs["camera_depth"].shape, dtype=obs["camera_depth"].dtype, ) self.observation_space = spaces.Dict(obs_spaces) def reset(self): self._reset_scene() return self._get_observation() def step(self, action): action = list(action) assert len(action) == len(self.action_space.high.tolist()) self._robot.setAngles(CONTROLLABLE_JOINTS, action, self._max_speeds) for _ in range(self._sim_steps): p.stepSimulation(physicsClientId=self._client) obs = self._get_observation() is_success = self._is_success() hand_idx = self._robot.link_dict["l_hand"].getIndex() hand_pos = np.array( p.getLinkState(self._robot.getRobotModel(), hand_idx, physicsClientId=self._client)[0]) obj_pos = np.array( p.getBasePositionAndOrientation(self._obj, physicsClientId=self._client)[0]) is_safety_violated = self._is_table_touched( ) or self._is_table_displaced() info = { "is_success": is_success, } reward = self.compute_reward(is_success, obj_pos, hand_pos, is_safety_violated) done = is_success or is_safety_violated return (obs, reward, done, info) def render(self, mode="human"): pass def close(self): if self._top_camera: self._robot.unsubscribeCamera(self._cam_top) self._robot.unsubscribeCamera(self._cam_bottom) if self._depth_camera: self._robot.unsubscribeCamera(self._cam_depth) self._simulation_manager.stopSimulation(self._client) def seed(self, seed=None): np.random.seed(seed or 0) def compute_reward(self, is_success, obj_pos, hand_pos, is_safety_violated): if is_success: return 10.0 if self._dense else 1.0 if self._dense: if is_safety_violated: return -10.0 return -np.linalg.norm(obj_pos - hand_pos, axis=-1).astype( np.float32) return 0.0 def _is_success(self): cont = p.getContactPoints(self._robot.getRobotModel(), self._obj) return len(cont) > 0 def _setup_scene(self): self._simulation_manager = SimulationManager() self._client = self._simulation_manager.launchSimulation( gui=self._gui, auto_step=False) p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0) self._robot = self._simulation_manager.spawnPepper( self._client, spawn_ground_plane=False) self._robot.goToPosture("Stand", 1.0) for _ in range(500): p.stepSimulation(physicsClientId=self._client) self._robot.setAngles(["KneePitch", "HipPitch", "LShoulderPitch"], [0.33, -0.9, -0.6], [0.5] * 3) for _ in range(500): p.stepSimulation(physicsClientId=self._client) self._table_init_pos = [0.35, 0, 0] self._table_init_ori = [0, 0, 0, 1] self._obj_init_pos = [0.35, 0, 0.65] self._obj_init_ori = [0, 0, 0, 1] path = Path(__file__).parent.parent / "assets" / "models" p.setAdditionalSearchPath(str(path), physicsClientId=self._client) self._obj = p.loadURDF("floor/floor.urdf", physicsClientId=self._client, useFixedBase=True) self._table = p.loadURDF( "adjustable_table/adjustable_table.urdf", self._table_init_pos, self._table_init_ori, physicsClientId=self._client, ) self._obj = p.loadURDF( "brick/brick.urdf", self._obj_init_pos, self._obj_init_ori, physicsClientId=self._client, flags=p.URDF_USE_INERTIA_FROM_FILE, ) # Let things fall down for _ in range(500): p.stepSimulation(physicsClientId=self._client) self.joints_initial_pose = self._robot.getAnglesPosition( self._robot.joint_dict.keys()) self._obj_start_pos = p.getBasePositionAndOrientation( self._obj, physicsClientId=self._client)[0] # Setup camera if self._top_camera: self._cam_top = self._robot.subscribeCamera( PepperVirtual.ID_CAMERA_TOP) self._cam_bottom = self._robot.subscribeCamera( PepperVirtual.ID_CAMERA_BOTTOM) if self._depth_camera: self._cam_depth = self._robot.subscribeCamera( PepperVirtual.ID_CAMERA_DEPTH) def _reset_scene(self): p.resetBasePositionAndOrientation( self._robot.robot_model, posObj=[0.0, 0.0, 0.0], ornObj=[0.0, 0.0, 0.0, 1.0], physicsClientId=self._client, ) self._reset_joint_state() p.resetBasePositionAndOrientation( self._table, posObj=self._table_init_pos, ornObj=self._table_init_ori, physicsClientId=self._client, ) obj_pos = self._sample_goal() p.resetBasePositionAndOrientation( self._obj, posObj=obj_pos, ornObj=self._obj_init_ori, physicsClientId=self._client, ) for _ in range(self._sim_steps): p.stepSimulation(physicsClientId=self._client) return self._get_observation() def _reset_joint_state(self): for joint, position in zip(self._robot.joint_dict.keys(), self.joints_initial_pose): p.setJointMotorControl2( self._robot.robot_model, self._robot.joint_dict[joint].getIndex(), p.VELOCITY_CONTROL, targetVelocity=0.0, physicsClientId=self._client, ) p.resetJointState( self._robot.robot_model, self._robot.joint_dict[joint].getIndex(), position, physicsClientId=self._client, ) def _get_observation(self): img_bottom = self._robot.getCameraFrame(self._cam_bottom) joint_p = self._robot.getAnglesPosition(CONTROLLABLE_JOINTS) # joint_v = self._robot.getAnglesVelocity(CONTROLLABLE_JOINTS) result = { "camera_bottom": img_bottom, # "joints_state": np.concatenate([joint_p, joint_v]).astype(np.float32), "joints_state": np.array(joint_p, dtype=np.float32) } if self._top_camera: img_top = self._robot.getCameraFrame(self._cam_top) result["camera_top"] = img_top if self._depth_camera: img_depth = self._robot.getCameraFrame(self._cam_depth) result["camera_depth"] = img_depth return result def _sample_goal(self): return np.append( (np.random.sample(2) * [0.2, 0.4] + self._obj_init_pos[:2] - [0.1, 0.2]).astype(np.float32), self._obj_init_pos[2], ) def _is_table_displaced(self): pose = p.getBasePositionAndOrientation(self._table, physicsClientId=self._client) current_pose = np.array([e for t in pose for e in t], dtype=np.float32) desired_pose = np.concatenate( [self._table_init_pos, self._table_init_ori]) return not np.allclose(desired_pose, current_pose, atol=0.01) def _is_table_touched(self): cont = p.getContactPoints(self._robot.getRobotModel(), self._table) return len(cont) > 0
if __name__ == "__main__": simulation_manager = SimulationManager() test_loader = unittest.TestLoader() test_runner = unittest.TextTestRunner() test_results = list() has_failed = False test_classes = [ PepperBaseTest, PepperJointTest, 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