add robot examples + update installation steps

README.md

@@ -5,23 +5,43 @@ This framework revolves mainly around 7 axes: simulators, worlds, robots, interf
 
 ## Requirements
 
-The framework has been tested with Python 2.7 and Ubuntu 16.04.
+The framework has been tested with Python 2.7 and Ubuntu 16.04 and 18.04. We plan to migrate soon to Python 3.5.
 
 ## Installation
 
 1. First download the `pip` Python package manager and create a virtual environment for Python 2.7 as described in the following link: https://packaging.python.org/guides/installing-using-pip-and-virtualenv/
-On Ubuntu, in the terminal, you can type: 
+On Ubuntu, in the terminal, you can type to download and install `pip` and `virtualenv`: 
 ```bash
 sudo apt install python-pip
 sudo pip install virtualenv
 ```
 
+You can then create the virtual environment by typing:
+```bash
+virtualenv -p /usr/bin/python2.7 <virtualenv_name>
+# activate the virtual environment
+source <virtualenv_name>/bin/activate
+```
+where `<virtualenv_name>` is a name of your choice for the virtual environment. For instance, it can be `py2.7`.
+
+To deactivate the virtual environment, just type:
+```bash
+deactivate
+```
+
 2. clone this repository and install the requirements and the setup.py
 ```bash
-pip install -r requirements.txt
-pip install -e .
+git clone https://github.com/robotlearn/pyrobolearn
+cd pyrobolearn
+pip install numpy cython
+pip install http://github.com/cornellius-gp/gpytorch/archive/alpha.zip
+pip install -e .  # this will install pyrobolearn as well as all the required packages (so no need for: pip install -r requirements.txt)
 ```
 
+## How to use it?
+
+Check the `README.md` file in the `examples` folder.
+
 ## Citation
 
 ```

examples/README.md

@@ -0,0 +1,7 @@
+## Examples
+
+In this folder, you will find different examples on how to use the framework.
+
+You can check the following folders:
+- `gym/cartpole`: policies are trained with different algorithms on the gym Cartpole environment.
+- `robots`: check how to load a specific robot into the world.

examples/gym/cartpole/README.md

@@ -0,0 +1,3 @@
+## Cartpole gym example
+
+In this folder, you can try different policies and RL algorithms to train them. Just run the file `python <policy>_<algo>.py`.

examples/robots/README.md

@@ -0,0 +1,5 @@
+## Robot examples
+
+You can try to load different robot by typing `python <robot>.py`.
+
+To turn the camera in the simulator, keep pressing the `ctrl` key and the left button on the mouse, and move this last one.

examples/robots/aibo.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Aibo robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Aibo
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Aibo(sim)  # , useFixedBase=True)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# # Position control using sliders
+# robot.addJointSlider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds())
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/allegrohand.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+"""Load the Allegro hand.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import AllegroHand
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+right_hand = AllegroHand(sim)  # , init_pos=(0.,0.,0.), init_orient=(0,0,1,0))
+
+# print information about the robot
+right_hand.printRobotInfo()
+# H = right_hand.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+# Position control using sliders
+right_hand.addJointSlider()
+
+for i in count():
+    right_hand.updateJointSlider()
+    # right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs())
+
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/ant.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Ant Mujoco model.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Ant
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Ant(sim)  # , useFixedBase=True)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider(robot.getLeftFrontLegIds() + robot.getRightFrontLegIds())
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/atlas.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Atlas robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Atlas
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Atlas(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# position control using sliders
+robot.addJointSlider(robot.getLeftLegIds())
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/ballbot.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Load the Ballbot robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Ballbot
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Ballbot(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+for i in count():
+    # robot.setJointVelocities([0, -1, 0])
+
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/baxter.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Baxter robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Baxter
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Baxter(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/bb8.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Load the BB8 robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import BB8
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = BB8(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+for i in count():
+    robot.setJointVelocities([0, -1, 0])
+
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/cartpole.py

@@ -0,0 +1,123 @@
+#!/usr/bin/env python
+"""Load the Cartpole robotic platform.
+"""
+
+import control
+from scipy.linalg import solve_continuous_are
+
+
+class LQR(object):
+    r"""Linear Quadratic Regulator
+
+    Type: Model-based (optimal control)
+
+    LQR assumes that the dynamics are described by a set of linear differential equations, and a quadratic cost.
+    That is, the dynamics can written as :math:`\dot{x} = A x + B u`, where :math:`x` is the state vector, and
+    :math:`u` is the control vector, and the cost is given by:
+
+    .. math:: J = x(T)^T F(T) x(T) + \int_0^T (x(t)^T Q x(t) + u(t)^T R u(t) + 2 x(t)^T N u(t)) dt
+
+    where :math:`Q` and :math:`R` represents weight matrices which allows to specify the relative importance
+    of each state/control variable. These are normally set by the user.
+
+    The goal is to find the feedback control law :math:`u` that minimizes the above cost :math:`J`. Solving it
+    gives us :math:`u = -K x`, where :math:`K = R^{-1} (B^T S + N^T)` with :math:`S` is found by solving the
+    continuous time Riccati differential equation :math:`S A + A^T S - (S B + N) R^{-1} (B^T S + N^T) + Q = 0`.
+
+    Thus, LQR requires thus the model/dynamics of the system to be given (i.e. :math:`A` and :math:`B`).
+    If the dynamical system is described by a set of nonlinear differential equations, we first have to linearize
+    them around fixed points.
+
+    Time complexity: O(M^3) where M is the size of the state vector
+    Note: A desired state xd can also be given to the system: u = -K (x - xd)   (P control)
+
+    See also:
+        - `ilqr.py`: iterative LQR
+        - `lqg.py`: LQG = LQR + LQE
+        - `ilqg.py`: iterative LQG
+    """
+
+    def __init__(self, A, B, Q=None, R=None, N=None):
+        if not self.isControllable(A, B):
+            raise ValueError("The system is not controllable")
+        self.A = A
+        self.B = B
+        if Q is None: Q = np.identity(A.shape[1])
+        self.Q = Q
+        if R is None: R = np.identity(B.shape[1])
+        self.R = R
+        self.N = N
+        self.K = None
+
+    @staticmethod
+    def isControllable(A, B):
+        return np.linalg.matrix_rank(control.ctrb(A, B)) == A.shape[0]
+
+    def getRiccatiSolution(self):
+        S = solve_continuous_are(self.A, self.B, self.Q, self.R, s=self.N)
+        return S
+
+    def getGainK(self):
+        #S = self.getRiccatiSolution()
+        #S1 = self.B.T.dot(S)
+        #if self.N is not None: S1 += self.N.T
+        #K = np.linalg.inv(self.R).dot(S1)
+
+        if self.N is None:
+            K, S, E = control.lqr(self.A, self.B, self.Q, self.R)
+        else:
+            K, S, E = control.lqr(self.A, self.B, self.Q, self.R, self.N)
+        return K
+
+    def compute(self, x, xd=None):
+        """Return the u."""
+
+        if self.K is None:
+            self.K = self.getGainK()
+
+        if xd is None:
+            return self.K.dot(x)
+        else:
+            return self.K.dot(xd - x)
+
+
+# Test
+if __name__ == "__main__":
+    import numpy as np
+    from itertools import count
+    from pyrobolearn.simulators import BulletSim
+    from pyrobolearn.worlds import World
+    from pyrobolearn.robots import CartPole
+
+    # Create simulator
+    sim = BulletSim()
+
+    # create world
+    world = World(sim)
+
+    # create robot
+    num_links = 2
+    robot = CartPole(sim, num_links=num_links)
+
+    # print information about the robot
+    robot.printRobotInfo()
+
+    robot.getSymbolicEquationsOfMotion()
+
+    eq_point = np.zeros((num_links + 1) * 2)  # state = [q, dq]
+    A, B = robot.linearizeEquationOfMotion(eq_point)
+
+    # LQR controller
+    lqr = LQR(A, B)
+    K = lqr.getGainK()
+
+    for i in count():
+        # control
+        x = np.concatenate((robot.getJointPositions(), robot.getJointVelocities()))
+        u = K.dot(eq_point - x)
+        robot.setJointTorques(u[0], 0)
+
+        print("U[0] = {}".format(u[0]))
+
+        # step in simulation
+        world.step(sleep_dt=1./240)

examples/robots/cassie.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Provide the Cassie robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Cassie
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Cassie(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    robot.moveJointHomePositions()
+    world.step(sleep_dt=1./240)

examples/robots/centauro.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the Centauro robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Centauro
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# load robot
+robot = Centauro(sim)  # , useFixedBase=True)
+
+# print information about the robot
+robot.printRobotInfo()
+print("Number of Legs: {}".format(robot.getNumberOfLegs()))
+print("Number of Arms: {}".format(robot.getNumberOfArms()))
+
+# robot.addJointSlider(robot.getRightFrontLegIds())
+robot.drive(speed=3)
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/cogimon.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Cogimon robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Cogimon
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Cogimon(sim, lower_body=False)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# # Position control using sliders
+robot.addJointSlider(robot.left_leg)
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/coman.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+"""Load the Coman robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Coman
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Coman(sim, useFixedBase=True)
+
+# print information about the robot
+robot.printRobotInfo()
+print(robot.link_names)
+
+# # Position control using sliders
+# robot.addJointSlider()
+
+robot.changeTransparency()
+# robot.drawLinkCoMs()
+robot.drawLinkFrames()
+# robot.drawBoundingBoxes(robot.right_leg[4])
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    # robot.computeAndDrawCoMPosition()
+    # robot.computeAndDrawProjectedCoMPosition()
+    world.step(sleep_dt=1./240)

examples/robots/crab.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the Crab robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Crab
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Crab(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+robot.addJointSlider(robot.right_middle_leg)
+
+# run simulation
+for i in count():
+    robot.updateJointSlider()
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/cubli.py

@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+"""Load the Cubli robot.
+"""
+
+import numpy as np
+from itertools import count
+from pyrobolearn.simulators import BulletSim, pybullet
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Cubli
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+scale = 1.  # Warning: this does not scale the mass...
+position = [0., 0., np.sqrt(2) / 2. * scale + 0.001]
+orientation = [0.383, 0, 0, 0.924]
+robot = Cubli(sim, position, orientation, scaling=scale)
+
+# print information about the robot
+robot.printRobotInfo()
+H = robot.calculateMassMatrix(qIdx=slice(6, 6+len(robot.joints)))  # floating base, thus keep only the last q
+print("Inertia matrix: H(q) = {}\n".format(H))
+
+# PD control
+Kp = 600.
+Kd = 2 * np.sqrt(Kp)
+desired_roll = np.pi / 4.
+
+for i in count():
+    # get state
+    quaternion = robot.getBaseOrientation(False)
+    w = robot.getBaseAngularVelocity()
+    euler = pybullet.getEulerFromQuaternion(quaternion.tolist())
+
+    # PD control
+    torques = [-Kp * (desired_roll - euler[0]) + Kd * w[0], 0., 0.]
+    robot.setJointTorques(torques)
+
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/darwin.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the Darwin robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Darwin
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Darwin(sim, useFixedBase=False)
+
+# print information about the robot
+robot.printRobotInfo()
+print(robot.link_names)
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/edo.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Load the e.Do robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Edo
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Edo(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+# H = robot.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/epuck.py

@@ -0,0 +1,35 @@
+#!/usr/bin/env python
+"""Load some Epuck robots.
+"""
+
+import numpy as np
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Epuck
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robots = []
+for _ in range(5):
+    x, y = np.random.uniform(low=-2, high=2, size=2)
+    robot = world.loadRobot(Epuck, position=(x, y, 0))
+    robots.append(robot)
+
+# print information about the robot
+robots[0].printRobotInfo()
+
+# Position control using sliders
+# robots[0].addJointSlider()
+
+# run simulator
+for _ in count():
+    # robots[0].updateJointSlider()
+    for robot in robots:
+        robot.drive(5)
+    world.step(sleep_dt=1./240)

examples/robots/f10_racecar.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the F10 racecar robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import F10Racecar
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = F10Racecar(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    robot.driveForward(10)
+    world.step(sleep_dt=1./240)

examples/robots/fetch.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Fetch robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Fetch
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Fetch(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/franka.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the Franka Emika robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Franka
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Franka(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+# H = robot.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+# Position control using sliders
+# robot.addJointSlider()
+
+for i in count():
+    # robot.updateJointSlider()
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/half_cheetah.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the HalfCheetah Mujoco model.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import HalfCheetah
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = HalfCheetah(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/hopper.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the Hopper Mujoco model.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Hopper
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Hopper(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/hubo.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Hubo robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Hubo
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Hubo(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/humanoid.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the Humanoid Mujoco model.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Humanoid
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Humanoid(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulation
+for i in count():
+    # robot.updateJointSlider()
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/husky.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the Husky robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Husky
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Husky(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    robot.driveForward(2)
+    world.step(sleep_dt=1./240)

examples/robots/hyq.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the HyQ robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import HyQ
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = HyQ(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# # Position control using sliders
+robot.addJointSlider(robot.getLeftFrontLegIds())
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    robot.computeAndDrawCoMPosition()
+    robot.computeAndDrawProjectedCoMPosition()
+    world.step(sleep_dt=1./240)

examples/robots/hyq2max.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+"""Provide the HyQ2Max robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import HyQ2Max
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+world.loadJapaneseMonastery()
+
+# create robot
+robot = HyQ2Max(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider(robot.getLeftFrontLegIds())
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    robot.computeAndDrawCoMPosition()
+    robot.computeAndDrawProjectedCoMPosition()
+
+    world.step(sleep_dt=1./240)

examples/robots/jaco.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the Jaco robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Jaco
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Jaco(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/kr5.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Load the Kuka KR5 robotic industrial platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import KR5
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = KR5(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+# H = robot.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/kuka_iiwa.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python
+"""Provide the Kuka IIWA robotic platform.
+"""
+
+import numpy as np
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import KukaIIWA
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = KukaIIWA(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+# H = robot.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+# print(robot.getLinkWorldPositions(flatten=False))
+
+K = 5000*np.identity(3)
+# D = 2 * np.sqrt(K)
+# D = np.zeros((3,3))
+D = 100 * np.identity(3)
+x_des = np.array([0.3, 0.0, 0.8])
+x_des = np.array([0.52557296, 0.09732758, 0.80817658])
+linkId = robot.getLinkIds('iiwa_link_ee')
+
+for i in count():
+    # print(robot.getLinkWorldPositions(flatten=False))
+
+    # get state
+    q = robot.getJointPositions()
+    dq = robot.getJointVelocities()
+    x = robot.getLinkWorldPositions(linkId)
+    dx = robot.getLinkWorldLinearVelocities(linkId)
+
+    # get (linear) jacobian
+    J = robot.getLinearJacobian(linkId, q)
+
+    # get coriolis, gravity compensation torques
+    torques = robot.getCoriolisAndGravityCompensationTorques(q, dq)
+
+    # Impedance control: attractor point
+    F = K.dot(x_des - x) - D.dot(dx)
+    # F = -D.dot(dx)
+    tau = J.T.dot(F)
+    print(tau)
+    torques += tau
+    robot.setJointTorques(torques)
+
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/kuka_lwr.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python
+"""Load the Kuka LWR robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import KukaLWR
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = KukaLWR(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+# H = robot.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/laikago.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the Laikago robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Laikago
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Laikago(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# # Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    robot.moveJointHomePositions()
+    world.step(sleep_dt=1./240)

examples/robots/littledog.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the Little Dog robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import LittleDog
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = LittleDog(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    # robot.computeAndDrawCoMPosition()
+    # robot.computeAndDrawProjectedCoMPosition()
+    world.step(sleep_dt=1./240)

examples/robots/manipulator2d.py

@@ -0,0 +1,27 @@
+#!/usr/bin/env python
+"""Load 2d manipulators.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Manipulator2D
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Manipulator2D(sim, init_pos=(0, -0.25, 0))
+robot1 = Manipulator2D(sim, init_pos=(0, 0.25, 0))
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/minitaur.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the Minitaur robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Minitaur
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Minitaur(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+robot.addJointSlider(robot.getLeftFrontLegIds())
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    # robot.computeAndDrawCoMPosition()
+    # robot.computeAndDrawProjectedCoMPosition()
+    world.step(sleep_dt=1./240)

examples/robots/mkz.py

@@ -0,0 +1,29 @@
+#!/usr/bin/env python
+"""Load the Lincoln MKZ car robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import MKZ
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = MKZ(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    robot.driveForward(2)
+    world.step(sleep_dt=1./240)

examples/robots/morphex.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the Morphex robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Morphex
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Morphex(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/nao.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Nao robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Nao
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Nao(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider(robot.getLeftArmIds())
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/opendog.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the OpenDog robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import OpenDog
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = OpenDog(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider(robot.getLeftFrontLegIds())
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/pepper.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python
+"""Load the Pepper robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Pepper
+
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Pepper(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for i in count():
+    # robot.updateJointSlider()
+    if i % 20 == 0:
+        robot.cameraTop.getRGBImage()
+    world.step(sleep_dt=1./240)

examples/robots/phantomx.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the Phantom X robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import PhantomX
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = PhantomX(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/pleurobot.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the Pleurobot robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Pleurobot
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Pleurobot(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    # robot.computeAndDrawCoMPosition()
+    # robot.computeAndDrawProjectedCoMPosition()
+    world.step(sleep_dt=1./240)

examples/robots/pr2.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the PR2 robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import PR2
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = PR2(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/quadcopter.py

@@ -0,0 +1,32 @@
+#!/usr/bin/env python
+"""Load the Quadcopter robotic platform.
+"""
+
+import numpy as np
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Quadcopter
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Quadcopter(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+rpm = robot.getStationaryRPM()
+print("Stationary RPM: {}".format(rpm))
+v = robot.rpmToRadPerSecond(rpm+20)
+v = [v, -v, v, -v]
+
+# run simulation
+for i in count():
+    robot.setJointVelocities(v)
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/rhex.py

@@ -0,0 +1,30 @@
+#!/usr/bin/env python
+"""Load the Rhex robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Rhex
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Rhex(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider(robot.right_back_leg)
+
+# run simulation
+for i in count():
+    # robot.updateJointSlider()
+    robot.drive(2)
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/rrbot.py

@@ -0,0 +1,150 @@
+#!/usr/bin/env python
+"""Load the RRBot robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import RRBot
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# load robot
+robot = RRBot(sim)
+# robot.addJointSlider()
+
+print("Robot: {}".format(robot))
+print("Total number of joints: {}".format(robot.getNumberOfJoints()))
+print("Joint names: {}".format(robot.getJointNames(range(robot.getNumberOfJoints()))))
+print("Link names: {}".format(robot.getLinkNames(range(robot.getNumberOfJoints()))))
+
+print("Number of DoFs: {}".format(robot.getNumberOfDoFs()))
+print("Robot actuated joint ids: {}".format(robot.joints))
+print("Actuated joint names: {}".format(robot.getJointNames()))
+print("Actuated link names: {}".format(robot.getLinkNames()))
+print("Current joint positions: {}".format(robot.getJointPositions()))
+
+print("Number of end-effectors: {}".format(robot.getNumberOfEndEffectors()))
+print("End-effector names: {}".format(robot.getEndEffectorNames()))
+
+print("Robot base position: {}".format(robot.getBasePosition()))
+robot.changeTransparency()
+visuals = robot.sim.getVisualShapeData(robot.id)
+visuals = {visual[1]: visual[3] for visual in visuals}
+
+# robot.drawLinkCoMs()
+robot.drawLinkFrames()
+# robot.drawBoundingBoxes()
+
+for i in robot.joints:
+    print("Link {}".format(i))
+    state = robot.sim.getLinkState(robot.id, i)
+    info = robot.sim.getJointInfo(robot.id, i)
+
+    print("\t CoM world position: {}".format(state[0]))
+    print("\t Local inertial frame position: {}".format(state[2]))
+    print("\t World link frame position: {}".format(state[4]))
+
+    print("\t CoM world orientation: {}".format(state[1]))
+    print("\t Local inertial frame orientation: {}".format(state[3]))
+    print("\t World link frame orientation: {}".format(state[5]))
+
+    print("\t Joint axis: {}".format(info[-4]))
+    if i in visuals:
+        print("\t Dimensions: {}".format(visuals[i]))
+
+for _ in count():
+    world.step(sleep_dt=1./240)
+
+raw_input('press enter')
+
+print("Inertia matrix: {}".format(np.array(sim.calculateMassMatrix(robot.id, [0.,0.,0.,0.,0.,0.]))))
+linkId = 2
+com_frame = robot.getLinkStates(linkId)[2]
+q = robot.getJointPositions()
+print(com_frame)
+# com_frame = [0.,0.,0.]
+print("Jacobian matrix: {}".format(np.vstack((sim.calculateJacobian(robot.id, linkId, com_frame, q.tolist(), [0.,0.], [0.,0.])))))
+
+Jlin = robot.calculateJacobian(linkId+1, localPosition=(0.,0.,0.))[:3]
+print("Jacobian matrix: {}".format(Jlin))
+
+robot.drawVelocityManipulabilityEllipsoid(linkId+1, Jlin)
+
+Jlin = robot.calculateJacobian(linkId)[:3]
+robot.drawVelocityManipulabilityEllipsoid(linkId, Jlin, color=(1,0,0,0.7))
+
+cnt = 0
+for i in count():
+    if i%240 == 0:
+        if cnt < 3:
+            Jlin = robot.calculateJacobian(linkId + 1, localPosition=(0., 0., 0.))[:3]
+            robot.drawVelocityManipulabilityEllipsoid(linkId + 1, Jlin)
+        cnt += 1
+    world.step(sleep_dt=1./240)
+    # robot.setJointTorques()
+
+raw_input('press enter')
+
+print(robot.getLinkNames())
+force = np.array([1., 0., 0.])
+pos = np.array([0., 0., 0.])
+sim.applyExternalForce(robot.id, 1, force, pos, flags=p.LINK_FRAME) # link_frame = 1
+
+slider = sim.addUserDebugParameter('force', -1000., 1000., 0)
+
+dq, ddq = [0., 0.], [0., 0.]
+J = sim.calculateJacobian(robot.id, 1, [0.,0.,0.], [0.,0.], dq, ddq)
+print(np.array(J[0]))
+
+a = robot.getJointPositions()
+# print(robot.getJacobianMatrix(1, np.array([0.,0.]))) # TODO: need to convert numpy array to list
+
+linkId = 2
+com_frame = robot.getLinkStates(linkId)[2]
+xdes = np.array(robot.getLinkWorldPositions(linkId))
+K = 100*np.identity(3)
+D = 2*np.sqrt(K)  # critically damped
+D = 3*D  # manually increase damping
+
+# run simulator
+for i in range(10000):
+    joint_states = p.getJointStates(robot.id, robot.joints)
+    # print("joint state: ", joint_states)
+    q = [joint_state[0] for joint_state in joint_states]
+    dq = [joint_state[1] for joint_state in joint_states]
+
+    # q = robot.getJointPositions().tolist()
+    # dq = robot.getJointVelocities().tolist()
+    x = np.array(robot.getLinkWorldPositions(linkId))
+    dx = np.array(robot.getLinkWorldLinearVelocities(linkId))
+    tau = robot.calculateID(q, dq, ddq)  # Coriolis, centrifugal and gravity compensation
+    Jlin = np.array(sim.calculateJacobian(robot.id, linkId, com_frame, q, [0.,0.], ddq)[0])
+    F = K.dot(xdes - x) - D.dot(dx) # compute cartesian forces
+    # print("force: {}".format(F))
+    tau += Jlin.T.dot(F) # cartesian PD with gravity compensation
+    # tau += Jlin.T.dot(- D.dot(dx))  # active compliance
+
+    # tau = Jlin.T.dot(F)
+
+    # compute manipulability measure :math:`w = sqrt(det(JJ^T))`
+    Jlin = Jlin[[0, 2], :]
+    w = np.sqrt(np.linalg.det(Jlin.dot(Jlin.T)))
+    # print("manipulability: {}".format(w))
+
+    # Impedance/Torque control
+    sim.setJointMotorControlArray(robot.id, robot.joint_indices, sim.TORQUE_CONTROL, forces=tau)
+
+    force = sim.readUserDebugParameter(slider)
+    if force > 0:
+        force = np.array([0., 0., 1.])
+    else:
+        force = np.array([0., 0., 0.])
+    sim.applyExternalForce(robot.id, linkId, force, pos, flags=p.LINK_FRAME)  # p.LINK_FRAME = 1
+
+    # robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/sawyer.py

@@ -0,0 +1,28 @@
+#!/usr/bin/env python
+"""Load the Sawyer robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Sawyer
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Sawyer(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# # Position control using sliders
+robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    robot.updateJointSlider()
+    world.step(sleep_dt=1./240)

examples/robots/sea_hexapod.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the SEA Hexapod robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import SEAHexapod
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = SEAHexapod(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/sea_snake.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the SEA Snake robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import SEASnake
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = SEASnake(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/softhand.py

@@ -0,0 +1,36 @@
+#!/usr/bin/env python
+"""Provide the Soft Hand robotic platform.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import SoftHand
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+left_hand = SoftHand(sim, init_pos=(-0.15, 0, 0), left=True)
+right_hand = SoftHand(sim, init_pos=(0.15, 0., 0.), init_orient=(0, 0, 1, 0), left=False)
+
+# print information about the robot
+left_hand.printRobotInfo()
+# H = left_hand.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+# Position control using sliders
+# left_hand.addJointSlider()
+
+left_hand.setJointPositions([0.] * left_hand.getNumberOfDoFs())
+right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs())
+
+for i in count():
+    # left_hand.updateJointSlider()
+    # left_hand.setJointPositions([0.] * left_hand.getNumberOfDoFs())
+    # right_hand.setJointPositions([0.] * right_hand.getNumberOfDoFs())
+
+    world.step(sleep_dt=1./240)

examples/robots/swimmer.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the Swimmer Mujoco model.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Swimmer
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Swimmer(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/walker2d.py

@@ -0,0 +1,25 @@
+#!/usr/bin/env python
+"""Load the Walker2D Mujoco model.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Walker2D
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = Walker2D(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# run simulation
+for i in count():
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/walkman.py

@@ -0,0 +1,33 @@
+#!/usr/bin/env python
+"""Load the WALK-MAN robot.
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import Walkman
+
+# Create simulator
+sim = BulletSim()
+
+# Create world
+world = BasicWorld(sim)
+world.loadSphere([2., 0, 2.], mass=0., color=(1, 0, 0, 1))
+world.loadSphere([2., 1., 2.], mass=0., color=(0, 0, 1, 1))
+
+# load robot
+robot = Walkman(sim, useFixedBase=False, lower_body=False)
+
+# print information about the robot
+robot.printRobotInfo()
+
+# # Position control using sliders
+robot.addJointSlider(robot.left_leg)
+
+# run simulator
+for i in count():
+    robot.updateJointSlider()
+    if i % 60 == 0:
+        img = robot.left_camera.getRGBImage()
+
+    world.step(sleep_dt=1./240)

examples/robots/wam.py

@@ -0,0 +1,46 @@
+#!/usr/bin/env python
+"""Load the WAM robotic platform.
+"""
+
+import numpy as np
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import WAM
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+robot = WAM(sim)
+
+# print information about the robot
+robot.printRobotInfo()
+# H = robot.calculateMassMatrix()
+# print("Inertia matrix: H(q) = {}".format(H))
+
+robot.setJointPositions([np.pi / 4, np.pi / 2], jointId=[0,1]) #2, 4])
+
+Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3]
+robot.drawVelocityManipulabilityEllipsoid(6, Jlin, color=(1,0,0,0.7))
+for _ in range(5):
+    world.step(sleep_dt=1./240)
+
+Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3]
+robot.drawVelocityManipulabilityEllipsoid(6, Jlin, color=(0, 0, 1, 0.7))
+for _ in range(45):
+    world.step(sleep_dt=1./240)
+
+Jlin = robot.calculateJacobian(6, localPosition=(0., 0., 0.))[:3]
+robot.drawVelocityManipulabilityEllipsoid(6, Jlin)
+
+for i in count():
+    if i%1000 == 0:
+        print("Joint Torques: {}".format(robot.getJointTorques()))
+        print("Gravity Torques: {}".format(robot.getGravityCompensationTorques()))
+        print("Compensation Torques: {}".format(robot.getCoriolisAndGravityCompensationTorques()))
+    # step in simulation
+    world.step(sleep_dt=1./240)

examples/robots/youbot.py

@@ -0,0 +1,38 @@
+#!/usr/bin/env python
+"""Load the various Youbot robotic platforms.
+
+These include: YoubotBase, KukaYoubotArm, Youbot, YoubotDualArm
+"""
+
+from itertools import count
+from pyrobolearn.simulators import BulletSim
+from pyrobolearn.worlds import BasicWorld
+from pyrobolearn.robots import YoubotBase, KukaYoubotArm, Youbot, YoubotDualArm
+
+# Create simulator
+sim = BulletSim()
+
+# create world
+world = BasicWorld(sim)
+
+# create robot
+youbot_base = YoubotBase(sim, init_pos=(0, -0.75))
+kuka_arm = KukaYoubotArm(sim, init_pos=(0, -0.25))
+youbot = Youbot(sim, init_pos=(0, 0.25))
+youbot_dual_arm = YoubotDualArm(sim, init_pos=(0, 0.75))
+
+robots = [youbot_base, kuka_arm, youbot, youbot_dual_arm]
+
+# print information about the robot
+for robot in robots:
+    robot.printRobotInfo()
+
+# Position control using sliders
+# robot.addJointSlider()
+
+# run simulator
+for _ in count():
+    # robots[0].updateJointSlider()
+    # for robot in robots:
+    #     robot.drive(5)
+    world.step(sleep_dt=1./240)

pyrobolearn/models/__init__.py

@@ -23,7 +23,7 @@ from gmm import GMM
 from gp import GPR
 
 # HMM
-from hmm import *
+# from hmm import *
 
 # DNN
 from nn import *

pyrobolearn/models/pca.py

@@ -1,5 +1,5 @@
 #!/usr/bin/env python
-"""Define the PCA model.
+"""Define the PCA model.
 """
 
 import numpy as np

pyrobolearn/robots/__init__.py

@@ -49,12 +49,14 @@ from walkman import Walkman
 from cogimon import Cogimon
 from darwin import Darwin
 # from kondo import KHR3HV
+from hubo import Hubo
 
 # Hexapod
 from crab import Crab
 from sea_hexapod import SEAHexapod
 from phantomx import PhantomX
 from morphex import Morphex
+from rhex import Rhex
 
 # Manipulators
 from rrbot import RRBot
@@ -64,6 +66,9 @@ from kuka_iiwa import KukaIIWA
 from jaco import Jaco
 from franka import Franka
 from sawyer import Sawyer
+from edo import Edo
+from kr5 import KR5
+from manipulator2d import Manipulator2D
 
 # Bi-Manipulators
 from baxter import Baxter
@@ -75,6 +80,8 @@ from softhand import SoftHand
 # Wheeled
 from epuck import Epuck
 from f10_racecar import F10Racecar
+from mkz import MKZ
+from husky import Husky
 
 # Wheeled + (single) manipulator
 from fetch import Fetch
@@ -104,7 +111,7 @@ from cubli import Cubli
 from sea_snake import SEASnake
 from bb8 import BB8
 from youbot import Youbot, YoubotBase, KukaYoubotArm, YoubotDualArm
-
+from ballbot import Ballbot
 
 # Robots #
 

pyrobolearn/robots/cartpole.py

@@ -3,6 +3,7 @@
 """
 
 import os
+import numpy as np
 import pybullet_data
 import sympy
 import sympy.physics.mechanics as mechanics
@@ -188,11 +189,11 @@ class CartPole(Robot):
 
         # Check if the robot has a fixed base and create the generalized coordinates and speeds based on that,
         # as well the base position, orientation and velocities
-        if robot.hasFixedBase():
+        if self.hasFixedBase():
             # generalized coordinates q(t) and speeds dq(t)
             q = mechanics.dynamicsymbols('q:{}'.format(len(self.joints)))
             dq = mechanics.dynamicsymbols('dq:{}'.format(len(self.joints)))
-            pos, orn = robot.getBasePositionAndOrientation(convert_to_numpy_quaternion=False)
+            pos, orn = self.getBasePositionAndOrientation(convert_to_numpy_quaternion=False)
             linVel, angVel = [0,0,0], [0,0,0]   # 0 because fixed base
             jointId = 0
         else:

pyrobolearn/robots/fetch.py

@@ -14,16 +14,16 @@ class Fetch(WheeledRobot, ManipulatorRobot):
 
     def __init__(self,
                  simulator,
-                 init_pos=(0, 0, 0),
+                 init_pos=(0, 0, 0.1),
                  init_orient=(0, 0, 0, 1),
                  useFixedBase=False,
                  scaling=1.,
                  urdf_path=os.path.dirname(__file__) + '/urdfs/fetch/fetch.urdf'):
         # check parameters
         if init_pos is None:
-            init_pos = (0., 0., 0.)
+            init_pos = (0., 0., 0.1)
         if len(init_pos) == 2:  # assume x, y are given
-            init_pos = tuple(init_pos) + (0.0,)
+            init_pos = tuple(init_pos) + (0.1,)
         if init_orient is None:
             init_orient = (0, 0, 0, 1)
         if useFixedBase is None:

pyrobolearn/robots/quadcopter.py

@@ -3,6 +3,7 @@
 """
 
 import os
+import numpy as np
 from uav import RotaryWingUAV
 
 

pyrobolearn/worlds/world.py

@@ -14,7 +14,7 @@ import cv2
 import time
 
 from pyrobolearn.utils.converter import QuaternionListConverter
-from pyrobolearn.utils.heightmap_generator import *
+# from pyrobolearn.utils.heightmap_generator import *  # TODO: problem with gdal installation
 from pyrobolearn.utils import hasMethod, hasVariable, isClass
 
 from pyrobolearn.robots import Robot, robot_names_to_classes

requirements.txt

@@ -1,9 +1,10 @@
 # To only install dependencies, type:
 # $ pip install -r requirements.txt
 
-numpy>=1.13.3
+# cython
+# numpy>=1.13.3
 numba>=0.42.0
-numpy-quaternion>=2019.1.13.15.7.3
+numpy-quaternion
 autograd>=1.2
 matplotlib>=2.0.2
 sympy>=1.3
@@ -26,19 +27,20 @@ gym>=0.10.9
 # interfaces
 inputs>=0.5
 gTTS>=2.0.3
-openni>=2.2.0.post6
-pyaudio>=0.2.8
+# openni>=2.2.0.post6
+# pyaudio>=0.2.8
 
 # models and algos
 sklearn>=0.0
 # gpytorch>=0.1.0rc4
-git+git://github.com/cornellius-gp/gpytorch/archive/alpha.zip
+# git+git://github.com/cornellius-gp/gpytorch/archive/alpha.zip
+# http://github.com/cornellius-gp/gpytorch/archive/alpha.zip
 GPy>=1.9.6
 GPyOpt>=1.2.5
-hmmlearn>=0.2.1
-keras>=2.2.4
+# hmmlearn>=0.2.1
+# keras>=2.2.4
 neat-python>=0.92
-tensorflow>=1.3.0
+# tensorflow>=1.3.0
 # tensorflow-gpu
 cma>=2.6.0
 # ray>=0.6.3 [rllib]

scripts/README.md

@@ -0,0 +1,3 @@
+## Scripts
+
+This folder contains bash scripts to install various packages on Ubuntu 16.04. Some of these packages are useful for optimization, ROS related libraries, but mostly for different interfaces (kinect, audio, openpose, etc.). By default, the various interfaces are not enabled in pyrobolearn.

setup.py

@@ -5,7 +5,10 @@
 # $ python setup.py install --home=<dir>
 
 from setuptools import setup, find_packages
-from pip.req import parse_requirements
+try: # for pip >= 10
+    from pip._internal.req import parse_requirements
+except ImportError: # for pip <= 9.0.3
+    from pip.req import parse_requirements
 
 # get description from readme file
 with open('README.md', 'r') as f:
@@ -25,10 +28,10 @@ setup(
     author_email='briandelhaisse@gmail.com',
     maintainer='Brian Delhaisse',
     maintainer_email='Brian Delhaisse',
-    license='MIT',
+    license='(c) Brian Delhaisse',
     url='https://github.com/robotlearn/pyrobolearn',
     platforms=['Linux Ubuntu'],
-    python_requires='2.7'
+    # python_requires='==2.7.*',
     # packages=['', 'worlds', 'robots', 'tools', 'pso', 'envs', 'algos', 'optim', 'tasks', 'tools.vr', 'tools.vr.htc', 'tools.vr.oculus',
     #           'tools.bci', 'tools.audio', 'tools.camera', 'tools.game_controllers', 'utils', 'utils.data_structures',
     #           'models', 'robots', 'robots.ros', 'robots.ros.coman', 'robots.ros.walkman', 'filters',