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(): 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 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(): 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(): 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 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_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_remove_pepper(self): """ Test the @removePepper method """ manager = SimulationManager() client = manager.launchSimulation(gui=False) pepper = manager.spawnPepper(client, spawn_ground_plane=True) manager.removePepper(pepper) with self.assertRaises(pybullet.error): pybullet.getBodyInfo(pepper.getRobotModel()) 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 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)
class PepperEnv(gym.Env): """docstring for PepperEnv""" def __init__(self): super(PepperEnv, 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.spawnPepper( self.client, spawn_ground_plane=True) time.sleep(1.0) # stand pose parameters pose = PepperPosture('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', 'LHand', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw', 'RElbowRoll', 'RWristYaw', 'RHand' ] 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([-1] * 3 + self.lower_limits), np.array([1] * 3 + self.upper_limits)) self.observation_space = spaces.Box( np.array([-1] * len(self.joint_names)), np.array([1] * len(self.joint_names))) def step(self, actions): if isinstance(actions, np.ndarray): actions = actions.tolist() # set joint angles self.robot.setAngles(self.joint_names, actions, 1.0) # get observations observation = { 'position': self.robot.getPosition(), 'anglesPosition': self.robot.getAnglesPosition(self.joint_names), 'anglesVelocity': self.robot.getAnglesVelocity(self.joint_names), # TODO: add more observations } # TODO: design your reward reward = 0 done = False info = {} return observation, reward, done, info def reset(self): self.simulation_manager.removePepper(self.robot) self.robot = self.simulation_manager.spawnPepper( self.client, spawn_ground_plane=True) time.sleep(1.0) def render(self, mode='human'): view_matrix = p.computeViewMatrixFromYawPitchRoll( cameraTargetPosition=[0.5, 0, 0.5], distance=.7, yaw=90, pitch=0, roll=0, upAxisIndex=2) proj_matrix = p.computeProjectionMatrixFOV(fov=60, aspect=float(960) / 720, nearVal=0.1, farVal=100.0) (_, _, px, _, _) = p.getCameraImage(width=960, height=720, viewMatrix=view_matrix, projectionMatrix=proj_matrix, renderer=p.ER_BULLET_HARDWARE_OPENGL) rgb_array = np.array(px, dtype=np.uint8) rgb_array = np.reshape(rgb_array, (720, 960, 4)) rgb_array = rgb_array[:, :, :3] return rgb_array def close(self): p.disconnect()
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()) obj_creation(client_id) #memProxy=initalmemory() #[choiceObj,choiceBasket,choiceFinish]=almemory(memProxy) #while choiceFinish==0: # [choiceObj,choiceBasket,choiceFinish]=almemory(memProxy) #print( [choiceObj,choiceBasket,choiceFinish]) choiceObj="banane" choiceBasket="jaune" qiSession = qiApp.session motionService = qiSession.service("ALMotion") init_cam(pepperSim) motionService.setAngles(["LShoulderPitch","RShoulderPitch"],[1,1],[1,1]) #take photo turn(motionService,pi) move(0.6,0,motionService,False) motionService.setAngles(["HipPitch"],[-0.10],[1]) motionService.setAngles(["HeadPitch", "HeadYaw"], [0.35,0], [1,1]) time.sleep(2) result=take_picture(pepperSim) motionService.setAngles(["HeadPitch", "HeadYaw"], [0,0], [1,1]) if choiceObj=='banane': choiceObj='banana' elif choiceObj=='vin': choiceObj='bottle' else: choiceObj='pizza' #go to the object if choiceObj==result[0][1]: move_left_obj(motionService) elif choiceObj==result[1][1]: move_middle_obj(motionService) else: move_right_obj(motionService) #go to the box if choiceBasket=='rouge': move_to_Red(motionService) elif choiceBasket=='vert': move_to_Green(motionService) else: move_to_Yellow(motionService) # block until stop is called. qiApp.run() # close nicely wrap.close()
if __name__ == '__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(qiApp, pepperSim) # /!\ keep instance to keep thread # code example : move head qiSession = qiApp.session motionService = qiSession.service("ALMotion") motionService.setAngles(["HeadPitch", "HeadYaw"], [-1,-1], [1,1]) # block until stop is called. qiApp.run() # close nicely
basePosition= [0.5,2.5,1], globalScaling = 0.7, physicsClientId=client) #walls2 = pybullet.loadURDF('C:\\Users\\cjrs2\\Downloads\\keras-yolo3\\proyecto_robo\\walls2.urdf', # physicsClientId=client) # nao = sim_manager.spawnNao( # client, # translation=[0.5,2,1], # quaternion=pybullet.getQuaternionFromEuler([0, 0, 3])) pepper = sim_manager.spawnPepper( client, translation=[0, -2, 0], quaternion=pybullet.getQuaternionFromEuler([0, 0, 1.5])) # nao.goToPosture('StandInit', 1) pepper.goToPosture("Stand", 1) sock_sender = SocketNumpyArray() sock_sender.initialize_sender('localhost', 9995) # nao.setAngles('HeadPitch', 0.25, 1) handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP ) print('Retriving camera frame') #x = 0
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
'LElbowYaw': -1.7, 'LElbowRoll': -1.4, 'LWristYaw': 1.5, 'RShoulderPitch': 0.6, 'RShoulderRoll': 0, 'RElbowYaw': 1.7, 'RElbowRoll': 1.4, 'RWristYaw': -1.5 } simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) if not DEMO_CAM_HUMAN: pepper = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True, translation=[-3, -1, 0]) cam = Cam(pepper, "top_cam", do_predictions=DEMO_TURN_TO_OBJ) cam.start() laser = Laser(pepper) laser.start() goToPosture(pepper, defaultPosture) if DEMO_CAM_HUMAN: human = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True, translation=[0, 0, 0]) humanCam = HumanCam(human) humanCam.start() p.connect(p.DIRECT)
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()
help="The port of the robot") args = parser.parse_args() try: session.connect("tcp://" + args.ip + ":" + str(args.port)) motion = session.service("ALMotion") except RuntimeError: sys.exit("Cannot open a qi session") simulation_manager = SimulationManager() client = simulation_manager.launchSimulation(gui=True) pepper_virtual = simulation_manager.spawnPepper( client, spawn_ground_plane=True) angle_names = list() angles_values = list() for name in pepper_virtual.joint_dict.keys(): if "Finger" in name or "Thumb" in name: continue else: angle_names.append(name) try: while True: angles_values = motion.getAngles(angle_names, True) pepper_virtual.setAngles(angle_names, angles_values, 1.0)
def setVisible(obj, visible=True): pos, ang = p.getBasePositionAndOrientation(obj) pos = list(pos) if visible: pos[-1] = 0 else: pos[-1] = -100 p.resetBasePositionAndOrientation(obj, pos, ang) simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) pepper = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True, translation=[0, 0, 0], quaternion=p.getQuaternionFromEuler( (0, 0, math.pi / 2))) p.connect(p.DIRECT) path = pybullet_data.getDataPath() + '/' table0 = p.loadURDF(path + 'table/table.urdf', basePosition=[-100, -100, -100]) table1 = p.loadURDF(path + 'table/table.urdf', basePosition=[-100, -100, -100]) table2 = p.loadURDF(path + 'table/table.urdf', basePosition=[-100, -100, -100]) chair0 = p.loadURDF("urdf/chair_1/chair.urdf", basePosition=[-100, -100, -100], globalScaling=1) #, baseOrientation=[0,0,1,0]) chair1 = p.loadURDF("urdf/chair_1/chair.urdf", basePosition=[-100, -100, -100], globalScaling=1) #, baseOrientation=[0,0,1,0]) chair2 = p.loadURDF("urdf/chair_1/chair.urdf",
def main(): #Spawn The robot used for the artefact simulation_manager = SimulationManager() client_id = simulation_manager.launchSimulation(gui=True) pepper = simulation_manager.spawnPepper(client_id,translation=[0, 0, 0],quaternion=[0, 0, 0, 1],spawn_ground_plane=True) #Allow the user to keep using the program until they would like to stop while True: voiceInp=Speech_Recognition.voiceRecog() voiceInp.voice() #Determine which game is being accessed if voiceInp.voiceWord == "coin": print("coin flip is started!") pepper.goToPosture("Stand",0.6) time.sleep(2) #Demonstrate arm motion for flipping coin pepper.setAngles("LShoulderPitch",0.4,0.1) coinPlaced=input("press enter once the coin is placed") # would be replaced with camera detection of the coin in the hand. time.sleep(2) pepper.setAngles("LElbowYaw",-2,1) pepper.setAngles("LWristYaw",-1,1) time.sleep(4) pepper.setAngles("LShoulderPitch",0.8,0.4) time.sleep(1) pepper.setAngles("LShoulderPitch",0,1) #Instantiate Coin Object robotCoin=Coin() #This would be replaced with an image recognition so that the robot can determine which way it would land but has not been completed robotCoin.flipCoin(random.randint(0,1)) if robotCoin.currentFace==0: print("Coin is heads") else: print("Coin is tails") elif voiceInp.voiceWord == "tic-tac-toe": print("tic-tac-toe started!") nc=Noughts_Crosses() while True: nc.playGame() nc.checkIfWon() if nc.gameFinished[0]==True and nc.gameFinished[1]==False: print() nc.displayBoard() if nc.turn==False: print("Player: ",nc.players[0].name," won!") else: print("Player: ",nc.players[1].name," won!") break elif nc.gameFinished[0]==True and nc.gameFinished[1]==True: print("tie game") break if nc.gameFinished==False: print() elif voiceInp.voiceWord == "rock paper scissors pepper": print("Rock paper scissors started!") #Instantiate RPS game rpsGame=RockPaperScissors() gameEnd=False pepper.goToPosture("Stand",0.6) #Demonstrating that camera feed can be used to detect a coin face for example, or which gesture has been thrown for rock paper scissors handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP,resolution=Camera.K_QVGA,fps=30.0) try: while True: img = pepper.getCameraFrame(handle) cv2.imshow("synthetic top camera", img) cv2.waitKey(1) except: pass while gameEnd==False: pepper.setAngles("LShoulderPitch",0.4,0.4) rpsGame.playRPS() if rpsGame.players[1].move.lower()=="rock": #Pepper will close his hand to demonstrate Rock time.sleep(2) pepper.setAngles("LHand",0,0.2) elif rpsGame.players[1].move.lower()=="scissors": #use other arm to represent show that scissors is being done. time.sleep(2) pepper.setAngles("LShoulderPitch",1,0.4) time.sleep(2) pepper.setAngles("RShoulderPitch",0.4,0.4) else: #Pepper will fully open his hand to demonstrate paper time.sleep(2) pepper.setAngles("LHand",100,0.2) gameEnd=rpsGame.rpsCheckWin() print("game over!") elif voiceInp.voiceWord == "rock-paper-scissors camera": rps.rock_paper_scissors() elif voiceInp.voiceWord == "exit": print("Thank you for using this program") break
def main(): simulation_manager = SimulationManager() client = simulation_manager.launchSimulation(gui=True) coord = [-2, 5, 0] pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=False) pepper2 = simulation_manager.spawnPepper(client, coord, spawn_ground_plane=False) pybullet.setAdditionalSearchPath(pybullet_data.getDataPath()) pybullet.loadURDF("plane.urdf", basePosition=[0, 0, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF( "table.urdf", basePosition=[-2, 0, 0], #baseOrientation=pybullet.getQuaternionFromEuler([1.57, 0, 0]), globalScaling=1, physicsClientId=client) pybullet.loadURDF("bar.urdf", basePosition=[0, 5, 0.5], baseOrientation=pybullet.getQuaternionFromEuler( [1.57, 0, 0]), globalScaling=1, physicsClientId=client) pybullet.loadURDF("robot.urdf", basePosition=[-2, 5, 0], baseOrientation=pybullet.getQuaternionFromEuler( [0, 0, 1.15]), globalScaling=1, physicsClientId=client) """ pybullet.loadURDF( "hat.urdf", basePosition=[-2, 5, 0], baseOrientation=pybullet.getQuaternionFromEuler([0, 0, 1.15]), globalScaling=1, physicsClientId=client) """ pybullet.loadURDF("Cagettejaune.urdf", basePosition=[1, -1, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF("Cagetterouge.urdf", basePosition=[1.5, -1.5, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF("Cagetteverte.urdf", basePosition=[1.7, -1, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF("deco.urdf", basePosition=[0, 6, 1.5], globalScaling=1, physicsClientId=client) position = [-0.25, 0.0, 0.25] i = random.choice(position) pybullet.loadURDF("totem.urdf", basePosition=[-1.5, i, 1.0], globalScaling=1, physicsClientId=client) del position[position.index(i)] j = random.choice(position) pybullet.loadURDF("totempomme.urdf", basePosition=[-1.5, j, 1], globalScaling=1, physicsClientId=client) del position[position.index(j)] pybullet.loadURDF("totemwine.urdf", basePosition=[-1.5, position[0], 1], globalScaling=1, 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(): 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( "table.urdf", basePosition=[-2, 0, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF( "Cagettejaune.urdf", basePosition=[1, -1, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF( "Cagetterouge.urdf", basePosition=[1.5, -1.5, 0], globalScaling=1, physicsClientId=client) pybullet.loadURDF( "Cagetteverte.urdf", basePosition=[1.7, -1, 0], globalScaling=1, physicsClientId=client) position=shuffle(["-0.25","0","0.25"]) pybullet.loadURDF( "totem.urdf", basePosition=[-1.5, position(0), 1], globalScaling=1, physicsClientId=client) pybullet.loadURDF( "totempomme.urdf", basePosition=[-1.5, position(1), 1], globalScaling=1, physicsClientId=client) pybullet.loadURDF( "totemwine.urdf", basePosition=[-1.5, position(2), 1], globalScaling=1, physicsClientId=client) pepper.showLaser(True) pepper.subscribeLaser() pepper.goToPosture("Stand", 0.6) almemory() 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
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) def Random_Point():
class PepperPushCamEnv(gym.GoalEnv): metadata = {"render.modes": ["human"]} def __init__( self, gui=False, sim_steps_per_action=10, use_depth_camera=False, use_top_camera=False, ): self._sim_steps = sim_steps_per_action self._gui = gui self._use_depth_camera = use_depth_camera self._use_top_camera = use_top_camera self._setup_scene() self._goal = self._sample_goal() obs = self._get_observation() self.action_space = spaces.Box(-1.0, 1.0, shape=(len(CONTROLLABLE_JOINTS) + 1, ), dtype="float32") obs_spaces = dict( camera_bottom=spaces.Box( 0, 255, shape=obs["observation"]["camera_bottom"].shape, dtype=obs["observation"]["camera_bottom"].dtype, ), joints_state=spaces.Box( -np.inf, np.inf, shape=obs["observation"]["joints_state"].shape, dtype=obs["observation"]["joints_state"].dtype, ), ) if self._use_top_camera: obs_spaces["camera_top"] = (spaces.Box( 0, 255, shape=obs["observation"]["camera_top"].shape, dtype=obs["observation"]["camera_top"].dtype, ), ) if self._use_depth_camera: obs_spaces["camera_depth"] = spaces.Box( 0, 65535, shape=obs["observation"]["camera_depth"].shape, dtype=obs["observation"]["camera_depth"].dtype, ) self.observation_space = spaces.Dict( dict( desired_goal=spaces.Box(-np.inf, np.inf, shape=obs["desired_goal"].shape, dtype="float32"), achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs["achieved_goal"].shape, dtype="float32"), observation=spaces.Dict(obs_spaces), )) def __del__(self): self.close() def reset(self): self._reset_scene() self._goal = self._sample_goal() if self._gui: self._place_ghosts() return self._get_observation() def step(self, action): """ Action in terms of desired joint positions. Last number is the speed of the movement. """ action = list(action) assert len(action) == len(self.action_space.high.tolist()) rescaled = [rescale_feature(i, f) for (i, f) in enumerate(action)] angles = rescaled[:-1] speed = rescaled[-1] self._robot.setAngles(CONTROLLABLE_JOINTS, angles, [speed] * len(angles)) for _ in range(self._sim_steps): p.stepSimulation(physicsClientId=self._client) obs = self._get_observation() is_success = self._is_success(obs["achieved_goal"], self._goal) is_safety_violated = self._is_table_touched( ) or self._is_table_displaced() info = { "is_success": is_success, } reward = self.compute_reward(obs["achieved_goal"], self._goal, info) done = is_success or is_safety_violated return (obs, reward, done, info) def render(self, mode="human"): pass def close(self): if self._use_top_camera: self._robot.unsubscribeCamera(self._cam_top) self._robot.unsubscribeCamera(self._cam_bottom) if self._use_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, achieved_goal, desired_goal, info): return (np.linalg.norm(desired_goal - achieved_goal, axis=-1) < DISTANCE_THRESHOLD).astype(np.float32) def _is_success(self, achieved_goal, desired_goal): return (np.linalg.norm(desired_goal - achieved_goal, axis=-1) < DISTANCE_THRESHOLD) def _setup_scene(self): self._simulation_manager = SimulationManager() self._client = self._simulation_manager.launchSimulation( gui=self._gui, auto_step=False, use_shared_memory=True) 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] 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] 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, ) # Setup camera if self._use_top_camera: self._cam_top = self._robot.subscribeCamera( PepperVirtual.ID_CAMERA_TOP) self._cam_bottom = self._robot.subscribeCamera( PepperVirtual.ID_CAMERA_BOTTOM) if self._use_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): obj_pos = p.getBasePositionAndOrientation( self._obj, physicsClientId=self._client)[0] img_bottom = self._robot.getCameraFrame(self._cam_bottom) joint_p = self._get_joints_states() result = { "camera_bottom": img_bottom.transpose(2, 0, 1), # "joints_state": np.concatenate([joint_p, joint_v]).astype(np.float32), "joints_state": np.array(joint_p, dtype=np.float32) } if self._use_top_camera: img_top = self._robot.getCameraFrame(self._cam_top) result["camera_top"] = img_top if self._use_depth_camera: img_depth = self._robot.getCameraFrame(self._cam_depth) result["camera_depth"] = img_depth return { "observation": result, "achieved_goal": np.array(obj_pos, dtype=np.float32), "desired_goal": self._goal, } def _sample_goal(self): return np.append( (np.random.sample(2) * [0.2, 0.4] + self._obj_start_pos[:2] - [0.1, 0.2]).astype(np.float32), self._obj_start_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 def _place_ghosts(self): p.resetBasePositionAndOrientation( self._ghost, posObj=self._goal, ornObj=self._obj_init_ori, physicsClientId=self._client, )
class PepperPushEnv(gym.GoalEnv): metadata = {"render.modes": ["human"]} def __init__(self, gui=False, sim_steps_per_action=10, max_motion_speed=0.5): self._sim_steps = sim_steps_per_action self._max_speeds = [max_motion_speed] * len(CONTROLLABLE_JOINTS) self._gui = gui 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") self.observation_space = spaces.Dict( dict( desired_goal=spaces.Box(-np.inf, np.inf, shape=obs["achieved_goal"].shape, dtype="float32"), achieved_goal=spaces.Box(-np.inf, np.inf, shape=obs["achieved_goal"].shape, dtype="float32"), observation=spaces.Box(-np.inf, np.inf, shape=obs["observation"].shape, dtype="float32"), )) def reset(self): self._reset_scene() self._goal = self._sample_goal() if self._gui: self._place_ghosts() 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(obs["achieved_goal"], self._goal) is_safety_violated = self._is_table_touched( ) or self._is_table_displaced() info = { "is_success": is_success, } reward = self.compute_reward(obs["achieved_goal"], self._goal, info) done = is_success or is_safety_violated return (obs, reward, done, info) def render(self, mode="human"): pass def close(self): self._simulation_manager.stopSimulation(self._client) def seed(self, seed=None): np.random.seed(seed or 0) def compute_reward(self, achieved_goal, desired_goal, info): return (np.linalg.norm(desired_goal - achieved_goal, axis=-1) < DISTANCE_THRESHOLD).astype(np.float32) def _is_success(self, achieved_goal, desired_goal): return (np.linalg.norm(desired_goal - achieved_goal, axis=-1) < DISTANCE_THRESHOLD) 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 _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): obj_pos = p.getBasePositionAndOrientation( self._obj, physicsClientId=self._client)[0] obj_vel = p.getBaseVelocity(self._obj, physicsClientId=self._client)[0] joint_p = self._robot.getAnglesPosition(CONTROLLABLE_JOINTS) joint_v = self._robot.getAnglesVelocity(CONTROLLABLE_JOINTS) hand_idx = self._robot.link_dict["l_hand"].getIndex() hand_pos = p.getLinkState(self._robot.getRobotModel(), hand_idx, physicsClientId=self._client)[0] obj_rel_pos = np.array(obj_pos) - np.array(hand_pos) return { "observation": np.concatenate([obj_pos, obj_vel, joint_p, joint_v, obj_rel_pos]).astype(np.float32), "achieved_goal": np.array(obj_pos, dtype=np.float32), "desired_goal": self._goal, } def _sample_goal(self): return np.append( (np.random.sample(2) * [0.2, 0.4] + self._obj_start_pos[:2] - [0.1, 0.2]).astype(np.float32), self._obj_start_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 def _place_ghosts(self): p.resetBasePositionAndOrientation( self._ghost, posObj=self._goal, ornObj=self._obj_init_ori, physicsClientId=self._client, )
class PepperEnv(gym.Env): metadata = {"render.modes": ["human"]} CONTROLLABLE_JOINTS = [ "HeadYaw", "HeadPitch", "HipRoll", "LShoulderPitch", "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand", ] FEATURE_LIMITS = [ (-2.0857, 2.0857), (-0.7068, 0.4451), (-0.5149, 0.5149), (-2.0857, 2.0857), (0.0087, 1.5620), (-2.0857, 2.0857), (-1.5620, -0.0087), (-1.8239, 1.8239), (0, 1), (0, 1), ] def __init__(self, gui=False, sim_steps_per_action=10, head_motion=True): self._sim_steps = sim_steps_per_action self._gui = gui self._setup_scene() self._goal = self._sample_goal() if not head_motion: self.CONTROLLABLE_JOINTS = self.CONTROLLABLE_JOINTS[3:] self.FEATURE_LIMITS = self.FEATURE_LIMITS[3:] self.action_space = spaces.Box(-1.0, 1.0, shape=(len(self.CONTROLLABLE_JOINTS) + 1, ), dtype="float32") self.observation_space = self._get_observation_space() def __del__(self): self.close() def reset(self): raise "Not implemented" def step(self, action): raise "Not implemented" def render(self, mode="human"): pass def close(self): if self._simulation_manager: self._simulation_manager.stopSimulation(self._client) def seed(self, seed=None): np.random.seed(seed or 0) def _perform_action(self, action): action = list(action) assert len(action) == len(self.action_space.high.tolist()) rescaled = [ self._rescale_feature(i, f) for (i, f) in enumerate(action) ] angles = rescaled[:-1] speed = rescaled[-1] self._robot.setAngles(self.CONTROLLABLE_JOINTS, angles, [speed] * len(angles)) for _ in range(self._sim_steps): p.stepSimulation(physicsClientId=self._client) 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 _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 obj_pos 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_joints_states(self): joint_p = self._robot.getAnglesPosition(self.CONTROLLABLE_JOINTS) scaled = [self._scale_feature(i, f) for (i, f) in enumerate(joint_p)] return scaled def _get_observation_space(self): raise "Not implemented" def _get_observation(self): raise "Not implemented" 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 def _rescale_feature(self, index, value): r = self.FEATURE_LIMITS[index] return (r[1] - r[0]) * (value + 1) / 2 + r[0] def _scale_feature(self, index, value): r = self.FEATURE_LIMITS[index] return (value - r[0]) * 2 / (r[1] - r[0]) - 1