diff --git a/doc/Doxyfile.extra.in b/doc/Doxyfile.extra.in
index 475ea97ddba7f4244872d7c08279cb1e48e8d36e..3f4492b687c73f87dd8614af0fa244483593a5a3 100644
--- a/doc/Doxyfile.extra.in
+++ b/doc/Doxyfile.extra.in
@@ -307,7 +307,7 @@ RECURSIVE = YES
# *.c *.cc *.cxx *.cpp *.c++ *.java *.ii *.ixx *.ipp *.i++ *.inl *.h *.hh *.hxx
# *.hpp *.h++ *.idl *.odl *.cs *.php *.php3 *.inc *.m *.mm *.py *.f90
-FILE_PATTERNS = *.cc *.cpp *.h *.hpp *.hxx *.hh *.dox *.md
+FILE_PATTERNS = *.cc *.cpp *.h *.hpp *.hxx *.hh *.dox *.md *.py
# If the value of the INPUT tag contains directories, you can use the
# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
diff --git a/doc/d-labs/1-directgeom.md b/doc/d-labs/1-directgeom.md
index 07c86fac2e358ccb6ea5450aa043a9da4879382f..85cdef6351c08da62038e73a9449941ace2a99be 100644
--- a/doc/d-labs/1-directgeom.md
+++ b/doc/d-labs/1-directgeom.md
@@ -1,8 +1,6 @@
-1. Move your body (aka direct geoemtry)
-=======================================
+# 1) Move your body (aka direct geometry)
-Objective
----------
+## Objective
In this first series of exercises, we are going to start using the
library Pinocchio. We will load in the simulator the model of a simple
@@ -14,98 +12,104 @@ The software Pinocchio is a C++ library provided with a python wrapping
that allows us to control it with a python terminal. Let's see how it
works.
-Tutorial 1.0. Tips
-------------------
+## 1.0) Tips
-Setup ~\~~\~\~
+### Setup
-For all the exercices, we suppose that Pinocchio is installed in a
-VirtualBox and the user ID is "student" (password student). The
-VirtualBox contains all the software to install. Alternatively,
-Pinocchio might be installed directly in any Ubuntu 14.04 / 16.04, after
-some work in other Linux, OSX, and likely in Windows (if you like to
-gamble).
+For this tutorial, you will need [Pinocchio](http://stack-of-tasks.github.io/pinocchio/download.html),
+[Gepetto GUI](https://github.com/humanoid-path-planner/gepetto-viewer-corba), and the description of the ur5 robot.
-Two directories have been created at the root of the homedir. The
-directory "student" is where you should develop your scripts. The
-directory "models" contains the models of the robot that we will use
-during the class.
+For this, the easiest way is to add [robotpkg apt repository](http://robotpkg.openrobots.org/debian.html) and launch:
+```
+sudo apt install robotpkg-pinocchio robotpkg-ur5-description robotpkg-gepetto-viewer-corba
+```
-Pinocchio has been installed using a software for "installation from
-sources" named +robot-pkg+. The sources of Pinocchio are available in
-+\~/robotpkg/wip/pinocchio/work.robotics/pinochio-1.2.2+. After
-compilation, the software has been installed in +/opt/openrobots'. See
-for example the python scripts in +/opt/openrobots/lib/python2.7+.
+### Python
-Python ~~\~~~\~ We remind you that you can open a python terminal in
-your own shell. Simply type : bash terminal ....
-student@student-virtualbox:\~\$ ipython >>> .... Afterwards
-you'll just have to type your commands in this newly opened terminal.
+We remind you that you can open a python terminal in
+your own shell. Simply type :
+```
+student@student-virtualbox:~$ ipython
+>>>
+```
+Afterwards you'll just have to type your commands in this newly opened terminal.
To close the python terminal, just type CTRL-D (CTRL-C first to
interrupt any on-going execution).
You can also write you command lines in a file and launch this script
without entering the interactive mode (ie. without starting a new python
-terminal). In your shell, just type: bash terminal ....
-student@student-virtualbox:\~\$ ipython script.py >>> ....
+terminal). In your shell, just type:
+```
+student@student-virtualbox:~$ ipython script.py
+>>>
+```
-Pinocchio ~\~~\~~~\~
+### Pinocchio
-*Basic mathematical objects:*
+#### Basic mathematical objects:
-In the following, we will use numpy Matrix class to represent matrices
+In the following, we will use numpy `Matrix` class to represent matrices
and vectors. In numpy, vectors simply are matrices with one column. See
-the following example. \[source, python\] ---- import numpy as np A =
-np.matrix(\[ \[1,2,3,4\],\[5,6,7,8\]\]) \# Define a 2x4 matrix b =
-np.zeros(\[4,1\]) \# Define a 4 vector (ie a 4x1 matrix) initialized
-with 0. c = A\*b \# Obtain c by multiplying A by b. ----
+the following example.
-A bunch of useful functions are packaged in the utils of pinocchio.
-
-\[source,python\]
------------------
-
-from pinocchio.utils import \* eye(6) \# Return a 6x6 identity matrix
-zero(6) \# Return a zero 6x1 vector zero(\[6,4\]) \# Return az zero 6x4
-matrix rand(6) \# Random 6x1 vector isapprox(zero(6),rand(6)) \# Test
-epsilon equality mprint(rand(\[6,6\])) \# Matlab-style print
-skew(rand(3)) \# Skew "cross-product" 3x3 matrix from a 3x1 vector
-cross(rand(3),rand(3)) \# Cross product of R\^3 rotate('x',0.4) \# Build
-a rotation matrix of 0.4rad around X. ----
-
-Specific classes are defined to represent objects of SE(3), se(3) and
-se(3)\^\*. Rigid displacements, elements of SE(3), are represented by
-the class SE3.
-
-\[source,python\]
------------------
-
-import pinocchio as se3 R = eye(3); p = zero(3) M0 = se3.SE3(R,p) M =
-se3.SE3.Random() M.translation = p; M.rotation = R ----
-
-Spatial velocities, elements of se(3) = M\^6, are represented by the
-class Motion.
+```py
+import numpy as np
+A = np.matrix([[1, 2, 3, 4], [5, 6, 7, 8]]) # Define a 2x4 matrix
+b = np.zeros([4, 1]) # Define a 4 vector (ie a 4x1 matrix) initialized with 0
+c = A * b # Obtain c by multiplying A by b.
+```
-\[source,python\]
------------------
-
-v = zero(3); w = zero(3) nu0 = se3.Motion(v,w) nu = se3.Motion.Random()
-nu.linear = v; nu.angular = w ----
-
-Spatial forces, elements of se(3)\^\* = F\^6, are represented by the
-class Force.
-
-\[source,python\]
------------------
-
-f = zero(3); tau = zero(3) phi0 = se3.Force(f,tau) phi =
-se3.Force.Random() phi.linear = f; phi.angular = tau ----
-
-Tutorial 1.1. Creating and displaying the robot
------------------------------------------------
+A bunch of useful functions are packaged in the utils of pinocchio.
-Robot kinematic tree ~~~~\~~~~~~\~~~~\~~\~~
+```py
+from pinocchio.utils import *
+eye(6) # Return a 6x6 identity matrix
+zero(6) # Return a zero 6x1 vector
+zero([6, 4]) # Return az zero 6x4
+matrix rand(6) # Random 6x1 vector
+isapprox(zero(6), rand(6)) # Test epsilon equality
+mprint(rand([6, 6])) # Matlab-style print
+skew(rand(3)) # Skew "cross-product" 3x3 matrix from a 3x1 vector
+cross(rand(3), rand(3)) # Cross product of R^3
+rotate('x', 0.4) # Build a rotation matrix of 0.4rad around X.
+```
+
+Specific classes are defined to represent objects of \f$SE(3)\f$, \f$se(3)\f$ and
+\f$se(3)^*\f$. Rigid displacements, elements of \f$SE(3)\f$, are represented by
+the class `SE3`.
+
+```py
+import pinocchio as se3
+R = eye(3); p = zero(3)
+M0 = se3.SE3(R, p)
+M = se3.SE3.Random()
+M.translation = p; M.rotation = R
+```
+
+Spatial velocities, elements of \f$se(3) = M^6\f$, are represented by the
+class `Motion`.
+
+```py
+v = zero(3); w = zero(3)
+nu0 = se3.Motion(v, w)
+nu = se3.Motion.Random()
+nu.linear = v; nu.angular = w
+```
+
+Spatial forces, elements of \f$se(3)^* = F^6\f$, are represented by the
+class `Force`.
+
+```py
+f = zero(3); tau = zero(3)
+phi0 = se3.Force(f, tau)
+phi = se3.Force.Random()
+phi.linear = f; phi.angular = tau
+```
+
+## 1.1) Creating and displaying the robot
+
+### Robot kinematic tree
The kinematic tree is represented by two C++ objects called Model (which
contains the model constants: lengths, masses, names, etc) and Data
@@ -115,154 +119,162 @@ called RobotWrapper and is generic.
For the next steps, we are going to work with the RobotWrapper.
-Import the class +RobotWrapper+ and create an instance of this class in
+Import the class `RobotWrapper` and create an instance of this class in
the python terminal. At initialization, RobotWrapper will read the model
description in the URDF file given as argument. In the following, we
will use the model of the UR5 robot, available in the directory "models"
of pinocchio (available in the homedir of the VBox).
-\[source, python\]
-------------------
+```py
+from pinocchio.robot_wrapper import RobotWrapper
-from pinocchio.robot\_wrapper import RobotWrapper
+PKG = '/opt/openrobots/share'
+URDF = PKG + '/ur5_description/urdf/ur5_gripper.urdf'
+robot = RobotWrapper(URDF, [PKG])
+```
-pkg = '/home/student/models/' urdf = pkg +
-'/ur\_description/urdf/ur5\_gripper.urdf' robot =
-RobotWrapper(urdf,\[pkg,\]) ---- The code of the RobotWrapper class is
-in
-/opt/openrobots/lib/python2.7/site-packages/pinocchio/robot\_wrapper.py
-. Do not hesitate to have a look at it and to take inspiration from the
+The code of the RobotWrapper class is in
+`/opt/openrobots/lib/python2.7/site-packages/pinocchio/robot_wrapper.py`.
+Do not hesitate to have a look at it and to take inspiration from the
implementation of the class functions.
UR5 is a fixed robot with one 6-DOF arms developed by the Danish company
Universal Robot. All its 6 joints are revolute joints. Its configuration
-is in R\^6 and is not subject to any constraint. The model of UR5 is
+is in R^6 and is not subject to any constraint. The model of UR5 is
described in a URDF file, with the visuals of the bodies of the robot
being described as meshed (i.e. polygon soups) using the Collada format
-".dae". Both the URDF and the DAE files are available in the attached
-ZIP archive. Uncompressed it in the VirtualBox, for example in the
-directory "/home/student/models".
-
-- UR5 model and code
- link:http://homepages.laas.fr/nmansard/teach/supaero2017/ur5.zip\[*ur5.zip*\]
+".dae". Both the URDF and the DAE files are available in the package
+`robotpkg-ur5-description`
-Exploring the model ~~~\~~~~~~\~~~~\~~
+### Exploring the model
-The robot model is available in robot.model. It contains the names of
-all the robot joint \[names\], the kinematic tree \[parents\] (i.e. the
+The robot model is available in `robot.model`. It contains the names of
+all the robot joint `names`, the kinematic tree `parents` (i.e. the
graph of parents, 0 being the root and having no parents), the position
-of the current joint in the parent coordinate frame \[jointPosition\],
+of the current joint in the parent coordinate frame `jointPosition`,
the mass, inertia and center-of-gravity position of all the bodies
-(condensed in a spatial inertia 6x6 matrix) \[inertias\] and the gravity
-of the associated world \[gravity\]. All these functions are documented
+(condensed in a spatial inertia 6x6 matrix) `inertias` and the gravity
+of the associated world `gravity`. All these functions are documented
and are available in the correponding class dictionnary.
-\[source,python\]
------------------
+```py
+for name, function in robot.model.__class__.__dict__.items():
+ print(" \*\*\*\* ", name, ": ", function.__doc__)
+```
-for name,function in robot.model.**class**.**dict**.items(): print "
-\*\*\*\* ", name, ": ", function.**doc** ----
-
-Similarly, the robot data are available in robot.data. All the variables
+Similarly, the robot data are available in `robot.data`. All the variables
allocated by the classical rigid-body dynamics algorithms are stored in
-robot.data and are available through the python wrapping. Similarly to
+`robot.data` and are available through the python wrapping. Similarly to
the model object, the function are documented and are available from the
class dictionnary. The most useful in the following will be the
placement of the frame associated which each joint output stored in
-robot.data.oMi.
+`robot.data.oMi`.
For example, the robot end effector corresponds to the output of the
-last joint, called +wrist\_1\_joint+. The ID of the joint in the joint
+last joint, called `wrist_1_joint`. The ID of the joint in the joint
list can be recovered from its name, and then used to access its
placement:
-\[source,python\]
------------------
-
-Get index of end effector
-=========================
+```py
+# Get index of end effector
-idx = robot.index('wrist\_3\_joint')
+idx = robot.index('wrist_3_joint')
-Compute and get the placement of joint number idx
-=================================================
+# Compute and get the placement of joint number idx
-placement = robot.position(q,idx) \# Be carreful, Python always returns
-references to values. \# You can often .copy() the object to avoid side
-effects \# Only get the placement placement =
-robot.data.oMi\[idx\].copy()
-
-------------------------------------------------------------------------
+placement = robot.position(q, idx)
+# Be carreful, Python always returns references to values.
+# You can often .copy() the object to avoid side effects
+# Only get the placement
+placement = robot.data.oMi[idx].copy()
+```
Finally, some recurring datas (used in Model and Data) have been wrapped
to functions in some python shortcuts, also available in RomeoWrapper:
-+The size of the robot configuration is given by nq. +The dimension of
-its tangent space (velocity) is nv. +The index of a joint in the tree
-can be accessed from its name by index (see above). +The classical
-algorithms are also binded: com, Jcom, mass, biais, joint gravity,
-position and velocity of each joint.
-\[source,python\]
------------------
+- The size of the robot configuration is given by `nq`.
+- The dimension of its tangent space (velocity) is `nv`.
+- The index of a joint in the tree can be accessed from its name by index (see above).
+- The classical algorithms are also binded: com, Jcom, mass, biais, joint gravity, position and velocity of each joint.
-q = zero(robot.nq) v = rand(robot.nv) robot.com(q) \# Compute the robot
-center of mass. robot.position(q,3) \# Compute the placement of joint 3
-----
+```py
+q = zero(robot.nq)
+v = rand(robot.nv)
+robot.com(q) # Compute the robot center of mass.
+robot.position(q, 3) # Compute the placement of joint 3
+```
-Display the robot ~\~~~~~~\~~~~~~~\~
+### Display the robot
-To display the robot, we need an external program called +Gepetto
-Viewer+. If you completed the installation in the previous page, you can
+To display the robot, we need an external program called *Gepetto
+Viewer*. If you completed the installation in the previous page, you can
launch this program, open a new terminal in an empty workspace.
-.... student@student-virtualbox:\~\$ gepetto-gui .... This will start a
-server waiting for instructions. We will now create a client that will
+```
+student@student-virtualbox:~$ gepetto-gui
+```
+
+This will start a server waiting for instructions. We will now create a client that will
ask the server to perform some requests (such as creating a window or
displaying our robot)
In a python terminal you can now load the visual model of the robot in
-the viewer \[source,python\] ---- robot.initDisplay(loadModel=True) ----
+the viewer:
+
+```py
+robot.initDisplay(loadModel=True)
+```
+
This will flush the robot model inside the GUI. The argument
-"loadModel=True" is mandatory when you start or restart the GUI. In
-later call to your scripts, you can set the argument to "False". A side
-effector of "=True" is that it will move the viewpoint inside the GUI to
+`loadModel=True` is mandatory when you start or restart the GUI. In
+later call to your scripts, you can set the argument to `False`. A side
+effector of `=True` is that it will move the viewpoint inside the GUI to
a reference zero position.
-More details about loading the model (optionnal)
-~~~~~~~~~~~~~~~~\~~~~~~~~~~~~~~~~~~~~~~\~~~~~~~~
+### More details about loading the model (optionnal)
You can access the visual object composing the robot model by
-robot.visual\_model.geometryObject. \[source,python\] ---- visualObj =
-robot.visual\_model.geometryObjects\[4\] \# 3D object representing the
-robot forarm visualName = visualObj.name \# Name associated to this
-object visualRef = robot.viewerNodeNames(visualObj) \# Viewer reference
-(string) representing this object ----
+`robot.visual_model.geometryObject`.
+
+```py
+visualObj = robot.visual_model.geometryObjects[4] # 3D object representing the robot forarm
+visualName = visualObj.name # Name associated to this object
+visualRef = robot.viewerNodeNames(visualObj) # Viewer reference (string) representing this object
+```
-Moving one object \[source,python\] ---- q1 = \[1,1,1, 1,0,0,0\] \#
-x,y,z , quaternion robot.viewer.gui.applyConfiguration(visualRef,q1)
-robot.viewer.gui.refresh() \# Refresh the window. ----
+Moving one object
+
+```py
+q1 = [1, 1, 1, 1, 0, 0, 0] # x, y, z, quaternion
+robot.viewer.gui.applyConfiguration(visualRef, q1)
+robot.viewer.gui.refresh() # Refresh the window.
+```
Additional objects can be created, like a sphere as follows.
-\[source,python\] ---- rgbt = \[1.0,0.2,0.2,1.0\] \#
-red-green-blue-transparency robot.viewer.gui.addSphere("world/sphere",
-.1, rgbt) \# .1 is the radius ----
+
+```py
+rgbt = [1.0, 0.2, 0.2, 1.0] # red, green, blue, transparency
+robot.viewer.gui.addSphere("world/sphere", .1, rgbt) # .1 is the radius
+```
The exhaustive list of the object that can be created is available in
the IDL of the GUI:
-+/opt/openrobots/share/idl/gepetto/corbaserver/graphical-interface.idl+
+`/opt/openrobots/share/idl/gepetto/corbaserver/graphical-interface.idl`
-Tutorial 1.2. Simple pick and place
------------------------------------
+## 1.2) Simple pick and place
*Objectives:* Display the robot at a given configuration or along a
given trajectory
-Pick: ~\~~\~\~
+### Pick:
+
+Say we have a target at position `[.5, .1, .2]` and we would like the
+robot to grasp it.
-Say we have a target at position \[.5,.1,.2\] and we would like the
-robot to grasp it. \[source,python\] ----
-robot.viewer.gui.applyConfiguration("world/sphere",\[.5,.1,.2,
-1.,0.,0.,0.\]) robot.viewer.gui.refresh() \# Refresh the window. ----
+```py
+robot.viewer.gui.applyConfiguration("world/sphere", [.5, .1, .2, 1.,0.,0.,0. ])
+robot.viewer.gui.refresh() # Refresh the window.
+```
First display a small sphere at this position to visualize it.
@@ -270,15 +282,15 @@ Then decide by any mean you want a configuration of the robot so that
the end effector is touching the sphere.
At the reference position you built, the end effector placement can be
-obtained by robot.position(q,6). Only the translation part of the
+obtained by `robot.position(q, 6)`. Only the translation part of the
placement has been selected. The rotation is free.
-\[Optional\] Say now that the object is a rectangle and not a sphere.
+*Optional* Say now that the object is a rectangle and not a sphere.
Pick the object at a reference position with the rotation that is
imposed, so that the end effector is aligned with one of the faces of
the rectangle.
-Place: ~~\~~~\~
+### Place:
Choose any trajectory you want in the configuration space, starting from
the reference position built in the previous exercice (it can be
@@ -290,14 +302,3 @@ can be used to slow down the loop.
At each instant of your loop, recompute the position of the ball and
display it so that it always "sticks" to the robot end effector.
-
-//// Homework --------
-
-Send by mail at nmansard@laas.fr a mail containing a single python file.
-The subject of the mail should start with +\[SUPAERO\] TP1+ When
-executed, the script should place the robot at the initial starting
-position where the end effector touches the sphere (optionally the
-rectangle) and move the robot with the sphere attached to the end
-effector.
-
-////
diff --git a/doc/d-labs/2-invgeom.md b/doc/d-labs/2-invgeom.md
index dfc4dd783da71a0a1dfd65a321f9f8b058119c1f..4c1f89d4c2e2cefa414e3fff0739924d70fe88cd 100644
--- a/doc/d-labs/2-invgeom.md
+++ b/doc/d-labs/2-invgeom.md
@@ -1,8 +1,6 @@
-2. Grasp an object (aka inverse Geometry)
-=========================================
+# 2) Grasp an object (aka inverse Geometry)
-Objectives
-----------
+## Objectives
The main objective of the first tutorial is to compute a configuration
of the robot minimizing a cost (maximizing a reward) and respecting some
@@ -10,16 +8,15 @@ given constraints. Additionally, the objective is to have a first hands
on the difficulties of working outside of real vector spaces and to
consider what are the guesses that are taken by an optimal solver.
-Tutorial 2.0. Technical prerequisites
--------------------------------------
+## 2.0) Technical prerequisites
-Python SciPy and MatplotLib ~~~~~~~~~~~\~~~~~~~~~~~~\~~\~~
+### Python SciPy and MatplotLib
-You will need the two libraries Python +SciPy+ (scientific Python) and
-+MatPlotLib+ (plot mathematical data).
+You will need the two Python libraries: *SciPy* (scientific Python) and
+*MatPlotLib* (plot mathematical data).
-SciPy can be installed by +sudo apt-get install python-scipy+. It is
-already installed if you are using the VirtualBox. Examples of calls of
+SciPy can be installed by `sudo apt-get install python-scipy`.
+Examples of calls of
these two functions are given below. We will use both solvers with
numerical (finite-differencing) differenciation, to avoid the extra work
of differencing the cost and constraint functions by hand. In general,
@@ -35,188 +32,228 @@ be added as penalty functions in the cost, but this requires additional
work). The constrained least-square implementation allows the user to
specify equality and inequality constraints, but not the callback. In
the following, start to use BFGS before moving to the constrained
-least-square only when constraints are really needed. \[source,python\]
----- \# Example of use a the optimization toolbox of SciPy. import numpy
-as np from scipy.optimize import fmin\_bfgs, fmin\_slsqp
+least-square only when constraints are really needed.
-def cost(x): '''Cost f(x,y) = x\^2 + 2y\^2 - 2xy - 2x ''' x0 = x\[0\] x1
-= x\[1\] return -1*(2*x0*x1 + 2*x0 - x0\*\*2 - 2\*x1\*\*2)
+```py
+# Example of use a the optimization toolbox of SciPy.
-def constraint\_eq(x): ''' Constraint x\^3 = y ''' return np.array(\[
-x\[0\]\*\*3-x\[1\] \])
+import numpy as np
+from scipy.optimize import fmin_bfgs, fmin_slsqp
-def constraint\_ineq(x): '''Constraint x>=2, y>=2''' return
-np.array(\[ x\[0\]-2,x\[1\]-2 \])
+def cost(x):
+ '''Cost f(x, y) = x² + 2y² - 2xy - 2x '''
+ x0, x1 = x
+ return -(2 * x0 * x1 + 2 * x0 - x0 ** 2 - 2 * x1 ** 2)
-class CallbackLogger: def **init**(self): self.nfeval = 1 def
-**call**(self,x): print '===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3:
-3.6f}'.format(self.nfeval, x\[0\], x\[1\], cost(x)) self.nfeval += 1
+def constraint_eq(x):
+ ''' Constraint x³ = y '''
+ return np.array([ x[0] ** 3 - x[1] ])
-x0 = np.array(\[0.0,0.0\]) \# Optimize cost without any constraints in
-BFGS, with traces. xopt\_bfgs = fmin\_bfgs(cost, x0,
-callback=CallbackLogger()) print '\n *\*\* Xopt in BFGS =
-',xopt\_bfgs,'\n\n\n\n'
+def constraint_ineq(x):
+ '''Constraint x >= 2, y >= 2'''
+ return np.array([ x[0] - 2, x[1] - 2 ])
-Optimize cost without any constraints in CLSQ
-=============================================
+class CallbackLogger:
+ def __init__(self):
+ self.nfeval = 1
-xopt\_lsq = fmin\_slsqp(cost,\[-1.0,1.0\], iprint=2, full\_output=1)
-print '\n *\*\* Xopt in LSQ = ',xopt\_lsq,'\n\n\n\n'
+ def __call__(self,x):
+ print('===CBK=== {0:4d} {1: 3.6f} {2: 3.6f} {3: 3.6f}'.format(self.nfeval, x[0], x[1], cost(x)))
+ self.nfeval += 1
-Optimize cost with equality and inequality constraints in CLSQ
-==============================================================
+x0 = np.array([0.0, 0.0]) # Optimize cost without any constraints in BFGS, with traces.
+xopt_bfgs = fmin_bfgs(cost, x0, callback=CallbackLogger())
+print('*** Xopt in BFGS =', xopt_bfgs)
-xopt\_clsq = fmin\_slsqp(cost,\[-1.0,1.0\], f\_eqcons=constraint\_eq,
-f\_ieqcons=constraint\_ineq, iprint=2, full\_output=1) print '\n *\*\*
-Xopt in c-lsq = ',xopt\_clsq,'\n\n\n\n' ---- Take care that all +SciPy+
+# Optimize cost without any constraints in CLSQ
+
+xopt_lsq = fmin_slsqp(cost, [-1.0, 1.0], iprint=2, full_output=1)
+print('*** Xopt in LSQ =', xopt_lsq)
+
+# Optimize cost with equality and inequality constraints in CLSQ
+
+xopt_clsq = fmin_slsqp(cost, [-1.0, 1.0], f_eqcons=constraint_eq, f_ieqcons=constraint_ineq, iprint=2, full_output=1)
+print('*** Xopt in c-lsq =', xopt_clsq)
+```
+
+Take care that all *SciPy*
always works with vectors represented as 1-dimensional array, while
Pinocchio works with vectors represented as matrices (which are in fact
two-dimensional arrays, with the second dimension being 1). You can pass
from a SciPy-like vector to a Pinocchio-like vector using:
-\[source,python\] ---- import numpy as np x = np.array(\[ 1.0, 2.0, 3.0
-\]) q = np.matrix(x).T x = q.getA()\[:,0\] ----
-
-The second library +MatPlotLib+ plots values on a 2D graph. Once more,
-documentation is available here:
-https://www.labri.fr/perso/nrougier/teaching/matplotlib/ An example is
-provided below.
-\[source, python\]
-------------------
+```py
+import numpy as np
+x = np.array([1.0, 2.0, 3.0])
+q = np.matrix(x).T
+x = q.getA()[:, 0]
+```
-import numpy as np import matplotlib.pyplot as plt \# In plt, the
-following functions are the most useful: \#
-ion,plot,draw,show,subplot,figure,title,savefig
+The second library *MatPlotLib* plots values on a 2D graph.
+A tutorial is available [here](https://www.labri.fr/perso/nrougier/teaching/matplotlib/).
+An example is provided below.
-For use in interactive python mode (ipthyon -i)
-===============================================
+```py
+import numpy as np
+import matplotlib.pyplot as plt
+# In plt, the following functions are the most useful:
+# ion, plot, draw, show, subplot, figure, title, savefig
+# For use in interactive python mode (ipthyon -i)
interactivePlot = False
-if interactivePlot: plt.ion() \# Plot functions now instantaneously
-display, shell is not blocked
+if interactivePlot:
+ plt.ion() # Plot functions now instantaneously display, shell is not blocked
-Build numpy array for x axis
-============================
+# Build numpy array for x axis
+x = 1e-3 * np.array(range(100))
+# Build numpy array for y axis
+y = x ** 2
-x = 1e-3 \* np.array (range (100)) \# Build numpy array for y axis y =
-x\*\*2
+fig = plt.figure()
+ax = fig.add_subplot('111')
+ax.plot(x, y)
+ax.legend(("x^2", ))
-fig = plt.figure () ax = fig.add\_subplot ('111') ax.plot (x, y)
-ax.legend (("x\^2",))
+if not interactivePlot:
+ # Display all the plots and block the shell.
+ # The script will only ends when all windows are closed.
+ plt.show()
+```
-if not interactivePlot: \# Display all the plots and block the shell. \#
-The script will only ends when all windows are closed. plt.show () ----
+### Robots
-Robots ~~\~~~\~
-
-We mostly use here the model UR5, used in the first tutorial. Refer to
-the instructions of Tuto 1 to load it.
+We mostly use here the model UR5, used in the first lab. Refer to
+the instructions of Lab 1 to load it.
Optionally, we might want to use a more complex robot model. Romeo is a
humanoid robot developed by the French-Japanese company Aldebaran
Robotics. It has two legs, two arms and a head, for a total of 31 joints
-(plus 6DOF on the free flyer). Its model is natively in Pinocchio (no
-additional dowload needed). Romeo can be loaded with: \[source,python\]
----- import pinocchio as se3 from pinocchio.romeo\_wrapper import
-RomeoWrapper path =
-'/home/nmansard/src/pinocchio/pinocchio/models/romeo/' urdf = path +
-'urdf/romeo.urdf' \# Explicitly specify that the first joint is a free
-flyer. robot = RomeoWrapper(urdf,\[path,\]) \# Load urdf model
-robot.initDisplay(loadModel=True) ---- Additionally, the index of right
-and left hands and feet are stored in romeo.rh, romeo.lh, romeo.rf and
-romeo.lf.
-
-Tutorial 2.1. Position the end effector
----------------------------------------
+(plus 6DOF on the free flyer). The description of Romeo can be obtained with:
+
+```
+sudo apt install robotpkg-romeo-description
+```
+
+Romeo can be loaded with:
+
+```py
+import pinocchio as se3
+from pinocchio.romeo_wrapper import RomeoWrapper
+
+PKG = '/opt/openrobots/share'
+URDF = PKG + 'romeo_description/urdf/romeo.urdf'
+
+robot = RomeoWrapper(URDF, [PKG]) # Load urdf model
+robot.initDisplay(loadModel=True)
+```
+
+Additionally, the index of right
+and left hands and feet are stored in `robot.rh`, `robot.lh`, `robot.rf` and
+`robot.lf`.
+
+## 2.1) Position the end effector
The first tutorial is to position (i.e. translation only) the end
effector of a manipulator robot to a given position. For this first
part, we will use the fixed serial-chain robot model.
-Recall first that the position (3D) of the joint with index "i" at
-position "q" can be access by the following two lines of code:
-\[source,python\] ---- \# Compute all joint placements and put the
-position of joint "i" in variable "p". import pinocchio as se3
-se3.forwardKinematics(robot.model,robot.data,q) p =
-robot.data.oMi\[i\].translation ----
+Recall first that the position (3D) of the joint with index `i` at
+position `q` can be access by the following two lines of code:
-*Question 1* Using this, build a cost function to be the norm of the
-difference between the end-effector position +p+ and a desired position
-+pdes+. The cost function is a function that accepts as input an
+```py
+# Compute all joint placements and put the position of joint "i" in variable "p".
+import pinocchio as se3
+se3.forwardKinematics(robot.model, robot.data, q)
+p = robot.data.oMi[i].translation
+```
+
+#### Question 1
+Using this, build a cost function to be the norm of the
+difference between the end-effector position `p` and a desired position
+`pdes`. The cost function is a function that accepts as input an
1-dimensional array and return a float.
-*Question 2* Then use +fmin\_bfgs+ to find a configuration q with the
-end effector at position +pdes+.
+#### Question 2
+Then use `fmin_bfgs` to find a configuration `q` with the
+end effector at position `pdes`.
-*Question 3* Finally, implements a callback function that display in
+#### Question 3
+Finally, implements a callback function that display in
Gepetto-Viewer every candidate configuration tried by the solver.
-Tutorial 2.2. Approaching the redundancy (optionnal)
-----------------------------------------------------
+## 2.2) Approaching the redundancy (optionnal)
The manipulator arm has 6 DOF, while the cost function only constraints
3 of them (the position of the end effector). A continuum of solutions
then exists. The two next questions are aiming at giving an intuition of
this continuum.
-*Question 4* Sample several configurations respecting +pdes+ by giving
+#### Question 4
+Sample several configurations respecting `pdes` by giving
various initial guesses to the solver. Store this sampling of solutions
in a list, then display this list in Gepetto-Viewer, each configuration
begin displayed during 1 second (pause of 1 seconds can be obtained
-using: import time; time.sleep(1.0)).
+using: `import time; time.sleep(1))`.
A configurations in this continuum can then be selected with particular
properties, like for example being the closest to a reference
configuration, or using some joints more than the others, or any other
criterion that you can imagine.
-*Question 5* Sum a secondary cost term to the first positioning cost, to
+#### Question 5
+Sum a secondary cost term to the first positioning cost, to
select the posture that maximizes the similarity (minimizes the norm of
the difference) to a reference posture. The relative importance of the
two cost terms can be adjusted by weighting the sum: find the weight so
that the reference position is obtained with a negligible error (below
millimeter) while the posture is properly taken into account.
-Tutorial 2.3. Placing the end-effector
---------------------------------------
+## 2.3) Placing the end-effector
The next step is to find a configuration of the robot so that the end
effector respects a reference placement, i.e. position and orientation.
-The stake is to find a metric in SE(3) to continuously quantify the
-distance between two placements. There is no canonical metric in SE(3),
+The stake is to find a metric in \f$SE(3)\f$ to continuously quantify the
+distance between two placements. There is no canonical metric in \f$SE(3)\f$,
i.e. no absolute way of weighting the position with respect to the
-orientation. Two metrics can be considered, namely the log in SE(3) or
-in R\^3 x SE(3). The tutorial will guide you through the first choice.
+orientation. Two metrics can be considered, namely the log in \f$SE(3)\f$ or
+in \f$R^3 \times SO(3)\f$. The tutorial will guide you through the first choice.
+
+The \f$SE(3)\f$ and \f$SO(3)\f$ logarithm are implemented in Pinocchio in the `explog`
+module.
-The SE(3) and SO(3) logarithm are implemented in Pinocchio in the explog
-module. \[source,python\] ---- from pinocchio.explog import log from
-pinocchio import SE3 nu = log(SE3.Random()) nu\_vec = nu.vector ----
+```py
+from pinocchio.explog import log
+from pinocchio import SE3
+nu = log(SE3.Random())
+nu_vec = nu.vector
+```
-*Question 6* Solve for the configuration that minimizes the norm of the
+#### Question 6
+Solve for the configuration that minimizes the norm of the
logarithm of the difference between the end effector placement and the
desired placement.
-Optionally, try other metrics, like the log metric of R\^3 x SO(3), or
+Optionally, try other metrics, like the log metric of \f$R^3 \times SO(3)\f$, or
the Froebenius norm of the homogeneous matrix.
-Tutorial 2.4. Working with a mobile robot (optionnal)
------------------------------------------------------
+## 2.4) Working with a mobile robot (optionnal)
Until now, the tutorial only worked with a simple manipulator robot,
i.e. whose configuration space is a real vector space. Consider now the
humanoid robot, whose first joint is a free joint: it has 6 degrees of
freedom (3 rotations, 3 translations) but its configuration vector is
-dimension 7. You can check it with +robot.model.nq+, that stores the
-dimension of the configuration, and +robot.model.nv+, that stores the
+dimension 7. You can check it with `robot.model.nq`, that stores the
+dimension of the configuration, and `robot.model.nv`, that stores the
dimension of the configuration velocity, i.e. the number of degrees of
-freedom. For the humanoid, nq = nv+1.
+freedom. For the humanoid, `nq = nv + 1`.
Indeed, the configuration coefficients 3 to 7 are indeed representing a
quaternion. The additional constraint is that these 4 coefficients must
be normalize.
-*Question 7* Display a configuration of the robot for which the norm of
+#### Question 7
+Display a configuration of the robot for which the norm of
the quaternion is bigger than one (e.g. 2.0). What happens?
During the search, the solver must respect this constraint. A solution
@@ -228,15 +265,16 @@ to the end-effector placement. An additional term should also be added
to the cost function to avoid excessive drift of the quaternion norm, in
particular with the norm going to 0.
-*Question 8* Use +fmin\_bfgs+ to compute a configuration respecting a
+#### Question 8
+Use `fmin_bfgs` to compute a configuration respecting a
given placement with the humanoid model, by normalizing the quaternion
at each step.
-*Question 9* (harder) Do the same with the solver C-LSQ +fmin\_slsqp+,
+#### Question 9 (harder)
+Do the same with the solver C-LSQ `fmin_slsqp`,
with the explicit constraint that the norm of the quaternion must be 1.
-Tutorial 2.5. Configuration of a parallel robot
------------------------------------------------
+## 2.5) Configuration of a parallel robot
A parallel robot is composed of several kinematic chains (called the
robot legs) that are all attached to the same end effector. This imposes
@@ -250,21 +288,14 @@ The kinematic structure of a paralel robot indeed induces loops in the
joint connection graph. In Pinocchio, we can only represents (one of)
the underlying kinematic tree. The loop constraints have to be handled
separately. An example that loads 4 manipulator arms is
-link:tp2\_ur5x4\_py.html\[available here\]. Each leg i (for i=0,1,2,3)
-of the robot is loaded in the list robots\[i\]. The loop constraints are
+[available here](ur5x4_8py_source.html). Each leg i (for i=0,1,2,3)
+of the robot is loaded in the list `robots[i]`. The loop constraints are
that the relative placement of every leg end-effector must stay the same
that in the initial configuration given as example in the above file.
-*Question 10* Consider now that the orientation of the tool plate is
-given by the quaternion Quaternion(0.7,0.2,0.2,0.6), with the
+#### Question 10
+Consider now that the orientation of the tool plate is
+given by the quaternion `Quaternion(0.7, 0.2, 0.2, 0.6)`, with the
translation that you like. Find using the above optimization routines
the configuration of each robot leg so that the loop constraints are all
met.
-
-//// Homework --------
-
-Send by mail at nmansard@laas.fr a mail containing a single python file.
-The subject of the mail should start with +\[SUPAERO\] TP2+ When
-executed, the script should place the parallel robot toolplate at the
-reference placement (whose rotation is Quaternion(0.7,0.2,0.2,0.6)) and
-then move the legs to meet the loop constraints. ////
diff --git a/doc/d-labs/3-invkine.md b/doc/d-labs/3-invkine.md
index 1891d17fb75061576c95b40e4d1e7fa4b23c9f7a..2d71f8d1c26b83aee5c1f34f776b0126d30beee3 100644
--- a/doc/d-labs/3-invkine.md
+++ b/doc/d-labs/3-invkine.md
@@ -1,139 +1,151 @@
-3. Drag and Drop (aka Inverse kinematics)
-=========================================
+# 3) Drag and Drop (aka Inverse kinematics)
-Objectives
-----------
+## Objectives
The main objective of the tutorial is to perform one or several tasks by
inverse kinematics, i.e. pseudo inversing a jacobian iteratively until
convergence of the task error.
-Tutorial 3.0. Technical prerequisites
--------------------------------------
+## 3.0) Technical prerequisites
-Robots ~~\~~~\~
+### Robots
We are going to use again the UR5 robot model, however this time mounted
-as a mobile robot. The source code of the mobile robot is available
-link:tp3\_mobilerobot\_py.html\[here\]. The robot has 3+6 DOF and can
+as a mobile robot. The source code of the mobile robot is
+[available here](mobilerobot_8py_source.html).
+The robot has 3+6 DOF and can
move (2 translations + 1 rotation) freely on the plane. Two operation
frames have been defined: at the front of the basis, and at the tip of
the tool. They are displayed when the robot moves.
Example of how to use the robot is has below.
-\[source,python\]
------------------
+```py
+import pinocchio as se3
+from mobilerobot import MobileRobotWrapper
+from pinocchio.utils import *
-import pinocchio as se3 from mobilerobot import MobileRobotWrapper from
-pinocchio.utils import \*
+PKG = '/opt/openrobots/share'
+URDF = PKG + '/ur5_description/urdf/ur5_gripper.urdf'
-pkg = '/home/student/models/' urdf = pkg +
-'ur\_description/urdf/ur5\_gripper.urdf'
-
-robot = MobileRobotWrapper(urdf,\[pkg,\])
+robot = MobileRobotWrapper(URDF, [PKG])
robot.initDisplay(loadModel=True)
-robot.viewer.gui.addFloor('world/floor')
-========================================
+# robot.viewer.gui.addFloor('world/floor')
-NQ = robot.model.nq NV = robot.model.nv
+NQ, NV = robot.model.nq, robot.model.nv
-q = rand(NQ) robot.display(q)
+q = rand(NQ)
+robot.display(q)
-IDX\_TOOL = 24 IDX\_BASIS = 23
+IDX_TOOL = 24
+IDX_BASIS = 23
-se3.framesKinematics(robot.model,robot.data) Mtool =
-robot.data.oMf\[IDX\_TOOL\] Mbasis = robot.data.oMf\[IDX\_BASIS\] ----
+se3.framesKinematics(robot.model, robot.data)
+Mtool = robot.data.oMf[IDX_TOOL]
+Mbasis = robot.data.oMf[IDX_BASIS]
+```
-Tutorial 3.1. Position the end effector
----------------------------------------
+## 3.1) Position the end effector
The first task will be concerned with the end effector. First define a
goal placement.
-\[source,python\]
------------------
+```py
+def place(name, M):
+ robot.viewer.gui.applyConfiguration(name, se3ToXYZQUAT(M))
+ robot.viewer.gui.refresh()
-from mobilerobot import M2v def place(name,M):
-robot.viewer.gui.applyConfiguration(name,M2v(M))
-robot.viewer.gui.refresh()
+def Rquat(x, y, z, w):
+ q = se3.Quaternion(x, y, z, w)
+ q.normalize()
+ return q.matrix()
-def Rquat(x,y,z,w): q = se3.Quaternion(x,y,z,w) q.normalize() return
-q.matrix()
+Mgoal = se3.SE3(Rquat(.4, .02, -.5, .7), np.matrix([.2, -.4, .7]).T)
+robot.viewer.gui.addXYZaxis('world/framegoal', [1., 0., 0., 1.], .015, 4)
+place('world/framegoal', Mgoal)
+```
-Mgoal = se3.SE3(Rquat(0.4,0.02, -0.5,0.7), np.matrix(\[.2,-.4,.7\]).T)
-robot.viewer.gui.addXYZaxis('world/framegoal',\[1.,0.,0.,1.\],.015,4)
-place('world/framegoal',Mgoal) ----
+The current placement of the tool at configuration `q` is available as
+follows:
-The current placement of the tool at configuration +q+ is available as
-follows: \[source,python\] ---- IDX\_TOOL = 24
-se3.forwardKinematics(robot.model,robot.data,q) \# Compute joint
-placements se3.framesKinematics(robot.model,robot.data) \# Also compute
-operational frame placements Mtool = robot.data.oMf\[IDX\_TOOL\] \# Get
-placement from world frame o to frame f oMf ----
+```py
+IDX_TOOL = 24
+se3.forwardKinematics(robot.model, robot.data, q) # Compute joint placements
+se3.framesKinematics(robot.model, robot.data) # Also compute operational frame placements
+Mtool = robot.data.oMf[IDX_TOOL] # Get placement from world frame o to frame f oMf
+```
The desired velocity of the tool in tool frame is given by the log:
-\[source,python\] ---- nu = se3.log(Mtool.inverse()\*Mgoal).vector ----
+
+```py
+nu = se3.log(Mtool.inverse() * Mgoal).vector
+```
The tool Jacobian, also in tool frame, is available as follows:
-\[source,python\]
------------------
+```py
+J = se3.frameJacobian(robot.model, robot.data, IDX_TOOL, q)
+```
-J = se3.frameJacobian(robot.model,robot.data,q,IDX\_TOOL,True,True)
--------------------------------------------------------------------
+Pseudoinverse operator is available in `numpy.linalg` toolbox.
-Pseudoinverse operator is available in +numpy.linalg+ toolbox.
-\[source,python\] ---- from numpy.linalg import pinv ----
+```py
+from numpy.linalg import pinv
+```
-The integration of joint velocity vq in configuration q can be done
-directly (q += vq dt). More generically, the se3 method integrate can be
+The integration of joint velocity `vq` in configuration `q` can be done
+directly (`q += vq * dt`). More generically, the se3 method integrate can be
used:
-\[source,python\]
------------------
+```py
+q = se3.integrate(robot.model, q, vq * dt)
+```
-q = se3.integrate(robot.model,q,vq\*dt) ----
+#### Question 1
-*Question 1* Implement a for-loop that computes the jacobian and the
+Implement a for-loop that computes the jacobian and the
desired velocity in tool frame, and deduced the joint velocity using the
pseudoinverse. At each iteration, also integrate the current velocity
and display the robot configuration.
-Tutorial 3.2. Position the basis on a line
-------------------------------------------
+## 3.2) Position the basis on a line
A line displaying "x=0" is also displayed in Gepetto viewer. Next step
is to servo the front of the basis on this line.
Similarly, the distance of the basis frame to the line, with
-corresponding jacobian, are: \[source,python\] ---- error =
-Mbasis.translation\[0\] J =
-se3.frameJacobian(robot.model,robot.data,q,IDX\_BASIS,False,True)\[0,:\]
-----
+corresponding jacobian, are:
+
+```py
+error = Mbasis.translation[0]
+J = se3.frameJacobian(robot.model, robot.data, IDX_BASIS, q)[0, :]
+```
Implement a second loop to servo the basis on the line. It becomes
interesting when both tasks are performed together. We can do that
-simply by summing both tasks. For that, the numpy method +hstack+ can be
+simply by summing both tasks. For that, the numpy method `vstack` can be
used to make a single error vector stacking the errors of tool and basis
-tasks, and similarly for the jacobians. \[source,python\] ---- nu =
-np.vstack(\[nu1, nu2\]) J = np.vstack(\[J1, J2\]) ----
+tasks, and similarly for the jacobians.
+
+```py
+nu = np.vstack([nu1, nu2])
+J = np.vstack([J1, J2])
+```
However, it is stronger to move the basis only in the null space of the
-basis. The null space projector of +J1+ can be computed using the
+basis. The null space projector of `J1` can be computed using the
pseudoinverse. Following the control law performing task 1 and task 2 in
the null space of task 1 is:
-\["latex"\] $vq_1 = J_1^+ v_1^*$ \["latex"\] $P_1 = I_9 - J_1^+ J_1$
-\["latex"\] $vq_2 = vq_1 + (J_2 P_1)^+ ( v_2^* - J_2 vq_1)$
+\f$vq_1 = J_1^+ v_1^*\f$
-*Question 2* Implement two loops: the first one regulate the tool
-placement alone. When the tool is properly placed, the second regulate
-the tool placement and the basis position in the null-space of the tool.
+\f$P_1 = I_9 - J_1^+ J_1\f$
+
+\f$vq_2 = vq_1 + (J_2 P_1)^+ ( v_2^* - J_2 vq_1)\f$
-//// Homework --------
+#### Question 2
-Send by mail at nmansard@laas.fr a mail containing a single python file.
-The subject of the mail should start with +\[SUPAERO\] TP3+ When
-executed, the script should execute question 2. ////
+Implement two loops: the first one regulate the tool
+placement alone. When the tool is properly placed, the second regulate
+the tool placement and the basis position in the null-space of the tool.
diff --git a/doc/d-labs/4-dyn.md b/doc/d-labs/4-dyn.md
index b9d1f1f77e9ef733e3864b13e6961afc455a2cfb..2aa67858d551e1211cf325ccb90b568fb3be9b59 100644
--- a/doc/d-labs/4-dyn.md
+++ b/doc/d-labs/4-dyn.md
@@ -1,102 +1,108 @@
-4. Snap your fingers (aka direct and inverse dynamics)
-======================================================
+# 4) Snap your fingers (aka direct and inverse dynamics)
-Objectives
-----------
+## Objectives
The main objective of the tutorial is to implement a simple torque
control inside a home-made contact simulator.
-Tutorial 4.0. Technical prerequisites
--------------------------------------
+## 4.0) Technical prerequisites
-Robots ~~\~~~\~
+### Robots
We are going to use a 4-finger hand, whose model is defined in Python
(no urdf model) using capsule volumes. The code of the robot is
-available in the link:hand\_robot.zip\[ZIP archive\]. It contains a
-+display+ class wrapping the Gepetto-viewer client, a +robot+ class
-implementing the robot hand and a simple example +hand\_example.py+.
+[available here](robot__hand_8py_source.html). It needs a
+`Display` class wrapping the Gepetto-viewer client [available here](display_8py_source.html),
+and contains a `Robot` class implementing the robot hand and a simple example si [available
+here](hand__example__8py_source.html).
-\[source,python\]
------------------
+```py
+from robot_hand import Robot
-from robot\_hand import Robot
-
-robot = Robot() robot.display(robot.q0) ----
+robot = Robot()
+robot.display(robot.q0)
+```
Take care that the hand is small: zoom in to see it in the window (or
press the space bar).
-Solver ~~\~~~\~
+### Solver
We will need a proper QP solver with inequality. QuadProg is a Python
wrap of a nice Golub-based solver. Install it with PIP
-\[source,python\]
------------------
-
-pip install quadprog
---------------------
+```
+pip install --user quadprog
+```
-QuadProg main function is solve\_qp. You have a bit of documentation
-using the Python help command +help(solve\_qp)+. A simple example
+QuadProg main function is `solve_qp`. You have a bit of documentation
+using the Python help command `help(solve_qp)`. A simple example
follows:
-\[source,python\]
------------------
+```py
+from quadprog import solve_qp
-from quadprog import solve\_qp
+# Solve min_x .5 xHx - gx s.t. Cx <= d
+x, _, _, _, _, _ = solve_qp(H, g, C, d)
+```
-Solve min\_x 1/2\*xHx - gx s.t Cx <= d x,*,*,*,*,\_ = solve\_qp( H,g,C,d ) ----
-==================================================================================
+## 4.1. Direct dynamics
-A more complete example is also in the archive given above.
+Choosing an arbitrary joint torque \f$\tau_q\f$, we now compute the robot
+acceleration and integrate it.
-Tutorial 4.1. Direct dynamics
------------------------------
+The dynamic equation of the robot is \f$M a_q + b = \tau_q\f$, with \f$M\f$ the mass,
+\f$a_q\f$ the joint acceleration and \f$b\f$ the drift. The mass matrix can be
+computed using `CRB` algorithm (function of \f$q\f$). The drift is computed
+using `RNE` algorithm (function of \f$q\f$, \f$v_q\f$ and \f$a_q\f$ with \f$a_q=0\f$).
-Choosing an arbitrary joint torque tauq, we now compute the robot
-acceleration and integrate it.
+```py
+import pinocchio as se3
-The dynamic equation of the robot is M aq + b = tauq, with M the mass,
-aq the joint acceleration and b the drift. The mass matrix can be
-computed using +CRB+ algorithm (function of q). The drift is computed
-using +RNE+ algorithm (function of q, vq and aq with aq=0).
-\[source,python\] ---- import pinocchio as se3 q = rand(robot.model.nq)
-vq = rand(robot.model.nv) aq0 = zero(robot.model.nv) b =
-se3.rnea(robot.model,robot.data,q,vq,aq0) \# compute dynamic drift --
-Coriolis, centrifugal, gravity M = se3.crba(robot.model,robot.data,q) \#
-compute mass matrix M ----
+q = rand(robot.model.nq)
+vq = rand(robot.model.nv)
+aq0 = zero(robot.model.nv)
+# compute dynamic drift -- Coriolis, centrifugal, gravity
+b = se3.rnea(robot.model, robot.data, q, vq, aq0)
+# compute mass matrix M
+M = se3.crba(robot.model, robot.data, q)
+```
These terms correspond to the inverse dynamics. They can be numerically
inverted to compute the direct dynamics.
-*Question 1* Using M and b computed by the above algorithms, and knowing
-a given set of joint torques tauq, compute aq so that M\*aq+b = tauq.
+#### Question 1
+Using \f$M\f$ and \f$b\f$ computed by the above algorithms, and knowing
+a given set of joint torques \f$\tau_q\f$, compute \f$a_q\f$ so that \f$M*a_q+b = \tau_q\f$.
-Once aq as been computed, it is straight forward to integrate it to
-velocity using vq += aq*dt. Integration to joint position is more
-complex in general. It is implemented in pinocchio: \[source,python\]
----- q = se3.integrate(robot.model,q,vq*dt) ---- In the particular case
-of only simple joints (like the robot hand), the same integration
-q+=vq\*Dt also holds.
+Once \f$a_q\f$ as been computed, it is straight forward to integrate it to
+velocity using \f$v_q += a_q * dt\f$. Integration to joint position is more
+complex in general. It is implemented in pinocchio:
-*Question 2* Implement the simulation of the robot hand moving freely
+```py
+q = se3.integrate(robot.model, q, vq * dt)
+```
+
+In the particular case of only simple joints (like the robot hand), the same integration
+\f$q += v_q * dt\f$ also holds.
+
+#### Question 2
+Implement the simulation of the robot hand moving freely
with constant (possibly 0) torques. Implement a variation where the
-torques are only joint friction (tauq = -Kf vq at each iteration).
+torques are only joint friction (\f$\tau_q = -K_f v_q\f$ at each iteration).
-Tutorial 4.2. PD and computed torques
--------------------------------------
+## 4.2) PD and computed torques
Now choose a reference joint position (possibly time varying, like in
the hand example). The joint torques can then be computed to track the
-desired position, with tauq = -Kp (q-qdes) - Kv vq. Both gains Kp and Kv
-should be properly chosen. Optimal tracking is obtained with Kv = 2
-sqrt(Kp). In general, a desired velocity is also tracked to avoid
+desired position, with \f$\tau_q = -K_p (q-q_{des}) - K_v v_q\f$.
+Both gains \f$K_p\f$ and \f$K_v\f$
+should be properly chosen. Optimal tracking is obtained with
+\f$K_v = 2 \sqrt{K_p}\f$. In general, a desired velocity is also tracked to avoid
tracking errors.
-*Question 3* Implement then simulate a PD, by compute the torques from a
+#### Question 3
+Implement then simulate a PD, by compute the torques from a
PD law, then integrate it using the simulator of question 2.
Here, there is a strong coupling between joints, due to the mass matrix
@@ -110,13 +116,13 @@ is done with the perfect M, while the control is computed with
approximate M (for example, using only the diagonal terms of the mass
matrix). Let's rather simulate contact.
-Tutorial 4.3. Collision checking
---------------------------------
+## 4.3) Collision checking
The robot hand is composed of capsules, i.e. level-set of constant
distance to a segment. Collision checking and distances are then easy to
implement. The source code of collision checking is available in the
-robot\_hand.py. Pinocchio also implement a complete and efficient
+[robot_hand.py](robot__hand_8py_source.html) file.
+Pinocchio also implement a complete and efficient
collision checking based on FCL, also not used in the tutorial.
Collision checking are done for a set of collision pairs that must be
@@ -124,43 +130,44 @@ specified to the robot. The collision checking method indeed compute the
distance between the two objects, along with the so-called witness
points. A method can also be used to display them.
-\[source,python\]
------------------
-
-from robot\_hand import Robot robot = Robot() robot.display(robot.q0)
+```py
+from robot_hand import Robot
+robot = Robot()
+robot.display(robot.q0)
-Create 10 witness points in the rendering window
-================================================
+# Create 10 witness points in the rendering window
for i in range(10):
-robot.viewer.viewer.gui.addCylinder('world/wa'+str(i), .01, .003, \[
-1.0,0,0,1\]) robot.viewer.viewer.gui.addCylinder('world/wb'+str(i), .01,
-.003, \[ 1.0,0,0,1\])
-robot.viewer.viewer.gui.setVisibility('world/wa'+str(i),'OFF')
-robot.viewer.viewer.gui.setVisibility('world/wb'+str(i),'OFF')
+ robot.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003, [1, 0, 0, 1])
+ robot.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003, [1, 0, 0, 1])
+ robot.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
+ robot.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
-Add 4 pairs between finger tips and palm
-========================================
+# Add 4 pairs between finger tips and palm
-robot.collisionPairs.append(\[2,8\])
-robot.collisionPairs.append(\[2,11\])
-robot.collisionPairs.append(\[2,14\])
-robot.collisionPairs.append(\[2,16\])
+robot.collisionPairs.append([2, 8])
+robot.collisionPairs.append([2, 11])
+robot.collisionPairs.append([2, 14])
+robot.collisionPairs.append([2, 16])
-Compute distance between object 2 and 8, i.e the first collision pair
-=====================================================================
+# Compute distance between object 2 and 8, i.e the first collision pair
-idx = 0 dist = robot.checkCollision(idx)
+idx = 0
+dist = robot.checkCollision(idx)
-Display the collision pair by adding two disks at the witness points.
-=====================================================================
+# Display the collision pair by adding two disks at the witness points.
-robot.displayCollision(idx,0)
------------------------------
+robot.displayCollision(idx, 0)
+```
The Jacobian of the corresponding pair can be computed using the
-collisionJacobian method \[source,python\] ---- J =
-robot.collisionJacobian(idx,q) ---- The jacobian is a 1xN matrix (row
+`collisionJacobian` method
+
+```py
+J = robot.collisionJacobian(idx, q)
+```
+
+The jacobian is a 1xN matrix (row
matrix) corresponding to the contact normal. Take care that some
information are stored in the visual objects when calling
checkCollision, that are later used by collisionJacobian. You have to
@@ -175,24 +182,25 @@ vector (same number of rows as the jacobian).
Now, the joint acceleration is constrained by the contact constraint. It
can be written as a minimization problem using Gauss principle
-\["latex"\]
-$min \quad \frac{1}{2} (\ddot{q} - \ddot{q}_0 )^T M (\ddot{q} - \ddot{q}_0 )$
-\["latex"\] \$s.t. \quad J \ddot{q} > 0 \$
+\f$min \quad \frac{1}{2} (\ddot q - \ddot q_0 )^T M (\ddot q - \ddot q_0 )\f$
+
+\f$s.t. \quad J \ddot q > 0 \f$
-where qddot\_0 is the free acceleration, i.e. the acceleration obtained
+where \f$\ddot q_0\f$ is the free acceleration, i.e. the acceleration obtained
in Question 2 where no constraint is active.
In theory, the acceleration should be above the "centrifugal"
acceleration (i.e. the acceleration caused by joint velocity only, often
-written Jdot qdot) but we neglect it here.
+written \f$\dot J \dot q\f$) but we neglect it here.
In case of penetration or negative velocity, having only position
acceleration is not enough. A "trick" is often to require the contact
acceleration to be above a proportional depending of the penetration
-distance: J qddot >= -dist, with dist the vector of stacked
+distance: \f$J \ddot q >= -dist\f$, with \f$dist\f$ the vector of stacked
distances.
-*Question 4* Implement a contact simulator using QuadProg, the results
+#### Question 4
+Implement a contact simulator using QuadProg, the results
of Question 2 and the jacobian matrix of constraints whose distance is
below 1e-3.
@@ -203,11 +211,13 @@ those that were not in collision at previous simulation step). When a
collision pair is detected that was not previously active, project the
current velocity on the null space of all contacts:
-\["latex"\] $\dot q = \dot q - J^+ J \dot q$
+\f$\dot q = \dot q - J^+ J \dot q\f$
-*Question 5* The complete loop should be as follows: tauq is computed
+#### Question 5
+
+The complete loop should be as follows: \f$\tau_q\f$ is computed
from a PD tracking a time-varying joint position (question 3). After
-computing tauq, all collision pairs must be checked to find those with
+computing \f$\tau_q\f$, all collision pairs must be checked to find those with
distances below 1e-3. Corresponding Jacobians must be computed and
stacked. If a new collision as appeared, the joint velocity must be
projected to nullify it. If not collision is active, the joint
@@ -215,10 +225,3 @@ acceleration is computed from inverting the mass matrix (question 2).
Otherwise, it is computed using QuadProg (question 4). The resulting
acceleration is integrated twice (question 1) before displaying the
robot starting a new simulation iteration.
-
-Homework
---------
-
-Send by mail at nmansard@laas.fr a mail containing a single python file.
-The subject of the mail should start with +\[SUPAERO\] TP4+ When
-executed, the script should execute question 5.
diff --git a/doc/d-labs/5-planner.md b/doc/d-labs/5-planner.md
index 97d85f59910457d9eb51f222d8cc7c019f2c95e6..6b9cb5b61fda2f59e2d39e2588119b025bf38abf 100644
--- a/doc/d-labs/5-planner.md
+++ b/doc/d-labs/5-planner.md
@@ -1,20 +1,19 @@
-5. look ahead (aka motion planning)
-===================================
+# 5) look ahead (aka motion planning)
-Objective
----------
+## Objective
The objective of this work is to introduce some key algorithm of motion
planning: collision checking, probabilistic roadmaps, visibility PRM,
-A\* (a-star), random shortcut.
+\f$A^*\f$ (a-star), random shortcut.
-Tutorial 5.0: prerequisites
----------------------------
+## 5.0) prerequisites
-*Prerequisite \#1* A robot model with simple (static actuated) dynamics,
-like a UR5 manipulator robot. See Tutorial 1 for loading the UR5 robot.
+### Prerequisite 1
+A robot model with simple (static actuated) dynamics,
+like a UR5 manipulator robot. See Lab 1 for loading the UR5 robot.
-*Prerequisite \#2* A collision checking library, provided by Pinocchio.
+### Prerequisite 2
+A collision checking library, provided by Pinocchio.
Pinocchio provides collision checking for a large class of 3D objects
(sphere, capsule, box, triangle soup) using library FCL. Any 3D object
@@ -25,52 +24,58 @@ connected with Gepetto viewer (help is welcome to correct this, it would
requires 4h work).
An example of loading a capsule in Pinocchio and Gepetto viewer is
-below: \[source,python\] ---- obs =
-se3.GeometryObject.CreateCapsule(rad,length) \# Pinocchio obstacle
-object obs.name = "obs" \# Set object name obs.parentJoint = 0 \# Set
-object parent = 0 = universe obs.placement = se3.SE3(
-rotate('x',0.1)*rotate('y',0.1)*rotate('z',0.1), np.matrix(0.1\[:3\]).T
-) \# Set object placement wrt parent
-robot.collision\_model.addGeometryObject(obs,robot.model,False) \# Add
-object to collision model robot.visual\_model
-.addGeometryObject(obs,robot.model,False) \# Add object to visual model
-\# Also create a geometric object in gepetto viewer, with according
-name. robot.viewer.gui.addCapsule( "world/pinocchio/"+obs.name,
-rad,length, \[ 1.0, 0.2, 0.2, 1.0 \] ) ----
+below:
+```py
+obs = se3.GeometryObject.CreateCapsule(rad, length) # Pinocchio obstacle object
+obs.name = "obs" # Set object name
+obs.parentJoint = 0 # Set object parent = 0 = universe
+obs.placement = se3.SE3(rotate('x', .1) * rotate('y', .1) * rotate('z', .1), np.matrix([.1, .1, .1]).T) # Set object placement wrt parent
+robot.collision_model.addGeometryObject(obs, robot.model, False) # Add object to collision model
+robot.visual_model.addGeometryObject(obs, robot.model, False) # Add object to visual model
+# Also create a geometric object in gepetto viewer, with according name.
+robot.viewer.gui.addCapsule("world/pinocchio/" + obs.name, rad, length, [1.0, 0.2, 0.2, 1.0])
+```
URDF specifications does not allow to define which collision pairs
should be tested. By default, Pinocchio does not load any collision
pair. A simple strategy is to add all pairs, but often, some meshes of
the models induce wrong collision. Then manually remove them by testing
valid configurations. To be clean, you can store the valid collision
-pair in a SRDF file. For UR5: \[source,python\] ----
-robot.collision\_model.addAllCollisionPairs() for idx in \[ 56,35,23 \]:
-robot.collision\_model.removeCollisionPair(robot.collision\_model.collisionPairs\[idx\])
-----
+pair in a SRDF file. For UR5:
+
+```py
+robot.collision_model.addAllCollisionPairs()
+for idx in [56, 35, 23]:
+ robot.collision_model.removeCollisionPair(robot.collision_model.collisionPairs[idx])
+```
Collision checking are done through the following algorithms:
-\[source,python\] ----
-se3.updateGeometryPlacements(robot.model,robot.data,robot.collision\_model,robot.collision\_data,q)
-se3.computeCollision(robot.collision\_model,robot.collision\_data,pairId)
-se3.computeCollisions(robot.collision\_model,robot.collision\_data,False)
-\# last arg to stop early. ---- Both collision algorithms requires a
-preliminary update of placement and return True if configuration is in
-collision (False otherwise).
-Tutorial 5.1: Testing collision
--------------------------------
+```py
+se3.updateGeometryPlacements(robot.model, robot.data, robot.collision_model, robot.collision_data, q)
+se3.computeCollision(robot.collision_model, robot.collision_data, pairId)
+se3.computeCollisions(robot.collision_model, robot.collision_data, False)
+# last arg to stop early.
+```
+
+Both collision algorithms requires a
+preliminary update of placement and return `True` if configuration is in
+collision (`False` otherwise).
+
+## 5.1) Testing collision
We need to define a simple function to check whether a configuration is
respecting the robot constraints (joint limits and collision, plus any
other inequality-defined constraints you might want).
-*Question 1* Implement the function +check+ taking a configuration q in
-argument and return True if and only if q is acceptable -- The solution
+#### Question 1
+
+Implement the function `check` taking a configuration `q` in
+argument and return `True` if and only if `q` is acceptable -- The solution
only uses the 2 collision algorithms of Pinocchio listed above and
standard python otherwise.
-Tutorial 5.2: Steering method
------------------------------
+## 5.2) Steering method
We need to define a local controller, aka a steering method, to define
the behavior of the robot when it tries to connect to configuration
@@ -87,28 +92,29 @@ Here we propose to implement the steering method and the path validation
in a single connected method. More versatile implementation is obtained
by defining two different functions.
-*Question 2* Implement a +connect+ function, that takes as argument an
-initial q1 and a final q2 configuration, and return True is it is
+#### Question 2
+Implement a `connect` function, that takes as argument an
+initial `q1` and a final `q2` configuration, and return `True` is it is
possible to connect both using linear interpolation while avoiding
collision. Optionally, the function should also returns the sampling of
the path as a list of intermediate configurations -- The solution does
not need any new Pinocchio calls.
-Tutorial 5.3: Nearest neighbors
--------------------------------
+## 5.3) Nearest neighbors
Finally, we need a k-nearest-neighbors algorithms.
-*Question 3* Implement a function +nearestNeighbors+ that takes as
-argument a new configuration q, a list of candidates qs, and the number
-of requested neighbors k, and returns the list of the k nearest
-neighbors of q in qs. Optionally, the distance function that scores how
-close a configuration q1 is close to a configuration q2 might be also
-provided. If qs contains less that k elements, simply returns them all
+#### Question 3
+
+Implement a function `nearest_neighbors` that takes as
+argument a new configuration `q`, a list of candidates `qs`, and the number
+of requested neighbors `k`, and returns the list of the `k` nearest
+neighbors of `q` in `qs`. Optionally, the distance function that scores how
+close a configuration `q1` is close to a configuration `q2` might be also
+provided. If `qs` contains less that `k` elements, simply returns them all
-- no new Pinocchio method is needed for the solution.
-Tutorial 5.2: Probabilistic roadmap
------------------------------------
+## 5.2) Probabilistic roadmap
Basically, probabilistic roadmaps are build by maintaining a graph of
configurations. At each iteration, a new configuration is randomly
@@ -124,30 +130,29 @@ neighbor in each of the already-sampled connected component.
Configuration qnew is added to the graph only if one of the two
following conditions is respected:
-- if qnew cannot be connected to any existing connected component
-
-- or if it can be connected to at least two connected component.
+- if qnew cannot be connected to any existing connected component
+- or if it can be connected to at least two connected component.
In the second case, the connected components that can be connected are
also merged.
A graph structure with connected components is
-link:tp5\_graph\_py.html\[provided here\].
+[provided here](graph_8py_source.html).
-*Question 4* Implement a +visibilityPRM+ that takes in argument two
-start and goal configurations qstart and qgoal, and the number of random
-sampling that must be achieved before failing. The returns True if qgoal
-can be connected to qstart. The graph must also be returned -- no fancy
+#### Question 4
+Implement a `visibilityPRM` that takes in argument two
+start and goal configurations `qstart` and `qgoal`, and the number of random
+sampling that must be achieved before failing. The returns `True` if `qgoal`
+can be connected to `qstart`. The graph must also be returned -- no fancy
Pinocchio algorithm is needed here.
The PRM can be visualized in Gepetto-viewer using the function
-+displayPRM+ link:tp5\_prm\_display\_py.html\[provided here\].
+`display_prm` [provided here](prm__display_8py_source.html).
-Tutorial 5.4: Searching a path in the roadmap
----------------------------------------------
+## 5.4) Searching a path in the roadmap
-A\* is an algorithm to find the shortest path in a graph (discrete
-problem). A\* iterativelly explore the nodes of the graph starting for
+\f$A^*\f$ is an algorithm to find the shortest path in a graph (discrete
+problem). \f$A^*\f$ iterativelly explore the nodes of the graph starting for
the given start. One a node gets explored, its exact cost from start
(cost to here) is exactly known. Then, all its children are added to the
"frontier" of the set of already-explored nodes. Cost from nodes of the
@@ -157,19 +162,18 @@ algorithm examines a node of the frontier, looking first at the node
that is the most likely to be in the shortest path using the heuristic
distance.
-See the fairly complete description of A\*
-link:http://theory.stanford.edu/\~amitp/GameProgramming/AStarComparison.html\#the-a-star-algorithm\[provided
-here\].
+See the fairly complete description of \f$A^*\f$
+[provided here](http://theory.stanford.edu/~amitp/GameProgramming/AStarComparison.html#the-a-star-algorithm).
-*Question 5* Implement the A\* algorithm. The A\* returns a sequence of
+#### Question 5
+Implement the \f$A^*\f$ algorithm. The \f$A^*\f$ returns a sequence of
node ID from start to goal. We only work here with the existing graph of
configuration, meaning that no new nodes a sampled, and no new collision
-test are computed. Pinocchio is not necessary here. A\* returns a list
+test are computed. Pinocchio is not necessary here. \f$A^*\f$ returns a list
containing the indexes of the nodes of the PRM graph that one should
cross to reach qgoal from qstart.
-Tutorial 5.4: Shortcut
-----------------------
+## 5.4) Shortcut
Being given a list of configurations, a random shortcut simply randomly
tries to shortcut some subpart of the list: it randomly selects the
@@ -178,22 +182,24 @@ the above-defined steering method). In case of success, it skips the
unnecessary subpart. The algorithm iterates a given number of time.
The shortcut can be run on either the list of configuration output by
-the A*, or on a sampling of the trajectory connecting the nodes of the
-A*. We propose here the second version.
+the \f$A^*\f$, or on a sampling of the trajectory connecting the nodes of the
+\f$A^*\f$. We propose here the second version.
-*Question 6* Defines a function +samplePath+ to uniformly sample that
-trajectory connecting the nodes selected by A*: for each edge of the A*
-optimal sequence, call +connect+ and add the resulting sampling to the
+#### Question 6
+Defines a function `sample_path` to uniformly sample that
+trajectory connecting the nodes selected by \f$A^*\f$: for each edge of the \f$A^*\f$
+optimal sequence, call `connect` and add the resulting sampling to the
previously-computed sample. It takes as argument the PRM graph and the
-list of indexes computed by A\* and returns a list of robot
+list of indexes computed by \f$A^*\f$ and returns a list of robot
configuration starting by qstart and ending by qgoal -- no Pinocchio
method is needed here.
The sampled path can be displayed in Gepetto-viewer using the function
-+displayPath+ link:prm\_display\[provided here\].
+`displayPath` [provided here](prm__display_8py_source.html).
-*Question 7* Implement the +shortcut+ algorithm that tries to randomly
+#### Question 7
+Implement the `shortcut` algorithm that tries to randomly
connect two configuration of the sampled path. It takes the list of
-configuration output by +samplePath+ and the number of random shortcut
+configuration output by `sample_path` and the number of random shortcut
it should tries. It returns an optimized list of configurations -- no
Pinocchio method is needed here.
diff --git a/doc/d-labs/6-wpg.md b/doc/d-labs/6-wpg.md
index 9702f0a397881c552af3b68ad39ee64b15c7c0f0..537a2b8841ae65a3701e11e54e6e59562d97da1d 100644
--- a/doc/d-labs/6-wpg.md
+++ b/doc/d-labs/6-wpg.md
@@ -1,20 +1,19 @@
-6. Take a walk (aka optimal control)
-====================================
+# 6) Take a walk (aka optimal control)
-Objective
----------
+## Objective
The objective of this work is to get a first touch of optimal control
The tutorial will guide you to generate a dynamically balanced walk
motion for a humanoid robot using a LQR to compute the robot
center-of-mass trajectory.
-Tutorial 6.0: prerequesites
----------------------------
+## 6.0) prerequesites
-*Prerequesite \#1* A humanoid robot models, with at least two legs.
+### Prerequesite 1
+A humanoid robot models, with at least two legs.
-*Prerequesite \#2* An inverse geometry solver based on BFGS.
+### Prerequesite 2
+An inverse geometry solver based on BFGS.
Yet, the inverse geometry only solves the motion of the robot for a
constant task, like reaching a specific position of the hand, or a
@@ -27,69 +26,44 @@ previous configuration of the robot, and (iii) turning off the default
verbose output of BFGS. For example, the robot can track a target moving
vertically using the following example:
-\[source,python\]
------------------
-
-cost.Mdes = se3.SE3( eye(3),np.matrix(\[0.2,0,0.1+t/100.\]) ) \#
-Reference target at time 0. q = np.copy(robot.q0) for t in range(100):
-cost.Mdes.translation = np.matrix(\[0.2,0,0.1+t/100.\]) q =
-fmin\_bfgs(cost, q,maxiter=10,disp=False) robot.display(q) ----
+```py
+cost.Mdes = se3.SE3(eye(3), np.matrix([0.2, 0, 0.1 + t / 100.])) # Reference target at time 0.
+q = np.copy(robot.q0)
+for t in range(100):
+ cost.Mdes.translation = np.matrix([0.2, 0, 0.1 + t / 100.])
+ q = fmin_bfgs(cost, q, maxiter=10, disp=False)
+ robot.display(q)
+```
Implement a motion of the right foot of the robot tracking a straight
line from the initial position of the robot to a position 10cm forward,
while keeping a constant rotation of the foot.
-Tutorial 6.1: defining input
-----------------------------
+## 6.1) defining input
The input of the walk generation algorithm is a sequence of steps with a
given timing. This input will be represented by two sequences as in the
-examples below. The class +FootSteps+ (link:footsteps.html\[*see code
-link*\]) can be used to define, store and access to the footstep plan.
-
-\[source, python\]
-------------------
-
-Define 6 steps forward, starting with the left foot and stoping at the same forward position.
-=============================================================================================
-
-footsteps = FootSteps( \[0.0,-0.1\] , \[0.0,0.1\] ) footsteps.addPhase(
-.3, 'none' ) footsteps.addPhase( .7, 'left' , \[0.1,+0.1\] )
-footsteps.addPhase( .1, 'none' ) footsteps.addPhase( .7, 'right',
-\[0.2,-0.1\] ) footsteps.addPhase( .1, 'none' ) footsteps.addPhase( .7,
-'left' , \[0.3,+0.1\] ) footsteps.addPhase( .1, 'none' )
-footsteps.addPhase( .7, 'right', \[0.4,-0.1\] ) footsteps.addPhase( .1,
-'none' ) footsteps.addPhase( .7, 'left' , \[0.5,+0.1\] )
-footsteps.addPhase( .1, 'none' ) footsteps.addPhase( .7, 'right',
-\[0.5,-0.1\] ) footsteps.addPhase( .5, 'none' ) ----
-
-The class stores three functions of time: left, right and flyingFoot.
-Each function is piecewise constant. For each function, the user can ask
-what is the value of this function at time t.
-
-The storage is composed of three lists for left, right and flyingFoot,
-and a list for time. The list of times stores the time intervals, i.e.
-each element of the list is the start of a time interval. The first
-element of the list is 0. The value of the functions
-left,right,flyingFoot one this time interval is stored at the same
-position is their respective list (i.e. value of left on interval \[
-time\[i\],time\[i+1\] \] is stored in left\[i\].
-
-The 4 lists are set up using function addPhase(). The values of
-functions left,right,flyingFoot can be accessed through the function
-getPhaseType(t), getLeftPosition(t), getRightPosition(t). PhaseType are
-'left' (meaning left foot is flying, right foot is fixed), 'right' (ie
-the opposite) or 'none' (meaning no foot is flying, both are fixed on
-the ground).
-
-Additionnally, functions getLeftNextPosition(t), getRightNextPosition(t)
-can be used to get the next position of the flying foot (in that case,
-additional work is needed to compute the position of flying foot at time
-t by interpolating getLeftPosition(t) and getLeftNextPosition(t).
-
-Functions getPhaseStart(t), getPhaseDuration(t) and getPhaseRemaining(t)
-can be used to get the starting time, the duration and the remaining
-time of the current phase at time t.
+examples below. The class `FootSteps` [provided here](foot__steps_8py_source.html)
+can be used to define, store and access to the footstep plan.
+
+```py
+# Define 6 steps forward, starting with the left foot and stoping at the same forward position.
+
+footsteps = FootSteps([.0, -.1] ,[.0, .1])
+footsteps.add_phase(.3, 'none')
+footsteps.add_phase(.7, 'left', [.1, .1])
+footsteps.add_phase(.1, 'none')
+footsteps.add_phase(.7, 'right', [.2, -.1])
+footsteps.add_phase(.1, 'none')
+footsteps.add_phase(.7, 'left', [.3, .1])
+footsteps.add_phase(.1, 'none')
+footsteps.add_phase(.7, 'right', [.4, -.1])
+footsteps.add_phase(.1, 'none')
+footsteps.add_phase(.7, 'left', [.5, .1])
+footsteps.add_phase(.1, 'none')
+footsteps.add_phase(.7, 'right', [.5, -.1])
+footsteps.add_phase(.5, 'none')
+```
A phase 'none' defines a double support phase (no foot moving). A phase
'left' (resp. 'right') defines a simple support phase while indicating
@@ -97,97 +71,92 @@ the flying foot. The time is a duration. The position is absolute.
Each interval corresponds to the following constant support phases:
-\["latex"\]
-\begin{tabular}{lll}
-$\left[t_0,t_1\right]$ &double support phase& left foot in steps [0], right foot in steps [1] \\
-$\left[t_1,t_2\right]$ &left foot support phase& right foot moving to steps [2]\\
-$\left[t_2,t_3\right]$ &double support phase,& \\
-$\left[t_3,t_4\right]$ &right foot support phase,& left foot moving to steps [3]\\
-&\vdots&\\
-$\left[t_{m-2}, t_{m-1}\right]$ &double support phase,& left foot in steps [p-2], right foot in steps [p-1]
-\end{tabular}
-\[source,python\]
------------------
-
-Example of use
-==============
-
-footsteps.getPhaseType(.4) \# return 'left'
-footsteps.getLeftPosition(0.4) \# return 0,0.1
-footsteps.getLeftNextPosition(0.4) \# return 0.1,0.1
-footsteps.getPhaseStart(0.4) \# return 0.3
-footsteps.getPhaseDuration(0.4) \# return 0.7
-footsteps.getPhaseRemaining(0.4) \# return 0.6
-footsteps.isDoubleFromLeftToRight(0) \# return False
-footsteps.isDoubleFromLeftToRight(1) \# return True ----
-
-Tutorial 6.2: computing reference ZMP
--------------------------------------
-
-Implement a python class called +ZmpRef+ that takes as input a sequence
+interval | Â | Â
+----------------------------------- | ------------------------- | ----------------------------------------------------
+\f$\left[t_0,t_1\right]\f$ | double support phase | left foot in steps [0], right foot in steps [1]
+\f$\left[t_1,t_2\right]\f$ | left foot support phase | right foot moving to steps [2]
+\f$\left[t_2,t_3\right]\f$ | double support phase, | Â
+\f$\left[t_3,t_4\right]\f$ | right foot support phase, | left foot moving to steps [3]
+\f$\vdots\f$ | \f$\vdots\f$ | \f$\vdots\f$
+\f$\left[t_{m-2}, t_{m-1}\right]\f$ | double support phase, | left foot in steps [p-2], right foot in steps [p-1]
+
+```py
+# Example of use
+
+footsteps.get_phase_type(.4) # return 'left'
+footsteps.get_left_position(0.4) # return 0,0.1
+footsteps.get_left_next_position(0.4) # return 0.1,0.1
+footsteps.get_phase_start(0.4) # return 0.3
+footsteps.get_phase_duration(0.4) # return 0.7
+footsteps.get_phase_remaining(0.4) # return 0.6
+footsteps.is_double_from_left_to_right(0) # return False
+footsteps.is_double_from_left_to_right(1) # return True
+```
+
+## 6.2) computing reference ZMP
+
+Implement a python class called `ZmpRef` that takes as input a sequence
of times and a sequence of steps. Objects of this class behave as a
function of time that returns a 2 dimensional vector:
-\[source, python\]
-------------------
-
-> > > zmp = ZmpRef (footsteps) zmp (2.5) array(\[ 0.41 , 0.096\]) ----
+```py
+zmp = ZmpRef(footsteps)
+zmp(2.5)
+array([0.41, 0.096])
+```
The function should be a piecewise affine function
-- starting in the middle of the ankles of the two first steps,
-- finishing in the middle of the two ankles of the two last steps,
-- constant under the foot support during single support phases.
+- starting in the middle of the ankles of the two first steps,
+- finishing in the middle of the two ankles of the two last steps,
+- constant under the foot support during single support phases.
You can use the template below.
-\[source, python\]
-------------------
+```py
+class ZmpRef(object):
+ def __init__(self, footsteps):
+ self.footsteps = footsteps
-class ZmpRef (object): def **init** (self, footsteps) : self.footsteps =
-footsteps \# Operator () def **call** (self, t): return array
-(self.steps \[0\]) ----
+ def __call__(self, t):
+ return np.array(self.footsteps[0])
+```
-For the inputs provided above, the graph of +zmp+ is given below.
+For the inputs provided above, the graph of `zmp` is given below.
-image::images/zmp-ref.png\[width="100%",alt="zmp\_ref against time"\]
-Tutorial 6.3: reference trajectory of the center of mass
---------------------------------------------------------
+## 6.3) reference trajectory of the center of mass
-Using the reference zmp trajectory implemented in Tutorial 6.3,
-implement a class +ComRef+ that computes the reference trajectory of the
+Using the reference zmp trajectory implemented above,
+implement a class `ComRef` that computes the reference trajectory of the
center of mass by optimal control.
To write the underlying optimization problem, you can use a factor
graph. A simple implementation is available in
-link:factor\_graph.html\[(*see the source code*)\]. An example of use is
+[this file](factor_8py_source.html). An example of use is
the following. Try to guess the solution before executing it.
-\[source,python\]
------------------
-
-f = FactorGraph(1,5) \# Define a factor of 5 variables of dimension 1
+```py
+f = FactorGraph(1, 5) # Define a factor of 5 variables of dimension 1
-M = eye(1) \# M is simply 1 written as a 1x1 matrix. for i in range(4):
-f.addFactorConstraint( \[ Factor( i,M ), Factor( i+1,-M ) \], zero(1) )
+M = eye(1) # M is simply 1 written as a 1x1 matrix.
+for i in range(4):
+ f.add_factor_constraint([Factor(i, M), Factor(i + 1, -M)], zero(1))
-f.addFactor( \[ Factor(0,M) \], M*10 ) f.addFactor( \[ Factor(4,M) \],
-M*20 )
+f.addFactor([Factor(0, M)], M * 10)
+f.addFactor([Factor(4, M)], M * 20)
x = f.solve()
--------------
+```
-Tutorial 6.4: reference trajectories of the feet
-------------------------------------------------
+## 6.4) reference trajectories of the feet
-Using the same method as in Tutorial 6.2, implement two classes
-+RightAnkleRef+ and +LeftAnkleRef+ that return reference positions of
+Using the same method as in 6.2, implement two classes
+`RightAnkleRef` and `LeftAnkleRef` that return reference positions of
the ankles as homogeneous matrices. Unlike zmp reference, trajectories
of the feet should be continuously differentiable.
-Tutorial 6.5: generate walk motion
-----------------------------------
+## 6.5) generate walk motion
Use the classes defined in the previous sections to generate a walk
-motion using the inverse kinematics solver of Tutorial 2.
+motion using the inverse kinematics solver of Lab 2.
diff --git a/doc/d-labs/7-learn.md b/doc/d-labs/7-learn.md
index 989c96b443c5f485358054d2de4d5003e81a29e5..765164edea5487100a70e7aac1ebab7f6cd82814 100644
--- a/doc/d-labs/7-learn.md
+++ b/doc/d-labs/7-learn.md
@@ -1,8 +1,6 @@
-7. Learning to flight (aka policy learning)
-===========================================
+# 7) Learning to flight (aka policy learning)
-Objective
----------
+## Objective
The objective of this tutorial is to study how to directly solve an
optimal control problem, either by computing a trajectory from current
@@ -13,72 +11,76 @@ standing configuration. The presented algorithms successively compute an
optimal trajectory, a discretized policy with a Q-table and with a
linear network, and a continuous policy with a deep neural network.
-Tutorial 7.0: prerequesites
----------------------------
+## 7.0) prerequesites
-We need a pendulum model and a neural network. All the source code for
-this tutorial is gathered in the archive (link:tuto7.zip\[*available at
-this url*\]).
+We need a pendulum model and a neural network.
-*Prerequesite \#1* Inverted pendulum model
+### Prerequesite 1
+
+Inverted pendulum model
Two models are provided. The first one is continuous and is implemented
-with Pinocchio and is available in pendulum.py with class Pendulum. The
+with Pinocchio and is available in [pendulum.py](pendulum_8py_source.html)
+with class `Pendulum`. The
code is generic for a N-pendulum. We will use the 1-dof model. The state
-is \[q,v\] the angle and angular velocity of the pendulum. The control
+is `[q, v]` the angle and angular velocity of the pendulum. The control
is the joint torque. The pendulum weights 1kg, measures 1m with COM at
-0.5m of the joint. Do not forget to start +gepetto-viewer-server+ before
+0.5m of the joint. Do not forget to start `gepetto-gui` before
rendering the model. Each time the simulator is integrated, it returns a
new state and the reward for the previous action (implement to be the
weighted sum of squared position, velocity and control). The state is
recorded as a member of the class and can be accessed through env.x. Do
not forget to copy it before modifying it.
-\[source, python\]
-------------------
-
-from pendulum import Pendulum from pinocchio.utils import \*
+```py
+from pendulum import Pendulum
+from pinocchio.utils import *
-env = Pendulum(1) \# Continuous pendulum NX = env.nobs \# ... training
-converges with q,qdot with 2x more neurones. NU = env.nu \# Control is
-dim-1: joint torque
+env = Pendulum(1) # Continuous pendulum
+NX = env.nobs # ... training converges with q,qdot with 2x more neurones.
+NU = env.nu # Control is dim-1: joint torque
-x = env.reset() \# Sample an initial state u = rand(NU) \# Sample a
-control x,reward = env.step(u) \# Integrate simulator for control u and
-get reward. env.render() \# Display model at state env.x ----
+x = env.reset() # Sample an initial state
+u = rand(NU) # Sample a control
+x, reward = env.step(u) # Integrate simulator for control u and get reward.
+env.render() # Display model at state env.x
+```
A second version of the same model is provided with a discrete dynamics,
-in dpendulum.py. The state is again \[q,v\], however discretized in NQ
+in [dpendulum.py](dpendulum_8py_source.html). The state is again `[q, v]`, however discretized in NQ
position and NV velocity. The state is then a integer equal to iq*NV+iv,
ranging from 0 to NQ*NV=NX. Controls are also discretized from 0 to NU.
-\[source, python\] ---- from dpendulum import DPendulum
-env = DPendulum() NX = env.nx \# Number of (discrete) states NU = env.nu
-\# Number of (discrete) controls env.reset() env.step(5) env.render()
-----
+```py
+from dpendulum import DPendulum
+
+env = DPendulum()
+NX = env.nx # Number of (discrete) states
+NU = env.nu # Number of (discrete) controls
+env.reset()
+env.step(5)
+env.render()
+```
Other models could be used. In particular, we used a similar API to the
Gym from OpenAI, that you might be interested to browsed and possibly
try with the following algorithms.
-*Prerequesite \#2* A neural network with optimizers
+### Prerequesite 2
+A neural network with optimizers
We will use the Tensor Flow from Google, available thanks to pip.
-\[source,python\]
------------------
-
-sudo apt-get install python-pip cython python-h5py export
-TF\_BINARY\_URL=https://storage.googleapis.com/tensorflow/linux/cpu/tensorflow-0.12.1-cp27-none-linux\_x86\_64.whl
-pip install --upgrade \$TF\_BINARY\_URL pip install tflearn==0.2.1 ----
+```
+pip install --user tensorflow tflearn
+```
-Tutorial 7.1. Optimizing an optimal trajectory
-----------------------------------------------
+## 7.1) Optimizing an optimal trajectory
For the first tutorial, we implement a nonlinear optimization program
optimizing the cost of a single trajectory for the continious pendulum.
The trajectory is represented by its initial state x0 and the vector of
-piecewise-constant control U=\[u0 ... uT-1\], with T the number of
+piecewise-constant control `U=[u0 ... uT-1]`, with `T` the number of
timestep. The cost is simply the integral of the cost function l(x,u)
returned by the pendulum environment.
@@ -86,11 +88,9 @@ Then the integral cost is optimized starting from a 0 control
trajectory, until the pendulum finally reaches a standing state. Observe
that the number of swings is possibly sub-optimal.
-The code to optimize the trajectory is available in ocp.py in the ZIP
-archive.
+The code to optimize the trajectory is available in [ocp.py](ocp_8py_source.html).
-Tutorial 7.2. Q-table resolution for discrete pendulum
-------------------------------------------------------
+## 7.2) Q-table resolution for discrete pendulum
We now consider the discrete model, with NX state and NU control.
Imagine this model as a chess board with a maze (possibly nonplanar)
@@ -109,10 +109,9 @@ Hamiltonian. Once the Q-table is computed, the optimal policy is simply
chosen by maximizing the vector of Q-values corresponding to the row of
state X.
-This algorithm is available in the file qtable.py of the ZIP archive.
+This algorithm is available in the file [qtable.py](qtable_8py_source.html).
-Tutorial 7.3. Q-table using a linear net
-----------------------------------------
+## 7.3) Q-table using a linear net
The idea is to similarly approximate the Q-value for the continuous
model. Since the continious model has both infinitely many states and
@@ -123,8 +122,8 @@ deep neural net. Firt, let's use a simple net for storing the Q-table.
Basically, the vectory of Q-values for all possible control u is
obtained by multiplying the Q-table by a one-hot vector (0 everywhere
except a single 1) corresponding to the state. The optimal policy is
-then the maximum of this vector: iu\^\* = argmax( Q\*h(ix) ), with h(ix)
-= \[ 0 0 ... 0 1 0 ... 0\], ix and iu being indexes of both state and
+then the maximum of this vector: `iu^* = argmax(Q*h(ix))`, with `h(ix)
+= [ 0 0 ... 0 1 0 ... 0]`, ix and iu being indexes of both state and
control. We use tensor flow to store array Q. The Q-value net is simply
the multiplication of Q by one-hot x, and the policy the argmax of the
result.
@@ -135,17 +134,15 @@ Hamiltion-Jacobi-Belman equation, we know that Q(x,u) = l(x,u) + max\_u2
Q(f(x,u),u2). We optimize the Q value so that this residual is minimized
along the samples collected from successive roll-outs inside the maze.
-The implementation of this algorithm is available in qnet.py in the ZIP
-archive. Observe that the convergence is not as fast as with the Q-Table
-algorithm.
+The implementation of this algorithm is available in [qnet.py](qnet_8py_source.html).
+Observe that the convergence is not as fast as with the Q-Table algorithm.
-Tutorial 7.4. Actor-critic network
-----------------------------------
+## 7.4) Actor-critic network
We will now optimize a continuous policy and the corresponding
-Q-function, using an "Actor-Critic" method proposed in "Continuous
+Q-function, using an "Actor-Critic" method proposed in ["Continuous
control with deep reinforcement learning", by Lillicrap et al,
-arXiv:1509.02971.
+arXiv:1509.02971](https://arxiv.org/abs/1509.02971).
Two networks are used to represent the Q function and the policy. The
first network has two inputs: state x and control u. Its outpout is a
@@ -165,20 +162,11 @@ the batch. Second, we regularize the optimization of both Q-value
(critic) and policy (actor) networks by storing a copy of both network,
and only slightly modifying these copy at each steps.
-The corresponding algorithm is implemented in the file continuous.py of
-the ZIP archive. The training phase requires 100 roll-outs and some
-minutes (maybe more on the virual machine). To make the training faster,
-backup of the network weights are available
-(link:tuto6\_backup.zip\[*following this link*\]). You can restore the
-networks weights by uncommenting the following code line:
-\[source,python\] ---- tf.train.Saver().restore(sess,
-"netvalues/actorcritic.pre.ckpt") tf.train.Saver().restore(sess,
-"netvalues/actorcritic.full.ckpt") ---- The "pre" backup contains
-partially-optimized weiths (with 30 rollouts). The "full" backup
-corresponds to fully-optimized weights after 100 roll-outs.
-
-Tutorial 7.5. Training the network with the OCP solver
-------------------------------------------------------
+The corresponding algorithm is implemented in the file [continuous.py](continuous_8py_source.html)
+The training phase requires 100 roll-outs and some
+minutes (maybe more on a virual machine).
+
+## 7.5) Training the network with the OCP solver
Using the OCP solver, you might compute a few optimal trajectories (say
10) starting from various initial conditions. Initialize the replay
@@ -196,14 +184,4 @@ The provided solvers (trajectory and policy) runs reasonably well for
the 1-pendulum. It is more difficult to tune for a more-complex
dynamics, such as a 2-pendulum. You may want to try on a quadcopter
robot (hence the title of the tutorial) but I except it to be a serious
-job. A model of the quadcopter (link:quadcopter.py\[*is available
-here*\]).
-
-Homework
---------
-
-Send by mail at nmansard@laas.fr a mail containing a single python file.
-The subject of the mail should start with +\[SUPAERO\] TP6+ When
-executed, the script should compute 5 optimal trajectory, feed them to
-the replay deque and train from scratch the actor-critic networks using
-batchs from these replay deque.
+job.
diff --git a/doc/d-labs/src/continuous.py b/doc/d-labs/src/continuous.py
new file mode 100644
index 0000000000000000000000000000000000000000..826f6ac9514e74410ad80d084d87ebb8511d8e2d
--- /dev/null
+++ b/doc/d-labs/src/continuous.py
@@ -0,0 +1,227 @@
+'''
+Deep actor-critic network,
+From "Continuous control with deep reinforcement learning", by Lillicrap et al, arXiv:1509.02971
+'''
+
+from pendulum import Pendulum
+import tensorflow as tf
+import numpy as np
+import tflearn
+import random
+from collections import deque
+import time
+import signal
+import matplotlib.pyplot as plt
+
+### --- Random seed
+RANDOM_SEED = int((time.time()%10)*1000)
+print "Seed = %d" % RANDOM_SEED
+np .random.seed (RANDOM_SEED)
+tf .set_random_seed (RANDOM_SEED)
+random.seed (RANDOM_SEED)
+n_init = tflearn.initializations.truncated_normal(seed=RANDOM_SEED)
+u_init = tflearn.initializations.uniform(minval=-0.003, maxval=0.003,\
+ seed=RANDOM_SEED)
+
+### --- Hyper paramaters
+NEPISODES = 100 # Max training steps
+NSTEPS = 100 # Max episode length
+QVALUE_LEARNING_RATE = 0.001 # Base learning rate for the Q-value Network
+POLICY_LEARNING_RATE = 0.0001 # Base learning rate for the policy network
+DECAY_RATE = 0.99 # Discount factor
+UPDATE_RATE = 0.01 # Homotopy rate to update the networks
+REPLAY_SIZE = 10000 # Size of replay buffer
+BATCH_SIZE = 64 # Number of points to be fed in stochastic gradient
+NH1 = NH2 = 250 # Hidden layer size
+
+### --- Environment
+env = Pendulum(1) # Continuous pendulum
+env.withSinCos = True # State is dim-3: (cosq,sinq,qdot) ...
+NX = env.nobs # ... training converges with q,qdot with 2x more neurones.
+NU = env.nu # Control is dim-1: joint torque
+
+### --- Q-value and policy networks
+
+class QValueNetwork:
+ def __init__(self):
+ nvars = len(tf.trainable_variables())
+
+ x = tflearn.input_data(shape=[None, NX])
+ u = tflearn.input_data(shape=[None, NU])
+
+ netx1 = tflearn.fully_connected(x, NH1, weights_init=n_init, activation='relu')
+ netx2 = tflearn.fully_connected(netx1, NH2, weights_init=n_init)
+ netu1 = tflearn.fully_connected(u, NH1, weights_init=n_init, activation='linear')
+ netu2 = tflearn.fully_connected(netu1, NH2, weights_init=n_init)
+ net = tflearn.activation (netx2+netu2,activation='relu')
+ qvalue = tflearn.fully_connected(net, 1, weights_init=u_init)
+
+ self.x = x # Network state <x> input in Q(x,u)
+ self.u = u # Network control <u> input in Q(x,u)
+ self.qvalue = qvalue # Network output <Q>
+ self.variables = tf.trainable_variables()[nvars:] # Variables to be trained
+ self.hidens = [ netx1, netx2, netu1, netu2 ] # Hidden layers for debug
+
+ def setupOptim(self):
+ qref = tf.placeholder(tf.float32, [None, 1])
+ loss = tflearn.mean_square(qref, self.qvalue)
+ optim = tf.train.AdamOptimizer(QVALUE_LEARNING_RATE).minimize(loss)
+ gradient = tf.gradients(self.qvalue, self.u)[0] / float(BATCH_SIZE)
+
+ self.qref = qref # Reference Q-values
+ self.optim = optim # Optimizer
+ self.gradient = gradient # Gradient of Q wrt the control dQ/du (for policy training)
+ return self
+
+ def setupTargetAssign(self,nominalNet,tau=UPDATE_RATE):
+ self.update_variables = \
+ [ target.assign( tau*ref + (1-tau)*target ) \
+ for target,ref in zip(self.variables,nominalNet.variables) ]
+ return self
+
+class PolicyNetwork:
+ def __init__(self):
+ nvars = len(tf.trainable_variables())
+
+ x = tflearn.input_data(shape=[None, NX])
+ net = tflearn.fully_connected(x, NH1, activation='relu', weights_init=n_init)
+ net = tflearn.fully_connected(net, NH2, activation='relu', weights_init=n_init)
+ policy = tflearn.fully_connected(net, NU, activation='tanh', weights_init=u_init)*env.umax
+
+ self.x = x # Network input <x> in Pi(x)
+ self.policy = policy # Network output <Pi>
+ self.variables = tf.trainable_variables()[nvars:] # Variables to be trained
+
+ def setupOptim(self):
+
+ qgradient = tf.placeholder(tf.float32, [None, NU])
+ grad = tf.gradients(self.policy, self.variables, -qgradient)
+ optim = tf.train.AdamOptimizer(POLICY_LEARNING_RATE).\
+ apply_gradients(zip(grad,self.variables))
+
+ self.qgradient = qgradient # Q-value gradient wrt control (input value)
+ self.optim = optim # Optimizer
+ return self
+
+ def setupTargetAssign(self,nominalNet,tau=UPDATE_RATE):
+ self.update_variables = \
+ [ target.assign( tau*ref + (1-tau)*target ) \
+ for target,ref in zip(self.variables,nominalNet.variables) ]
+ return self
+
+### --- Replay memory
+class ReplayItem:
+ def __init__(self,x,u,r,d,x2):
+ self.x = x
+ self.u = u
+ self.reward = r
+ self.done = d
+ self.x2 = x2
+
+replayDeque = deque()
+
+### --- Tensor flow initialization
+
+policy = PolicyNetwork(). setupOptim()
+policyTarget = PolicyNetwork(). setupTargetAssign(policy)
+
+qvalue = QValueNetwork(). setupOptim()
+qvalueTarget = QValueNetwork(). setupTargetAssign(qvalue)
+
+sess = tf.InteractiveSession()
+tf.global_variables_initializer().run()
+
+# Uncomment to save or restore networks
+#tf.train.Saver().restore(sess, "netvalues/actorcritic.pre.ckpt")
+#tf.train.Saver().save (sess, "netvalues/actorcritic.full.ckpt")
+
+def rendertrial(maxiter=NSTEPS,verbose=True):
+ x = env.reset()
+ rsum = 0.
+ for i in range(maxiter):
+ u = sess.run(policy.policy, feed_dict={ policy.x: x.T })
+ x, reward = env.step(u)
+ env.render()
+ time.sleep(1e-2)
+ rsum += reward
+ if verbose: print 'Lasted ',i,' timestep -- total reward:',rsum
+signal.signal(signal.SIGTSTP, lambda x,y:rendertrial()) # Roll-out when CTRL-Z is pressed
+
+### History of search
+h_rwd = []
+h_qva = []
+h_ste = []
+
+### --- Training
+for episode in range(1,NEPISODES):
+ x = env.reset().T
+ rsum = 0.0
+
+ for step in range(NSTEPS):
+ u = sess.run(policy.policy, feed_dict={ policy.x: x }) # Greedy policy ...
+ u += 1. / (1. + episode + step) # ... with noise
+ x2,r = env.step(u)
+ x2 = x2.T
+ done = False # pendulum scenario is endless.
+
+ replayDeque.append(ReplayItem(x,u,r,done,x2)) # Feed replay memory ...
+ if len(replayDeque)>REPLAY_SIZE: replayDeque.popleft() # ... with FIFO forgetting.
+
+ rsum += r
+ if done or np.linalg.norm(x-x2)<1e-3: break # Break when pendulum is still.
+ x = x2
+
+ # Start optimizing networks when memory size > batch size.
+ if len(replayDeque) > BATCH_SIZE:
+ batch = random.sample(replayDeque,BATCH_SIZE) # Random batch from replay memory.
+ x_batch = np.vstack([ b.x for b in batch ])
+ u_batch = np.vstack([ b.u for b in batch ])
+ r_batch = np.vstack([ b.reward for b in batch ])
+ d_batch = np.vstack([ b.done for b in batch ])
+ x2_batch = np.vstack([ b.x2 for b in batch ])
+
+ # Compute Q(x,u) from target network
+ u2_batch = sess.run(policyTarget.policy, feed_dict={ policyTarget .x : x2_batch})
+ q2_batch = sess.run(qvalueTarget.qvalue, feed_dict={ qvalueTarget.x : x2_batch,
+ qvalueTarget.u : u2_batch })
+ qref_batch = r_batch + (d_batch==False)*(DECAY_RATE*q2_batch)
+
+ # Update qvalue to solve HJB constraint: q = r + q'
+ sess.run(qvalue.optim, feed_dict={ qvalue.x : x_batch,
+ qvalue.u : u_batch,
+ qvalue.qref : qref_batch })
+
+ # Compute approximate policy gradient ...
+ u_targ = sess.run(policy.policy, feed_dict={ policy.x : x_batch} )
+ qgrad = sess.run(qvalue.gradient, feed_dict={ qvalue.x : x_batch,
+ qvalue.u : u_targ })
+ # ... and take an optimization step along this gradient.
+ sess.run(policy.optim,feed_dict= { policy.x : x_batch,
+ policy.qgradient : qgrad })
+
+ # Update target networks by homotopy.
+ sess.run(policyTarget. update_variables)
+ sess.run(qvalueTarget.update_variables)
+
+ # \\\END_FOR step in range(NSTEPS)
+
+ # Display and logging (not mandatory).
+ maxq = np.max( sess.run(qvalue.qvalue,feed_dict={ qvalue.x : x_batch,
+ qvalue.u : u_batch }) ) \
+ if 'x_batch' in locals() else 0
+ print 'Ep#{:3d}: lasted {:d} steps, reward={:3.0f}, max qvalue={:2.3f}' \
+ .format(episode, step,rsum, maxq)
+ h_rwd.append(rsum)
+ h_qva.append(maxq)
+ h_ste.append(step)
+ if not (episode+1) % 20: rendertrial(100)
+
+# \\\END_FOR episode in range(NEPISODES)
+
+print "Average reward during trials: %.3f" % (sum(h_rwd)/NEPISODES)
+rendertrial()
+plt.plot( np.cumsum(h_rwd)/range(1,NEPISODES) )
+plt.show()
+
+
+
diff --git a/doc/d-labs/src/display.py b/doc/d-labs/src/display.py
new file mode 100644
index 0000000000000000000000000000000000000000..9597b7d202a56d3fbcd8840041573e44be8e2ea6
--- /dev/null
+++ b/doc/d-labs/src/display.py
@@ -0,0 +1,55 @@
+# Typical header of a Python script using Pinocchio
+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+import pinocchio as se3
+import gepetto.corbaserver
+
+# Example of a class Display that connect to Gepetto-viewer and implement a
+# 'place' method to set the position/rotation of a 3D visual object in a scene.
+class Display():
+ '''
+ Class Display: Example of a class implementing a client for the Gepetto-viewer server. The main
+ method of the class is 'place', that sets the position/rotation of a 3D visual object in a scene.
+ '''
+ def __init__(self,windowName = "pinocchio" ):
+ '''
+ This function connect with the Gepetto-viewer server and open a window with the given name.
+ If the window already exists, it is kept in the current state. Otherwise, the newly-created
+ window is set up with a scene named 'world'.
+ '''
+
+ # Create the client and connect it with the display server.
+ try:
+ self.viewer=gepetto.corbaserver.Client()
+ except:
+ print "Error while starting the viewer client. "
+ print "Check whether Gepetto-viewer is properly started"
+
+ # Open a window for displaying your model.
+ try:
+ # If the window already exists, do not do anything.
+ windowID = self.viewer.gui.getWindowID (windowName)
+ print "Warning: window '"+windowName+"' already created."
+ print "The previously created objects will not be destroyed and do not have to be created again."
+ except:
+ # Otherwise, create the empty window.
+ windowID = self.viewer.gui.createWindow (windowName)
+ # Start a new "scene" in this window, named "world", with just a floor.
+ self.viewer.gui.createScene("world")
+ self.viewer.gui.addSceneToWindow("world",windowID)
+
+ # Finally, refresh the layout to obtain your first rendering.
+ self.viewer.gui.refresh()
+
+ def place(self,objName,M,refresh=True):
+ '''
+ This function places (ie changes both translation and rotation) of the object
+ names "objName" in place given by the SE3 object "M". By default, immediately refresh
+ the layout. If multiple objects have to be placed at the same time, do the refresh
+ only at the end of the list.
+ '''
+ self.viewer.gui.applyConfiguration(objName,
+ se3ToXYZQUAT(M))
+ if refresh: self.viewer.gui.refresh()
+
diff --git a/doc/d-labs/src/dpendulum.py b/doc/d-labs/src/dpendulum.py
new file mode 100644
index 0000000000000000000000000000000000000000..0b33a68c4712b2be44f9e9aba88c366d00d5c3f9
--- /dev/null
+++ b/doc/d-labs/src/dpendulum.py
@@ -0,0 +1,149 @@
+from pendulum import Pendulum
+import numpy as np
+from numpy import pi
+import time
+
+p = Pendulum(1)
+
+NQ = 20 # Discretization steps for position
+NV = 19 # Discretization steps for velocity
+VMAX = 5 # Max velocity (v in [-vmax,vmax])
+NU = 9 # Discretization steps for torque
+UMAX = 5 # Max torque (u in [-umax,umax])
+DT = 3e-1
+
+DQ = 2*pi/NQ
+DV = 2.0*(VMAX)/NV
+DU = 2.0*(UMAX)/NU
+
+# Continuous to discrete
+def c2dq(q):
+ q = (q+pi)%(2*pi)
+ return int(round(q/DQ)) % NQ
+
+def c2dv(v):
+ v = np.clip(v,-VMAX+1e-3,VMAX-1e-3)
+ return int(np.floor((v+VMAX)/DV))
+
+def c2du(u):
+ u = np.clip(u,-UMAX+1e-3,UMAX-1e-3)
+ return int(np.floor((u+UMAX)/DU))
+
+def c2d(qv):
+ '''From continuous to discrete.'''
+ return c2dq(qv[0]),c2dv(qv[1])
+
+# Discrete to continuous
+def d2cq(iq):
+ iq = np.clip(iq,0,NQ-1)
+ return iq*DQ - pi
+
+def d2cv(iv):
+ iv = np.clip(iv,0,NV-1) - (NV-1)/2
+ return iv*DV
+
+def d2cu(iu):
+ iu = np.clip(iu,0,NU-1) - (NU-1)/2
+ return iu*DU
+
+def d2c(iqv):
+ '''From discrete to continuous'''
+ return d2cq(iqv[0]),d2cv(iqv[1])
+
+def x2i(x): return x[0]+x[1]*NQ
+def i2x(i): return [ i%NQ, i/NQ ]
+
+# --- PENDULUM
+
+class DPendulum:
+ def __init__(self):
+ self.pendulum = Pendulum(1)
+ self.pendulum.DT = DT
+ self.pendulum.NDT = 5
+
+ @property
+ def nqv(self): return [NQ,NV]
+ @property
+ def nx(self): return NQ*NV
+ @property
+ def nu(self): return NU
+ @property
+ def goal(self): return x2i(c2d([0.,0.]))
+
+ def reset(self,x=None):
+ if x is None:
+ x = [ np.random.randint(0,NQ), np.random.randint(0,NV) ]
+ else: x = i2x(x)
+ assert(len(x)==2)
+ self.x = x
+ return x2i(self.x)
+
+ def step(self,iu):
+ self.x = self.dynamics(self.x,iu)
+ reward = 1 if x2i(self.x)==self.goal else 0
+ return x2i(self.x),reward
+
+ def render(self):
+ q = d2cq(self.x[0])
+ self.pendulum.display(np.matrix([q,]))
+ time.sleep(self.pendulum.DT)
+
+ def dynamics(self,ix,iu):
+ x = np.matrix(d2c (ix)).T
+ u = d2cu(iu)
+
+ self.xc,_ = self.pendulum.dynamics(x,u)
+ return c2d(x.T.tolist()[0])
+
+
+'''
+env = DPendulum()
+
+print env.reset(x2i([14,11]))
+hq = []
+hv = []
+hqc = []
+hvc = []
+u = 0
+for i in range(100):
+ ix,r=env.step(u)
+ q,v = i2x(ix)
+ env.render()
+ if d2cv(v)==0.0: u = MAXU-1 if u==0 else 0
+ hq.append( d2cq(env.x[0]) )
+ hv.append( d2cv(env.x[1]) )
+ hqc.append( env.xc[0,0] )
+ hvc.append( env.xc[1,0] )
+
+'''
+
+'''
+
+
+EPS = 1e-3
+q = 0.0
+v = -VMAX
+hq = []
+hv = []
+hiq = []
+hiv = []
+hqa = []
+hva = []
+
+while q<2*pi:
+ hq.append(q)
+ iq = c2dq(q)
+ hiq.append(iq)
+ hqa.append(d2cq(iq))
+ q += EPS
+while v<VMAX:
+ iv = c2dv(v)
+ hv.append(v)
+ hiv.append(iv)
+ hva.append(d2cv(iv))
+ v += EPS
+
+
+
+
+'''
diff --git a/doc/d-labs/src/factor.py b/doc/d-labs/src/factor.py
new file mode 100644
index 0000000000000000000000000000000000000000..09952a96a9089db8cac97e711fbc58ad6aa14366
--- /dev/null
+++ b/doc/d-labs/src/factor.py
@@ -0,0 +1,100 @@
+from pinocchio.utils import zero, eye
+import numpy as np
+import numpy.linalg as npl
+
+'''
+This file implements a sparse linear problem (quadric cost, linear constraints -- LCQP)
+where the decision variables are denoted by x=(x1 ... xn), n being the number of factors.
+The problem can be written:
+ min Sum_i=1^p || A_i x - b_i ||^2
+x1...xn
+
+so that forall j=1:q C_j x = d_i
+
+Matrices A_i and C_j are block sparse, i.e. they are acting only on some (few) of the variables
+x1 .. xn.
+
+The file implements the main class FactorGraph, which stores the LCQP problem and solve it.
+It also provides a secondary class Factor, used to set up FactorGraph
+'''
+
+
+class Factor(object):
+ '''
+ A factor is a part of a linear constraint corresponding either a cost ||A x - b|| or
+ a constraint Cx = d.
+ In both cases, we have Ax = sum A_i x_i, where some A_i are null. One object of class
+ Factor stores one of the A_i, along with the correspond <i> index. It is simply a pair
+ (index, matrix).
+
+ This class is used as a arguments of some of the setup functions of FactorGraph.
+ '''
+ def __init__(self, index, matrix):
+ self.index = index
+ self.matrix = matrix
+
+
+class FactorGraph(object):
+ '''
+ The class FactorGraph stores a block-sparse linear-constrained quadratic program (LCQP)
+ of variable x=(x1...xn). The size of the problem is set up at construction of the object.
+ Methods add_factor() and add_factor_constraint() are used to set up the problem.
+ Method solve() is used to compute the solution to the problem.
+ '''
+ def __init__(self, variableSize, nbVariables):
+ '''
+ Initialize a QP sparse problem as min || A x - b || so that C x = d
+ where x = (x1, .., xn), and dim(xi) = variableSize and n = nbVariables
+ After construction, A, b, C and d are allocated and set to 0.
+ '''
+ self.nx = variableSize
+ self.N = nbVariables
+ self.A = zero([0, self.N * self.nx])
+ self.b = zero(0)
+ self.C = zero([0, self.N * self.nx])
+ self.d = zero(0)
+
+ def matrix_form_factor(self, factors):
+ '''
+ Internal function: not designed to be called by the user.
+ Create a factor matrix [ A1 0 A2 0 A3 ... ] where the Ai's are placed at
+ the indexes of the factors.
+ '''
+ assert(len(factors) > 0)
+ nr = factors[0].matrix.shape[0] # nb rows of the factor
+ nc = self.nx * self.N # nb cols
+
+ # Define and fill the new rows to be added
+ A = zero([nr, nc]) # new factor to be added to self.A
+ for factor in factors:
+ assert(factor.matrix.shape == (nr, self.nx))
+ A[:, self.nx * factor.index:self.nx * (factor.index + 1)] = factor.matrix
+ return A
+
+ def add_factor(self, factors, reference):
+ '''
+ Add a factor || sum_{i} factor[i].matrix * x_{factor[i].index} - reference ||
+ to the cost.
+ '''
+ # Add the new rows to the cost matrix.
+ self.A = np.vstack([self.A, self.matrix_form_factor(factors)])
+ self.b = np.vstack([self.b, reference])
+
+ def add_factor_constraint(self, factors, reference):
+ '''
+ Add a factor sum_{i} factor[i].matrix * x_{factor[i].index} = reference
+ to the constraints.
+ '''
+ # Add the new rows to the cost matrix.
+ self.C = np.vstack([self.C, self.matrix_form_factor(factors)])
+ self.d = np.vstack([self.d, reference])
+
+ def solve(self, eps=1e-8):
+ '''
+ Implement a LCQP solver, with numerical threshold eps.
+ '''
+ Cp = npl.pinv(self.C, eps)
+ xopt = Cp * self.d
+ P = eye(self.nx * self.N) - Cp * self.C
+ xopt += npl.pinv(self.A * P, eps) * (self.b - self.A * xopt)
+ return xopt
diff --git a/doc/d-labs/src/foot_steps.py b/doc/d-labs/src/foot_steps.py
new file mode 100644
index 0000000000000000000000000000000000000000..56184d03276ad89503857d79bbafdc2201297c55
--- /dev/null
+++ b/doc/d-labs/src/foot_steps.py
@@ -0,0 +1,100 @@
+class FootSteps(object):
+ '''
+ The class stores three functions of time: left, right and flying_foot.
+ Each function is piecewise constant. For each function, the user can ask
+ what is the value of this function at time t.
+
+ The storage is composed of three lists for left, right and flying_foot, and a list for time.
+ The list of times stores the time intervals, i.e. each element of the list is
+ the start of a time interval. The first element of the list is 0.
+ The value of the functions left, right, flying_foot one this time interval is stored at
+ the same position is their respective list (i.e. value of left on interval
+ [time[i], time[i+1]] is stored in left[i].
+
+ The 4 lists are set up using function add_phase().
+ The values of functions left, right, flying_foot can be accessed through the function
+ get_phase_type(t), get_left_position(t), get_right_position(t).
+ phase_type are 'left' (meaning left foot is flying, right foot is fixed), 'right' (ie the opposite)
+ or 'none' (meaning no foot is flying, both are fixed on the ground).
+
+ Additionnally, functions get_left_next_position(t),
+ get_right_next_position(t) can be used to get the next position of the
+ flying foot (in that case, additional work is needed to compute the
+ position of flying foot at time t by interpolating get_left_position(t)
+ and get_left_next_position(t).
+
+ Functions get_phase_start(t), get_phase_duration(t) and get_phase_remaining(t)
+ can be used to get the starting time, the duration and the remaining time of the
+ current phase at time t.
+ '''
+
+ def __init__(self, right, left):
+ '''
+ The class is initiated from the initial positions of left and right feet.
+ '''
+ self.right = [right]
+ self.left = [left]
+ self.time = [0.]
+ self.flying_foot = []
+
+ def add_phase(self, duration, foot, position=None):
+ '''
+ Add a phase lasting <duration> where the flyhing foot <foot> (either 'left' or 'right')
+ moves to <position> (being a vector or a SE3 placement).
+ Alternatively, <foot> might be set to 'none' (i.e double support). In that case, <position>
+ is not specified (or is set to None, default).
+ '''
+ assert(foot == 'left' or foot == 'right' or foot == 'none')
+ self.time.append(self.time[-1] + duration)
+ self.right.append(self.right[-1])
+ self.left.append(self.left[-1])
+ self.flying_foot.append(foot)
+ if foot == 'left':
+ self.left[-1] = position
+ elif foot == 'right':
+ self.right[-1] = position
+
+ def get_index_from_time(self, t):
+ '''Return the index i of the interval containing t, i.e. t in time[i], time[i+1] '''
+ if t > self.time[-1]:
+ return len(self.time) - 1
+ return next(i for i, ti in enumerate(self.time) if ti > t) - 1
+
+ def get_phase_type(self, t):
+ i = self.get_index_from_time(t)
+ return self.flying_foot[i]
+
+ def is_double_from_left_to_right(self, t):
+ '''
+ Suppose that phase at time <t> is a double support phase.
+ Return True if the previous phase is left and/or the next phase is right.
+ '''
+ assert(self.get_phase_type(t) == 'none')
+ i = self.get_index_from_time(t)
+ return self.flying_foot[i - 1] == 'left' if i > 0 else self.flying_foot[i + 1] == 'right'
+
+ def get_left_position(self, t):
+ return self.left[self.get_index_from_time(t)]
+
+ def get_right_position(self, t):
+ return self.right[self.get_index_from_time(t)]
+
+ def get_left_next_position(self, t):
+ i = self.get_index_from_time(t)
+ i = i + 1 if i + 1 < len(self.time) else i
+ return self.left[i]
+
+ def get_right_next_position(self, t):
+ i = self.get_index_from_time(t)
+ i = i + 1 if i + 1 < len(self.time) else i
+ return self.right[i]
+
+ def get_phase_start(self, t):
+ return self.time[self.get_index_from_time(t)]
+
+ def get_phase_duration(self, t):
+ i = self.get_index_from_time(t)
+ return self.time[i + 1] - self.time[i]
+
+ def get_phase_remaining(self, t):
+ return self.time[self.get_index_from_time(t) + 1] - t
diff --git a/doc/d-labs/src/graph.py b/doc/d-labs/src/graph.py
new file mode 100644
index 0000000000000000000000000000000000000000..f7ce9e97b9bf4578ea49e1c487d15636b3235efb
--- /dev/null
+++ b/doc/d-labs/src/graph.py
@@ -0,0 +1,54 @@
+class Graph(object):
+ def __init__(self):
+ self.children = {} # dictionnary giving the list of childrens for each node.
+ self.q = [] # configuration associated to each node.
+ self.connex = [] # ID of the connex component the node is belonging to.
+ self.nconnex = 0 # number of connex components.
+ self.existing_connex = [] # List of existing connex component ID.
+
+ def add_node(self, q=None, new_connex=False):
+ '''
+ Create the memory to store a new edge. Initialize all components to None.
+ Create an empty list of children.
+ '''
+ idx = len(self.children)
+ self.children[idx] = []
+ self.q.append(q)
+ self.connex.append(None)
+ if new_connex:
+ self.new_connex(idx)
+ return idx
+
+ def add_edge(self, first, second, orientation=0):
+ '''
+ Add edge from first to second. Also add edge from second to first if orientation
+ is null.
+ '''
+ assert(first in self.children and second in self.children)
+ if orientation >= 0:
+ self.children[first].append(second)
+ if orientation <= 0:
+ self.children[second].append(first)
+
+ def new_connex(self, idx):
+ '''
+ Create a new connex component for node <idx>
+ '''
+ self.connex[idx] = self.nconnex
+ self.existing_connex.append(self.nconnex)
+ self.nconnex += 1
+
+ def rename_connex(self, past, future):
+ '''
+ Change the index of the all the nodes belonging to a connex component.
+ Useful when merging two connex components.
+ '''
+ try:
+ self.existing_connex.remove(past)
+ self.connex = [c if c != past else future for c in self.connex]
+ except:
+ pass
+
+ def connexIndexes(self, connex):
+ '''Return the list of all node indexes belonging to connex component <connex>.'''
+ return [i for i, c in enumerate(self.connex) if c == connex]
diff --git a/doc/d-labs/src/mobilerobot.py b/doc/d-labs/src/mobilerobot.py
new file mode 100644
index 0000000000000000000000000000000000000000..c591fbd3e31c934420a38b29c7472332c74e8f9c
--- /dev/null
+++ b/doc/d-labs/src/mobilerobot.py
@@ -0,0 +1,70 @@
+import pinocchio as se3
+from pinocchio.robot_wrapper import RobotWrapper
+from pinocchio.utils import eye, se3ToXYZQUAT
+
+import numpy as np
+
+
+class MobileRobotWrapper(RobotWrapper):
+ '''
+ The class models a mobile robot with UR5 arm, feature 3+6 DOF.
+ The configuration of the basis is represented by [X, Y, cos, sin], with the additionnal constraint
+ that (cos, sin) should have norm 1. Hence take care when sampling and integrating configurations.
+ The robot is depicted as an orange box with two black cylinders (wheels) atop of which is the
+ classical (realistic) UR5 model.
+ The model also features to OPERATIONAL frames, named "mobile" (on the front of the mobile basis, 30cm
+ above the ground) and "tool" (at the very end of the effector).
+ '''
+
+ def __init__(self, urdf, pkgs):
+ super(MobileRobotWrapper, self).__init__(self, urdf, pkgs, se3.JointModelPlanar())
+
+ M0 = se3.SE3(eye(3), np.matrix([.0, .0, .6]).T)
+ self.model.jointPlacements[2] = M0 * self.model.jointPlacements[2]
+ self.visual_model.geometryObjects[0].placement = M0 * self.visual_model.geometryObjects[0].placement
+ self.visual_data.oMg[0] = M0 * self.visual_data.oMg[0]
+
+ # Placement of the "mobile" frame wrt basis center.
+ basisMop = se3.SE3(eye(3), np.matrix([.3, .0, .1]).T)
+ self.model.addFrame(se3.Frame('mobile', 1, 1, basisMop, se3.FrameType.OP_FRAME))
+
+ # Placement of the tool frame wrt end effector frame (located at the center of the wrist)
+ effMop = se3.SE3(eye(3), np.matrix([.0, .08, .095]).T)
+ self.model.addFrame(se3.Frame('tool', 6, 6, effMop, se3.FrameType.OP_FRAME))
+
+ # Create data again after setting frames
+ self.data = self.model.createData()
+
+ def initDisplay(self, loadModel):
+ RobotWrapper.initDisplay(self, loadModel=loadModel)
+ if loadModel and not hasattr(self, 'display'):
+ RobotWrapper.initDisplay(self, loadModel=False)
+
+ try:
+ # self.viewer.gui.deleteNode('world/mobilebasis', True)
+ self.viewer.gui.addBox('world/mobilebasis', .25, .25, .25, [.8, .2, .2, 1])
+ self.viewer.gui.addCylinder('world/mobilewheel1', .05, .45, [.1, .0, .0, 1])
+ self.viewer.gui.addCylinder('world/mobilewheel2', .05, .45, [.1, .0, .0, 1])
+
+ self.viewer.gui.setStaticTransform('world/mobilebasis', [.0, .0, .35, 1.0, .0, .0, .0])
+ self.viewer.gui.setStaticTransform('world/mobilewheel1', [+0.15, .0, .05, .7, .7, .0, .0])
+ self.viewer.gui.setStaticTransform('world/mobilewheel2', [-0.15, .0, .05, .7, .7, .0, .0])
+
+ self.viewer.gui.addXYZaxis('world/framebasis', [1., 0., 0., 1.], .03, .1)
+ self.viewer.gui.addXYZaxis('world/frametool', [1., 0., 0., 1.], .03, .1)
+ except:
+ print("Error when adding 3d objects in the viewer ... ignore")
+
+ def display(self, q):
+ RobotWrapper.display(self, q)
+ M1 = self.data.oMi[1]
+ self.viewer.gui.applyConfiguration('world/mobilebasis', se3ToXYZQUAT(M1))
+ self.viewer.gui.applyConfiguration('world/mobilewheel1', se3ToXYZQUAT(M1))
+ self.viewer.gui.applyConfiguration('world/mobilewheel2', se3ToXYZQUAT(M1))
+ self.viewer.gui.refresh()
+
+ se3.framesKinematics(self.model, self.data)
+ self.viewer.gui.applyConfiguration('world/framebasis', se3ToXYZQUAT(self.data.oMf[-2]))
+ self.viewer.gui.applyConfiguration('world/frametool', se3ToXYZQUAT(self.data.oMf[-1]))
+
+ self.viewer.gui.refresh()
diff --git a/doc/d-labs/src/ocp.py b/doc/d-labs/src/ocp.py
new file mode 100644
index 0000000000000000000000000000000000000000..27e8bea6f738f076496cfd22463ee22412e8e18f
--- /dev/null
+++ b/doc/d-labs/src/ocp.py
@@ -0,0 +1,68 @@
+'''
+Example of optimal control resolution by direct optimization of a single trajectory.
+'''
+
+from pendulum import Pendulum
+from scipy.optimize import *
+from pinocchio.utils import *
+import pinocchio as se3
+from numpy.linalg import norm
+import time
+import signal
+import matplotlib.pyplot as plt
+
+env = Pendulum(1)
+NSTEPS = 50
+x0 = env.reset().copy()
+
+def cost(U):
+ '''Cost for a trajectory starting at state X0 with control U'''
+ env.reset(x0)
+ csum = 0.0
+ for t in range(NSTEPS):
+ u = U[env.nu*t:env.nu*(t+1)] # Control at time step <t>
+ _,r = env.step(u) # Pendulum step, with reward r
+ csum += r # Cumulative sum
+ return -csum # Returns cost ie negative reward
+
+def display(U,verbose=False):
+ '''Display the trajectory on Gepetto viewer.'''
+ x = x0.copy()
+ if verbose: print "U = ", " ".join(map(lambda u:'%.1f'%u,np.asarray(U).flatten()))
+ for i in range(len(U)/env.nu):
+ env.dynamics(x,U[env.nu*i:env.nu*(i+1)],True)
+ env.display(x)
+ time.sleep(5e-2)
+ if verbose: print "X%d"%i,x.T
+
+class CallBack:
+ '''Call back function used to follow optimizer steps.'''
+ def __init__(self):
+ self.iter = 0
+ self.withdisplay = False
+ self.h_rwd = []
+ def __call__(self,U):
+ print 'Iteration ',self.iter, " ".join(map(lambda u:'%.1f'%u,np.asarray(U).flatten()))
+ self.iter += 1
+ self.U = U.copy()
+ self.h_rwd.append(cost(U))
+ if self.withdisplay: r = display(U) # Display if CTRL-Z has been pressed.
+ def setWithDisplay(self,boolean = None):
+ self.withdisplay = not self.withdisplay if boolean is None else boolean
+callback = CallBack()
+signal.signal(signal.SIGTSTP, lambda x,y:callback.setWithDisplay())
+
+### --- OCP resolution
+U0 = zero(NSTEPS*env.nu)-env.umax # Initial guess for the control trajectory.
+bounds = [ [-env.umax,env.umax], ]*env.nu*NSTEPS # Set control bounds to environment umax.
+
+# Start BFGS optimization routine
+U,c,info = fmin_l_bfgs_b(cost,x0=U0,callback=callback,
+ approx_grad=True,bounds=bounds)
+
+# When done, display the trajectory in Gepetto-viewer
+display(U,True)
+
+plt.plot(callback.h_rwd)
+plt.show()
+
diff --git a/doc/d-labs/src/pendulum.py b/doc/d-labs/src/pendulum.py
new file mode 100644
index 0000000000000000000000000000000000000000..b2c46a2950d58278a66baabb934cfef5b063abda
--- /dev/null
+++ b/doc/d-labs/src/pendulum.py
@@ -0,0 +1,188 @@
+'''
+Create a simulation environment for a N-pendulum.
+Example of use:
+
+env = Pendulum(N)
+env.reset()
+
+for i in range(1000):
+ env.step(zero(env.nu))
+ env.render()
+
+'''
+
+
+from pinocchio.utils import *
+from pinocchio.explog import exp,log
+from numpy.linalg import pinv,norm
+import pinocchio as se3
+import gepetto.corbaserver
+from display import Display
+from numpy.linalg import pinv,norm,inv
+import time
+
+
+class Visual:
+ '''
+ Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
+ * the name of the 3D objects inside Gepetto viewer.
+ * the ID of the joint in the kinematic tree to which the body is attached.
+ * the placement of the body with respect to the joint frame.
+ This class is only used in the list Robot.visuals (see below).
+ '''
+ def __init__(self,name,jointParent,placement):
+ self.name = name # Name in gepetto viewer
+ self.jointParent = jointParent # ID (int) of the joint
+ self.placement = placement # placement of the body wrt joint, i.e. bodyMjoint
+ def place(self,display,oMjoint):
+ oMbody = oMjoint*self.placement
+ display.place(self.name,oMbody,False)
+
+class Pendulum:
+ '''
+ Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
+ The configuration is nq=7. The velocity is the same.
+ The members of the class are:
+ * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
+ * model: the kinematic tree of the robot.
+ * data: the temporary variables to be used by the kinematic algorithms.
+ * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
+ an object Visual (see above).
+
+ See tp1.py for an example of use.
+ '''
+
+ def __init__(self,nbJoint=1):
+ '''Create a Pinocchio model of a N-pendulum, with N the argument <nbJoint>.'''
+ self.viewer = Display()
+ self.visuals = []
+ self.model = se3.Model.BuildEmptyModel()
+ self.createPendulum(nbJoint)
+ self.data = self.model.createData()
+
+ self.q0 = zero(self.model.nq)
+
+ self.DT = 5e-2 # Step length
+ self.NDT = 2 # Number of Euler steps per integration (internal)
+ self.Kf = .10 # Friction coefficient
+ self.vmax = 8.0 # Max velocity (clipped if larger)
+ self.umax = 2.0 # Max torque (clipped if larger)
+ self.withSinCos = False # If true, state is [cos(q),sin(q),qdot], else [q,qdot]
+
+ def createPendulum(self,nbJoint,rootId=0,prefix='',jointPlacement=None):
+ color = [red,green,blue,transparency] = [1,1,0.78,1.0]
+ colorred = [1.0,0.0,0.0,1.0]
+
+ jointId = rootId
+ jointPlacement = jointPlacement if jointPlacement!=None else se3.SE3.Identity()
+ length = 1.0
+ mass = length
+ inertia = se3.Inertia(mass,
+ np.matrix([0.0,0.0,length/2]).T,
+ mass/5*np.diagflat([ 1e-2,length**2, 1e-2 ]) )
+
+ for i in range(nbJoint):
+ istr = str(i)
+ name = prefix+"joint"+istr
+ jointName,bodyName = [name+"_joint",name+"_body"]
+ jointId = self.model.addJoint(jointId,se3.JointModelRY(),jointPlacement,jointName)
+ self.model.appendBodyToJoint(jointId,inertia,se3.SE3.Identity())
+ try:self.viewer.viewer.gui.addSphere('world/'+prefix+'sphere'+istr, 0.15,colorred)
+ except: pass
+ self.visuals.append( Visual('world/'+prefix+'sphere'+istr,jointId,se3.SE3.Identity()) )
+ try:self.viewer.viewer.gui.addCapsule('world/'+prefix+'arm'+istr, .1,.8*length,color)
+ except:pass
+ self.visuals.append( Visual('world/'+prefix+'arm'+istr,jointId,
+ se3.SE3(eye(3),np.matrix([0.,0.,length/2]))))
+ jointPlacement = se3.SE3(eye(3),np.matrix([0.0,0.0,length]).T)
+
+ self.model.addFrame( se3.Frame('tip',jointId,0,jointPlacement,se3.FrameType.OP_FRAME) )
+
+ def display(self,q):
+ se3.forwardKinematics(self.model,self.data,q)
+ for visual in self.visuals:
+ visual.place( self.viewer,self.data.oMi[visual.jointParent] )
+ self.viewer.viewer.gui.refresh()
+
+
+ @property
+ def nq(self): return self.model.nq
+ @property
+ def nv(self): return self.model.nv
+ @property
+ def nx(self): return self.nq+self.nv
+ @property
+ def nobs(self): return self.nx+self.withSinCos
+ @property
+ def nu(self): return self.nv
+
+ def reset(self,x0=None):
+ if x0 is None:
+ q0 = np.pi*(rand(self.nq)*2-1)
+ v0 = rand(self.nv)*2-1
+ x0 = np.vstack([q0,v0])
+ assert len(x0)==self.nx
+ self.x = x0.copy()
+ self.r = 0.0
+ return self.obs(self.x)
+
+ def step(self,u):
+ assert(len(u)==self.nu)
+ _,self.r = self.dynamics(self.x,u)
+ return self.obs(self.x),self.r
+
+ def obs(self,x):
+ if self.withSinCos:
+ return np.vstack([ np.vstack([np.cos(qi),np.sin(qi)]) for qi in x[:self.nq] ]
+ + [x[self.nq:]],)
+ else: return x.copy()
+
+ def tip(self,q):
+ '''Return the altitude of pendulum tip'''
+ se3.framesKinematics(self.model,self.data,q)
+ return self.data.oMf[1].translation[2,0]
+
+ def dynamics(self,x,u,display=False):
+ '''
+ Dynamic function: x,u -> xnext=f(x,y).
+ Put the result in x (the initial value is destroyed).
+ Also compute the cost of making this step.
+ Return x for convenience along with the cost.
+ '''
+
+ modulePi = lambda th: (th+np.pi)%(2*np.pi)-np.pi
+ sumsq = lambda x : np.sum(np.square(x))
+
+ cost = 0.0
+ q = modulePi(x[:self.nq])
+ v = x[self.nq:]
+ u = np.clip(np.reshape(np.matrix(u),[self.nu,1]),-self.umax,self.umax)
+
+
+ DT = self.DT/self.NDT
+ for i in range(self.NDT):
+ se3.computeAllTerms(self.model,self.data,q,v)
+ M = self.data.M
+ b = self.data.nle
+ #tau = u-self.Kf*v
+ a = inv(M)*(u-self.Kf*v-b)
+
+ v += a*DT
+ q += v*DT
+ cost += (sumsq(q) + 1e-1*sumsq(v) + 1e-3*sumsq(u))*DT
+
+ if display:
+ self.display(q)
+ time.sleep(1e-4)
+
+ x[:self.nq] = modulePi(q)
+ x[self.nq:] = np.clip(v,-self.vmax,self.vmax)
+
+ return x,-cost
+
+ def render(self):
+ q = self.x[:self.nq]
+ self.display(q)
+ time.sleep(self.DT/10)
+
+
diff --git a/doc/d-labs/src/prm_display.py b/doc/d-labs/src/prm_display.py
new file mode 100644
index 0000000000000000000000000000000000000000..a12f30b70165d6aa7b459744cc5395c3cc1a81d0
--- /dev/null
+++ b/doc/d-labs/src/prm_display.py
@@ -0,0 +1,41 @@
+import time
+from pinocchio.utils import se3ToXYZQUAT
+
+
+def display_prm(robot, graph):
+ '''Take a graph object containing a list of configurations q and
+ a dictionnary of graph relations edge. Display the configurations by the correspond
+ placement of the robot end effector. Display the graph relation by vertices connecting
+ the robot end effector positions.
+ '''
+
+ gui = robot.viewer.gui
+
+ try:
+ gui.deleteNode('world/prm', True)
+ except:
+ pass
+ gui.createRoadmap('world/prm', [1., .2, .2, .8], 1e-2, 1e-2, [1., .2, .2, .8])
+
+ for q in graph.q:
+ gui.addNodeToRoadmap('world/prm', se3ToXYZQUAT(robot.position(q, 6)))
+
+ for parent, children in graph.children.items():
+ for child in children:
+ if child > parent:
+ q1, q2 = graph.q[parent], graph.q[child]
+ p1 = robot.position(q1, 6).translation.ravel().tolist()[0]
+ p2 = robot.position(q2, 6).translation.ravel().tolist()[0]
+ gui.addEdgeToRoadmap('world/prm', p1, p2)
+
+ gui.refresh()
+
+
+def display_path(robot, path, sleeptime=1e-2):
+ '''
+ Display a path, i.e. a sequence of robot configuration, by moving the robot
+ to each list element.
+ '''
+ for q in path:
+ robot.display(q)
+ time.sleep(sleeptime)
diff --git a/doc/d-labs/src/qnet.py b/doc/d-labs/src/qnet.py
new file mode 100644
index 0000000000000000000000000000000000000000..d0870433004cbd68a0716ac9e50fcd5bec42c4f3
--- /dev/null
+++ b/doc/d-labs/src/qnet.py
@@ -0,0 +1,104 @@
+'''
+Example of Q-table learning with a simple discretized 1-pendulum environment using a linear Q network.
+'''
+
+import numpy as np
+import random
+import tensorflow as tf
+import matplotlib.pyplot as plt
+from dpendulum import DPendulum
+import signal
+import time
+
+### --- Random seed
+RANDOM_SEED = int((time.time()%10)*1000)
+print "Seed = %d" % RANDOM_SEED
+np.random.seed(RANDOM_SEED)
+tf.set_random_seed(RANDOM_SEED)
+
+### --- Hyper paramaters
+NEPISODES = 500 # Number of training episodes
+NSTEPS = 50 # Max episode length
+LEARNING_RATE = 0.1 # Step length in optimizer
+DECAY_RATE = 0.99 # Discount factor
+
+### --- Environment
+env = DPendulum()
+NX = env.nx
+NU = env.nu
+
+### --- Q-value networks
+class QValueNetwork:
+ def __init__(self):
+ x = tf.placeholder(shape=[1,NX],dtype=tf.float32)
+ W = tf.Variable(tf.random_uniform([NX,NU],0,0.01,seed=100))
+ qvalue = tf.matmul(x,W)
+ u = tf.argmax(qvalue,1)
+
+ qref = tf.placeholder(shape=[1,NU],dtype=tf.float32)
+ loss = tf.reduce_sum(tf.square(qref - qvalue))
+ optim = tf.train.GradientDescentOptimizer(LEARNING_RATE).minimize(loss)
+
+ self.x = x # Network input
+ self.qvalue = qvalue # Q-value as a function of x
+ self.u = u # Policy as a function of x
+ self.qref = qref # Reference Q-value at next step (to be set to l+Q o f)
+ self.optim = optim # Optimizer
+
+### --- Tensor flow initialization
+tf.reset_default_graph()
+qvalue = QValueNetwork()
+sess = tf.InteractiveSession()
+tf.global_variables_initializer().run()
+
+def onehot(ix,n=NX):
+ '''Return a vector which is 0 everywhere except index <i> set to 1.'''
+ return np.array([[ (i==ix) for i in range(n) ],],np.float)
+
+def disturb(u,i):
+ u += int(np.random.randn()*10/(i/50+10))
+ return np.clip(u,0,NU-1)
+
+def rendertrial(maxiter=100):
+ x = env.reset()
+ for i in range(maxiter):
+ u = sess.run(qvalue.u,feed_dict={ qvalue.x:onehot(x) })
+ x,r = env.step(u)
+ env.render()
+ if r==1: print 'Reward!'; break
+signal.signal(signal.SIGTSTP, lambda x,y:rendertrial()) # Roll-out when CTRL-Z is pressed
+
+### --- History of search
+h_rwd = [] # Learning history (for plot).
+
+### --- Training
+for episode in range(1,NEPISODES):
+ x = env.reset()
+ rsum = 0.0
+
+ for step in range(NSTEPS-1):
+ u = sess.run(qvalue.u,feed_dict={ qvalue.x: onehot(x) })[0] # Greedy policy ...
+ u = disturb(u,episode) # ... with noise
+ x2,reward = env.step(u)
+
+ # Compute reference Q-value at state x respecting HJB
+ Q2 = sess.run(qvalue.qvalue,feed_dict={ qvalue.x: onehot(x2) })
+ Qref = sess.run(qvalue.qvalue,feed_dict={ qvalue.x: onehot(x ) })
+ Qref[0,u] = reward + DECAY_RATE*np.max(Q2)
+
+ # Update Q-table to better fit HJB
+ sess.run(qvalue.optim,feed_dict={ qvalue.x : onehot(x),
+ qvalue.qref : Qref })
+
+ rsum += reward
+ x = x2
+ if reward == 1: break
+
+ h_rwd.append(rsum)
+ if not episode%20: print 'Episode #%d done with %d sucess' % (episode,sum(h_rwd[-20:]))
+
+print "Total rate of success: %.3f" % (sum(h_rwd)/NEPISODES)
+rendertrial()
+plt.plot( np.cumsum(h_rwd)/range(1,NEPISODES) )
+plt.show()
+
diff --git a/doc/d-labs/src/qtable.py b/doc/d-labs/src/qtable.py
new file mode 100644
index 0000000000000000000000000000000000000000..996111564ca3d5f84375f691d31bc55d4e120ac1
--- /dev/null
+++ b/doc/d-labs/src/qtable.py
@@ -0,0 +1,63 @@
+'''
+Example of Q-table learning with a simple discretized 1-pendulum environment.
+'''
+
+import numpy as np
+from dpendulum import DPendulum
+import matplotlib.pyplot as plt
+import signal
+import time
+
+### --- Random seed
+RANDOM_SEED = int((time.time()%10)*1000)
+print "Seed = %d" % RANDOM_SEED
+np.random.seed(RANDOM_SEED)
+
+### --- Hyper paramaters
+NEPISODES = 500 # Number of training episodes
+NSTEPS = 50 # Max episode length
+LEARNING_RATE = 0.85 #
+DECAY_RATE = 0.99 # Discount factor
+
+### --- Environment
+env = DPendulum()
+NX = env.nx # Number of (discrete) states
+NU = env.nu # Number of (discrete) controls
+
+Q = np.zeros([env.nx,env.nu]) # Q-table initialized to 0
+
+def rendertrial(maxiter=100):
+ '''Roll-out from random state using greedy policy.'''
+ s = env.reset()
+ for i in range(maxiter):
+ a = np.argmax(Q[s,:])
+ s,r = env.step(a)
+ env.render()
+ if r==1: print 'Reward!'; break
+
+signal.signal(signal.SIGTSTP, lambda x,y:rendertrial()) # Roll-out when CTRL-Z is pressed
+h_rwd = [] # Learning history (for plot).
+
+for episode in range(1,NEPISODES):
+ x = env.reset()
+ rsum = 0.0
+ for steps in range(NSTEPS):
+ u = np.argmax(Q[x,:] + np.random.randn(1,NU)/episode) # Greedy action with noise
+ x2,reward = env.step(u)
+
+ # Compute reference Q-value at state x respecting HJB
+ Qref = reward + DECAY_RATE*np.max(Q[x2,:])
+
+ # Update Q-Table to better fit HJB
+ Q[x,u] += LEARNING_RATE*(Qref-Q[x,u])
+ x = x2
+ rsum += reward
+ if reward==1: break
+
+ h_rwd.append(rsum)
+ if not episode%20: print 'Episode #%d done with %d sucess' % (episode,sum(h_rwd[-20:]))
+
+print "Total rate of success: %.3f" % (sum(h_rwd)/NEPISODES)
+rendertrial()
+plt.plot( np.cumsum(h_rwd)/range(1,NEPISODES) )
+plt.show()
diff --git a/doc/d-labs/src/robot_hand.py b/doc/d-labs/src/robot_hand.py
new file mode 100644
index 0000000000000000000000000000000000000000..f861cdd9bce82bdfbc18996a83697e1551399898
--- /dev/null
+++ b/doc/d-labs/src/robot_hand.py
@@ -0,0 +1,317 @@
+from math import pi
+
+import numpy as np
+from numpy.linalg import norm, pinv
+
+import pinocchio as se3
+from pinocchio.utils import cross, zero, rotate, eye
+from display import Display
+
+
+class Visual(object):
+ '''
+ Class representing one 3D mesh of the robot, to be attached to a joint. The class contains:
+ * the name of the 3D objects inside Gepetto viewer.
+ * the ID of the joint in the kinematic tree to which the body is attached.
+ * the placement of the body with respect to the joint frame.
+ This class is only used in the list Robot.visuals (see below).
+
+ The visual are supposed mostly to be capsules. In that case, the object also contains
+ radius and length of the capsule.
+ The collision checking computes collision test, distance, and witness points.
+ Using the supporting robot, the collision Jacobian returns a 1xN matrix corresponding
+ to the normal direction.
+ '''
+ def __init__(self, name, jointParent, placement, radius=.1, length=None):
+ '''Length and radius are used in case of capsule objects'''
+ self.name = name # Name in gepetto viewer
+ self.jointParent = jointParent # ID (int) of the joint
+ self.placement = placement # placement of the body wrt joint, i.e. bodyMjoint
+ if length is not None:
+ self.length = length
+ self.radius = radius
+
+ def place(self, display, oMjoint):
+ oMbody = oMjoint * self.placement
+ display.place(self.name, oMbody, False)
+
+ def isCapsule(self):
+ return hasattr(self, 'length') and hasattr(self, 'radius')
+
+ def collision(self, c2, data=None, oMj1=None, oMj2=None):
+ if data is not None:
+ oMj1 = data.oMi[self.jointParent]
+ oMj2 = data.oMi[c2.jointParent]
+ M1 = oMj1 * self.placement
+ M2 = oMj2 * c2.placement
+
+ assert(self.isCapsule() and c2.isCapsule())
+ l1 = self.length
+ r1 = self.radius
+ l2 = c2.length
+ r2 = c2.radius
+
+ a1 = M1.act(np.matrix([0, 0, -l1 / 2]).T)
+ b1 = M2.act(np.matrix([0, 0, -l2 / 2]).T)
+ a2 = M1.act(np.matrix([0, 0, +l1 / 2]).T)
+ b2 = M2.act(np.matrix([0, 0, +l2 / 2]).T)
+
+ ab = pinv(np.hstack([a1 - a2, b2 - b1])) * (b2 - a2)
+
+ if all(0 <= ab <= 1):
+ asat = bsat = False
+ pa = a2 + ab[0, 0] * (a1 - a2)
+ pb = b2 + ab[1, 0] * (b1 - b2)
+ else:
+ asat = bsat = True
+ i = np.argmin(np.vstack([ab, 1 - ab]))
+
+ pa = a2 if i == 0 else a1
+ pb = b2 if i == 1 else b1
+ if i == 0 or i == 2: # fix a to pa, search b
+ b = (pinv(b1 - b2) * (pa - b2))[0, 0]
+ if b < 0:
+ pb = b2
+ elif b > 1:
+ pb = b1
+ else:
+ pb = b2 + b * (b1 - b2)
+ bsat = False
+ else: # fix b
+ a = (pinv(a1 - a2) * (pb - a2))[0, 0]
+ if a < 0:
+ pa = a2
+ elif a > 1:
+ pa = a1
+ else:
+ pa = a2 + a * (a1 - a2)
+ asat = False
+
+ dist = norm(pa - pb) - (r1 + r2)
+ if norm(pa - pb) > 1e-3:
+ # Compute witness points
+ ab = pa - pb
+ ab /= norm(ab)
+ wa = pa - ab * r1
+ wb = pb + ab * r2
+
+ # Compute normal matrix
+ x = np.matrix([1., 0, 0]).T
+ r1 = cross(ab, x)
+ if norm(r1) < 1e-2:
+ x = np.matrix([0, 1., 0]).T
+ r1 = cross(ab, x)
+ r1 /= norm(r1)
+ r2 = cross(ab, r1)
+ R = np.hstack([r1, r2, ab])
+
+ self.dist = dist
+ c2.dist = dist
+ self.w = wa
+ c2.w = wb
+ self.R = R
+ c2.R = R
+
+ return dist
+
+ def jacobian(self, c2, robot, q):
+ Ja = se3.jacobian(robot.model, robot.data, q, self.jointParent, False, True)
+ Jb = se3.jacobian(robot.model, robot.data, q, c2.jointParent, False, True)
+
+ Xa = se3.SE3(self.R, self.w).action
+ Xb = se3.SE3(c2.R, c2.w).action
+
+ J = (Xa * Ja)[2, :] - (Xb * Jb)[2, :]
+ return J
+
+ def displayCollision(self, viewer, name='world/wa'):
+ viewer.viewer.gui.setVisibility(name, 'ON')
+ viewer.place(name, se3.SE3(self.R, self.w))
+
+
+class Robot(object):
+ '''
+ Define a class Robot with 7DOF (shoulder=3 + elbow=1 + wrist=3).
+ The configuration is nq=7. The velocity is the same.
+ The members of the class are:
+ * viewer: a display encapsulating a gepetto viewer client to create 3D objects and place them.
+ * model: the kinematic tree of the robot.
+ * data: the temporary variables to be used by the kinematic algorithms.
+ * visuals: the list of all the 'visual' 3D objects to render the robot, each element of the list being
+ an object Visual (see above).
+
+ CollisionPairs is a list of visual indexes.
+ Reference to the collision pair is used in the collision test and jacobian of the collision
+ (which are simply proxy method to methods of the visual class).
+ '''
+
+ def __init__(self):
+ self.viewer = Display()
+ self.visuals = []
+ self.model = se3.Model.BuildEmptyModel()
+ self.createHand()
+ self.data = self.model.createData()
+ self.q0 = zero(self.model.nq)
+ # self.q0[3] = 1.0
+ self.v0 = zero(self.model.nv)
+ self.collisionPairs = []
+
+ def createHand(self, root_id=0, prefix='', joint_placement=None):
+ def trans(x, y, z):
+ return se3.SE3(eye(3), np.matrix([x, y, z]).T)
+
+ def inertia(m, c):
+ return se3.Inertia(m, np.matrix(c, np.double).T, eye(3) * m ** 2)
+
+ def joint_name(body):
+ return prefix + body + '_joint'
+
+ def body_name(body):
+ return 'world/' + prefix + body
+
+ color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
+ joint_id = root_id
+ cm = 1e-2
+
+ joint_placement = joint_placement if joint_placement is not None else se3.SE3.Identity()
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('wrist'))
+ self.model.appendBodyToJoint(joint_id, inertia(3, [0, 0, 0]), se3.SE3.Identity())
+
+ L, W, H = 3 * cm, 5 * cm, 1 * cm
+ self.viewer.viewer.gui.addSphere(body_name('wrist'), .02, color)
+ self.viewer.viewer.gui.addBox(body_name('wpalm'), L / 2, W / 2, H, color)
+ self.visuals.append(Visual(body_name('wpalm'), joint_id, trans(L / 2, 0, 0)))
+
+ self.viewer.viewer.gui.addCapsule(body_name('wpalmb'), H, W, color)
+ self.visuals.append(Visual(body_name('wpalmb'), joint_id, se3.SE3(rotate('x', pi / 2), zero(3)), H, W))
+
+ self.viewer.viewer.gui.addCapsule(body_name('wpalmt'), H, W, color)
+ pos = se3.SE3(rotate('x', pi / 2), np.matrix([L, 0, 0]).T)
+ self.visuals.append(Visual(body_name('wpalmt'), joint_id, pos, H, W))
+
+ self.viewer.viewer.gui.addCapsule(body_name('wpalml'), H, L, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([L / 2, -W / 2, 0]).T)
+ self.visuals.append(Visual(body_name('wpalml'), joint_id, pos, H, L))
+
+ self.viewer.viewer.gui.addCapsule(body_name('wpalmr'), H, L, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([L / 2, +W / 2, 0]).T)
+ self.visuals.append(Visual(body_name('wpalmr'), joint_id, pos, H, L))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([5 * cm, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('palm'))
+ self.model.appendBodyToJoint(joint_id, inertia(2, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('palm2'), 1 * cm, W, color)
+ self.visuals.append(Visual(body_name('palm2'), joint_id, se3.SE3(rotate('x', pi / 2), zero(3)), H, W))
+
+ FL = 4 * cm
+ palmIdx = joint_id
+
+ joint_placement = se3.SE3(eye(3), np.matrix([2 * cm, W / 2, 0]).T)
+ joint_id = self.model.addJoint(palmIdx, se3.JointModelRY(), joint_placement, joint_name('finger11'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('finger11'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
+ self.visuals.append(Visual(body_name('finger11'), joint_id, pos, H, FL - 2 * H))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([FL, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('finger12'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+
+ self.viewer.viewer.gui.addCapsule(body_name('finger12'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
+ self.visuals.append(Visual(body_name('finger12'), joint_id, pos, H, FL - 2 * H))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('finger13'))
+ self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addSphere(body_name('finger13'), H, color)
+ self.visuals.append(Visual(body_name('finger13'), joint_id, trans(2 * H, 0, 0), H, 0))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([2 * cm, 0, 0]).T)
+ joint_id = self.model.addJoint(palmIdx, se3.JointModelRY(), joint_placement, joint_name('finger21'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('finger21'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
+ self.visuals.append(Visual(body_name('finger21'), joint_id, pos, H, FL - 2 * H))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([FL, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('finger22'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('finger22'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
+ self.visuals.append(Visual(body_name('finger22'), joint_id, pos, H, FL - 2 * H))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([FL - H, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('finger23'))
+ self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addSphere(body_name('finger23'), H, color)
+ self.visuals.append(Visual(body_name('finger23'), joint_id, trans(H, 0, 0), H, 0))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([2 * cm, -W / 2, 0]).T)
+ joint_id = self.model.addJoint(palmIdx, se3.JointModelRY(), joint_placement, joint_name('finger31'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('finger31'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
+ self.visuals.append(Visual(body_name('finger31'), joint_id, pos, H, FL - 2 * H))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([FL, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('finger32'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('finger32'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('y', pi / 2), np.matrix([FL / 2 - H, 0, 0]).T)
+ self.visuals.append(Visual(body_name('finger32'), joint_id, pos, H, FL - 2 * H))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([FL - 2 * H, 0, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRY(), joint_placement, joint_name('finger33'))
+ self.model.appendBodyToJoint(joint_id, inertia(.3, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addSphere(body_name('finger33'), H, color)
+ self.visuals.append(Visual(body_name('finger33'), joint_id, trans(2 * H, 0, 0), H, 0))
+
+ joint_placement = se3.SE3(eye(3), np.matrix([1 * cm, -W / 2 - H * 1.5, 0]).T)
+ joint_id = self.model.addJoint(1, se3.JointModelRY(), joint_placement, joint_name('thumb1'))
+ self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('thumb1'), H, 2 * cm, color)
+ pos = se3.SE3(rotate('z', pi / 3) * rotate('x', pi / 2), np.matrix([1 * cm, -1 * cm, 0]).T)
+ self.visuals.append(Visual(body_name('thumb1'), joint_id, pos, 2 * cm))
+
+ joint_placement = se3.SE3(rotate('z', pi / 3) * rotate('x', pi), np.matrix([3 * cm, -1.8 * cm, 0]).T)
+ joint_id = self.model.addJoint(joint_id, se3.JointModelRZ(), joint_placement, joint_name('thumb2'))
+ self.model.appendBodyToJoint(joint_id, inertia(.4, [0, 0, 0]), se3.SE3.Identity())
+ self.viewer.viewer.gui.addCapsule(body_name('thumb2'), H, FL - 2 * H, color)
+ pos = se3.SE3(rotate('x', pi / 3), np.matrix([-0.7 * cm, .8 * cm, -0.5 * cm]).T)
+ self.visuals.append(Visual(body_name('thumb2'), joint_id, pos, H, FL - 2 * H))
+
+ # Prepare some patches to represent collision points. Yet unvisible.
+ for i in range(10):
+ self.viewer.viewer.gui.addCylinder('world/wa%i' % i, .01, .003, [1.0, 0, 0, 1])
+ self.viewer.viewer.gui.addCylinder('world/wb%i' % i, .01, .003, [1.0, 0, 0, 1])
+ self.viewer.viewer.gui.setVisibility('world/wa%i' % i, 'OFF')
+ self.viewer.viewer.gui.setVisibility('world/wb%i' % i, 'OFF')
+
+ def checkCollision(self, pairIndex):
+ ia, ib = self.collisionPairs[pairIndex]
+ va = self.visuals[ia]
+ vb = self.visuals[ib]
+ dist = va.collision(vb, self.data)
+ return dist
+
+ def collisionJacobian(self, pairIndex, q):
+ ia, ib = self.collisionPairs[pairIndex]
+ va = self.visuals[ia]
+ vb = self.visuals[ib]
+ return va.jacobian(vb, self, q)
+
+ def displayCollision(self, pairIndex, meshIndex, onlyOne=False):
+ ia, ib = self.collisionPairs[pairIndex]
+ va = self.visuals[ia]
+ vb = self.visuals[ib]
+ va.displayCollision(self.viewer, 'world/wa%i' % meshIndex)
+ vb.displayCollision(self.viewer, 'world/wb%i' % meshIndex)
+ self.viewer.viewer.gui.setVisibility('world/wa%i' % meshIndex, 'ON')
+ self.viewer.viewer.gui.setVisibility('world/wb%i' % meshIndex, 'ON')
+
+ def display(self, q):
+ se3.forwardKinematics(self.model, self.data, q)
+ for visual in self.visuals:
+ visual.place(self.viewer, self.data.oMi[visual.jointParent])
+ self.viewer.viewer.gui.refresh()
diff --git a/doc/d-labs/src/ur5x4.py b/doc/d-labs/src/ur5x4.py
new file mode 100644
index 0000000000000000000000000000000000000000..ae77d9d059713f679fe40b3788d30b012c63091e
--- /dev/null
+++ b/doc/d-labs/src/ur5x4.py
@@ -0,0 +1,48 @@
+'''
+Load 4 times the UR5 model, plus a plate object on top of them, to feature a simple parallel robot.
+No optimization, this file is just an example of how to load the models.
+'''
+
+from pinocchio import SE3
+from pinocchio.robot_wrapper import RobotWrapper
+from pinocchio.utils import rotate, zero, eye, se3ToXYZQUAT, urdf
+
+import numpy as np
+
+PKG = '/opt/openrobots/share'
+URDF = PKG + '/ur5_description/urdf/ur5_gripper.urdf'
+
+
+def loadRobot(M0, name):
+ '''
+ This function load a UR5 robot n a new model, move the basis to placement <M0>
+ and add the corresponding visuals in gepetto viewer with name prefix given by string <name>.
+ It returns the robot wrapper (model,data).
+ '''
+ robot = RobotWrapper(urdf, [PKG])
+ robot.model.jointPlacements[1] = M0 * robot.model.jointPlacements[1]
+ robot.visual_model.geometryObjects[0].placement = M0 * robot.visual_model.geometryObjects[0].placement
+ robot.visual_data.oMg[0] = M0 * robot.visual_data.oMg[0]
+ robot.initDisplay(loadModel=True, viewerRootNodeName="world/" + name)
+ return robot
+
+
+robots = []
+# Load 4 Ur5 robots, placed at 0.3m from origin in the 4 directions x,y,-x,-y.
+Mt = SE3(eye(3), np.matrix([.3, 0, 0]).T) # First robot is simply translated
+for i in range(4):
+ robots.append(loadRobot(SE3(rotate('z', np.pi / 2 * i), zero(3)) * Mt, "robot%d" % i))
+
+# Set up the robots configuration with end effector pointed upward.
+q0 = np.matrix([np.pi / 4, -np.pi / 4, -np.pi / 2, np.pi / 4, np.pi / 2, 0]).T
+for i in range(4):
+ robots[i].display(q0)
+
+# Add a new object featuring the parallel robot tool plate.
+gepettoViewer = robots[0].viewer.gui
+w, h, d = 0.25, 0.25, 0.005
+color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]
+gepettoViewer.addBox('world/toolplate', w, h, d, color)
+Mtool = SE3(rotate('z', 1.268), np.matrix([0, 0, .77]).T)
+gepettoViewer.applyConfiguration('world/toolplate', se3ToXYZQUAT(Mtool))
+gepettoViewer.refresh()