add robot examples + update installation steps

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)