Ejemplo n.º 1
0
    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)
Ejemplo n.º 2
0
 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
     
Ejemplo n.º 3
0
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
Ejemplo n.º 4
0
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)
Ejemplo n.º 5
0
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)
Ejemplo n.º 6
0
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
Ejemplo n.º 7
0
    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
Ejemplo n.º 8
0
    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_
Ejemplo n.º 10
0
    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
Ejemplo n.º 11
0
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)
Ejemplo n.º 12
0
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)

Ejemplo n.º 13
0
# 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
Ejemplo n.º 14
0
    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
Ejemplo n.º 15
0
    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
Ejemplo n.º 16
0
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()
Ejemplo n.º 17
0
    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
Ejemplo n.º 18
0
    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
Ejemplo n.º 19
0
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: