diff --git a/README.md b/README.md index ac21a29..ef09d82 100644 --- a/README.md +++ b/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 +# activate the virtual environment +source /bin/activate +``` +where `` 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 ``` diff --git a/examples/README.md b/examples/README.md new file mode 100644 index 0000000..b9869dc --- /dev/null +++ b/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. diff --git a/examples/gym/cartpole/README.md b/examples/gym/cartpole/README.md new file mode 100644 index 0000000..d6fc482 --- /dev/null +++ b/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 _.py`. diff --git a/examples/robots/README.md b/examples/robots/README.md new file mode 100644 index 0000000..3a46ca9 --- /dev/null +++ b/examples/robots/README.md @@ -0,0 +1,5 @@ +## Robot examples + +You can try to load different robot by typing `python .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. diff --git a/examples/robots/aibo.py b/examples/robots/aibo.py new file mode 100644 index 0000000..c07acc6 --- /dev/null +++ b/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) diff --git a/examples/robots/allegrohand.py b/examples/robots/allegrohand.py new file mode 100644 index 0000000..52d2274 --- /dev/null +++ b/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) diff --git a/examples/robots/ant.py b/examples/robots/ant.py new file mode 100644 index 0000000..ccea860 --- /dev/null +++ b/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) diff --git a/examples/robots/atlas.py b/examples/robots/atlas.py new file mode 100644 index 0000000..b446491 --- /dev/null +++ b/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) diff --git a/examples/robots/ballbot.py b/examples/robots/ballbot.py new file mode 100644 index 0000000..5f30a1b --- /dev/null +++ b/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) diff --git a/examples/robots/baxter.py b/examples/robots/baxter.py new file mode 100644 index 0000000..2dd6cb7 --- /dev/null +++ b/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) diff --git a/examples/robots/bb8.py b/examples/robots/bb8.py new file mode 100644 index 0000000..d48fd4e --- /dev/null +++ b/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) diff --git a/examples/robots/cartpole.py b/examples/robots/cartpole.py new file mode 100644 index 0000000..4fb011c --- /dev/null +++ b/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) diff --git a/examples/robots/cassie.py b/examples/robots/cassie.py new file mode 100644 index 0000000..5d000cc --- /dev/null +++ b/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) diff --git a/examples/robots/centauro.py b/examples/robots/centauro.py new file mode 100644 index 0000000..233cda9 --- /dev/null +++ b/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) diff --git a/examples/robots/cogimon.py b/examples/robots/cogimon.py new file mode 100644 index 0000000..aa27c35 --- /dev/null +++ b/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) diff --git a/examples/robots/coman.py b/examples/robots/coman.py new file mode 100644 index 0000000..772bd81 --- /dev/null +++ b/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) diff --git a/examples/robots/crab.py b/examples/robots/crab.py new file mode 100644 index 0000000..4e2904f --- /dev/null +++ b/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) diff --git a/examples/robots/cubli.py b/examples/robots/cubli.py new file mode 100644 index 0000000..a11d715 --- /dev/null +++ b/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) diff --git a/examples/robots/darwin.py b/examples/robots/darwin.py new file mode 100644 index 0000000..6e203ce --- /dev/null +++ b/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) diff --git a/examples/robots/edo.py b/examples/robots/edo.py new file mode 100644 index 0000000..2ef5bd3 --- /dev/null +++ b/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) diff --git a/examples/robots/epuck.py b/examples/robots/epuck.py new file mode 100644 index 0000000..8eb38e8 --- /dev/null +++ b/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) diff --git a/examples/robots/f10_racecar.py b/examples/robots/f10_racecar.py new file mode 100644 index 0000000..0d9cc31 --- /dev/null +++ b/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) diff --git a/examples/robots/fetch.py b/examples/robots/fetch.py new file mode 100644 index 0000000..cfc46ba --- /dev/null +++ b/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) diff --git a/examples/robots/franka.py b/examples/robots/franka.py new file mode 100644 index 0000000..bade6b6 --- /dev/null +++ b/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) diff --git a/examples/robots/half_cheetah.py b/examples/robots/half_cheetah.py new file mode 100644 index 0000000..4ed1d2b --- /dev/null +++ b/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) diff --git a/examples/robots/hopper.py b/examples/robots/hopper.py new file mode 100644 index 0000000..5c26472 --- /dev/null +++ b/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) diff --git a/examples/robots/hubo.py b/examples/robots/hubo.py new file mode 100644 index 0000000..b6c3341 --- /dev/null +++ b/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) diff --git a/examples/robots/humanoid.py b/examples/robots/humanoid.py new file mode 100644 index 0000000..aadc1ce --- /dev/null +++ b/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) diff --git a/examples/robots/husky.py b/examples/robots/husky.py new file mode 100644 index 0000000..8df769d --- /dev/null +++ b/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) diff --git a/examples/robots/hyq.py b/examples/robots/hyq.py new file mode 100644 index 0000000..2364992 --- /dev/null +++ b/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) diff --git a/examples/robots/hyq2max.py b/examples/robots/hyq2max.py new file mode 100644 index 0000000..036a662 --- /dev/null +++ b/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) diff --git a/examples/robots/jaco.py b/examples/robots/jaco.py new file mode 100644 index 0000000..756557d --- /dev/null +++ b/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) diff --git a/examples/robots/kr5.py b/examples/robots/kr5.py new file mode 100644 index 0000000..1183f33 --- /dev/null +++ b/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) diff --git a/examples/robots/kuka_iiwa.py b/examples/robots/kuka_iiwa.py new file mode 100644 index 0000000..8e51f00 --- /dev/null +++ b/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) diff --git a/examples/robots/kuka_lwr.py b/examples/robots/kuka_lwr.py new file mode 100644 index 0000000..7f904f3 --- /dev/null +++ b/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) diff --git a/examples/robots/laikago.py b/examples/robots/laikago.py new file mode 100644 index 0000000..277a23f --- /dev/null +++ b/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) diff --git a/examples/robots/littledog.py b/examples/robots/littledog.py new file mode 100644 index 0000000..5e59f6e --- /dev/null +++ b/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) diff --git a/examples/robots/manipulator2d.py b/examples/robots/manipulator2d.py new file mode 100644 index 0000000..573200f --- /dev/null +++ b/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) diff --git a/examples/robots/minitaur.py b/examples/robots/minitaur.py new file mode 100644 index 0000000..2b8efb5 --- /dev/null +++ b/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) diff --git a/examples/robots/mkz.py b/examples/robots/mkz.py new file mode 100644 index 0000000..312bc07 --- /dev/null +++ b/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) diff --git a/examples/robots/morphex.py b/examples/robots/morphex.py new file mode 100644 index 0000000..d38ddec --- /dev/null +++ b/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) diff --git a/examples/robots/nao.py b/examples/robots/nao.py new file mode 100644 index 0000000..e1bd4cb --- /dev/null +++ b/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) diff --git a/examples/robots/opendog.py b/examples/robots/opendog.py new file mode 100644 index 0000000..e9d201f --- /dev/null +++ b/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) diff --git a/examples/robots/pepper.py b/examples/robots/pepper.py new file mode 100644 index 0000000..c9ab77b --- /dev/null +++ b/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) diff --git a/examples/robots/phantomx.py b/examples/robots/phantomx.py new file mode 100644 index 0000000..b96b783 --- /dev/null +++ b/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) diff --git a/examples/robots/pleurobot.py b/examples/robots/pleurobot.py new file mode 100644 index 0000000..e151639 --- /dev/null +++ b/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) diff --git a/examples/robots/pr2.py b/examples/robots/pr2.py new file mode 100644 index 0000000..547fe29 --- /dev/null +++ b/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) diff --git a/examples/robots/quadcopter.py b/examples/robots/quadcopter.py new file mode 100644 index 0000000..be5eda8 --- /dev/null +++ b/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) diff --git a/examples/robots/rhex.py b/examples/robots/rhex.py new file mode 100644 index 0000000..1e050f6 --- /dev/null +++ b/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) diff --git a/examples/robots/rrbot.py b/examples/robots/rrbot.py new file mode 100644 index 0000000..849c1bd --- /dev/null +++ b/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) diff --git a/examples/robots/sawyer.py b/examples/robots/sawyer.py new file mode 100644 index 0000000..2293152 --- /dev/null +++ b/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) diff --git a/examples/robots/sea_hexapod.py b/examples/robots/sea_hexapod.py new file mode 100644 index 0000000..17a34af --- /dev/null +++ b/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) diff --git a/examples/robots/sea_snake.py b/examples/robots/sea_snake.py new file mode 100644 index 0000000..0d4ac78 --- /dev/null +++ b/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) diff --git a/examples/robots/softhand.py b/examples/robots/softhand.py new file mode 100644 index 0000000..5c63c27 --- /dev/null +++ b/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) diff --git a/examples/robots/swimmer.py b/examples/robots/swimmer.py new file mode 100644 index 0000000..56f977f --- /dev/null +++ b/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) diff --git a/examples/robots/walker2d.py b/examples/robots/walker2d.py new file mode 100644 index 0000000..3a30bd1 --- /dev/null +++ b/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) diff --git a/examples/robots/walkman.py b/examples/robots/walkman.py new file mode 100644 index 0000000..13a2d43 --- /dev/null +++ b/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) diff --git a/examples/robots/wam.py b/examples/robots/wam.py new file mode 100644 index 0000000..e6f562a --- /dev/null +++ b/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) diff --git a/examples/robots/youbot.py b/examples/robots/youbot.py new file mode 100644 index 0000000..214a48f --- /dev/null +++ b/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) diff --git a/pyrobolearn/models/__init__.py b/pyrobolearn/models/__init__.py index abcbccf..7d3eb59 100644 --- a/pyrobolearn/models/__init__.py +++ b/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 * diff --git a/pyrobolearn/models/pca.py b/pyrobolearn/models/pca.py index 286e2e9..267c897 100644 --- a/pyrobolearn/models/pca.py +++ b/pyrobolearn/models/pca.py @@ -1,5 +1,5 @@ #!/usr/bin/env python -"""Define the PCA model. +"""Define the PCA model. """ import numpy as np diff --git a/pyrobolearn/robots/__init__.py b/pyrobolearn/robots/__init__.py index c242878..2953d9b 100644 --- a/pyrobolearn/robots/__init__.py +++ b/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 # diff --git a/pyrobolearn/robots/cartpole.py b/pyrobolearn/robots/cartpole.py index d932411..9464c96 100644 --- a/pyrobolearn/robots/cartpole.py +++ b/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: diff --git a/pyrobolearn/robots/fetch.py b/pyrobolearn/robots/fetch.py index ab53887..c65162c 100644 --- a/pyrobolearn/robots/fetch.py +++ b/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: diff --git a/pyrobolearn/robots/quadcopter.py b/pyrobolearn/robots/quadcopter.py index b890d25..c46edc0 100644 --- a/pyrobolearn/robots/quadcopter.py +++ b/pyrobolearn/robots/quadcopter.py @@ -3,6 +3,7 @@ """ import os +import numpy as np from uav import RotaryWingUAV diff --git a/pyrobolearn/worlds/world.py b/pyrobolearn/worlds/world.py index 793a556..fafbd92 100644 --- a/pyrobolearn/worlds/world.py +++ b/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 diff --git a/requirements.txt b/requirements.txt index dc8ba17..2079650 100644 --- a/requirements.txt +++ b/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] diff --git a/scripts/README.md b/scripts/README.md new file mode 100644 index 0000000..156f724 --- /dev/null +++ b/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. diff --git a/setup.py b/setup.py index e22117f..a2c419f 100755 --- a/setup.py +++ b/setup.py @@ -5,7 +5,10 @@ # $ python setup.py install --home= 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',