def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False, use_shared_memory=True)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False,
use_shared_memory=True,
auto_step=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False, auto_step=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
# Ensure that stopping the simulation one more time won't raise an
# error
try:
manager.stopSimulation(client)
self.assertTrue(True)
except Exception:
self.assertTrue(False)
def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False, use_shared_memory=True)
self.assertEqual(client, 0)
manager.stopSimulation(client)
def test_gravity(self):
"""
Test the @setGravity and @getGravity methods
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
gravity = manager.getGravity(client)
self.assertIsInstance(gravity, list)
self.assertEqual(len(gravity), 3)
# Test with an invalid client
gravity = manager.getGravity(-1)
self.assertIsNone(gravity)
gravities = [[0.0, 0.0, -9.81], [0.0, 0.0, 9.81], [1.0, 3.0, 0.0],
[-1.0, 3.5, 2.0]]
for gravity in gravities:
manager.setGravity(client, gravity)
value = manager.getGravity(client)
self.assertIsInstance(value, list)
for i in range(len(gravity)):
self.assertEqual(value[i], gravity[i])
# Test with an invalid client
with self.assertRaises(pybullet.error):
manager.setGravity(-1, gravities[0])
manager.stopSimulation(client)
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF(
"duck_vhacd.urdf",
basePosition=[1, -1, 0.5],
globalScaling=10.0,
physicsClientId=client)
pepper.showLaser(True)
pepper.subscribeLaser()
pepper.goToPosture("Stand", 0.6)
while True:
laser_list = pepper.getRightLaserValue()
laser_list.extend(pepper.getFrontLaserValue())
laser_list.extend(pepper.getLeftLaserValue())
if all(laser == 5.6 for laser in laser_list):
print("Nothing detected")
else:
print("Detected")
pass
def test_camera_removal(self):
"""
Ensure that the camera processes are stopped when removing a robot,
stopping or resetting a simulation
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
pepper = manager.spawnPepper(client, spawn_ground_plane=True)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
camera = pepper.getCamera(handle)
self.assertTrue(camera.isActive())
manager.removePepper(pepper)
self.assertFalse(camera.isActive())
pepper = manager.spawnPepper(client, spawn_ground_plane=True)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
camera = pepper.getCamera(handle)
self.assertTrue(camera.isActive())
manager.resetSimulation(client)
self.assertFalse(camera.isActive())
pepper = manager.spawnPepper(client, spawn_ground_plane=True)
handle = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
camera = pepper.getCamera(handle)
self.assertTrue(camera.isActive())
manager.stopSimulation(client)
self.assertFalse(camera.isActive())
class Simulation:
"""
Classe gérant la simu
"""
def __init__(self):
self.sim = SimulationManager()
self.client = self.sim.launchSimulation()
self.robot = Robot(self.sim, self.client)
self.createScene()
self.robot.start()
def createScene(self):
pb.setAdditionalSearchPath(pd.getDataPath())
pb.loadURDF("duck_vhacd.urdf", basePosition=[4, 2, 0], globalScaling=5)
pb.loadURDF("sphere2red.urdf",
basePosition=[-3, 0.05, 1],
globalScaling=1)
time.sleep(5)
def getPepper(self):
return self.pepper
def getClient(self):
return self.client
def getSimulationManager(self):
return self.simulationManager
def generate_world_2(self):
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pybullet.setAdditionalSearchPath(
"/home/tp/Projet/g7_dupuis_dez_flachard")
StartPosTable = [5.2, 1, 0.2]
StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0])
PlaneId = pybullet.loadURDF("table.urdf", StartPosTable,
StartOrientationTable)
StartPosLego = [2, 0, 0.1]
PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego,
StartOrientationTable)
StartPosBox1 = [3.55, 1.5, -0.3]
StartPosBox2 = [3.1, 2, -0.3]
StartOrientationbox2 = pybullet.getQuaternionFromEuler([0, 0, 1.57])
Box1 = pybullet.loadURDF("box.urdf", StartPosBox1,
StartOrientationTable)
Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2,
StartOrientationbox2)
pepper = simulation_manager.spawnPepper(client_id,
spawn_ground_plane=True)
pepper.angular_velocity = 1.0
pepper.goToPosture("Stand", 0.6)
pepper.setAngles("HeadPitch", 0.27, 0.27)
return pepper
def main():
global my_model
global canMove
# global attribute to replicate a state machine
canMove = True
CURR_DIR = os.getcwd()
my_model = tf.keras.models.load_model(CURR_DIR + '\saved_model\model_3000')
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client_id,
translation=[-2, 0, 0],
spawn_ground_plane=True)
pepper2 = simulation_manager.spawnPepper(client_id,
translation=[0, 0, 0],
quaternion=[0, 0, -4, 0],
spawn_ground_plane=True)
ObjectSpawner()
#cam = Camera(pepper)
mover = AleaMove(pepper2, 'StandZero')
pepClone = CloningBehavior(pepper)
#cam.start()
mover.start()
pepClone.start()
#cam.join()
mover.join()
pepClone.join()
def main():
image_name = 'image.png'
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM)
#pepper.goToPosture("Crouch", 0.6)
#time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
#pepper.goToPosture("StandZero", 0.6)
#time.sleep(1)
flag_pic = 0 #flag to launch the image recognition
while True:
img = pepper.getCameraFrame() #capture camera frame
cv2.imshow("bottom camera", img) #show the capture
cv2.waitKey(1)
flag_pic = flag_pic + 1 #debug flag
if flag_pic == 10:
print('photo taken') #debug
flag_pic = 0
cv2.imwrite(image_name,
img) #write the image in the current directory
result = Algo_detect(image_name, score) #COCO detection
def main():
# qi App Session
qiApp = qi.Application(sys.argv)
# Bullet Simulator
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pepperSim = simulation_manager.spawnPepper(client_id,
translation=[0, 0, 0],
quaternion=[0, 0, 0, 1],
spawn_ground_plane=True)
# wrap qi App Session with Simulated Pepper
wrap = NaoqibulletWrapper.NaoqibulletWrapper(
qiApp, pepperSim) # /!\ keep wrap instance to keep thread
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("table.urdf",
basePosition=[-2, 0, 0],
globalScaling=1,
physicsClientId=client_id)
pybullet.loadURDF("totem.urdf",
basePosition=[-1.5, 0, 1],
globalScaling=0.7,
physicsClientId=client_id)
pybullet.loadURDF("totempomme.urdf",
basePosition=[-1.5, -0.25, 1],
globalScaling=0.7,
physicsClientId=client_id)
pybullet.loadURDF("totemwine.urdf",
basePosition=[-1.5, 0.25, 1],
globalScaling=0.7,
physicsClientId=client_id)
qiSession = qiApp.session
motionService = qiSession.service("ALMotion")
#add function here
init_cam(pepperSim)
motionService.setAngles(["LShoulderPitch", "RShoulderPitch"], [1, 1],
[1, 1])
#take photo
turn(motionService, pi)
take_picture(pepperSim)
#go to the object
move_right_obj(motionService)
#go to the box
move_to_Blue(motionService)
# block until stop is called.
qiApp.run()
# close nicely
wrap.close()
def test_reset_simulation(self):
"""
Test the @resetSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.resetSimulation(client)
manager.stopSimulation(client)
def test_set_light_position(self):
"""
Test the @setLightPosition method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.setLightPosition(client, [10, 20, 2])
manager.setLightPosition(client, [0, 0, 10])
manager.setLightPosition(client, [-5, -20, 50])
manager.stopSimulation(client)
def main():
sim_manager = SimulationManager()
client = sim_manager.launchSimulation(gui=True)
# client_direct_1 = sim_manager.launchSimulation(gui=False)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF('plane.urdf')
duck = pybullet.loadURDF('duck_vhacd.urdf',
basePosition=[0, 0, 0],
globalScaling=5.0,
physicsClientId=client)
# r2d2 = pybullet.loadURDF(
# "r2d2.urdf",
# basePosition = [-2, 0, 0],
# globalScaling = 5.0,
# physicsClientId = client_direct_1)
nao = sim_manager.spawnNao(client,
translation=[2, 0, 0],
quaternion=pybullet.getQuaternionFromEuler(
[0, 0, 3]))
pepper = sim_manager.spawnPepper(
client,
translation=[0, -2, 0],
quaternion=pybullet.getQuaternionFromEuler([0, 0, 1.5]))
romeo = sim_manager.spawnRomeo(client,
translation=[0, 2, 0],
quaternion=pybullet.getQuaternionFromEuler(
[0, 0, -1.5]))
nao.goToPosture('StandInit', 1)
pepper.goToPosture('StandInit', 1)
romeo.goToPosture('StandInit', 1)
nao.subscribeCamera(NaoVirtual.ID_CAMERA_TOP)
pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
romeo.subscribeCamera(RomeoVirtual.ID_CAMERA_DEPTH)
nao.setAngles('HeadPitch', 0.25, 1)
while True:
nao_cam = nao.getCameraFrame()
cv2.imshow('Nao Cam', nao_cam)
pepper_cam = pepper.getCameraFrame()
cv2.imshow('Pepper Cam', pepper_cam)
romeo_cam = romeo.getCameraFrame()
cv2.imshow('Romeo Cam', romeo_cam)
cv2.waitKey(1)
pass
def generate_world_1(self):
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pybullet.setAdditionalSearchPath(
"/home/tp/Projet/g7_dupuis_dez_flachard")
StartPosTable = [4, 0, 0.2]
StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0])
PlaneId = pybullet.loadURDF("table.urdf", StartPosTable,
StartOrientationTable)
StartPosLego = [2, 0, 0.1]
PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego,
StartOrientationTable)
StartPosTot1 = [3.7, 0, 0.75]
StartPosTot2 = [3.7, 0.4, 0.75]
StartPosTot3 = [3.7, -0.4, 0.75]
StartPosBox1 = [-1, 1, -0.3]
StartPosBox2 = [-1, -1, -0.3]
StartPosBox3 = [-1, 0, -0.3]
Box1 = pybullet.loadURDF("box.urdf", StartPosBox1,
StartOrientationTable)
Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2,
StartOrientationTable)
Box3 = pybullet.loadURDF("box3.urdf", StartPosBox3,
StartOrientationTable)
Choix = [StartPosTot1, StartPosTot2, StartPosTot3]
Positions = []
while (len(Choix) > 0):
pos = choice(Choix)
Positions.append(pos)
Choix.remove(pos)
Tot1_Id = pybullet.loadURDF("duck.urdf", Positions[0],
StartOrientationTable)
Tot2_Id = pybullet.loadURDF("eye.urdf", Positions[1],
StartOrientationTable)
Tot3_Id = pybullet.loadURDF("objet3.urdf", Positions[2],
StartOrientationTable)
pepper = simulation_manager.spawnPepper(client_id,
spawn_ground_plane=True)
pepper.angular_velocity = 1.0
pepper.goToPosture("Stand", 0.6)
pepper.setAngles("HeadPitch", 0.27, 0.27)
return pepper
def test_spawn_nao(self):
"""
Test the @spawnNao method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
nao = manager.spawnNao(client,
translation=[2, 4, 1],
quaternion=[0, 0.1, 0, 1],
spawn_ground_plane=True)
self.assertIsInstance(nao, NaoVirtual)
self.assertNotEqual(len(nao.joint_dict.keys()), 0)
manager.stopSimulation(client)
def test_spawn_romeo(self):
"""
Test the @spawnRomeo method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
romeo = manager.spawnRomeo(client,
translation=[1, 1, 4],
quaternion=[0, 0, 0.4, 1],
spawn_ground_plane=True)
self.assertIsInstance(romeo, RomeoVirtual)
self.assertNotEqual(len(romeo.joint_dict.keys()), 0)
manager.stopSimulation(client)
def test_remove_nao(self):
"""
Test the @removeNao method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
nao = manager.spawnNao(client, spawn_ground_plane=True)
manager.removeNao(nao)
with self.assertRaises(pybullet.error):
pybullet.getBodyInfo(nao.getRobotModel())
manager.stopSimulation(client)
def test_step_simulation(self):
"""
Test the @stepSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False, auto_step=False)
manager.stepSimulation(client)
with self.assertRaises(pybullet.error):
manager.stepSimulation(client + 1)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False,
use_shared_memory=True,
auto_step=False)
manager.stepSimulation(client)
with self.assertRaises(pybullet.error):
manager.stepSimulation(client + 1)
manager.stopSimulation(client)
def test_spawn_pepper(self):
"""
Test the @spawnPepper method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
pepper = manager.spawnPepper(client,
translation=[0, 0, 0],
quaternion=[0, 0, 0, 1],
spawn_ground_plane=True)
self.assertIsInstance(pepper, PepperVirtual)
self.assertNotEqual(len(pepper.joint_dict.keys()), 0)
manager.stopSimulation(client)
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.goToPosture("Crouch", 0.6)
time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
pepper.goToPosture("StandZero", 0.6)
time.sleep(1)
pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM)
while True:
img = pepper.getCameraFrame()
cv2.imshow("bottom camera", img)
cv2.waitKey(1)
def main(session):
global asked_object
global asked_rangement
# Create the simulation with the Pepper
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
# Original positions of the objects (give random positions)
liste = [-0.2, 0, 0.2]
gene_positions(liste)
# Initialization of the decor and the robot
init_decor(client)
init_posture(pepper)
# The user asks an object and a rangement
[liste_asked_rangement, liste_asked_object] = user_request(session)
# Step 1 : Detect the 3 objects and their position
pepper.moveTo(1.9, 0, 0) # Move to the position to take the picture
get_image(pepper)
dico_objets = analyse_image(client)
for i in range(len(liste_asked_rangement)):
numero_obj = dico_objets[
liste_asked_object[i]] # Give the position of the object asked
# Step 2 : Take the object asked
grab_object(pepper, numero_obj)
# Step 3 : Store the object in the right place
if (liste_asked_rangement[i] == "table"):
drop_night(pepper)
elif (liste_asked_rangement[i] == "fauteuil"):
drop_chair(pepper)
elif (liste_asked_rangement[i] == "carton"):
drop_carton(pepper)
# Step 4 : DAB
dab(pepper)
while True:
pass
def test_stop_simulation(self):
"""
Test the @stopSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.stopSimulation(client)
with self.assertRaises(pybullet.error):
pybullet.stepSimulation(physicsClientId=client)
# Try stopping an invalid simulation instance
try:
manager.stopSimulation(-1)
self.assertTrue(True)
except Exception:
self.assertTrue(False)
def test_set_light_position(self):
"""
Test the @setLightPosition method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.setLightPosition(client, [10, 20, 2])
manager.setLightPosition(client, [0, 0, 10])
manager.setLightPosition(client, [-5, -20, 50])
with self.assertRaises(pybullet.error):
manager.setLightPosition(client, "not a list")
with self.assertRaises(pybullet.error):
manager.setLightPosition(client,
[1, 2, 3, "wrong amount of elements"])
manager.stopSimulation(client)
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.goToPosture("Crouch", 0.6)
time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
pepper.goToPosture("StandZero", 0.6)
time.sleep(1)
# Subscribe to the top RGB camera in QVGA (default value, specified here
# for more clarity) and 15fps
handle_top = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP,
resolution=Camera.K_QVGA,
fps=15.0)
# Same process for the bottom camera
handle_bottom = pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM,
resolution=Camera.K_QVGA,
fps=15.0)
try:
while True:
img_top = pepper.getCameraFrame(handle_top)
img_bottom = pepper.getCameraFrame(handle_bottom)
cv2.imshow("top camera", img_top)
cv2.imshow("bottom camera", img_bottom)
cv2.waitKey(1)
except KeyboardInterrupt:
pass
finally:
# No need to manually unsubscribe from the cameras when calling
# stopSimulation, the method will automatically unsubscribe from the
# cameras
simulation_manager.stopSimulation(client)
class VirtualPepperMotionControll:
def __init__(self):
# self.head_sub = rospy.Subscriber('/pepper_dcm/Head_controller/command', JointTrajectory, self.callback)
# self.left_arm_sub = rospy.Subscriber('/pepper_dcm/LeftArm_controller/command', JointTrajectory, self.callback)
# self.right_arm_sub = rospy.Subscriber('/pepper_dcm/RightArm_controller/command', JointTrajectory, self.callback)
# self.left_hand_sub = rospy.Subscriber('/pepper_dcm/LeftHand_controller/command', JointTrajectory, self.callback)
# self.right_hand_sub = rospy.Subscriber('/pepper_dcm/RightHand_controller/command', JointTrajectory, self.callback)
self.simulation_manager = SimulationManager()
self.client_id = self.simulation_manager.launchSimulation(gui=True)
self.pepper = self.simulation_manager.spawnPepper(
self.client_id, spawn_ground_plane=True)
self.wrap = PepperRosWrapper()
self.wrap.launchWrapper(self.pepper, "/naoqi_driver")
#self.pepper.subscribeCamera(PepperVirtual.ID_CAMERA_TOP)
# def callback(self, msg):
# print(msg.joint_names)
# print(list(msg.points[0].positions))
# self.pepper.setAngles(msg.joint_names, list(msg.points[0].positions))
def __del__(self):
self.wrap.stopWrapper()
self.simulation_manager.stopSimulation(self.client_id)
from qibullet import PepperVirtual
import time
import random
from CardScanner import cardsearch # abreviates the name so it is easier to use . can also try "FROM CardScanner import cardsearch,x,y because x and y are global variables"
Robot = "pepper"
for x in range(
2
): # creates two random variables to get pepper to point at a random location
random_x = random.randint(-3, 0) #gridwidth max 5
random_y = random.randint(0, 4) #gridheight max 4
#if __name__ == "__main__":
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
#Spawn PEPPER
Robot = simulation_manager.spawnPepper(client_id, spawn_ground_plane=True)
#Pointing
def Point_at_Match():
Reset()
Robot.setAngles(
"RShoulderRoll", (-1 * (2.2 / 20) * int(cardsearch()[1])), 0.1
) ## rolls shoulder in x direction should, come after movement in y direction
time.sleep(1.0)
## pepper.setAngles("RShoulderRoll", 0.0, 0.1)
Robot.setAngles("RShoulderPitch", ((2.2 / 20) * int(cardsearch()[2])), 0.1)
time.sleep(1.0)
class Simulation(threading.Thread):
def __init__(self):
threading.Thread.__init__(self)
self.manager = SimulationManager()
self.client_id = self.manager.launchSimulation(gui=True)
p.connect(p.DIRECT)
self.robot = Robot(self.manager, self.client_id)
self.createScene()
self.initUI()
def initUI(self):
self.joint_parameters = list()
for name, joint in self.robot.pepper.joint_dict.items():
if "Finger" not in name and "Thumb" not in name:
self.joint_parameters.append(
(
p.addUserDebugParameter(
name,
joint.getLowerLimit(),
joint.getUpperLimit(),
self.robot.pepper.getAnglesPosition(name)
),
name
)
)
def createScene(self):
p.setAdditionalSearchPath(pybullet_data.getDataPath())
p.loadURDF("duck_vhacd.urdf", basePosition=[random.randrange(-3,3),random.randrange(-3,3),0.5], globalScaling=10, )
for i in range(24):
self.createSphere(0.2, random.choice([-5, -4, -3, -2, -1, 1, 2, 4, 5]), random.choice([-5,-4, -3,-2,-1,1,2,3,4,5]), 0.3,
color=(random.randint(0, 255), random.randint(0, 255), random.randint(0, 255)))
def createWall(self, w, h, d, x, y, z):
wall_visual = p.createVisualShape(p.GEOM_BOX, halfExtents=[w, h, d])
wall_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[w, h, d])
wall_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=wall_collision, baseVisualShapeIndex=wall_visual, basePosition=[x, y, z])
def createSphere(self, radius, x, y, z, color=(255, 0, 0)):
sphere_visual = p.createVisualShape(p.GEOM_SPHERE, rgbaColor=[color[0]/255, color[1]/255, color[2]/255, 1], radius=radius)
sphere_collision = p.createCollisionShape(p.GEOM_SPHERE, radius=radius)
sphere_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=sphere_collision,baseVisualShapeIndex=sphere_visual, basePosition=[x, y, z])
def createCube(self, width, x, y, color=(255, 0, 0)):
cube_visual = p.createVisualShape(p.GEOM_BOX, rgbaColor=[color[0]/255, color[1]/255, color[2]/255, 1], halfExtents=[0.5, 0.5, 0.5])
cube_collision = p.createCollisionShape(p.GEOM_BOX, halfExtents=[width, width, width])
cube_body = p.createMultiBody(baseMass=0, baseCollisionShapeIndex=cube_collision, baseVisualShapeIndex=cube_visual, basePosition=[x,y,width/2])
def signal_handler(self, signal, frame):
#print("CTRL-C detected")
for t in self.robot.threads:
t.kill()
self.robot.kill()
self.manager.stopSimulation(self.client_id)
sys.exit(0)
def run(self):
self.robot.start()
"""
#Used to follow the robot position but lower performances
while True:
p.resetDebugVisualizerCamera(cameraDistance= p.getDebugVisualizerCamera(self.client_id)[10],
cameraYaw= p.getDebugVisualizerCamera(self.client_id)[8],
cameraPitch= p.getDebugVisualizerCamera(self.client_id)[9],
cameraTargetPosition= [self.robot.pepper.getPosition()[0], self.robot.pepper.getPosition()[1], 0.5])
"""
"""
#Used to find new posture for PiLDIM in case we add more.
for joint_parameter in self.joint_parameters:
self.robot.pepper.setAngles(
joint_parameter[1],
p.readUserDebugParameter(joint_parameter[0]), 1.0
)
"""
def main():
nb_clients = 10
iterations = 10
if iterations < nb_clients:
nb_clients = iterations
task_list = list()
iterations_per_task = int(iterations / nb_clients)
simulation_manager = SimulationManager()
client_list = list()
pepper_list = list()
keys = list()
collision_links = list()
gui = False
for i in range(nb_clients):
client = simulation_manager.launchSimulation(gui=gui)
pepper = simulation_manager.spawnPepper(client,
spawn_ground_plane=True)
if gui:
gui = False
client_list.append(client)
pepper_list.append(pepper)
collision_links = ['r_wrist', 'LBicep', 'l_wrist', 'RBicep', 'Pelvis']
for key, value in pepper_list[0].joint_dict.items():
if "Finger" in key or "Thumb" in key or "Hand" in key or "Head" in key:
continue
else:
keys.append(key)
for i in range(nb_clients):
task_list.append(
Task(pepper_list[i], keys, collision_links, iterations_per_task))
mean_error = [0] * len(keys)
for task in task_list:
task.start()
print("Task " + str(task.pepper.getPhysicsClientId()) + " started")
for task in task_list:
task.join()
print("Task " + str(task.pepper.getPhysicsClientId()) +
" finished after " + str(iterations_per_task) + " iteration(s)")
temp_error = [sum(x) for x in zip(mean_error, task.getResult())]
mean_error = list(temp_error)
for client in client_list:
simulation_manager.stopSimulation(client)
# Normalize the error given the iteration number
normalized_mean_error = [x / iterations for x in mean_error]
plt.bar(range(len(normalized_mean_error)), normalized_mean_error)
plt.xticks(range(len(normalized_mean_error)),
tuple(keys),
rotation='vertical',
horizontalalignment='left')
plt.show()
class PepperReachCamEnv(gym.Env):
metadata = {"render.modes": ["human"]}
def __init__(
self,
gui=False,
sim_steps_per_action=10,
max_motion_speed=0.3,
dense=True,
depth_camera=False,
top_camera=False,
):
self._sim_steps = sim_steps_per_action
self._max_speeds = [max_motion_speed] * len(CONTROLLABLE_JOINTS)
self._gui = gui
self._dense = dense
self._depth_camera = depth_camera
self._top_camera = top_camera
self._setup_scene()
self._goal = self._sample_goal()
obs = self._get_observation()
self.action_space = spaces.Box(-2.0857,
2.0857,
shape=(len(CONTROLLABLE_JOINTS), ),
dtype="float32")
obs_spaces = dict(
camera_bottom=spaces.Box(
0,
255,
shape=obs["camera_bottom"].shape,
dtype=obs["camera_bottom"].dtype,
),
joints_state=spaces.Box(
-np.inf,
np.inf,
shape=obs["joints_state"].shape,
dtype=obs["joints_state"].dtype,
),
)
if self._top_camera:
obs_spaces["camera_top"] = (spaces.Box(
0,
255,
shape=obs["camera_top"].shape,
dtype=obs["camera_top"].dtype,
), )
if self._depth_camera:
obs_spaces["camera_depth"] = spaces.Box(
0,
65_535,
shape=obs["camera_depth"].shape,
dtype=obs["camera_depth"].dtype,
)
self.observation_space = spaces.Dict(obs_spaces)
def reset(self):
self._reset_scene()
return self._get_observation()
def step(self, action):
action = list(action)
assert len(action) == len(self.action_space.high.tolist())
self._robot.setAngles(CONTROLLABLE_JOINTS, action, self._max_speeds)
for _ in range(self._sim_steps):
p.stepSimulation(physicsClientId=self._client)
obs = self._get_observation()
is_success = self._is_success()
hand_idx = self._robot.link_dict["l_hand"].getIndex()
hand_pos = np.array(
p.getLinkState(self._robot.getRobotModel(),
hand_idx,
physicsClientId=self._client)[0])
obj_pos = np.array(
p.getBasePositionAndOrientation(self._obj,
physicsClientId=self._client)[0])
is_safety_violated = self._is_table_touched(
) or self._is_table_displaced()
info = {
"is_success": is_success,
}
reward = self.compute_reward(is_success, obj_pos, hand_pos,
is_safety_violated)
done = is_success or is_safety_violated
return (obs, reward, done, info)
def render(self, mode="human"):
pass
def close(self):
if self._top_camera:
self._robot.unsubscribeCamera(self._cam_top)
self._robot.unsubscribeCamera(self._cam_bottom)
if self._depth_camera:
self._robot.unsubscribeCamera(self._cam_depth)
self._simulation_manager.stopSimulation(self._client)
def seed(self, seed=None):
np.random.seed(seed or 0)
def compute_reward(self, is_success, obj_pos, hand_pos,
is_safety_violated):
if is_success:
return 10.0 if self._dense else 1.0
if self._dense:
if is_safety_violated:
return -10.0
return -np.linalg.norm(obj_pos - hand_pos, axis=-1).astype(
np.float32)
return 0.0
def _is_success(self):
cont = p.getContactPoints(self._robot.getRobotModel(), self._obj)
return len(cont) > 0
def _setup_scene(self):
self._simulation_manager = SimulationManager()
self._client = self._simulation_manager.launchSimulation(
gui=self._gui, auto_step=False)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self._robot = self._simulation_manager.spawnPepper(
self._client, spawn_ground_plane=False)
self._robot.goToPosture("Stand", 1.0)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._robot.setAngles(["KneePitch", "HipPitch", "LShoulderPitch"],
[0.33, -0.9, -0.6], [0.5] * 3)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._table_init_pos = [0.35, 0, 0]
self._table_init_ori = [0, 0, 0, 1]
self._obj_init_pos = [0.35, 0, 0.65]
self._obj_init_ori = [0, 0, 0, 1]
path = Path(__file__).parent.parent / "assets" / "models"
p.setAdditionalSearchPath(str(path), physicsClientId=self._client)
self._obj = p.loadURDF("floor/floor.urdf",
physicsClientId=self._client,
useFixedBase=True)
self._table = p.loadURDF(
"adjustable_table/adjustable_table.urdf",
self._table_init_pos,
self._table_init_ori,
physicsClientId=self._client,
)
self._obj = p.loadURDF(
"brick/brick.urdf",
self._obj_init_pos,
self._obj_init_ori,
physicsClientId=self._client,
flags=p.URDF_USE_INERTIA_FROM_FILE,
)
# Let things fall down
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self.joints_initial_pose = self._robot.getAnglesPosition(
self._robot.joint_dict.keys())
self._obj_start_pos = p.getBasePositionAndOrientation(
self._obj, physicsClientId=self._client)[0]
# Setup camera
if self._top_camera:
self._cam_top = self._robot.subscribeCamera(
PepperVirtual.ID_CAMERA_TOP)
self._cam_bottom = self._robot.subscribeCamera(
PepperVirtual.ID_CAMERA_BOTTOM)
if self._depth_camera:
self._cam_depth = self._robot.subscribeCamera(
PepperVirtual.ID_CAMERA_DEPTH)
def _reset_scene(self):
p.resetBasePositionAndOrientation(
self._robot.robot_model,
posObj=[0.0, 0.0, 0.0],
ornObj=[0.0, 0.0, 0.0, 1.0],
physicsClientId=self._client,
)
self._reset_joint_state()
p.resetBasePositionAndOrientation(
self._table,
posObj=self._table_init_pos,
ornObj=self._table_init_ori,
physicsClientId=self._client,
)
obj_pos = self._sample_goal()
p.resetBasePositionAndOrientation(
self._obj,
posObj=obj_pos,
ornObj=self._obj_init_ori,
physicsClientId=self._client,
)
for _ in range(self._sim_steps):
p.stepSimulation(physicsClientId=self._client)
return self._get_observation()
def _reset_joint_state(self):
for joint, position in zip(self._robot.joint_dict.keys(),
self.joints_initial_pose):
p.setJointMotorControl2(
self._robot.robot_model,
self._robot.joint_dict[joint].getIndex(),
p.VELOCITY_CONTROL,
targetVelocity=0.0,
physicsClientId=self._client,
)
p.resetJointState(
self._robot.robot_model,
self._robot.joint_dict[joint].getIndex(),
position,
physicsClientId=self._client,
)
def _get_observation(self):
img_bottom = self._robot.getCameraFrame(self._cam_bottom)
joint_p = self._robot.getAnglesPosition(CONTROLLABLE_JOINTS)
# joint_v = self._robot.getAnglesVelocity(CONTROLLABLE_JOINTS)
result = {
"camera_bottom": img_bottom,
# "joints_state": np.concatenate([joint_p, joint_v]).astype(np.float32),
"joints_state": np.array(joint_p, dtype=np.float32)
}
if self._top_camera:
img_top = self._robot.getCameraFrame(self._cam_top)
result["camera_top"] = img_top
if self._depth_camera:
img_depth = self._robot.getCameraFrame(self._cam_depth)
result["camera_depth"] = img_depth
return result
def _sample_goal(self):
return np.append(
(np.random.sample(2) * [0.2, 0.4] + self._obj_init_pos[:2] -
[0.1, 0.2]).astype(np.float32),
self._obj_init_pos[2],
)
def _is_table_displaced(self):
pose = p.getBasePositionAndOrientation(self._table,
physicsClientId=self._client)
current_pose = np.array([e for t in pose for e in t], dtype=np.float32)
desired_pose = np.concatenate(
[self._table_init_pos, self._table_init_ori])
return not np.allclose(desired_pose, current_pose, atol=0.01)
def _is_table_touched(self):
cont = p.getContactPoints(self._robot.getRobotModel(), self._table)
return len(cont) > 0
if __name__ == "__main__":
simulation_manager = SimulationManager()
test_loader = unittest.TestLoader()
test_runner = unittest.TextTestRunner()
test_results = list()
has_failed = False
test_classes = [
PepperBaseTest,
PepperJointTest,
PepperLaserTest,
PepperCameraTest,
PepperPostureTest,
PepperSelfCollisionTest]
physics_client = simulation_manager.launchSimulation(gui=False)
for test_class in test_classes:
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadMJCF("mjcf/ground_plane.xml")
test_results.append(
test_runner.run(test_loader.loadTestsFromTestCase(test_class)))
simulation_manager.resetSimulation(physics_client)
simulation_manager.stopSimulation(physics_client)
print("------------------------------------------------------------------")
for i in range(len(test_results)):
test_count = test_results[i].testsRun
test_failures = test_results[i].failures
