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 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():
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 _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 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():
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 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 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 setUpClass(cls):
"""
Launches a simulation and spawns a Pepper virtual robot
"""
SensorTest.simulation = SimulationManager()
SensorTest.client = SensorTest.simulation.launchSimulation(gui=False)
SensorTest.sensor = DummySensor(SensorTest.client)
def test_reset_simulation(self):
"""
Test the @resetSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.resetSimulation(client)
manager.stopSimulation(client)
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 __init__(self, gui=False):
self._simulation_manager = SimulationManager()
self._client_id = self._simulation_manager.launchSimulation(gui=gui)
self._agent = self._simulation_manager.spawnPepper(
self._client_id, spawn_ground_plane=True)
self._laser_handle = self._agent.subscribeLaser()
self._camera_handle = self._agent.subscribeCamera(
PepperVirtual.ID_CAMERA_BOTTOM)
def _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 setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
DummyFsrTest.simulation = SimulationManager()
DummyFsrTest.client = DummyFsrTest.simulation.launchSimulation(
gui=False)
DummyFsrTest.robot = DummyRobot()
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
ImuTest.simulation = SimulationManager()
ImuTest.client = ImuTest.simulation.launchSimulation(gui=False)
ImuTest.robot = ImuTest.simulation.spawnNao(ImuTest.client,
spawn_ground_plane=True)
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
PostureTest.simulation = SimulationManager()
PostureTest.client = PostureTest.simulation.launchSimulation(gui=False)
PostureTest.robot = PostureTest.simulation.spawnRomeo(
PostureTest.client, spawn_ground_plane=True)
def setUpClass(cls):
"""
Launches a simulation and spawns a Pepper virtual robot
"""
VirtualRobotTest.simulation = SimulationManager()
VirtualRobotTest.client = VirtualRobotTest.simulation.launchSimulation(
gui=False)
VirtualRobotTest.robot = DummyVirtual()
def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
def setUpClass(cls):
"""
Launches a simulation and spawns the Pepper virtual robot
"""
JointTest.simulation = SimulationManager()
JointTest.client = JointTest.simulation.launchSimulation(gui=False)
JointTest.robot = JointTest.simulation.spawnPepper(
JointTest.client, spawn_ground_plane=True)
def setUpClass(cls):
"""
Launches a simulation and spawns the Romeo virtual robot
"""
CameraTest.simulation = SimulationManager()
CameraTest.client = CameraTest.simulation.launchSimulation(gui=False)
CameraTest.robot = CameraTest.simulation.spawnRomeo(
CameraTest.client, spawn_ground_plane=True)
def 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 setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
FsrTest.simulation = SimulationManager()
FsrTest.client = FsrTest.simulation.launchSimulation(gui=False)
FsrTest.fsr_names = NaoFsr.RFOOT + NaoFsr.LFOOT
FsrTest.fsr_groups = [NaoFsr.RFOOT, NaoFsr.LFOOT]
FsrTest.robot = FsrTest.simulation.spawnNao(FsrTest.client,
spawn_ground_plane=True)
def 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 __init__(self):
# self.head_sub = rospy.Subscriber('/pepper_dcm/Head_controller/command', JointTrajectory, self.callback)
# self.left_arm_sub = rospy.Subscriber('/pepper_dcm/LeftArm_controller/command', JointTrajectory, self.callback)
# self.right_arm_sub = rospy.Subscriber('/pepper_dcm/RightArm_controller/command', JointTrajectory, self.callback)
# self.left_hand_sub = rospy.Subscriber('/pepper_dcm/LeftHand_controller/command', JointTrajectory, self.callback)
# self.right_hand_sub = rospy.Subscriber('/pepper_dcm/RightHand_controller/command', JointTrajectory, self.callback)
self.simulation_manager = SimulationManager()
self.client_id = self.simulation_manager.launchSimulation(gui=True)
self.pepper = self.simulation_manager.spawnPepper(
self.client_id, spawn_ground_plane=True)
self.wrap = PepperRosWrapper()
self.wrap.launchWrapper(self.pepper, "/naoqi_driver")
def generate_world_1(self):
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pybullet.setAdditionalSearchPath(
"/home/tp/Projet/g7_dupuis_dez_flachard")
StartPosTable = [4, 0, 0.2]
StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0])
PlaneId = pybullet.loadURDF("table.urdf", StartPosTable,
StartOrientationTable)
StartPosLego = [2, 0, 0.1]
PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego,
StartOrientationTable)
StartPosTot1 = [3.7, 0, 0.75]
StartPosTot2 = [3.7, 0.4, 0.75]
StartPosTot3 = [3.7, -0.4, 0.75]
StartPosBox1 = [-1, 1, -0.3]
StartPosBox2 = [-1, -1, -0.3]
StartPosBox3 = [-1, 0, -0.3]
Box1 = pybullet.loadURDF("box.urdf", StartPosBox1,
StartOrientationTable)
Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2,
StartOrientationTable)
Box3 = pybullet.loadURDF("box3.urdf", StartPosBox3,
StartOrientationTable)
Choix = [StartPosTot1, StartPosTot2, StartPosTot3]
Positions = []
while (len(Choix) > 0):
pos = choice(Choix)
Positions.append(pos)
Choix.remove(pos)
Tot1_Id = pybullet.loadURDF("duck.urdf", Positions[0],
StartOrientationTable)
Tot2_Id = pybullet.loadURDF("eye.urdf", Positions[1],
StartOrientationTable)
Tot3_Id = pybullet.loadURDF("objet3.urdf", Positions[2],
StartOrientationTable)
pepper = simulation_manager.spawnPepper(client_id,
spawn_ground_plane=True)
pepper.angular_velocity = 1.0
pepper.goToPosture("Stand", 0.6)
pepper.setAngles("HeadPitch", 0.27, 0.27)
return pepper
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_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_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_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)
