def __init__(self, config_path, scene_path, host='127.0.0.1', port=19997, tracked_objects=['left_hand_tracker', 'right_hand_tracker']):
from pypot.vrep import from_vrep
with open(config_path) as cf:
config = json.load(cf)
self.robot = from_vrep(config, host, port, scene_path,
tracked_objects)
def init(self):
with open('../../utils/poppy_config.json') as f:
poppy_config = json.load(f)
scene_path = '../../utils/poppy-standing2.ttt'
self.poppy = from_vrep(poppy_config,'127.0.0.1', 19997, scene_path)
# poppy.start_sync()
# for m in poppy.motors:
# m.compliant = False
def before_all(context):
from pypot.vrep import from_vrep
with open('./contents/poppy_config.json') as f:
poppy_config = json.load(f)
scene_path = './contents/poppy-standing2.ttt'
context.poppy = from_vrep(poppy_config, '127.0.0.1', 19997, scene_path)
context.poppy.start_sync()
for m in context.poppy.motors:
m.compliant = False
def main():
x_home = [0.060175, -0.0049936, 0.31573]
q_home = [0, 0, 0, 0, 0]
robot = from_vrep(scene = '../model_1/luxo_1_kinematic.ttt',
config = '../model_1/config_luxo_1.json'
)
robot.stop_simulation()
robot._controllers[0].io.call_remote_api('simxCloseScene')
close_all_connections()
# robot = {
# "motors":{
# "foot":{
# "id":12,
# "orientation":"direct",
# "type":"AX-12",
# "offset":0.0
# },
# "body_bottom":{
# "id":13,
# "orientation":"direct",
# "type":"AX-12",
# "angle_limit":[-90.0, 90.0],
# "offset":0.0
# },
# "body_middle":{
# "id":14,
# "orientation":"direct",
# "type":"AX-12",
# "angle_limit":[-130.0, 130.0],
# "offset":0.0
# },
# "body_top":{
# "id":15,
# "orientation":"direct",
# "type":"AX-12",
# "angle_limit":[-130.0, 130.0],
# "offset":0.0
# },
# "head":{
# "id":16,
# "orientation":"direct",
# "type":"AX-12",
# "offset":0.0
# }
# }
# }
f = VrepDirectFunctionWoPypot(robot)
g = PolynomialModel(3,5,4,q_home)
trainer = GoalBabblingTrainer(g,f,q_home,x_home, '../model_1/luxo_1_data_20_5.json')
trainer.train(200,4,0.05)
def main():
x_home = [0.060175, -0.0049936, 0.31573]
q_home = [0, 0, 0, 0, 0]
robot = from_vrep(scene='../model_1/luxo_1_kinematic.ttt',
config='../model_1/config_luxo_1.json')
robot.stop_simulation()
robot._controllers[0].io.call_remote_api('simxCloseScene')
close_all_connections()
# robot = {
# "motors":{
# "foot":{
# "id":12,
# "orientation":"direct",
# "type":"AX-12",
# "offset":0.0
# },
# "body_bottom":{
# "id":13,
# "orientation":"direct",
# "type":"AX-12",
# "angle_limit":[-90.0, 90.0],
# "offset":0.0
# },
# "body_middle":{
# "id":14,
# "orientation":"direct",
# "type":"AX-12",
# "angle_limit":[-130.0, 130.0],
# "offset":0.0
# },
# "body_top":{
# "id":15,
# "orientation":"direct",
# "type":"AX-12",
# "angle_limit":[-130.0, 130.0],
# "offset":0.0
# },
# "head":{
# "id":16,
# "orientation":"direct",
# "type":"AX-12",
# "offset":0.0
# }
# }
# }
f = VrepDirectFunctionWoPypot(robot)
g = PolynomialModel(3, 5, 4, q_home)
trainer = GoalBabblingTrainer(g, f, q_home, x_home,
'../model_1/luxo_1_data_20_5.json')
trainer.train(200, 4, 0.05)
def Leachy(*args, **kwargs):
if 'simulator' not in kwargs:
if 'config' not in kwargs:
config = os.path.join(os.path.dirname(__file__), 'configuration',
'leachy.json')
kwargs['config'] = config
robot = Reachy(*args, **kwargs)
if 'simulator' in kwargs:
config = os.path.join(os.path.dirname(__file__), 'configuration',
'leachy.json')
if 'scene' not in kwargs:
kwargs['scene'] = 'leachy.ttt'
if kwargs['scene'] == 'keep-existing':
scene = None
else:
scene = os.path.join(os.path.dirname(__file__), 'vrep-scene',
kwargs['scene'])
try:
id = kwargs['id'] if 'id' in kwargs else None
shared_vrep_io = kwargs[
'shared_vrep_io'] if 'shared_vrep_io' in kwargs else None
robot = from_vrep(config,
'127.0.0.1',
19997,
scene,
shared_vrep_io=shared_vrep_io,
id=id)
except VrepConnectionError:
raise IOError('Connection to V-REP failed!')
robot.simulated = True
with open(config) as f:
robot.config = json.load(f)
urdf_file = os.path.join(os.path.dirname(__file__), 'leachy.urdf')
robot.urdf_file = urdf_file
setup(robot)
robot.urdf_file = robot.urdf_file.replace('reachy.urdf', 'leachy.urdf')
if 'brunel_hand' in kwargs:
robot.urdf_file = robot.urdf_file.replace('leachy.urdf',
'leachy_hand.urdf')
robot.ik_chain = IkChain(robot, tip=tips['brunel_hand'])
else:
robot.ik_chain = IkChain(robot, tip=tips['hand'])
return robot
def setup(self):
done = False
self.port = 2000
while not done and (1024 < self.port < 65535):
try:
self._vrep_proc, self.port = spawn_vrep(
self.gui, self.scene, start=True)
# print 'started', self.port
time.sleep(2)
self.robot = from_vrep(
self.robot_config, '127.0.0.1', self.port, tracked_collisions=self.tracked_collisions)
done = True
except VrepConnectionError:
print 'Error connecting to port:', self.port
stop_vrep(self.port)
self.port += 1
def __new__(cls,
base_path=None,
config=None,
simulator=None,
scene=None,
host='localhost',
port=19997,
id=None,
shared_vrep_io=None,
use_snap=False,
snap_host='0.0.0.0',
snap_port=6969,
snap_quiet=True,
use_http=False,
http_host='0.0.0.0',
http_port=8080,
http_quiet=True,
use_remote=False,
remote_host='0.0.0.0',
remote_port=4242,
use_ws=False,
ws_host='0.0.0.0',
ws_port=9009,
start_background_services=True,
sync=True,
**extra):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used : 'vrep', 'poppy-simu', or 'dummy-robot'
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature, use "keep-existing" to keep the current VRep scene - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param int id: robot id in simulator (useful when using a scene with multiple robots)
:param vrep_io: use an already connected VrepIO (useful when using a scene with multiple robots)
:type vrep_io: :class:`~pypot.vrep.io.VrepIO`
:param bool use_snap: start or not the Snap! API
:param str snap_host: host of Snap! API
:param int snap_port: port of the Snap!
:param bool use_http: start or not the HTTP API
:param str http_host: host of HTTP API
:param int http_port: port of the HTTP API
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
You can also add extra keyword arguments to disable sensor. For instance, to use a DummyCamera, you can add the argument: camera='dummy'.
.. warning:: You can not specify a particular config when using a simulated robot!
"""
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
creature = camelcase_to_underscore(cls.__name__)
base_path = (os.path.dirname(__import__(creature).__file__)
if base_path is None else base_path)
default_config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
if config is None:
config = default_config
if simulator is not None:
if simulator == 'vrep':
from pypot.vrep import from_vrep, VrepConnectionError
scene_path = os.path.join(base_path, 'vrep-scene')
if scene != "keep-existing":
if scene is None:
scene = '{}.ttt'.format(creature)
elif not os.path.exists(scene):
if ((os.path.basename(scene) != scene)
or (not os.path.exists(
os.path.join(scene_path, scene)))):
raise ValueError(
'Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
# vrep.simxStart no longer listen on localhost
if host == 'localhost':
host = '127.0.0.1'
try:
poppy_creature = from_vrep(
config,
host,
port,
scene if scene != "keep-existing" else None,
id=id,
shared_vrep_io=shared_vrep_io)
except VrepConnectionError:
raise IOError('Connection to V-REP failed!')
elif simulator == 'poppy-simu':
use_http = True
poppy_creature = use_dummy_robot(config)
elif simulator == 'dummy':
poppy_creature = use_dummy_robot(config)
else:
raise ValueError(
'Unknown simulation mode: "{}"'.format(simulator))
poppy_creature.simulated = True
else:
for _ in range(MAX_SETUP_TRIALS):
try:
poppy_creature = from_json(config, sync, **extra)
logger.info('Init successful')
break
except Exception as e:
logger.warning('Init fail: {}'.format(str(e)))
exc_type, exc_inst, tb = sys.exc_info()
else:
raise OSError(
'Could not initialize robot: {} '.format(exc_inst))
poppy_creature.simulated = False
with open(config) as f:
poppy_creature.config = json.load(f)
urdf_file = os.path.join(
os.path.join(base_path, '{}.urdf'.format(creature)))
poppy_creature.urdf_file = urdf_file
if use_snap:
poppy_creature.snap = SnapRobotServer(poppy_creature,
snap_host,
snap_port,
quiet=snap_quiet)
snap_url = 'http://snap.berkeley.edu/snapsource/snap.html'
block_url = 'http://{}:{}/snap-blocks.xml'.format(
find_local_ip(), snap_port)
url = '{}#open:{}'.format(snap_url, block_url)
logger.info(
'SnapRobotServer is now running on: http://{}:{}\n'.format(
snap_host, snap_port))
logger.info(
'You can open Snap! interface with loaded blocks at "{}"\n'.
format(url))
if use_http:
from pypot.server.httpserver import HTTPRobotServer
poppy_creature.http = HTTPRobotServer(poppy_creature,
http_host,
http_port,
cross_domain_origin="*",
quiet=http_quiet)
logger.info(
'HTTPRobotServer is now running on: http://{}:{}\n'.format(
http_host, http_port))
if use_remote:
from pypot.server import RemoteRobotServer
poppy_creature.remote = RemoteRobotServer(poppy_creature,
remote_host, remote_port)
logger.info(
'RemoteRobotServer is now running on: http://{}:{}\n'.format(
remote_host, remote_port))
if use_ws:
from pypot.server import WsRobotServer
poppy_creature.ws = WsRobotServer(poppy_creature, ws_host, ws_port)
logger.info('Ws server is now running on: ws://{}:{}\n'.format(
ws_host, ws_port))
cls.setup(poppy_creature)
if start_background_services:
cls.start_background_services(poppy_creature)
return poppy_creature
class Env(object):
action_bound = [-1, 1] # action will be angle move between [-1,1]
state_dim = 4 # theta1 & theta2, distance to goal,get_point
action_dim = 2
arm1l = 0.185
arm2l = 0.515
arm1_theta = 0
arm2_theta = -10 * np.pi / 180
get_point = False
grab_counter = 0
theta_bound = np.array([[-75, 55], [-130, -10]])
point_l = 5
poppy = from_vrep('poppy.json',
scene='/home/eddiesyn/V-REP/experiment.ttt')
def __init__(self):
self.arm_info = np.zeros(2)
self.EE = np.zeros(2)
self.arm_info[0] = self.arm1_theta
self.arm_info[1] = self.arm2_theta
self.point_info = np.array([0.7, 0.512])
self.point_info_init = self.point_info.copy()
self.EE[0] = -self.arm2l * np.sin(np.sum(
self.arm_info)) - self.arm1l * np.sin(self.arm_info[0])
self.EE[1] = self.arm2l * np.cos(np.sum(
self.arm_info)) + self.arm1l * np.cos(self.arm_info[0])
# self.poppy = from_vrep('poppy.json', scene = '/home/eddiesyn/V-REP/experiment.ttt')
for m in self.poppy.motors:
if m.id == 41:
self.motor_41 = m
if m.id == 44:
self.motor_44 = m
self.motor_41.compliant = False
self.motor_44.compliant = False
self.motor_41.torque_limit = 15
self.motor_44.torque_limit = 15
self.motor_41.moving_speed = 10
self.motor_44.moving_speed = 10
move(self.motor_44, self.motor_44.present_position, -10)
move(self.motor_41, self.motor_41.present_position, 0)
def step(self, action):
action_ = action * 180 / np.pi # action is np.array(2,)
goal_position_1 = np.clip((self.arm_info + action_)[0], -75, 55)
goal_position_2 = np.clip((self.arm_info + action_)[1], -130, -10)
self.poppy.reset_simulation()
move(self.motor_41, self.motor_41.present_position, goal_position_1)
move(self.motor_44, self.motor_44.present_position, goal_position_2)
self.arm_info[0] = self.motor_41.present_position
self.arm_info[1] = self.motor_44.present_position
self.EE[0] = -self.arm2l * np.sin(np.sum(
self.arm_info)) - self.arm1l * np.sin(self.arm_info[0])
self.EE[1] = self.arm2l * np.cos(np.sum(
self.arm_info)) + self.arm1l * np.cos(self.arm_info[0])
s = self.get_state()
r = self._r_func(s[2])
return s, r
def _r_func(self, distance):
t = 50
abs_distance = distance
r = -abs_distance / 200
if abs_distance < self.point_l and (not self.get_point):
r += 1.
self.grab_counter += 1
if self.grab_counter > t:
r += 10.
self.get_point = True
elif abs_distance > self.point_l:
self.grab_counter = 0
self.get_point = False
return r
def reset(self):
self.poppy.reset_simulation()
self.get_point = False
self.grab_counter = 0
self.arm_info[0] = -75 + 130 * np.random.random()
self.arm_info[1] = -130 + 120 * np.random.random()
move(self.motor_41, self.motor_41.present_position, self.arm_info[0])
move(self.motor_44, self.motor_44.present_position, self.arm_info[1])
self.arm_info[0] = self.motor_41.present_position
self.arm_info[1] = self.motor_44.present_position
s_ = self.get_state()
return s_
def get_state(self):
state_ = np.zeros(4)
state_[:2] = self.arm_info
state_[2] = np.linalg.norm(self.point_info - self.EE)
state_[3] = 1 if self.grab_counter > 0 else 0
return state_
def __new__(cls,
base_path=None, config=None,
simulator=None, scene=None, host='127.0.0.1', port=19997, id=0,
use_snap=False, snap_host='0.0.0.0', snap_port=6969,
use_http=False, http_host='0.0.0.0', http_port=8080,
use_remote=False, remote_host='0.0.0.0', remote_port=4242,
sync=True):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used (only vrep for the moment)
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
.. warning:: You can not specify a particular config when using a simulated robot!
"""
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
creature = camelcase_to_underscore(cls.__name__)
base_path = (os.path.dirname(__import__(creature).__file__)
if base_path is None else base_path)
if config is None:
config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
if simulator is not None:
if simulator != 'vrep':
raise ValueError('Unknown simulation mode: "{}"'.format(simulator))
from pypot.vrep import from_vrep
scene_path = os.path.join(base_path, 'vrep-scene')
if scene is None:
scene = '{}.ttt'.format(creature)
if not os.path.exists(scene):
if ((os.path.basename(scene) != scene) or
(not os.path.exists(os.path.join(scene_path, scene)))):
raise ValueError('Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
poppy_creature = from_vrep(config, host, port, scene)
poppy_creature.simulated = True
else:
poppy_creature = from_json(config, sync)
poppy_creature.simulated = False
if use_snap:
poppy_creature.snap = SnapRobotServer(poppy_creature, snap_host, snap_port)
if(use_http):
from pypot.server.httpserver import HTTPRobotServer
poppy_creature.http = HTTPRobotServer(poppy_creature, http_host, http_port, cross_domain_origin="*")
if(use_remote):
from pypot.server import RemoteRobotServer
poppy_creature.remote = RemoteRobotServer(poppy_creature, remote_host, remote_port)
cls.setup(poppy_creature)
return poppy_creature
if __name__ == '__main__':
DT = 30.
class JustWaitingPrimitive(LoopPrimitive):
def update(self):
if self.elapsed_time > DT:
self.stop(wait=False)
configfile = os.path.join(os.path.dirname(poppytools.__file__),
'configuration', 'poppy_config.json')
with open(configfile) as f:
poppy_config = json.load(f)
scene_path = os.path.join(os.path.dirname(pypot.__file__), '..', 'samples',
'notebooks', 'poppy-sitting.ttt')
poppy = from_vrep(poppy_config, '127.0.0.1', 19997, scene_path)
time.sleep(0.1)
p = JustWaitingPrimitive(poppy, 50.)
t0 = time.time()
p.start()
p.wait_to_stop()
print 'Running {}s of v-rep simulation took {}s'.format(
DT,
time.time() - t0)
import os
import poppytools
import json
import time
from pypot.vrep import from_vrep
from poppytools.primitive.walking import WalkingGaitFromMat
with open("/Users/wyoc/Projects/Poppy/tests/poppy_config.json") as f:
poppy_config = json.load(f)
scene_path = '/Users/wyoc/Projects/Poppy/tests/poppy-standing2.ttt'
poppy = from_vrep(poppy_config, '127.0.0.1', 19997, scene_path)
cpg_filename = os.path.join(os.path.dirname(poppytools.__file__), 'behavior', 'IROS_Normal_Gait.mat')
walk = WalkingGaitFromMat(poppy, cpg_filename)
walk.start()
# To give enough "time" to really start the walk
# And prevent the script to stop too soon.
time.sleep(5)
# Import libraries
from pypot.vrep import from_vrep, close_all_connections
from pypot.robot import from_config
import time
import numpy as np
import itertools
import random
import pypot.dynamixel
# if in the vrep env
vrep = False
if vrep:
close_all_connections()
poppy = from_vrep('poppy.json', scene='experiment.ttt')
#initial motor angle
if vrep:
arm_theta1 = 0
arm_theta2 = 0
arm_theta3 = -10
for m in poppy.motors:
if m.id == 41:
motor_41 = m
if m.id == 42:
motor_42 = m
if m.id == 44:
motor_44 = m
else:
arm_theta1 = 90
def __new__(cls,
base_path=None,
config=None,
simulator=None,
scene=None,
host='127.0.0.1',
port=19997,
id=0,
use_snap=False,
snap_host='0.0.0.0',
snap_port=6969,
sync=True):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used (only vrep for the moment)
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
.. warning:: You can not specify a particular config when using a simulated robot!
"""
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
creature = camelcase_to_underscore(cls.__name__)
base_path = (os.path.dirname(__import__(creature).__file__)
if base_path is None else base_path)
if config is None:
config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
if simulator is not None:
if simulator != 'vrep':
raise ValueError(
'Unknown simulation mode: "{}"'.format(simulator))
from pypot.vrep import from_vrep
scene_path = os.path.join(base_path, 'vrep-scene')
if scene is None:
scene = '{}.ttt'.format(creature)
if not os.path.exists(scene):
if ((os.path.basename(scene) != scene) or
(not os.path.exists(os.path.join(scene_path, scene)))):
raise ValueError(
'Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
poppy_creature = from_vrep(config, host, port, scene)
poppy_creature.simulated = True
else:
poppy_creature = from_json(config, sync)
poppy_creature.simulated = False
if use_snap:
poppy_creature.snap = SnapRobotServer(poppy_creature, snap_host,
snap_port)
cls.setup(poppy_creature)
return poppy_creature
def __new__(cls,
base_path=None, config="monrobot.json",
simulator=None, scene=None, host='localhost', port=19997, id=0,
use_snap=True, snap_host='0.0.0.0', snap_port=6969, snap_quiet=True,
use_http=False, http_host='0.0.0.0', http_port=8080, http_quiet=True,
use_remote=False, remote_host='0.0.0.0', remote_port=4242,
start_background_services=True, sync=True,
**extra):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used : 'vrep' or 'poppy-simu'
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param bool use_snap: start or not the Snap! API
:param str snap_host: host of Snap! API
:param int snap_port: port of the Snap!
:param bool use_http: start or not the HTTP API
:param str http_host: host of HTTP API
:param int http_port: port of the HTTP API
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
You can also add extra keyword arguments to disable sensor. For instance, to use a DummyCamera, you can add the argument: camera='dummy'.
.. warning:: You can not specify a particular config when using a simulated robot!
"""
print(config)
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
#creature = camelcase_to_underscore(cls.__name__)
#base_path = (os.path.dirname(__import__(creature).__file__)
# if base_path is None else base_path)
#default_config = os.path.join(os.path.join(base_path, 'configuration'),
# '{}.json'.format(creature))
if config is None:
config = default_config
if simulator is not None:
if simulator == 'vrep':
from pypot.vrep import from_vrep, VrepConnectionError
scene_path = os.path.join(base_path, 'vrep-scene')
if scene is None:
scene = '{}.ttt'.format(creature)
if not os.path.exists(scene):
if ((os.path.basename(scene) != scene) or
(not os.path.exists(os.path.join(scene_path, scene)))):
raise ValueError('Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
try:
poppy_creature = from_vrep(config, host, port, scene)
except VrepConnectionError:
raise IOError('Connection to V-REP failed!')
elif simulator == 'poppy-simu':
use_http = True
poppy_creature = use_dummy_robot(config)
else:
raise ValueError('Unknown simulation mode: "{}"'.format(simulator))
poppy_creature.simulated = True
else:
try:
poppy_creature = from_json(config, sync, **extra)
except IndexError as e:
raise IOError('Connection to the robot failed! {}'.format(e.message))
poppy_creature.simulated = False
with open(config) as f:
poppy_creature.config = json.load(f)
# urdf_file = os.path.join(os.path.join(base_path,
# '{}.urdf'.format(creature)))
# poppy_creature.urdf_file = urdf_file
if use_snap:
poppy_creature.snap = SnapRobotServer(
poppy_creature, snap_host, snap_port, quiet=snap_quiet)
snap_url = 'http://snap.berkeley.edu/snapsource/snap.html'
block_url = 'http://{}:{}/snap-blocks.xml'.format(find_local_ip(), snap_port)
url = '{}#open:{}'.format(snap_url, block_url)
print('SnapRobotServer is now running on: http://{}:{}\n'.format(snap_host, snap_port))
print('You can open Snap! interface with loaded blocks at "{}"\n'.format(url))
if use_http:
from pypot.server.httpserver import HTTPRobotServer
poppy_creature.http = HTTPRobotServer(poppy_creature, http_host, http_port,
cross_domain_origin="*", quiet=http_quiet)
print('HTTPRobotServer is now running on: http://{}:{}\n'.format(http_host, http_port))
if use_remote:
from pypot.server import RemoteRobotServer
poppy_creature.remote = RemoteRobotServer(poppy_creature, remote_host, remote_port)
print('RemoteRobotServer is now running on: http://{}:{}\n'.format(remote_host, remote_port))
cls.setup(poppy_creature)
if start_background_services:
cls.start_background_services(poppy_creature)
return poppy_creature
if __name__ == '__main__':
# Load the robot configuration
configfile = os.path.join(os.path.dirname(poppytools.__file__),
'configuration', 'poppy_config.json')
with open(configfile) as f:
robot_config = json.load(f)
# Load a V-REP scene with poppy sitting
scene = os.path.join(os.path.dirname(pypot.__file__), '..', 'samples',
'notebooks', 'poppy-sitting.ttt')
# Connect to the simulated robot
robot = from_vrep(robot_config,
'127.0.0.1',
19997,
scene,
tracked_objects=['head_visual'])
http = SnapRobotServer(robot, '127.0.0.1', 8080)
snap_url = 'http://snap.berkeley.edu/snapsource/snap.html'
blocks_url = 'http://localhost:8080/snap-blocks.xml'
url = '{}#open:{}'.format(snap_url, blocks_url)
print 'Snap Server should now be ready!'
print 'You can now connect to "{}"'.format(url)
http.run()
def __new__(cls,
base_path=None, config=None,
simulator=None, scene=None, host='localhost', port=19997, id=None, shared_vrep_io=None,
use_snap=False, snap_host='0.0.0.0', snap_port=6969, snap_quiet=True,
use_http=False, http_host='0.0.0.0', http_port=8080, http_quiet=True,
use_remote=False, remote_host='0.0.0.0', remote_port=4242,
use_ws=False, ws_host='0.0.0.0', ws_port=9009,
start_background_services=True, sync=True,
**extra):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used : 'vrep', 'poppy-simu', or 'dummy-robot'
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature, use "keep-existing" to keep the current VRep scene - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param int id: robot id in simulator (useful when using a scene with multiple robots)
:param vrep_io: use an already connected VrepIO (useful when using a scene with multiple robots)
:type vrep_io: :class:`~pypot.vrep.io.VrepIO`
:param bool use_snap: start or not the Snap! API
:param str snap_host: host of Snap! API
:param int snap_port: port of the Snap!
:param bool use_http: start or not the HTTP API
:param str http_host: host of HTTP API
:param int http_port: port of the HTTP API
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
You can also add extra keyword arguments to disable sensor. For instance, to use a DummyCamera, you can add the argument: camera='dummy'.
.. warning:: You can not specify a particular config when using a simulated robot!
"""
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
creature = camelcase_to_underscore(cls.__name__)
base_path = (os.path.dirname(__import__(creature).__file__)
if base_path is None else base_path)
default_config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
if config is None:
config = default_config
if simulator is not None:
if simulator == 'vrep':
from pypot.vrep import from_vrep, VrepConnectionError
scene_path = os.path.join(base_path, 'vrep-scene')
if scene != "keep-existing":
if scene is None:
scene = '{}.ttt'.format(creature)
elif not os.path.exists(scene):
if ((os.path.basename(scene) != scene) or
(not os.path.exists(os.path.join(scene_path, scene)))):
raise ValueError('Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
# vrep.simxStart no longer listen on localhost
if host == 'localhost':
host = '127.0.0.1'
try:
poppy_creature = from_vrep(config, host, port, scene if scene != "keep-existing" else None, id=id, shared_vrep_io=shared_vrep_io)
except VrepConnectionError:
raise IOError('Connection to V-REP failed!')
elif simulator == 'poppy-simu':
use_http = True
poppy_creature = use_dummy_robot(config)
elif simulator == 'dummy':
poppy_creature = use_dummy_robot(config)
else:
raise ValueError('Unknown simulation mode: "{}"'.format(simulator))
poppy_creature.simulated = True
else:
for _ in range(MAX_SETUP_TRIALS):
try:
poppy_creature = from_json(config, sync, **extra)
logger.info('Init successful')
break
except Exception as e:
logger.warning('Init fail: {}'.format(str(e)))
exc_type, exc_inst, tb = sys.exc_info()
else:
raise OSError('Could not initalize robot: {} '.format(exc_inst))
poppy_creature.simulated = False
with open(config) as f:
poppy_creature.config = json.load(f)
urdf_file = os.path.join(os.path.join(base_path,
'{}.urdf'.format(creature)))
poppy_creature.urdf_file = urdf_file
if use_snap:
poppy_creature.snap = SnapRobotServer(
poppy_creature, snap_host, snap_port, quiet=snap_quiet)
snap_url = 'http://snap.berkeley.edu/snapsource/snap.html'
block_url = 'http://{}:{}/snap-blocks.xml'.format(find_local_ip(), snap_port)
url = '{}#open:{}'.format(snap_url, block_url)
logger.info('SnapRobotServer is now running on: http://{}:{}\n'.format(snap_host, snap_port))
logger.info('You can open Snap! interface with loaded blocks at "{}"\n'.format(url))
if use_http:
from pypot.server.httpserver import HTTPRobotServer
poppy_creature.http = HTTPRobotServer(poppy_creature, http_host, http_port,
cross_domain_origin="*", quiet=http_quiet)
logger.info('HTTPRobotServer is now running on: http://{}:{}\n'.format(http_host, http_port))
if use_remote:
from pypot.server import RemoteRobotServer
poppy_creature.remote = RemoteRobotServer(poppy_creature, remote_host, remote_port)
logger.info('RemoteRobotServer is now running on: http://{}:{}\n'.format(remote_host, remote_port))
if use_ws:
from pypot.server import WsRobotServer
poppy_creature.ws = WsRobotServer(poppy_creature, ws_host, ws_port)
logger.info('Ws server is now running on: ws://{}:{}\n'.format(ws_host, ws_port))
cls.setup(poppy_creature)
if start_background_services:
cls.start_background_services(poppy_creature)
return poppy_creature
def __new__(cls,
base_path=None,
config=None,
simulator=None,
scene=None,
host='localhost',
port=19997,
id=0,
use_snap=False,
snap_host='0.0.0.0',
snap_port=6969,
snap_quiet=True,
use_http=False,
http_host='0.0.0.0',
http_port=8080,
http_quiet=True,
use_remote=False,
remote_host='0.0.0.0',
remote_port=4242,
start_background_services=True,
sync=True,
**extra):
""" Poppy Creature Factory.
Creates a Robot (real or simulated) and specifies it to make it a specific Poppy Creature.
:param str config: path to a specific json config (if None uses the default config of the poppy creature - e.g. poppy_humanoid.json)
:param str simulator: name of the simulator used : 'vrep' or 'poppy-simu'
:param str scene: specify a particular simulation scene (if None uses the default scene of the poppy creature - e.g. poppy_humanoid.ttt)
:param str host: host of the simulator
:param int port: port of the simulator
:param bool use_snap: start or not the Snap! API
:param str snap_host: host of Snap! API
:param int snap_port: port of the Snap!
:param bool use_http: start or not the HTTP API
:param str http_host: host of HTTP API
:param int http_port: port of the HTTP API
:param int id: id of robot in the v-rep scene (not used yet!)
:param bool sync: choose if automatically starts the synchronization loops
You can also add extra keyword arguments to disable sensor. For instance, to use a DummyCamera, you can add the argument: camera='dummy'.
.. warning:: You can not specify a particular config when using a simulated robot!
"""
if config and simulator:
raise ValueError('Cannot set a specific config '
'when using a simulated version!')
creature = camelcase_to_underscore(cls.__name__)
base_path = (os.path.dirname(__import__(creature).__file__)
if base_path is None else base_path)
default_config = os.path.join(os.path.join(base_path, 'configuration'),
'{}.json'.format(creature))
if config is None:
config = default_config
if simulator is not None:
if simulator == 'vrep':
from pypot.vrep import from_vrep, VrepConnectionError
scene_path = os.path.join(base_path, 'vrep-scene')
if scene is None:
scene = '{}.ttt'.format(creature)
if not os.path.exists(scene):
if ((os.path.basename(scene) != scene) or
(not os.path.exists(os.path.join(scene_path, scene)))):
raise ValueError(
'Could not find the scene "{}"!'.format(scene))
scene = os.path.join(scene_path, scene)
# TODO: use the id so we can have multiple poppy creatures
# inside a single vrep scene
try:
poppy_creature = from_vrep(config, host, port, scene)
except VrepConnectionError:
raise IOError('Connection to V-REP failed!')
elif simulator == 'poppy-simu':
use_http = True
poppy_creature = use_dummy_robot(config)
else:
raise ValueError(
'Unknown simulation mode: "{}"'.format(simulator))
poppy_creature.simulated = True
else:
try:
poppy_creature = from_json(config, sync, **extra)
except IndexError as e:
raise IOError('Connection to the robot failed! {}'.format(
str(e)))
poppy_creature.simulated = False
with open(config) as f:
poppy_creature.config = json.load(f)
urdf_file = os.path.join(
os.path.join(base_path, '{}.urdf'.format(creature)))
poppy_creature.urdf_file = urdf_file
if use_snap:
poppy_creature.snap = SnapRobotServer(poppy_creature,
snap_host,
snap_port,
quiet=snap_quiet)
snap_url = 'http://snap.berkeley.edu/snapsource/snap.html'
block_url = 'http://{}:{}/snap-blocks.xml'.format(
find_local_ip(), snap_port)
url = '{}#open:{}'.format(snap_url, block_url)
print('SnapRobotServer is now running on: http://{}:{}\n'.format(
snap_host, snap_port))
print('You can open Snap! interface with loaded blocks at "{}"\n'.
format(url))
if use_http:
from pypot.server.httpserver import HTTPRobotServer
poppy_creature.http = HTTPRobotServer(poppy_creature,
http_host,
http_port,
cross_domain_origin="*",
quiet=http_quiet)
print('HTTPRobotServer is now running on: http://{}:{}\n'.format(
http_host, http_port))
if use_remote:
from pypot.server import RemoteRobotServer
poppy_creature.remote = RemoteRobotServer(poppy_creature,
remote_host, remote_port)
print('RemoteRobotServer is now running on: http://{}:{}\n'.format(
remote_host, remote_port))
cls.setup(poppy_creature)
if start_background_services:
cls.start_background_services(poppy_creature)
return poppy_creature
from pypot.vrep import from_vrep, close_all_connections
import time
import numpy as np
# For real robot===================
from pypot.robot import from_config
import pypot.dynamixel
import itertools
import random
import json
#==================================
vrep = False # if in the vrep env
if vrep:
close_all_connections()
poppy = from_vrep('poppy.json',
scene='experiment.ttt') # /home/eddiesyn/V-REP/
else:
ports = pypot.dynamixel.get_available_ports()
#print("availble is :",ports)
# with open('poppy.json', 'r') as f:
# config = json.load(f)
# .set_angle_limit
dxl_io_up = pypot.dynamixel.DxlIO(ports[0])
dxl_io_down = pypot.dynamixel.DxlIO(ports[1])
lower_io = dxl_io_down
upper_io = dxl_io_up
# upper_io.set_angle_limits({41: })
poppy_up = dxl_io_up.scan(range(31, 55))
poppy_down = dxl_io_down.scan(range(11, 26))
with open('config_g.json', 'r') as f: