def main():
image_name = 'image.png'
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM)
#pepper.goToPosture("Crouch", 0.6)
#time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
#pepper.goToPosture("StandZero", 0.6)
#time.sleep(1)
flag_pic = 0 #flag to launch the image recognition
while True:
img = pepper.getCameraFrame() #capture camera frame
cv2.imshow("bottom camera", img) #show the capture
cv2.waitKey(1)
flag_pic = flag_pic + 1 #debug flag
if flag_pic == 10:
print('photo taken') #debug
flag_pic = 0
cv2.imwrite(image_name,
img) #write the image in the current directory
result = Algo_detect(image_name, score) #COCO detection
def generate_world_2(self):
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pybullet.setAdditionalSearchPath(
"/home/tp/Projet/g7_dupuis_dez_flachard")
StartPosTable = [5.2, 1, 0.2]
StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0])
PlaneId = pybullet.loadURDF("table.urdf", StartPosTable,
StartOrientationTable)
StartPosLego = [2, 0, 0.1]
PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego,
StartOrientationTable)
StartPosBox1 = [3.55, 1.5, -0.3]
StartPosBox2 = [3.1, 2, -0.3]
StartOrientationbox2 = pybullet.getQuaternionFromEuler([0, 0, 1.57])
Box1 = pybullet.loadURDF("box.urdf", StartPosBox1,
StartOrientationTable)
Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2,
StartOrientationbox2)
pepper = simulation_manager.spawnPepper(client_id,
spawn_ground_plane=True)
pepper.angular_velocity = 1.0
pepper.goToPosture("Stand", 0.6)
pepper.setAngles("HeadPitch", 0.27, 0.27)
return pepper
def main():
global my_model
global canMove
# global attribute to replicate a state machine
canMove = True
CURR_DIR = os.getcwd()
my_model = tf.keras.models.load_model(CURR_DIR + '\saved_model\model_3000')
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client_id,
translation=[-2, 0, 0],
spawn_ground_plane=True)
pepper2 = simulation_manager.spawnPepper(client_id,
translation=[0, 0, 0],
quaternion=[0, 0, -4, 0],
spawn_ground_plane=True)
ObjectSpawner()
#cam = Camera(pepper)
mover = AleaMove(pepper2, 'StandZero')
pepClone = CloningBehavior(pepper)
#cam.start()
mover.start()
pepClone.start()
#cam.join()
mover.join()
pepClone.join()
def _setup_scene(self):
self._simulation_manager = SimulationManager()
self._client = self._simulation_manager.launchSimulation(
gui=self._gui, auto_step=False)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self._robot = self._simulation_manager.spawnPepper(
self._client, spawn_ground_plane=True)
self._robot.goToPosture("Stand", 1.0)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._robot.setAngles(["KneePitch", "HipPitch", "LShoulderPitch"],
[0.33, -0.9, -0.6], [0.5] * 3)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
path = Path(__file__).parent.parent / "assets" / "models"
p.setAdditionalSearchPath(str(path), physicsClientId=self._client)
self._table_init_pos = [0.35, 0, 0]
self._table_init_ori = [0, 0, 0, 1]
self._obj_init_pos = [0.35, 0, 0.65]
self._obj_init_ori = [0, 0, 0, 1]
self._table = p.loadURDF(
"adjustable_table/adjustable_table.urdf",
self._table_init_pos,
self._table_init_ori,
physicsClientId=self._client,
)
self._obj = p.loadURDF("brick/brick.urdf",
self._obj_init_pos,
self._obj_init_ori,
physicsClientId=self._client,
flags=p.URDF_USE_INERTIA_FROM_FILE)
# Let things fall down
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self.joints_initial_pose = self._robot.getAnglesPosition(
self._robot.joint_dict.keys())
self._obj_start_pos = p.getBasePositionAndOrientation(
self._obj, physicsClientId=self._client)[0]
if self._gui:
# load ghosts
self._ghost = p.loadURDF(
"brick/brick_ghost.urdf",
self._obj_start_pos,
self._obj_init_ori,
physicsClientId=self._client,
useFixedBase=True,
)
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF(
"duck_vhacd.urdf",
basePosition=[1, -1, 0.5],
globalScaling=10.0,
physicsClientId=client)
pepper.showLaser(True)
pepper.subscribeLaser()
pepper.goToPosture("Stand", 0.6)
while True:
laser_list = pepper.getRightLaserValue()
laser_list.extend(pepper.getFrontLaserValue())
laser_list.extend(pepper.getLeftLaserValue())
if all(laser == 5.6 for laser in laser_list):
print("Nothing detected")
else:
print("Detected")
pass
def main():
# qi App Session
qiApp = qi.Application(sys.argv)
# Bullet Simulator
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pepperSim = simulation_manager.spawnPepper(client_id,
translation=[0, 0, 0],
quaternion=[0, 0, 0, 1],
spawn_ground_plane=True)
# wrap qi App Session with Simulated Pepper
wrap = NaoqibulletWrapper.NaoqibulletWrapper(
qiApp, pepperSim) # /!\ keep wrap instance to keep thread
pybullet.setAdditionalSearchPath(pybullet_data.getDataPath())
pybullet.loadURDF("table.urdf",
basePosition=[-2, 0, 0],
globalScaling=1,
physicsClientId=client_id)
pybullet.loadURDF("totem.urdf",
basePosition=[-1.5, 0, 1],
globalScaling=0.7,
physicsClientId=client_id)
pybullet.loadURDF("totempomme.urdf",
basePosition=[-1.5, -0.25, 1],
globalScaling=0.7,
physicsClientId=client_id)
pybullet.loadURDF("totemwine.urdf",
basePosition=[-1.5, 0.25, 1],
globalScaling=0.7,
physicsClientId=client_id)
qiSession = qiApp.session
motionService = qiSession.service("ALMotion")
#add function here
init_cam(pepperSim)
motionService.setAngles(["LShoulderPitch", "RShoulderPitch"], [1, 1],
[1, 1])
#take photo
turn(motionService, pi)
take_picture(pepperSim)
#go to the object
move_right_obj(motionService)
#go to the box
move_to_Blue(motionService)
# block until stop is called.
qiApp.run()
# close nicely
wrap.close()
def __init__(self, gui=False):
self._simulation_manager = SimulationManager()
self._client_id = self._simulation_manager.launchSimulation(gui=gui)
self._agent = self._simulation_manager.spawnPepper(
self._client_id, spawn_ground_plane=True)
self._laser_handle = self._agent.subscribeLaser()
self._camera_handle = self._agent.subscribeCamera(
PepperVirtual.ID_CAMERA_BOTTOM)
def __init__(self):
threading.Thread.__init__(self)
self.manager = SimulationManager()
self.client_id = self.manager.launchSimulation(gui=True)
p.connect(p.DIRECT)
self.robot = Robot(self.manager, self.client_id)
self.createScene()
self.initUI()
def _setup_scene(self):
self._simulation_manager = SimulationManager()
self._client = self._simulation_manager.launchSimulation(
gui=self._gui, auto_step=False)
p.configureDebugVisualizer(p.COV_ENABLE_GUI, 0)
self._robot = self._simulation_manager.spawnPepper(
self._client, spawn_ground_plane=False)
self._robot.goToPosture("Stand", 1.0)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._robot.setAngles(
["KneePitch", "HipPitch", "LShoulderPitch", "HeadPitch"],
[0.33, -0.9, -0.6, 0.3], [0.5] * 4)
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self._table_init_pos = [0.35, 0, 0]
self._table_init_ori = [0, 0, 0, 1]
self._obj_init_pos = [0.35, 0, 0.71]
self._obj_init_ori = [0, 0, 0, 1]
dirname = os.path.dirname(__file__)
assets_path = os.path.join(dirname, "../assets/models")
p.setAdditionalSearchPath(assets_path, physicsClientId=self._client)
self._floor = p.loadURDF("floor/floor.urdf",
physicsClientId=self._client,
useFixedBase=True)
self._table = p.loadURDF(
"adjustable_table/adjustable_table.urdf",
self._table_init_pos,
self._table_init_ori,
physicsClientId=self._client,
)
self._obj = p.loadURDF(
"brick/brick.urdf",
self._obj_init_pos,
self._obj_init_ori,
physicsClientId=self._client,
flags=p.URDF_USE_INERTIA_FROM_FILE,
)
# Let things fall down
for _ in range(500):
p.stepSimulation(physicsClientId=self._client)
self.joints_initial_pose = self._robot.getAnglesPosition(
self._robot.joint_dict.keys())
self._obj_start_pos = p.getBasePositionAndOrientation(
self._obj, physicsClientId=self._client)[0]
def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
def __init__(self):
# self.head_sub = rospy.Subscriber('/pepper_dcm/Head_controller/command', JointTrajectory, self.callback)
# self.left_arm_sub = rospy.Subscriber('/pepper_dcm/LeftArm_controller/command', JointTrajectory, self.callback)
# self.right_arm_sub = rospy.Subscriber('/pepper_dcm/RightArm_controller/command', JointTrajectory, self.callback)
# self.left_hand_sub = rospy.Subscriber('/pepper_dcm/LeftHand_controller/command', JointTrajectory, self.callback)
# self.right_hand_sub = rospy.Subscriber('/pepper_dcm/RightHand_controller/command', JointTrajectory, self.callback)
self.simulation_manager = SimulationManager()
self.client_id = self.simulation_manager.launchSimulation(gui=True)
self.pepper = self.simulation_manager.spawnPepper(
self.client_id, spawn_ground_plane=True)
self.wrap = PepperRosWrapper()
self.wrap.launchWrapper(self.pepper, "/naoqi_driver")
def generate_world_1(self):
simulation_manager = SimulationManager()
client_id = simulation_manager.launchSimulation(gui=True)
pybullet.setAdditionalSearchPath(
"/home/tp/Projet/g7_dupuis_dez_flachard")
StartPosTable = [4, 0, 0.2]
StartOrientationTable = pybullet.getQuaternionFromEuler([0, 0, 0])
PlaneId = pybullet.loadURDF("table.urdf", StartPosTable,
StartOrientationTable)
StartPosLego = [2, 0, 0.1]
PlaneId = pybullet.loadURDF("sofa_2.urdf", StartPosLego,
StartOrientationTable)
StartPosTot1 = [3.7, 0, 0.75]
StartPosTot2 = [3.7, 0.4, 0.75]
StartPosTot3 = [3.7, -0.4, 0.75]
StartPosBox1 = [-1, 1, -0.3]
StartPosBox2 = [-1, -1, -0.3]
StartPosBox3 = [-1, 0, -0.3]
Box1 = pybullet.loadURDF("box.urdf", StartPosBox1,
StartOrientationTable)
Box2 = pybullet.loadURDF("box2.urdf", StartPosBox2,
StartOrientationTable)
Box3 = pybullet.loadURDF("box3.urdf", StartPosBox3,
StartOrientationTable)
Choix = [StartPosTot1, StartPosTot2, StartPosTot3]
Positions = []
while (len(Choix) > 0):
pos = choice(Choix)
Positions.append(pos)
Choix.remove(pos)
Tot1_Id = pybullet.loadURDF("duck.urdf", Positions[0],
StartOrientationTable)
Tot2_Id = pybullet.loadURDF("eye.urdf", Positions[1],
StartOrientationTable)
Tot3_Id = pybullet.loadURDF("objet3.urdf", Positions[2],
StartOrientationTable)
pepper = simulation_manager.spawnPepper(client_id,
spawn_ground_plane=True)
pepper.angular_velocity = 1.0
pepper.goToPosture("Stand", 0.6)
pepper.setAngles("HeadPitch", 0.27, 0.27)
return pepper
class Simulation:
"""
Classe gérant la simu
"""
def __init__(self):
self.sim = SimulationManager()
self.client = self.sim.launchSimulation()
self.robot = Robot(self.sim, self.client)
self.createScene()
self.robot.start()
def createScene(self):
pb.setAdditionalSearchPath(pd.getDataPath())
pb.loadURDF("duck_vhacd.urdf", basePosition=[4, 2, 0], globalScaling=5)
pb.loadURDF("sphere2red.urdf",
basePosition=[-3, 0.05, 1],
globalScaling=1)
time.sleep(5)
def getPepper(self):
return self.pepper
def getClient(self):
return self.client
def getSimulationManager(self):
return self.simulationManager
def main():
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
pepper.goToPosture("Crouch", 0.6)
time.sleep(1)
pepper.goToPosture("Stand", 0.6)
time.sleep(1)
pepper.goToPosture("StandZero", 0.6)
time.sleep(1)
pepper.subscribeCamera(PepperVirtual.ID_CAMERA_BOTTOM)
while True:
img = pepper.getCameraFrame()
cv2.imshow("bottom camera", img)
cv2.waitKey(1)
def setUpClass(cls):
"""
Launches a simulation and spawns a Pepper virtual robot
"""
SensorTest.simulation = SimulationManager()
SensorTest.client = SensorTest.simulation.launchSimulation(gui=False)
SensorTest.sensor = DummySensor(SensorTest.client)
def main(session):
global asked_object
global asked_rangement
# Create the simulation with the Pepper
simulation_manager = SimulationManager()
client = simulation_manager.launchSimulation(gui=True)
pepper = simulation_manager.spawnPepper(client, spawn_ground_plane=True)
# Original positions of the objects (give random positions)
liste = [-0.2, 0, 0.2]
gene_positions(liste)
# Initialization of the decor and the robot
init_decor(client)
init_posture(pepper)
# The user asks an object and a rangement
[liste_asked_rangement, liste_asked_object] = user_request(session)
# Step 1 : Detect the 3 objects and their position
pepper.moveTo(1.9, 0, 0) # Move to the position to take the picture
get_image(pepper)
dico_objets = analyse_image(client)
for i in range(len(liste_asked_rangement)):
numero_obj = dico_objets[
liste_asked_object[i]] # Give the position of the object asked
# Step 2 : Take the object asked
grab_object(pepper, numero_obj)
# Step 3 : Store the object in the right place
if (liste_asked_rangement[i] == "table"):
drop_night(pepper)
elif (liste_asked_rangement[i] == "fauteuil"):
drop_chair(pepper)
elif (liste_asked_rangement[i] == "carton"):
drop_carton(pepper)
# Step 4 : DAB
dab(pepper)
while True:
pass
def __init__(self):
super(NaoEnv, self).__init__()
self.simulation_manager = SimulationManager()
self.client = self.simulation_manager.launchSimulation(gui=True)
self.simulation_manager.setLightPosition(self.client, [0, 0, 100])
self.robot = self.simulation_manager.spawnNao(self.client,
spawn_ground_plane=True)
time.sleep(1.0)
# stand pose parameters
pose = NaoPosture('Stand')
pose_dict = {}
for joint_name, joint_value in zip(pose.joint_names,
pose.joint_values):
pose_dict[joint_name] = joint_value
# joint parameters
self.joint_names = [
'LShoulderPitch', 'LShoulderRoll', 'LElbowYaw', 'LElbowRoll',
'LWristYaw', 'RShoulderPitch', 'RShoulderRoll', 'RElbowYaw',
'RElbowRoll', 'RWristYaw', 'LHipYawPitch', 'LHipRoll', 'LHipPitch',
'LKneePitch', 'LAnklePitch', 'LAnkleRoll', 'RHipYawPitch',
'RHipRoll', 'RHipPitch', 'RKneePitch', 'RAnklePitch', 'RAnkleRoll'
]
self.lower_limits = []
self.upper_limits = []
self.init_angles = []
for joint_name in self.joint_names:
joint = self.robot.joint_dict[joint_name]
self.lower_limits.append(joint.getLowerLimit())
self.upper_limits.append(joint.getUpperLimit())
self.init_angles.append(pose_dict[joint_name])
self.link_names = []
for joint_name in self.joint_names:
linkName = p.getJointInfo(
self.robot.getRobotModel(),
self.robot.joint_dict[joint_name].getIndex())[12].decode(
"utf-8")
self.link_names.append(linkName)
self.action_space = spaces.Box(np.array(self.lower_limits),
np.array(self.upper_limits))
self.observation_space = spaces.Box(
np.array([-1] * len(self.joint_names)),
np.array([1] * len(self.joint_names)))
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
ImuTest.simulation = SimulationManager()
ImuTest.client = ImuTest.simulation.launchSimulation(gui=False)
ImuTest.robot = ImuTest.simulation.spawnNao(ImuTest.client,
spawn_ground_plane=True)
def setUpClass(cls):
"""
Launches a simulation and spawns a Pepper virtual robot
"""
VirtualRobotTest.simulation = SimulationManager()
VirtualRobotTest.client = VirtualRobotTest.simulation.launchSimulation(
gui=False)
VirtualRobotTest.robot = DummyVirtual()
def setUpClass(cls):
"""
Launches a simulation and spawns the Pepper virtual robot
"""
JointTest.simulation = SimulationManager()
JointTest.client = JointTest.simulation.launchSimulation(gui=False)
JointTest.robot = JointTest.simulation.spawnPepper(
JointTest.client, spawn_ground_plane=True)
def setUpClass(cls):
"""
Launches a simulation and spawns the Romeo virtual robot
"""
CameraTest.simulation = SimulationManager()
CameraTest.client = CameraTest.simulation.launchSimulation(gui=False)
CameraTest.robot = CameraTest.simulation.spawnRomeo(
CameraTest.client, spawn_ground_plane=True)
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
DummyFsrTest.simulation = SimulationManager()
DummyFsrTest.client = DummyFsrTest.simulation.launchSimulation(
gui=False)
DummyFsrTest.robot = DummyRobot()
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
PostureTest.simulation = SimulationManager()
PostureTest.client = PostureTest.simulation.launchSimulation(gui=False)
PostureTest.robot = PostureTest.simulation.spawnRomeo(
PostureTest.client, spawn_ground_plane=True)
def test_reset_simulation(self):
"""
Test the @resetSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
manager.resetSimulation(client)
manager.stopSimulation(client)
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
FsrTest.simulation = SimulationManager()
FsrTest.client = FsrTest.simulation.launchSimulation(gui=False)
FsrTest.fsr_names = NaoFsr.RFOOT + NaoFsr.LFOOT
FsrTest.fsr_groups = [NaoFsr.RFOOT, NaoFsr.LFOOT]
FsrTest.robot = FsrTest.simulation.spawnNao(FsrTest.client,
spawn_ground_plane=True)
def test_remove_nao(self):
"""
Test the @removeNao method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
nao = manager.spawnNao(client, spawn_ground_plane=True)
manager.removeNao(nao)
with self.assertRaises(pybullet.error):
pybullet.getBodyInfo(nao.getRobotModel())
manager.stopSimulation(client)
def test_launch_simulation(self):
"""
Test the @launchSimulation method
"""
manager = SimulationManager()
client = manager.launchSimulation(gui=False)
self.assertEqual(client, 0)
manager.stopSimulation(client)
client = manager.launchSimulation(gui=False, use_shared_memory=True)
self.assertEqual(client, 0)
manager.stopSimulation(client)
# Ensure that stopping the simulation one more time won't raise an
# error
try:
manager.stopSimulation(client)
self.assertTrue(True)
except Exception:
self.assertTrue(False)
def setUpClass(cls):
"""
Launches a simulation and spawns the NAO virtual robot
"""
CameraTest.simulation = SimulationManager()
CameraTest.client = CameraTest.simulation.launchSimulation(gui=False)
CameraTest.robot = CameraTest.simulation.spawnNao(
CameraTest.client, spawn_ground_plane=True)
CameraTest.cameras = {
NaoVirtual.ID_CAMERA_TOP: CameraTest.robot.camera_top,
NaoVirtual.ID_CAMERA_BOTTOM: CameraTest.robot.camera_bottom
}
def setUpClass(cls):
"""
Launches a simulation and spawns the Romeo virtual robot
"""
CameraTest.simulation = SimulationManager()
CameraTest.client = CameraTest.simulation.launchSimulation(gui=False)
CameraTest.robot = CameraTest.simulation.spawnRomeo(
CameraTest.client, spawn_ground_plane=True)
CameraTest.cameras = {
RomeoVirtual.ID_CAMERA_LEFT: CameraTest.robot.camera_left,
RomeoVirtual.ID_CAMERA_RIGHT: CameraTest.robot.camera_right,
RomeoVirtual.ID_CAMERA_DEPTH: CameraTest.robot.camera_depth
}
def setUpClass(cls):
"""
Launches a simulation and spawns the Pepper virtual robot
"""
CameraTest.simulation = SimulationManager()
CameraTest.client = CameraTest.simulation.launchSimulation(gui=False)
CameraTest.robot = CameraTest.simulation.spawnPepper(
CameraTest.client, spawn_ground_plane=True)
CameraTest.cameras = {
PepperVirtual.ID_CAMERA_TOP: CameraTest.robot.camera_top,
PepperVirtual.ID_CAMERA_BOTTOM: CameraTest.robot.camera_bottom,
PepperVirtual.ID_CAMERA_DEPTH: CameraTest.robot.camera_depth
}
