diff --git a/pinocchio/Makefile b/pinocchio/Makefile
index 5137c92398537ddc5d382b950a3c2f5517437588..8201ea540641026465fca22f9aeffa9c7de4cac6 100644
--- a/pinocchio/Makefile
+++ b/pinocchio/Makefile
@@ -3,6 +3,7 @@
#
VERSION= 1.2.1
+PKGREVISION = 1
DISTNAME= pinocchio-${VERSION}
MASTER_SITES= ${MASTER_SITE_OPENROBOTS:=${PKGBASE}/}
diff --git a/pinocchio/distinfo b/pinocchio/distinfo
index d044381bad3d3e47ea6bcf63702a974c3c29c008..f02c53ea1b4cf71f8f400c85ce17cae34cb261a4 100644
--- a/pinocchio/distinfo
+++ b/pinocchio/distinfo
@@ -1,3 +1,4 @@
SHA1 (pinocchio-1.2.1.tar.gz) = 27b19328b5e438ee1437eea0b37fb5f54724b244
RMD160 (pinocchio-1.2.1.tar.gz) = 1680b89e1860dc4565a4f3b79c0ba1136a80b1ab
Size (pinocchio-1.2.1.tar.gz) = 8974603 bytes
+SHA1 (patch-aa) = 0e75f494fff0f5f0951a192426cd3a0be6980c3d
diff --git a/pinocchio/patches/patch-aa b/pinocchio/patches/patch-aa
new file mode 100644
index 0000000000000000000000000000000000000000..22a40f6ada018695f15d682a27314b28c0b197aa
--- /dev/null
+++ b/pinocchio/patches/patch-aa
@@ -0,0 +1,748 @@
+diff --git bindings/python/algorithm/expose-com.cpp bindings/python/algorithm/expose-com.cpp
+index 0cc1951..78aeb72 100644
+--- bindings/python/algorithm/expose-com.cpp
++++ bindings/python/algorithm/expose-com.cpp
+@@ -18,11 +18,15 @@
+ #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+ #include "pinocchio/algorithm/center-of-mass.hpp"
+
++#include <boost/python/overloads.hpp>
++
+ namespace se3
+ {
+ namespace python
+ {
+
++ BOOST_PYTHON_FUNCTION_OVERLOADS(jacobianCenterOfMass_overload,jacobianCenterOfMass,3,5)
++
+ static SE3::Vector3
+ com_0_proxy(const Model& model,
+ Data & data,
+@@ -85,10 +89,11 @@ namespace se3
+ "and returns the center of mass position of the full model expressed in the world frame.");
+
+ bp::def("jacobianCenterOfMass",jacobianCenterOfMass,
+- bp::args("Model","Data",
++ jacobianCenterOfMass_overload(bp::args("Model","Data",
+ "Joint configuration q (size Model::nq)"),
+- "Computes the jacobian of the center of mass, puts the result in Data and return it.",
+- bp::return_value_policy<bp::return_by_value>());
++ "Computes the jacobian of the center of mass, puts the result in Data and return it.")[
++ bp::return_value_policy<bp::return_by_value>()]);
++
+ }
+
+ } // namespace python
+diff --git bindings/python/multibody/model.hpp bindings/python/multibody/model.hpp
+index dfb3bc9..36c047c 100644
+--- bindings/python/multibody/model.hpp
++++ bindings/python/multibody/model.hpp
+@@ -20,6 +20,7 @@
+ #define __se3_python_model_hpp__
+
+ #include <boost/python/suite/indexing/vector_indexing_suite.hpp>
++#include <boost/python/overloads.hpp>
+ #include <eigenpy/memory.hpp>
+
+ #include "pinocchio/multibody/model.hpp"
+@@ -35,6 +36,9 @@ namespace se3
+ {
+ namespace bp = boost::python;
+
++ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getFrameId_overload,Model::getFrameId,1,2)
++ BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(existFrame_overload,Model::existFrame,1,2)
++
+ struct ModelPythonVisitor
+ : public boost::python::def_visitor< ModelPythonVisitor >
+ {
+@@ -127,8 +131,12 @@ namespace se3
+ .def("existBodyName", &Model::existBodyName, bp::args("name"), "Check if a frame of type BODY exists, given its name")
+ .def("getJointId",&Model::getJointId, bp::args("name"), "Return the index of a joint given by its name")
+ .def("existJointName", &Model::existJointName, bp::args("name"), "Check if a joint given by its name exists")
+- .def("getFrameId",&Model::getFrameId,bp::args("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector.")
+- .def("existFrame",&Model::existFrame,bp::args("name"),"Returns true if the frame given by its name exists inside the Model.")
++
++ .def("getFrameId",&Model::getFrameId,existFrame_overload(bp::arg("name"),"Returns the index of the frame given by its name. If the frame is not in the frames vector, it returns the current size of the frames vector."))
++ .def("getFrameId",&Model::getFrameId,existFrame_overload(bp::args("name","type"),"Returns the index of the frame given by its name and its type. If the frame is not in the frames vector, it returns the current size of the frames vector."))
++
++ .def("existFrame",&Model::existFrame,existFrame_overload(bp::arg("name"),"Returns true if the frame given by its name exists inside the Model."))
++ .def("existFrame",&Model::existFrame,existFrame_overload(bp::args("name","type"),"Returns true if the frame given by its name exists inside the Model with the given type."))
+
+ .def("addFrame",(bool (Model::*)(const std::string &,const JointIndex, const FrameIndex, const SE3 &,const FrameType &)) &Model::addFrame,bp::args("name","parent_id","placement","type"),"Add a frame to the vector of frames. See also Frame for more details. Returns False if the frame already exists.")
+ .def("addFrame",(bool (Model::*)(const Frame &)) &Model::addFrame,bp::args("frame"),"Add a frame to the vector of frames.")
+diff --git bindings/python/scripts/derivative/dcrba.py bindings/python/scripts/derivative/dcrba.py
+new file mode 100644
+index 0000000..014c201
+--- /dev/null
++++ bindings/python/scripts/derivative/dcrba.py
+@@ -0,0 +1,465 @@
++#
++# Copyright (c) 2016 CNRS
++#
++# This file is part of Pinocchio
++# Pinocchio is free software: you can redistribute it
++# and/or modify it under the terms of the GNU Lesser General Public
++# License as published by the Free Software Foundation, either version
++# 3 of the License, or (at your option) any later version.
++# Pinocchio is distributed in the hope that it will be
++# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
++# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
++# General Lesser Public License for more details. You should have
++# received a copy of the GNU Lesser General Public License along with
++# Pinocchio If not, see
++# <http://www.gnu.org/licenses/>.
++
++import pinocchio as se3
++from pinocchio.robot_wrapper import RobotWrapper
++from pinocchio.utils import *
++from numpy.linalg import norm
++import lambdas
++from lambdas import Mcross,ancestors,parent,iv,td,quad,adj,adjdual
++
++def hessian(robot,q,crossterms=False):
++ '''
++ Compute the Hessian tensor of all the robot joints.
++ If crossterms, then also compute the Si x Si terms,
++ that are not part of the true Hessian but enters in some computations like VRNEA.
++ '''
++ lambdas.setRobotArgs(robot)
++ H=np.zeros([6,robot.model.nv,robot.model.nv])
++ se3.computeJacobians(robot.model,robot.data,q)
++ J = robot.data.J
++ skiplast = -1 if not crossterms else None
++ for joint_i in range(1,robot.model.njoints):
++ for joint_j in ancestors(joint_i)[:skiplast]: # j is a child of i
++ for i in iv(joint_i):
++ for j in iv(joint_j):
++ Si = J[:,i]
++ Sj = J[:,j]
++ H[:,i,j] = np.asarray(Mcross(Sj, Si))[:,0]
++ return H
++
++
++# CRBA ====> Mp[i,j] += Si.transpose()*Yk*Sj
++# D-CRBA ==> TSi Y Sj + Si Y TSj - Si YSdx Sj + Si SdxY Sj
++# Term a = term d && term b = term c --- or are null
++# -> term a is null if j<=diff
++# -> term d is null if diff>k
++# -> then a-d is nonzero only when k>=diff>j
++# Due to simplification, terms cancel to the following:
++# if i<d (always true) M[i,j,d] = Si.T Yd TjSd (= -Si.T Yd Sd x Sj)
++# if j<d (possibly false) M[i,j,d] += TdSi.T Yd Sd (= +Si.T Sdx Yd Sj)
++# where Yd is the composite inertia of the minimal subtree rooted either at j or d.
++
++class DCRBA:
++ '''
++ Compute the derivative (tangent application) of the mass matrix M wrt q,
++ which is a NVxNVxNV tensor.
++ '''
++
++ def __init__(self,robot):
++ self.robot = robot
++ lambdas.setRobotArgs(robot)
++
++ def pre(self,q):
++ robot = self.robot
++ self.H = hessian(robot,q)
++ self.J = robot.data.J.copy()
++ se3.crba(robot.model,robot.data,q)
++ self.Y =[ (robot.data.oMi[i]*robot.data.Ycrb[i]).matrix() for i in range(0,robot.model.njoints) ]
++
++ self.dM = np.zeros([robot.model.nv,]*3)
++
++ def __call__(self):
++ robot = self.robot
++ J = self.J
++ Y = self.Y
++ dM = self.dM
++ H = self.H
++
++ for j in range(robot.model.njoints-1,0,-1):
++ for joint_diff in range(1,robot.model.njoints): # diff should be a descendant or ancestor of j
++ if j not in ancestors(joint_diff) and joint_diff not in ancestors(j): continue
++
++ for i in ancestors(min(parent(joint_diff),j)):
++
++ i0,i1 = iv(i)[0],iv(i)[-1]+1
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++
++ Si = J[:,i0:i1]
++ Sj = J[:,j0:j1]
++
++ for d in iv(joint_diff):
++
++ T_iSd = np.matrix(H[:,d,i0:i1]) # this is 0 if d<=i (<=j)
++ T_jSd = np.matrix(H[:,d,j0:j1]) # this is 0 is d<=j
++
++ '''
++ assert( norm(T_iSd)<1e-6 or not joint_diff<i ) # d<i => TiSd=0
++ assert( norm(T_jSd)<1e-6 or not joint_diff<j ) # d<j => TjSd=0
++ assert( norm(T_jSd)<1e-6 or not norm(T_iSd)<1e-6 ) # TiSd=0 => TjSd=0
++ assert( norm(T_iSd)>1e-6 )
++ '''
++
++ Yd = Y[max(j,joint_diff)]
++
++ dM [i0:i1,j0:j1,d] = T_iSd.T * Yd * Sj
++ if j<joint_diff:
++ dM[i0:i1,j0:j1,d] += Si.T * Yd * T_jSd
++
++ # Make dM triangular by copying strict-upper triangle to lower one.
++ if i!=j: dM[j0:j1,i0:i1,d] = dM[i0:i1,j0:j1,d].T
++ else: dM[j0:j1,i0:i1,d] += np.triu(dM[i0:i1,j0:j1,d],1).T
++
++ return dM
++
++# -----------------------------------------------------------------------------------
++# -----------------------------------------------------------------------------------
++# -----------------------------------------------------------------------------------
++class VRNEA:
++ '''
++ Compute the C tensor so that nle(q,vq) = vq C vq.
++ Noting Cv = C*vq for an arbitrary robot velocity vq:
++ Since Mdot = (Cv+Cv')/2, then C = dCv/dvq and dM/dq = (Cv+Cv')
++
++ Q is index by i,j,k i.e. tau_k = v.T*Q[:,:,k]*v
++ At level k, Qk is a lower triangular matrix (same shape as H) where:
++ * rows i s.t. i>k are equals to Sk.T*Ycrb_i*S_ixS_j (j the col index),
++ * rows i s.t. i<=k are equals to Sk.T*Ycrb_k*S_ixS_j (j the col index)
++ To avoid the call to Ycrb_i>k, the first par is set up while computing Q_k
++ '''
++
++ def __init__(self,robot):
++ self.robot = robot
++ lambdas.setRobotArgs(robot)
++ self.YJ = zero([6,robot.model.nv])
++ self.Q = np.zeros([robot.model.nv,]*3)
++
++ def __call__(self,q):
++ robot = self.robot
++ H = hessian(robot,q,crossterms=True)
++ J = robot.data.J
++ YJ = self.YJ
++ Q = self.Q
++
++ # The terms in SxS YS corresponds to f = vxYv
++ # The terms in S YSxS corresponds to a = vxv (==> f = Yvxv)
++ for k in range(robot.model.njoints-1,0,-1):
++ k0,k1 = iv(k)[0],iv(k)[-1]+1
++ Yk = (robot.data.oMi[k]*robot.data.Ycrb[k]).matrix()
++ Sk = J[:,k0:k1]
++ for j in ancestors(k): # Fill YJ = [ ... Yk*Sj ... ]_j
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++ YJ[:,j0:j1] = Yk*J[:,j0:j1]
++
++ # Fill the diagonal of the current level of Q = Q[k,:k,:k]
++ for i in ancestors(k):
++ i0,i1 = iv(i)[0],iv(i)[-1]+1
++ Si = J[:,i0:i1]
++
++ Q[k0:k1,i0:i1,i0:i1] = -td(H[:,k0:k1,i0:i1],YJ[:,i0:i1],[0,0]) # = (Si x Sk)' * Yk Si
++
++ # Fill the nondiag of the current level Q[k,:k,:k]
++ for j in ancestors(i)[:-1]:
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++ Sj = J[:,j0:j1]
++
++ # = Sk' * Yk * Si x Sj
++ Q[k0:k1,i0:i1,j0:j1] = td(YJ[:,k0:k1], H[:,i0:i1,j0:j1],[0,0])
++ # = (Si x Sk)' * Yk Sj
++ Q[k0:k1,i0:i1,j0:j1] -= td(H[:,k0:k1,i0:i1],YJ[:,j0:j1], [0,0])
++ # = (Sj x Sk)' * Yk Si
++ Q[k0:k1,j0:j1,i0:i1] =- td(H[:,k0:k1,j0:j1],YJ[:,i0:i1], [0,0])
++
++ # Fill the border elements of levels below k Q[kk,k,:] and Q[kk,:,k] with kk<k
++ for kk in ancestors(k)[:-1]:
++ kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
++ Skk = J[:,kk0:kk1]
++
++ for j in ancestors(k):
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++ Sj = J[:,j0:j1]
++
++ # = Skk' Yk Sk x Sj = (Yk Skk)' Sk x Sj
++ if k!=j:
++ Q[kk0:kk1,k0:k1,j0:j1] = td(YJ[:,kk0:kk1],H[:,k0:k1,j0:j1 ],[0,0])
++ # = (Sk x Skk)' Yk Sj
++ Q[kk0:kk1,k0:k1,j0:j1] += td(H[:,k0:k1,kk0:kk1].T,YJ[:,j0:j1], [2,0])
++ # = (Sj x Skk)' Yk Sk
++ # Switch because j can be > or < than kk
++ if j<kk:
++ Q[kk0:kk1,j0:j1,k0:k1] = -Q[j0:j1,kk0:kk1,k0:k1].transpose(1,0,2)
++ elif j>=kk:
++ Q[kk0:kk1,j0:j1,k0:k1] = td(H[:,j0:j1,kk0:kk1].T,YJ[:,k0:k1], [2,0])
++
++ return Q
++
++# -----------------------------------------------------------------------------------
++# -----------------------------------------------------------------------------------
++# -----------------------------------------------------------------------------------
++class Coriolis:
++ def __init__(self,robot):
++ self.robot = robot
++ lambdas.setRobotArgs(robot)
++ NV = robot.model.nv
++ NJ = robot.model.njoints
++ self.C = zero([NV,NV])
++ self.YS = zero([6,NV])
++ self.Sxv = zero([6,NV])
++
++ def __call__(self,q,vq):
++ robot = self.robot
++ NV = robot.model.nv
++ NJ = robot.model.njoints
++ C = self.C
++ YS = self.YS
++ Sxv = self.Sxv
++ H = hessian(robot,q)
++
++ se3.computeAllTerms(robot.model,robot.data,q,vq)
++ J = robot.data.J
++ oMi=robot.data.oMi
++ v = [ (oMi[i]*robot.data.v[i]).vector for i in range(NJ) ]
++ Y = [ (oMi[i]*robot.data.Ycrb[i]).matrix() for i in range(NJ) ]
++
++ # --- Precomputations
++ # Centroidal matrix
++ for j in range(NJ):
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++ YS[:,j0:j1] = Y[j]*J[:,j0:j1]
++
++ # velocity-jacobian cross products.
++ for j in range(NJ):
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++ vj = v[j]
++ Sxv[:,j0:j1] = adj(vj)*J[:,j0:j1]
++
++ # --- Main loop
++ for i in range(NJ-1,0,-1):
++ i0,i1 = iv(i)[0],iv(i)[-1]+1
++ Yi = Y[i]
++ Si = J[:,i0:i1]
++ vp = v[robot.model.parents[i]]
++
++ SxvY = Sxv[:,i0:i1].T*Yi
++ for j in ancestors(i):
++ j0,j1 = iv(j)[0],iv(j)[-1]+1
++ Sj = J[:,j0: j1 ]
++
++ # COR ===> Si' Yi Sj x vj
++ C[i0:i1,j0:j1] = YS[:,i0:i1].T*Sxv[:,j0:j1]
++ # CEN ===> Si' vi x Yi Sj = - (Si x vi)' Yi Sj
++ C[i0:i1,j0:j1] -= SxvY*Sj
++
++ vxYS = adjdual(v[i])*Yi*Si
++ YSxv = Yi*Sxv[:,i0:i1]
++ Yv = Yi*vp
++ for ii in ancestors(i)[:-1]:
++ ii0,ii1 = iv(ii)[0],iv(ii)[-1]+1
++ Sii = J[:,ii0:ii1]
++ # COR ===> Sii' Yi Si x vi
++ C[ii0:ii1,i0:i1] = Sii.T*YSxv
++ # CEN ===> Sii' vi x Yi Si = (Sii x vi)' Yi Si
++ C[ii0:ii1,i0:i1] += Sii.T*vxYS
++ # CEN ===> Sii' Si x Yi vi = (Sii x Si)' Yi vi
++ C[ii0:ii1,i0:i1] += np.matrix(td(H[:,i0:i1,ii0:ii1],Yv,[0,0])[:,:,0]).T
++
++ return C
++
++# --- DRNEA -------------------------------------------------------------------------
++# --- DRNEA -------------------------------------------------------------------------
++# --- DRNEA -------------------------------------------------------------------------
++
++from lambdas import MCross,FCross
++
++
++class DRNEA:
++ def __init__(self,robot):
++ self.robot = robot
++ lambdas.setRobotArgs(robot)
++
++ def __call__(self,q,vq,aq):
++ robot = self.robot
++ se3.rnea(robot.model,robot.data,q,vq,aq)
++ NJ = robot.model.njoints
++ NV = robot.model.nv
++ J = robot.data.J
++ oMi = robot.data.oMi
++ v = [ (oMi[i]*robot.data.v [i]).vector for i in range(NJ) ]
++ a = [ (oMi[i]*robot.data.a_gf[i]).vector for i in range(NJ) ]
++ f = [ (oMi[i]*robot.data.f [i]).vector for i in range(NJ) ]
++ Y = [ (oMi[i]*robot.model.inertias[i]).matrix() for i in range(NJ) ]
++ Ycrb = [ (oMi[i]*robot.data.Ycrb[i]) .matrix() for i in range(NJ) ]
++
++ Tkf = [ zero([6,NV]) for i in range(NJ) ]
++ Yvx = [ zero([6,6]) for i in range(NJ) ]
++ adjf = lambda f: -np.bmat([ [zero([3,3]),skew(f[:3])] , [skew(f[:3]),skew(f[3:])] ])
++
++ R = self.R = zero([NV,NV])
++
++ for i in reversed(range(1,NJ)): # i is the torque index : dtau_i = R[i,:] dq
++ i0,i1 = iv(i)[0],iv(i)[-1]+1
++ Yi = Y[i]
++ Yci = Ycrb[i]
++ Si = J[:,i0:i1]
++ vi = v[i]
++ ai = a[i]
++ fi = f[i]
++ aqi = aq[i0:i1]
++ vqi = vq[i0:i1]
++ dvi = Si*vqi
++ li = parent(i)
++
++ Yvx[ i] += Y[i]*adj(v[i])
++ Yvx[ i] -= adjf(Y[i]*v[i])
++ Yvx[ i] -= adjdual(v[i])*Yi
++
++ Yvx[li] += Yvx[i]
++
++ for k in ancestors(i): # k is the derivative index: dtau = R[:,k] dq_k
++ k0,k1 = iv(k)[0],iv(k)[-1]+1
++ Sk = J[:,k0:k1]
++ lk = parent(k)
++
++ # Si' Yi Tk ai = Si' Yi ( Sk x (ai-alk) + (vi-vlk) x (Sk x vlk) )
++ # Tk Si' Yi ai = Si' Yi ( - Sk x a[lk] + (vi-vlk) x (Sk x vlk) )
++ Tkf = Yci*(- MCross(Sk,a[lk]) + MCross(MCross(Sk,v[lk]),v[lk]))
++
++ # Tk Si' fs = Tk Si' Ycrb[i] ai + Si' Ys (vs-vi) x (Sk x vlk)
++ # = "" + Si' Ys vs x (Sk x vlk) - Si' Ys vi x (Sk x vlk)
++ Tkf += Yvx[i]*MCross(Sk,v[lk])
++
++ R[i0:i1,k0:k1] = Si.T*Tkf
++ if i==k:
++ for kk in ancestors(k)[:-1]:
++ kk0,kk1 = iv(kk)[0],iv(kk)[-1]+1
++ Skk = J[:,kk0:kk1]
++ R[kk0:kk1,k0:k1] = Skk.T * FCross(Sk,f[i])
++ R[kk0:kk1,k0:k1] += Skk.T * Tkf
++
++ self.a = a
++ self.v = v
++ self.f = f
++ self.Y = Y
++ self.Ycrb = Ycrb
++ self.Yvx = Yvx
++
++ return R
++
++# -----------------------------------------------------------------------------------
++# -----------------------------------------------------------------------------------
++# -----------------------------------------------------------------------------------
++# --- UNIT TEST ---
++if __name__ == '__main__':
++ np.random.seed(0)
++
++ robot = RobotWrapper('/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf',
++ [ '/home/nmansard/src/pinocchio/pinocchio/models/romeo/', ],
++ se3.JointModelFreeFlyer()
++ )
++ q = rand(robot.model.nq); q[3:7] /= norm(q[3:7])
++ vq = rand(robot.model.nv)
++
++ # --- HESSIAN ---
++ # --- HESSIAN ---
++ # --- HESSIAN ---
++ H = hessian(robot,q)
++
++ # Compute the Hessian matrix H using RNEA, so that acor = v*H*v
++ vq1 = vq*0
++ Htrue = np.zeros([6,robot.model.nv,robot.model.nv])
++ for joint_i in range(1,robot.model.njoints):
++ for joint_j in ancestors(joint_i)[:-1]:
++ for i in iv(joint_i):
++ for j in iv(joint_j):
++ vq1 *= 0
++ vq1[i] = vq1[j] = 1.0
++ se3.computeAllTerms(robot.model,robot.data,q,vq1)
++ Htrue[:,i,j] = (robot.data.oMi[joint_i]*robot.data.a[joint_i]).vector.T
++
++ print 'Check hessian = \t\t', norm(H-Htrue)
++
++ # --- dCRBA ---
++ # --- dCRBA ---
++ # --- dCRBA ---
++
++ dcrba = DCRBA(robot)
++ dcrba.pre(q)
++ Mp = dcrba()
++
++ # --- Validate dM/dq by finite diff
++ dM = np.zeros([robot.model.nv,]*3)
++ eps = 1e-6
++ dq = zero(robot.model.nv)
++
++ for diff in range(robot.model.nv):
++
++ dM[:,:,diff] = -se3.crba(robot.model,robot.data,q)
++
++ dq *=0; dq[diff] = eps
++ qdq = se3.integrate(robot.model,q,dq)
++
++ dM[:,:,diff] += se3.crba(robot.model,robot.data,qdq)
++
++ dM /= eps
++
++ print 'Check dCRBA = \t\t\t', max([ norm(Mp[:,:,diff]-dM[:,:,diff]) for diff in range(robot.model.nv) ])
++
++
++ # --- vRNEA ---
++ # --- vRNEA ---
++ # --- vRNEA ---
++
++ vrnea = VRNEA(robot)
++ Q = vrnea(q)
++
++ # --- Compute C from rnea, for future check
++ robot.model.gravity = se3.Motion.Zero()
++ rnea0 = lambda q,vq: se3.nle(robot.model,robot.data,q,vq)
++ vq1 = vq*0
++ C = np.zeros([robot.model.nv,]*3)
++ for i in range(robot.model.nv):
++ vq1 *= 0; vq1[i] = 1
++ C[:,i,i] = rnea0(q,vq1).T
++
++ for i in range(robot.model.nv):
++ for j in range(robot.model.nv):
++ if i==j: continue
++ vq1 *= 0
++ vq1[i] = vq1[j] = 1.0
++ C[:,i,j] = (rnea0(q,vq1).T-C[:,i,i]-C[:,j,j]) /2
++
++ print "Check d/dv rnea = \t\t",norm(quad(Q,vq)-rnea0(q,vq))
++ print "Check C = Q+Q.T = \t\t", norm((Q+Q.transpose(0,2,1))/2-C)
++ print "Check dM = C+C.T /2 \t\t", norm( Mp - (C+C.transpose(1,0,2)) )
++ print "Check dM = Q+Q.T+Q.T+Q.T /2 \t", norm( Mp -
++ (Q+Q.transpose(0,2,1)+Q.transpose(1,0,2)+Q.transpose(2,0,1))/2 )
++
++ # --- CORIOLIS
++ # --- CORIOLIS
++ # --- CORIOLIS
++ coriolis = Coriolis(robot)
++ C = coriolis(q,vq)
++ print "Check coriolis \t\t\t",norm(C*vq-rnea0(q,vq))
++
++ # --- DRNEA
++ # --- DRNEA
++ # --- DRNEA
++ drnea = DRNEA(robot)
++ aq = rand(robot.model.nv)
++ R = drnea(q,vq,aq)
++
++ NV = robot.model.nv
++ Rd = zero([NV,NV])
++ eps = 1e-8
++ r0 = se3.rnea(robot.model,robot.data,q,vq,aq).copy()
++ for i in range(NV):
++ dq = zero(NV); dq[i]=eps
++ qdq = se3.integrate(robot.model,q,dq)
++ Rd[:,i] = (se3.rnea(robot.model,robot.data,qdq,vq,aq)-r0)/eps
++ print "Check drnea \t\t\t",norm(Rd-R)
++
++
++
+diff --git bindings/python/scripts/derivative/lambdas.py bindings/python/scripts/derivative/lambdas.py
+new file mode 100644
+index 0000000..320b0c8
+--- /dev/null
++++ bindings/python/scripts/derivative/lambdas.py
+@@ -0,0 +1,76 @@
++#
++# Copyright (c) 2016 CNRS
++#
++# This file is part of Pinocchio
++# Pinocchio is free software: you can redistribute it
++# and/or modify it under the terms of the GNU Lesser General Public
++# License as published by the Free Software Foundation, either version
++# 3 of the License, or (at your option) any later version.
++# Pinocchio is distributed in the hope that it will be
++# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
++# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
++# General Lesser Public License for more details. You should have
++# received a copy of the GNU Lesser General Public License along with
++# Pinocchio If not, see
++# <http://www.gnu.org/licenses/>.
++
++import numpy as np
++from pinocchio import Motion,Force
++from pinocchio.utils import skew,zero
++
++def jFromIdx(idxv,robot):
++ '''Return the joint index from the velocity index'''
++ for j in range(1,robot.model.njoint):
++ if idxv in range(robot.model.joints[j].idx_v,
++ robot.model.joints[j].idx_v+robot.model.joints[j].nv):
++ return j
++
++parent = lambda i,robot: robot.model.parents[i]
++iv = lambda i,robot: range(robot.model.joints[i].idx_v,
++ robot.model.joints[i].idx_v+robot.model.joints[i].nv)
++ancestors = lambda j,robot,res=[]: res if j==0 else ancestors(robot.model.parents[j],robot,[j,]+res)
++
++class ancestorOf:
++ def __init__(self,i,robot): self.dec=i; self.robot=robot
++ def __contains__(self,anc):
++ dec = self.dec
++ while(dec>0):
++ if anc==dec: return True
++ else: dec = self.robot.model.parents[dec]
++#descendants = lambda root,robot: filter( lambda i: root in ancestorOf(i,robot), range(root,robot.model.njoints) )
++descendants = lambda root,robot: robot.model.subtrees[root]
++
++
++def setRobotArgs(robot):
++ ancestors.__defaults__ = (robot,)+ancestors.__defaults__
++ descendants.__defaults__ = (robot,)
++ #ancestorsOf.__init__.__defaults__ = (robot,)
++ iv.__defaults__ = (robot,)
++ parent.__defaults__ = (robot,)
++ jFromIdx.__defaults__ = (robot,)
++
++# --- SE3 operators
++Mcross = lambda x,y: Motion(x).cross(Motion(y)).vector
++Mcross.__doc__ = "Motion cross product"
++
++Fcross = lambda x,y: Motion(x).cross(Force(y)).vector
++Fcross.__doc__ = "Force cross product"
++
++MCross = lambda V,v: np.bmat([ Mcross(V[:,i],v) for i in range(V.shape[1]) ])
++FCross = lambda V,f: np.bmat([ Fcross(V[:,i],f) for i in range(V.shape[1]) ])
++
++
++adj = lambda nu: np.bmat([[ skew(nu[3:]),skew(nu[:3])],[zero([3,3]),skew(nu[3:])]])
++adj.__doc__ = "Motion pre-cross product (ie adjoint, lie bracket operator)"
++
++adjdual = lambda nu: np.bmat([[ skew(nu[3:]),zero([3,3])],[skew(nu[:3]),skew(nu[3:])]])
++adjdual.__doc__ = "Force pre-cross product adjdual(a) = -adj(a)' "
++
++td = np.tensordot
++quad = lambda H,v: np.matrix(td(td(H,v,[2,0]),v,[1,0])).T
++quad.__doc__ = '''Tensor product v'*H*v, with H.shape = [ nop, nv, nv ]'''
++
++def np_prettyprint(sarg = '{: 0.5f}',eps=5e-7):
++ mformat = lambda x,sarg = sarg,eps=eps: sarg.format(x) if abs(x)>eps else ' 0. '
++ np.set_printoptions(formatter={'float': mformat})
++
+diff --git bindings/python/scripts/derivative/xm.py bindings/python/scripts/derivative/xm.py
+new file mode 100644
+index 0000000..6d74b3e
+--- /dev/null
++++ bindings/python/scripts/derivative/xm.py
+@@ -0,0 +1,46 @@
++#
++# Copyright (c) 2016 CNRS
++#
++# This file is part of Pinocchio
++# Pinocchio is free software: you can redistribute it
++# and/or modify it under the terms of the GNU Lesser General Public
++# License as published by the Free Software Foundation, either version
++# 3 of the License, or (at your option) any later version.
++# Pinocchio is distributed in the hope that it will be
++# useful, but WITHOUT ANY WARRANTY; without even the implied warranty
++# of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
++# General Lesser Public License for more details. You should have
++# received a copy of the GNU Lesser General Public License along with
++# Pinocchio If not, see
++# <http://www.gnu.org/licenses/>.
++
++import pinocchio as se3
++from pinocchio.robot_wrapper import RobotWrapper
++from pinocchio.utils import *
++from dcrba import *
++
++np.random.seed(0)
++
++robot = RobotWrapper('/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf',
++ [ '/home/nmansard/src/pinocchio/pinocchio/models/romeo/', ],
++ se3.JointModelFreeFlyer()
++ )
++q = rand(robot.model.nq); q[3:7] /= norm(q[3:7])
++vq = rand(robot.model.nv)
++aq = rand(robot.model.nv)
++
++# d/dq M(q)
++dcrba = DCRBA(robot)
++dcrba.pre(q)
++Mp = dcrba()
++
++# d/dvq RNEA(q,vq) = C(q,vq)
++coriolis = Coriolis(robot)
++C = coriolis(q,vq)
++
++# d/dq RNEA(q,vq,aq)
++drnea = DRNEA(robot)
++aq = rand(robot.model.nv)
++R = drnea(q,vq,aq)
++
++
+diff --git bindings/python/spatial/se3.hpp bindings/python/spatial/se3.hpp
+index 880a279..7c1558d 100644
+--- bindings/python/spatial/se3.hpp
++++ bindings/python/spatial/se3.hpp
+@@ -22,6 +22,10 @@
+ #include <eigenpy/memory.hpp>
+
+ #include "pinocchio/spatial/se3.hpp"
++#include "pinocchio/spatial/motion.hpp"
++#include "pinocchio/spatial/force.hpp"
++#include "pinocchio/spatial/inertia.hpp"
++
+ #include "pinocchio/bindings/python/utils/copyable.hpp"
+ #include "pinocchio/bindings/python/utils/printable.hpp"
+
+@@ -72,17 +76,34 @@ namespace se3
+ .def("setRandom",&SE3PythonVisitor::setRandom,"Set *this to a random placement.")
+
+ .def("inverse", &SE3::inverse)
++
+ .def("act", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::act,
+ bp::args("point"),
+ "Returns a point which is the result of the entry point transforms by *this.")
+ .def("actInv", (Vector3 (SE3::*)(const Vector3 &) const) &SE3::actInv,
+ bp::args("point"),
+ "Returns a point which is the result of the entry point by the inverse of *this.")
++
+ .def("act", (SE3 (SE3::*)(const SE3 & other) const) &SE3::act,
+ bp::args("M"), "Returns the result of *this * M.")
+ .def("actInv", (SE3 (SE3::*)(const SE3 & other) const) &SE3::actInv,
+ bp::args("M"), "Returns the result of the inverse of *this times M.")
+
++ .def("act", (Motion (SE3::*)(const Motion &) const) &SE3::act,
++ bp::args("motion"), "Returns the result action of *this onto a Motion.")
++ .def("actInv", (Motion (SE3::*)(const Motion &) const) &SE3::actInv,
++ bp::args("motion"), "Returns the result of the inverse of *this onto a Motion.")
++
++ .def("act", (Force (SE3::*)(const Force &) const) &SE3::act,
++ bp::args("force"), "Returns the result of *this onto a Force.")
++ .def("actInv", (Force (SE3::*)(const Force &) const) &SE3::actInv,
++ bp::args("force"), "Returns the result of the inverse of *this onto an Inertia.")
++
++ .def("act", (Inertia (SE3::*)(const Inertia &) const) &SE3::act,
++ bp::args("inertia"), "Returns the result of *this onto a Force.")
++ .def("actInv", (Inertia (SE3::*)(const Inertia &) const) &SE3::actInv,
++ bp::args("inertia"), "Returns the result of the inverse of *this onto an Inertia.")
++
+ .def("isApprox",(bool (SE3::*)(const SE3 & other, const Scalar & prec)) &SE3::isApprox,bp::args("other","prec"),"Returns true if *this is approximately equal to other, within the precision given by prec.")
+ .def("isApprox",(bool (SE3::*)(const SE3 & other)) &SE3::isApprox,bp::args("other"),"Returns true if *this is approximately equal to other.")
+
+@@ -91,6 +112,9 @@ namespace se3
+
+ .def("__invert__",&SE3::inverse,"Returns the inverse of *this.")
+ .def(bp::self * bp::self)
++ .def("__mul__",&__mul__<Motion>)
++ .def("__mul__",&__mul__<Force>)
++ .def("__mul__",&__mul__<Inertia>)
+ .add_property("np",&SE3::toHomogeneousMatrix)
+
+ .def("Identity",&SE3::Identity,"Returns the identity transformation.")
+@@ -118,6 +142,10 @@ namespace se3
+
+ static void setIdentity(SE3 & self) { self.setIdentity(); }
+ static void setRandom(SE3 & self) { self.setRandom(); }
++
++ template<typename Spatial>
++ static Spatial __mul__(const SE3 & self, const Spatial & other)
++ { return self.act(other); }
+ };
+
+