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
