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 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 _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=True) 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) path = Path(__file__).parent.parent / "assets" / "models" p.setAdditionalSearchPath(str(path), 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] 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] if self._gui: # load ghosts self._ghost = p.loadURDF( "brick/brick_ghost.urdf", self._obj_start_pos, self._obj_init_ori, physicsClientId=self._client, useFixedBase=True, )
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 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 __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 __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 _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", "HeadPitch"], [0.33, -0.9, -0.6, 0.3], [0.5] * 4) 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.71] self._obj_init_ori = [0, 0, 0, 1] dirname = os.path.dirname(__file__) assets_path = os.path.join(dirname, "../assets/models") p.setAdditionalSearchPath(assets_path, physicsClientId=self._client) self._floor = 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]
def test_launch_simulation(self): """ Test the @launchSimulation method """ manager = SimulationManager() client = manager.launchSimulation(gui=False) self.assertEqual(client, 0) manager.stopSimulation(client)
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")
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
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 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 setUpClass(cls): """ Launches a simulation and spawns a Pepper virtual robot """ SensorTest.simulation = SimulationManager() SensorTest.client = SensorTest.simulation.launchSimulation(gui=False) SensorTest.sensor = DummySensor(SensorTest.client)
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 __init__(self): super(NaoEnv, self).__init__() self.simulation_manager = SimulationManager() self.client = self.simulation_manager.launchSimulation(gui=True) self.simulation_manager.setLightPosition(self.client, [0, 0, 100]) self.robot = self.simulation_manager.spawnNao(self.client, spawn_ground_plane=True) time.sleep(1.0) # stand pose parameters pose = NaoPosture('Stand') pose_dict = {} for joint_name, joint_value in zip(pose.joint_names, pose.joint_values): pose_dict[joint_name] = joint_value # joint parameters self.joint_names = [ 'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll', 'LWristYaw', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'LHipYawPitch', 'LHipRoll', 'LHipPitch', 'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipYawPitch', 'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll' ] self.lower_limits = [] self.upper_limits = [] self.init_angles = [] for joint_name in self.joint_names: joint = self.robot.joint_dict[joint_name] self.lower_limits.append(joint.getLowerLimit()) self.upper_limits.append(joint.getUpperLimit()) self.init_angles.append(pose_dict[joint_name]) self.link_names = [] for joint_name in self.joint_names: linkName = p.getJointInfo( self.robot.getRobotModel(), self.robot.joint_dict[joint_name].getIndex())[12].decode( "utf-8") self.link_names.append(linkName) self.action_space = spaces.Box(np.array(self.lower_limits), np.array(self.upper_limits)) self.observation_space = spaces.Box( np.array([-1] * len(self.joint_names)), np.array([1] * len(self.joint_names)))
def setUpClass(cls): """ Launches a simulation and spawns the NAO virtual robot """ ImuTest.simulation = SimulationManager() ImuTest.client = ImuTest.simulation.launchSimulation(gui=False) ImuTest.robot = ImuTest.simulation.spawnNao(ImuTest.client, spawn_ground_plane=True)
def setUpClass(cls): """ Launches a simulation and spawns a Pepper virtual robot """ VirtualRobotTest.simulation = SimulationManager() VirtualRobotTest.client = VirtualRobotTest.simulation.launchSimulation( gui=False) VirtualRobotTest.robot = DummyVirtual()
def setUpClass(cls): """ Launches a simulation and spawns the Pepper virtual robot """ JointTest.simulation = SimulationManager() JointTest.client = JointTest.simulation.launchSimulation(gui=False) JointTest.robot = JointTest.simulation.spawnPepper( JointTest.client, spawn_ground_plane=True)
def setUpClass(cls): """ Launches a simulation and spawns the Romeo virtual robot """ CameraTest.simulation = SimulationManager() CameraTest.client = CameraTest.simulation.launchSimulation(gui=False) CameraTest.robot = CameraTest.simulation.spawnRomeo( CameraTest.client, spawn_ground_plane=True)
def setUpClass(cls): """ Launches a simulation and spawns the NAO virtual robot """ DummyFsrTest.simulation = SimulationManager() DummyFsrTest.client = DummyFsrTest.simulation.launchSimulation( gui=False) DummyFsrTest.robot = DummyRobot()
def setUpClass(cls): """ Launches a simulation and spawns the NAO virtual robot """ PostureTest.simulation = SimulationManager() PostureTest.client = PostureTest.simulation.launchSimulation(gui=False) PostureTest.robot = PostureTest.simulation.spawnRomeo( PostureTest.client, spawn_ground_plane=True)
def test_reset_simulation(self): """ Test the @resetSimulation method """ manager = SimulationManager() client = manager.launchSimulation(gui=False) manager.resetSimulation(client) manager.stopSimulation(client)
def setUpClass(cls): """ Launches a simulation and spawns the NAO virtual robot """ FsrTest.simulation = SimulationManager() FsrTest.client = FsrTest.simulation.launchSimulation(gui=False) FsrTest.fsr_names = NaoFsr.RFOOT + NaoFsr.LFOOT FsrTest.fsr_groups = [NaoFsr.RFOOT, NaoFsr.LFOOT] FsrTest.robot = FsrTest.simulation.spawnNao(FsrTest.client, spawn_ground_plane=True)
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_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) # 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 setUpClass(cls): """ Launches a simulation and spawns the NAO virtual robot """ CameraTest.simulation = SimulationManager() CameraTest.client = CameraTest.simulation.launchSimulation(gui=False) CameraTest.robot = CameraTest.simulation.spawnNao( CameraTest.client, spawn_ground_plane=True) CameraTest.cameras = { NaoVirtual.ID_CAMERA_TOP: CameraTest.robot.camera_top, NaoVirtual.ID_CAMERA_BOTTOM: CameraTest.robot.camera_bottom }
def setUpClass(cls): """ Launches a simulation and spawns the Romeo virtual robot """ CameraTest.simulation = SimulationManager() CameraTest.client = CameraTest.simulation.launchSimulation(gui=False) CameraTest.robot = CameraTest.simulation.spawnRomeo( CameraTest.client, spawn_ground_plane=True) CameraTest.cameras = { RomeoVirtual.ID_CAMERA_LEFT: CameraTest.robot.camera_left, RomeoVirtual.ID_CAMERA_RIGHT: CameraTest.robot.camera_right, RomeoVirtual.ID_CAMERA_DEPTH: CameraTest.robot.camera_depth }
def setUpClass(cls): """ Launches a simulation and spawns the Pepper virtual robot """ CameraTest.simulation = SimulationManager() CameraTest.client = CameraTest.simulation.launchSimulation(gui=False) CameraTest.robot = CameraTest.simulation.spawnPepper( CameraTest.client, spawn_ground_plane=True) CameraTest.cameras = { PepperVirtual.ID_CAMERA_TOP: CameraTest.robot.camera_top, PepperVirtual.ID_CAMERA_BOTTOM: CameraTest.robot.camera_bottom, PepperVirtual.ID_CAMERA_DEPTH: CameraTest.robot.camera_depth }