diff --git a/sot-dynamic-pinocchio-v3/Makefile b/sot-dynamic-pinocchio-v3/Makefile
index 90f2ef46ac41c34daa44ce9791993b83ecf444a2..ec1259b4f4f975fcbe9f8842f155d646fe44f109 100644
--- a/sot-dynamic-pinocchio-v3/Makefile
+++ b/sot-dynamic-pinocchio-v3/Makefile
@@ -2,8 +2,7 @@
# Created: Rohan Budhiraja on Fri, 8 Apr 2016
#
-VERSION= 3.1.4
-PKGREVISION= 2
+VERSION= 3.1.5
DISTNAME= sot-dynamic-pinocchio-v3-${VERSION}
MASTER_SITES= ${MASTER_SITE_OPENROBOTS:=sot-dynamic-pinocchio-v3/}
MASTER_REPOSITORY= ${MASTER_REPOSITORY_GITHUB}stack-of-tasks/sot-dynamic-pinocchio/
diff --git a/sot-dynamic-pinocchio-v3/distinfo b/sot-dynamic-pinocchio-v3/distinfo
index d5beaf4c13fb5641c99722ea2818d06a9f2e153f..4c8add2a7f80a29de799f3caa06eded538e4a64d 100644
--- a/sot-dynamic-pinocchio-v3/distinfo
+++ b/sot-dynamic-pinocchio-v3/distinfo
@@ -1,5 +1,3 @@
-SHA1 (sot-dynamic-pinocchio-v3-3.1.4.tar.gz) = 201f892726b25cf98f81b998d04e874256a83849
-RMD160 (sot-dynamic-pinocchio-v3-3.1.4.tar.gz) = d46fa22e0fcad72e93a80555c13260fab6b958c1
-Size (sot-dynamic-pinocchio-v3-3.1.4.tar.gz) = 834002 bytes
-SHA1 (patch-aa) = d75b4ea46ccff74e3e31f1b847151418b1a5f9d7
-SHA1 (patch-ab) = a9a6a1ae86fcaffdb0db40cbe03c196986af5ce7
+SHA1 (sot-dynamic-pinocchio-v3-3.1.5.tar.gz) = 4fbf0521dbad767cb633d6cbd8f172c5f2cd699c
+RMD160 (sot-dynamic-pinocchio-v3-3.1.5.tar.gz) = 2b48a31e1f04fc6ff40d3dfd98440a8d6bb42ea2
+Size (sot-dynamic-pinocchio-v3-3.1.5.tar.gz) = 835143 bytes
diff --git a/sot-dynamic-pinocchio-v3/patches/patch-aa b/sot-dynamic-pinocchio-v3/patches/patch-aa
deleted file mode 100644
index defb0882b12124f3c7d5e4ca26377a7fc7dedf8d..0000000000000000000000000000000000000000
--- a/sot-dynamic-pinocchio-v3/patches/patch-aa
+++ /dev/null
@@ -1,666 +0,0 @@
-Create internal Signals. Avoid unnecessary computations. Change the signal dependencies
-
---- include/sot-dynamic-pinocchio/dynamic-pinocchio.h 2017-04-11 18:50:38.595980728 +0200
-+++ include/sot-dynamic-pinocchio/dynamic-pinocchio.h 2017-04-11 18:52:20.770729291 +0200
-@@ -137,9 +137,19 @@
- dg::SignalPtr<dg::Vector,int> jointAccelerationSIN;
- dg::SignalPtr<dg::Vector,int> freeFlyerAccelerationSIN;
-
-- dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
-+ dg::SignalTimeDependent<dg::Vector,int> pinocchioPosSINTERN;
-+ dg::SignalTimeDependent<dg::Vector,int> pinocchioVelSINTERN;
-+ dg::SignalTimeDependent<dg::Vector,int> pinocchioAccSINTERN;
-
-- int& computeNewtonEuler( int& dummy,int time );
-+ dg::SignalTimeDependent<Dummy,int> newtonEulerSINTERN;
-+ dg::SignalTimeDependent<Dummy,int> jacobiansSINTERN;
-+ dg::SignalTimeDependent<Dummy,int> forwardKinematicsSINTERN;
-+ dg::SignalTimeDependent<Dummy,int> ccrbaSINTERN;
-+
-+ int& computeNewtonEuler(int& dummy,const int& time );
-+ int& computeForwardKinematics(int& dummy,const int& time );
-+ int& computeCcrba( int& dummy,const int& time );
-+ int& computeJacobians( int& dummy,const int& time );
-
- dg::SignalTimeDependent<dg::Vector,int> zmpSOUT;
- dg::SignalTimeDependent<dg::Matrix,int> JcomSOUT;
-@@ -187,50 +197,50 @@
- ///
- /// \param[out] result vector
- /// \return result vector
-- dg::Vector& getLowerPositionLimits(dg::Vector& res,const int&);
-+ dg::Vector& getLowerPositionLimits(dg::Vector& res,const int&) const ;
-
- /// \brief Get joint position upper limits
- ///
- /// \param[out] result vector
- /// \return result vector
-- dg::Vector& getUpperPositionLimits(dg::Vector& res,const int&);
-+ dg::Vector& getUpperPositionLimits(dg::Vector& res,const int&) const;
-
- /// \brief Get joint velocity upper limits
- ///
- /// \param[out] result vector
- /// \return result vector
-- dg::Vector& getUpperVelocityLimits(dg::Vector& res,const int&);
-+ dg::Vector& getUpperVelocityLimits(dg::Vector& res,const int&) const;
-
- /// \brief Get joint effort upper limits
- ///
- /// \param[out] result vector
- /// \return result vector
-- dg::Vector& getMaxEffortLimits(dg::Vector& res,const int&);
-+ dg::Vector& getMaxEffortLimits(dg::Vector& res,const int&) const;
-
-
- // dg::Vector& getAnklePositionInFootFrame() const;
-
- protected:
-- dg::Matrix& computeGenericJacobian(bool isFrame,
-- int jointId,
-- dg::Matrix& res,int time );
-- dg::Matrix& computeGenericEndeffJacobian(bool isFrame,
-- int jointId,
-- dg::Matrix& res,int time );
-- MatrixHomogeneous& computeGenericPosition(bool isFrame,
-- int jointId,
-- MatrixHomogeneous& res,int time );
-- dg::Vector& computeGenericVelocity(int jointId,dg::Vector& res,int time );
-- dg::Vector& computeGenericAcceleration(int jointId,dg::Vector& res,int time );
--
-- dg::Vector& computeZmp( dg::Vector& res,int time );
-- dg::Vector& computeMomenta( dg::Vector &res, int time);
-- dg::Vector& computeAngularMomentum( dg::Vector &res, int time);
-- dg::Matrix& computeJcom( dg::Matrix& res,int time );
-- dg::Vector& computeCom( dg::Vector& res,int time );
-- dg::Matrix& computeInertia( dg::Matrix& res,int time );
-- dg::Matrix& computeInertiaReal( dg::Matrix& res,int time );
-- double& computeFootHeight( double& res,int time );
-+ dg::Matrix& computeGenericJacobian(const bool isFrame,
-+ const int jointId,
-+ dg::Matrix& res,const int& time );
-+ dg::Matrix& computeGenericEndeffJacobian(const bool isFrame,
-+ const int jointId,
-+ dg::Matrix& res,const int& time );
-+ MatrixHomogeneous& computeGenericPosition(const bool isFrame,
-+ const int jointId,
-+ MatrixHomogeneous& res,const int& time );
-+ dg::Vector& computeGenericVelocity(const int jointId,dg::Vector& res,const int& time );
-+ dg::Vector& computeGenericAcceleration(const int jointId,dg::Vector& res,const int& time );
-+
-+ dg::Vector& computeZmp( dg::Vector& res,const int& time );
-+ dg::Vector& computeMomenta( dg::Vector &res,const int& time);
-+ dg::Vector& computeAngularMomentum( dg::Vector &res,const int& time);
-+ dg::Matrix& computeJcom( dg::Matrix& res,const int& time );
-+ dg::Vector& computeCom( dg::Vector& res,const int& time );
-+ dg::Matrix& computeInertia( dg::Matrix& res,const int& time );
-+ dg::Matrix& computeInertiaReal( dg::Matrix& res,const int& time );
-+ double& computeFootHeight( double& res,const int& time );
-
- dg::Vector& computeTorqueDrift( dg::Vector& res,const int& time );
-
-@@ -246,9 +256,9 @@
- private:
- /// \brief map of joints in construction.
- /// map: jointName -> (jointType,jointPosition (in parent frame), function_ptr to pinocchio Joint)
-- dg::Vector getPinocchioPos(int);
-- dg::Vector getPinocchioVel(int);
-- dg::Vector getPinocchioAcc(int);
-+ dg::Vector& getPinocchioPos(dg::Vector& q,const int& time);
-+ dg::Vector& getPinocchioVel(dg::Vector& v, const int& time);
-+ dg::Vector& getPinocchioAcc(dg::Vector& a, const int&time);
-
- //\brief Index list for the first dof of (spherical joints)/ (spherical part of free-flyer joint).
- std::vector<int> sphericalJoints;
-
-
-
---- src/sot-dynamic-pinocchio.cpp 2017-04-11 18:50:37.843960691 +0200
-+++ src/sot-dynamic-pinocchio.cpp 2017-04-11 18:52:20.774729399 +0200
-@@ -54,28 +54,43 @@
- ,jointAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::acceleration")
- ,freeFlyerAccelerationSIN(NULL,"sotDynamic("+name+")::input(vector)::ffacceleration")
-
-+ ,pinocchioPosSINTERN( boost::bind(&DynamicPinocchio::getPinocchioPos,this,_1, _2),
-+ jointPositionSIN<<freeFlyerPositionSIN,
-+ "sotDynamic("+name+")::intern(dummy)::pinocchioPos" )
-+ ,pinocchioVelSINTERN( boost::bind(&DynamicPinocchio::getPinocchioVel,this,_1, _2),
-+ jointVelocitySIN<<freeFlyerVelocitySIN,
-+ "sotDynamic("+name+")::intern(dummy)::pinocchioVel" )
-+ ,pinocchioAccSINTERN( boost::bind(&DynamicPinocchio::getPinocchioAcc,this,_1, _2),
-+ jointAccelerationSIN<<freeFlyerAccelerationSIN,
-+ "sotDynamic("+name+")::intern(dummy)::pinocchioAcc" )
-+
- ,newtonEulerSINTERN( boost::bind(&DynamicPinocchio::computeNewtonEuler,this,_1,_2),
-- jointPositionSIN<<freeFlyerPositionSIN
-- <<jointVelocitySIN<<freeFlyerVelocitySIN
-- <<jointAccelerationSIN<<freeFlyerAccelerationSIN,
-+ pinocchioPosSINTERN<<pinocchioVelSINTERN<<pinocchioAccSINTERN,
- "sotDynamic("+name+")::intern(dummy)::newtoneuler" )
--
-+ ,jacobiansSINTERN( boost::bind(&DynamicPinocchio::computeJacobians,this,_1, _2),
-+ pinocchioPosSINTERN,
-+ "sotDynamic("+name+")::intern(dummy)::computeJacobians" )
-+ ,forwardKinematicsSINTERN( boost::bind(&DynamicPinocchio::computeForwardKinematics,this,_1, _2),
-+ pinocchioPosSINTERN<<pinocchioVelSINTERN<<pinocchioAccSINTERN,
-+ "sotDynamic("+name+")::intern(dummy)::computeForwardKinematics" )
-+ ,ccrbaSINTERN( boost::bind(&DynamicPinocchio::computeCcrba,this,_1,_2),
-+ pinocchioPosSINTERN<<pinocchioVelSINTERN,
-+ "sotDynamic("+name+")::intern(dummy)::computeCcrba" )
- ,zmpSOUT( boost::bind(&DynamicPinocchio::computeZmp,this,_1,_2),
- newtonEulerSINTERN,
- "sotDynamic("+name+")::output(vector)::zmp" )
- ,JcomSOUT( boost::bind(&DynamicPinocchio::computeJcom,this,_1,_2),
-- newtonEulerSINTERN,
-+ pinocchioPosSINTERN,
- "sotDynamic("+name+")::output(matrix)::Jcom" )
- ,comSOUT( boost::bind(&DynamicPinocchio::computeCom,this,_1,_2),
-- newtonEulerSINTERN,
-+ forwardKinematicsSINTERN,
- "sotDynamic("+name+")::output(vector)::com" )
- ,inertiaSOUT( boost::bind(&DynamicPinocchio::computeInertia,this,_1,_2),
-- newtonEulerSINTERN,
-+ pinocchioPosSINTERN,
- "sotDynamic("+name+")::output(matrix)::inertia" )
- ,footHeightSOUT( boost::bind(&DynamicPinocchio::computeFootHeight,this,_1,_2),
-- newtonEulerSINTERN,
-+ pinocchioPosSINTERN,
- "sotDynamic("+name+")::output(double)::footHeight" )
--
- ,upperJlSOUT( boost::bind(&DynamicPinocchio::getUpperPositionLimits,this,_1,_2),
- sotNOSIGNAL,
- "sotDynamic("+name+")::output(vector)::upperJl" )
-@@ -98,10 +113,10 @@
- inertiaSOUT << gearRatioSOUT << inertiaRotorSOUT,
- "sotDynamic("+name+")::output(matrix)::inertiaReal" )
- ,MomentaSOUT( boost::bind(&DynamicPinocchio::computeMomenta,this,_1,_2),
-- inertiaSOUT,
-+ ccrbaSINTERN,
- "sotDynamic("+name+")::output(vector)::momenta" )
- ,AngularMomentumSOUT( boost::bind(&DynamicPinocchio::computeAngularMomentum,this,_1,_2),
-- inertiaSOUT,
-+ ccrbaSINTERN,
- "sotDynamic("+name+")::output(vector)::angularmomentum" )
- ,dynamicDriftSOUT( boost::bind(&DynamicPinocchio::computeTorqueDrift,this,_1,_2),
- newtonEulerSINTERN,
-@@ -245,7 +260,7 @@
- /*--------------------------------GETTERS-------------------------------------------*/
-
- dg::Vector& DynamicPinocchio::
--getLowerPositionLimits(dg::Vector& res, const int&)
-+getLowerPositionLimits(dg::Vector& res, const int&) const
- {
- sotDEBUGIN(15);
- assert(m_model);
-@@ -285,7 +300,7 @@
- }
-
- dg::Vector& DynamicPinocchio::
--getUpperPositionLimits(dg::Vector& res, const int&)
-+getUpperPositionLimits(dg::Vector& res, const int&) const
- {
- sotDEBUGIN(15);
- assert(m_model);
-@@ -325,7 +340,7 @@
- }
-
- dg::Vector& DynamicPinocchio::
--getUpperVelocityLimits(dg::Vector& res, const int&)
-+getUpperVelocityLimits(dg::Vector& res, const int&) const
- {
- sotDEBUGIN(15);
- assert(m_model);
-@@ -339,7 +354,7 @@
- }
-
- dg::Vector& DynamicPinocchio::
--getMaxEffortLimits(dg::Vector& res, const int&)
-+getMaxEffortLimits(dg::Vector& res, const int&) const
- {
- sotDEBUGIN(15);
- assert(m_model);
-@@ -353,11 +368,10 @@
-
-
- /* ---------------- INTERNAL ------------------------------------------------ */
--dg::Vector DynamicPinocchio::getPinocchioPos(int time)
-+dg::Vector& DynamicPinocchio::getPinocchioPos(dg::Vector& q,const int& time)
- {
- sotDEBUGIN(15);
- dg::Vector qJoints=jointPositionSIN.access(time);
-- dg::Vector q;
- if (!sphericalJoints.empty()){
- if( freeFlyerPositionSIN) {
- dg::Vector qFF=freeFlyerPositionSIN.access(time);
-@@ -397,29 +411,36 @@
- return q;
- }
-
--Eigen::VectorXd DynamicPinocchio::getPinocchioVel(int time)
-+dg::Vector& DynamicPinocchio::getPinocchioVel(dg::Vector& v, const int& time)
- {
- const Eigen::VectorXd vJoints=jointVelocitySIN.access(time);
- if(freeFlyerVelocitySIN){
- const Eigen::VectorXd vFF=freeFlyerVelocitySIN.access(time);
-- Eigen::VectorXd v(vJoints.size() + vFF.size());
-+ if(v.size() != vJoints.size() + vFF.size())
-+ v.resize(vJoints.size() + vFF.size());
- v << vFF,vJoints;
- return v;
- }
-- else
-- return vJoints;
-+ else {
-+ v = vJoints;
-+ return v;
-+ }
- }
-
--Eigen::VectorXd DynamicPinocchio::getPinocchioAcc(int time)
-+dg::Vector& DynamicPinocchio::getPinocchioAcc(dg::Vector& a, const int& time)
- {
- const Eigen::VectorXd aJoints=jointAccelerationSIN.access(time);
- if(freeFlyerAccelerationSIN){
- const Eigen::VectorXd aFF=freeFlyerAccelerationSIN.access(time);
-- Eigen::VectorXd a(aJoints.size() + aFF.size());
-+ if (a.size() !=aJoints.size() + aFF.size())
-+ a.resize(aJoints.size() + aFF.size());
- a << aFF,aJoints;
- return a;
- }
-- else return aJoints;
-+ else {
-+ a = aJoints;
-+ return a;
-+ }
- }
-
- /* --- SIGNAL ACTIVATION ---------------------------------------------------- */
-@@ -433,14 +454,14 @@
- long int frameId = m_model->getFrameId(jointName);
- sig = new dg::SignalTimeDependent< dg::Matrix,int >
- ( boost::bind(&DynamicPinocchio::computeGenericJacobian,this,true,frameId,_1,_2),
-- newtonEulerSINTERN,
-+ jacobiansSINTERN,
- "sotDynamic("+name+")::output(matrix)::"+signame );
- }
- else if(m_model->existJointName(jointName)) {
- long int jointId = m_model->getJointId(jointName);
- sig = new dg::SignalTimeDependent< dg::Matrix,int >
- ( boost::bind(&DynamicPinocchio::computeGenericJacobian,this,false,jointId,_1,_2),
-- newtonEulerSINTERN,
-+ jacobiansSINTERN,
- "sotDynamic("+name+")::output(matrix)::"+signame );
- }
- else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
-@@ -462,14 +483,14 @@
- long int frameId = m_model->getFrameId(jointName);
- sig = new dg::SignalTimeDependent< dg::Matrix,int >
- ( boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian,this,true,frameId,_1,_2),
-- newtonEulerSINTERN,
-+ jacobiansSINTERN,
- "sotDynamic("+name+")::output(matrix)::"+signame );
- }
- else if(m_model->existJointName(jointName)) {
- long int jointId = m_model->getJointId(jointName);
- sig = new dg::SignalTimeDependent< dg::Matrix,int >
- ( boost::bind(&DynamicPinocchio::computeGenericEndeffJacobian,this,false,jointId,_1,_2),
-- newtonEulerSINTERN,
-+ jacobiansSINTERN,
- "sotDynamic("+name+")::output(matrix)::"+signame );
- }
- else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
-@@ -490,14 +511,14 @@
- long int frameId = m_model->getFrameId(jointName);
- sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
- ( boost::bind(&DynamicPinocchio::computeGenericPosition,this,true,frameId,_1,_2),
-- newtonEulerSINTERN,
-+ forwardKinematicsSINTERN,
- "sotDynamic("+name+")::output(matrixHomo)::"+signame );
- }
- else if(m_model->existJointName(jointName)) {
- long int jointId = m_model->getJointId(jointName);
- sig = new dg::SignalTimeDependent< MatrixHomogeneous,int >
- ( boost::bind(&DynamicPinocchio::computeGenericPosition,this,false,jointId,_1,_2),
-- newtonEulerSINTERN,
-+ forwardKinematicsSINTERN,
- "sotDynamic("+name+")::output(matrixHomo)::"+signame );
- }
- else SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
-@@ -519,7 +540,7 @@
- SignalTimeDependent< dg::Vector,int > * sig
- = new SignalTimeDependent< dg::Vector,int >
- ( boost::bind(&DynamicPinocchio::computeGenericVelocity,this,jointId,_1,_2),
-- newtonEulerSINTERN,
-+ forwardKinematicsSINTERN,
- "sotDynamic("+name+")::output(dg::Vector)::"+signame );
- genericSignalRefs.push_back( sig );
- signalRegistration( *sig );
-@@ -537,7 +558,7 @@
- dg::SignalTimeDependent< dg::Vector,int > * sig
- = new dg::SignalTimeDependent< dg::Vector,int >
- ( boost::bind(&DynamicPinocchio::computeGenericAcceleration,this,jointId,_1,_2),
-- newtonEulerSINTERN,
-+ forwardKinematicsSINTERN,
- "sotDynamic("+name+")::output(matrixHomo)::"+signame );
-
- genericSignalRefs.push_back( sig );
-@@ -547,9 +568,6 @@
- return *sig;
- }
-
--
--
--
- void DynamicPinocchio::
- destroyJacobianSignal( const std::string& signame )
- {
-@@ -658,7 +676,7 @@
-
- /* --------------------- COMPUTE ------------------------------------------------- */
-
--dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,int time )
-+dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,const int& time )
- {
- //TODO: To be verified
- sotDEBUGIN(25);
-@@ -667,9 +685,9 @@
- res.resize(3);
- newtonEulerSINTERN(time);
-
-- se3::Force ftau = m_data->oMi[1].act(m_data->f[1]);
-- se3::Force::Vector3 tau = ftau.angular();
-- se3::Force::Vector3 f = ftau.linear();
-+ const se3::Force& ftau = m_data->oMi[1].act(m_data->f[1]);
-+ const se3::Force::Vector3& tau = ftau.angular();
-+ const se3::Force::Vector3& f = ftau.linear();
- res(0) = -tau[1]/f[2];
- res(1) = tau[0]/f[2];
- res(2) = 0;
-@@ -680,62 +698,101 @@
- }
-
- //In world coordinates
-+
-+
-+//Updates the jacobian matrix in m_data
-+int& DynamicPinocchio::computeJacobians(int& dummy, const int& time) {
-+ sotDEBUGIN(25);
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
-+ se3::computeJacobians(*m_model,*m_data, q);
-+ sotDEBUG(25)<<"Jacobians updated"<<std::endl;
-+ sotDEBUGOUT(25);
-+ return dummy;
-+}
-+int& DynamicPinocchio::computeForwardKinematics(int& dummy, const int& time) {
-+ sotDEBUGIN(25);
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
-+ const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
-+ const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
-+ se3::forwardKinematics(*m_model, *m_data, q, v, a);
-+ sotDEBUG(25)<<"Kinematics updated"<<std::endl;
-+ sotDEBUGOUT(25);
-+ return dummy;
-+}
-+
-+int& DynamicPinocchio::computeCcrba(int& dummy, const int& time) {
-+ sotDEBUGIN(25);
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
-+ const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
-+ se3::ccrba(*m_model,*m_data, q, v);
-+ sotDEBUG(25)<<"Inertia and Momentum updated"<<std::endl;
-+ sotDEBUGOUT(25);
-+ return dummy;
-+}
-+
- dg::Matrix& DynamicPinocchio::
--computeGenericJacobian(bool isFrame, int jointId, dg::Matrix& res,int time )
-+computeGenericJacobian(const bool isFrame, const int jointId, dg::Matrix& res,const int& time )
- {
- sotDEBUGIN(25);
- assert(m_model);
- assert(m_data);
-- newtonEulerSINTERN(time);
-+ if(res.rows()!=6 && res.cols()!=m_model->nv)
- res.resize(6,m_model->nv);
-- se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
-+ jacobiansSINTERN(time);
-
-- se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
-+ //TODO: Find a way to remove tmp object
-+ se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
- //Computes Jacobian in world coordinates.
- if(isFrame){
-- se3::framesForwardKinematics(*m_model,*m_data);
-- se3::getFrameJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
-+ //se3::framesForwardKinematics(*m_model,*m_data);
-+ se3::getFrameJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
- }
-- else se3::getJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
-- res = m_output;
-+ else se3::getJacobian<false>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
-+ res = tmp;
- sotDEBUGOUT(25);
- return res;
- }
-
- dg::Matrix& DynamicPinocchio::
--computeGenericEndeffJacobian(bool isFrame, int jointId,dg::Matrix& res,int time )
-+computeGenericEndeffJacobian(const bool isFrame, const int jointId,dg::Matrix& res,const int& time )
- {
- sotDEBUGIN(25);
- assert(m_model);
- assert(m_data);
-- newtonEulerSINTERN(time);
-+ if(res.rows()!=6 && res.cols()!=m_model->nv)
- res.resize(6,m_model->nv);
-- //In local coordinates.
-- se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
-- se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
-- std::string temp;
-+ jacobiansSINTERN(time);
-+
-+ //TODO: Find a way to remove tmp object
-+ se3::Data::Matrix6x tmp = Eigen::MatrixXd::Zero(6,m_model->nv);
-+ //std::string temp;
- if(isFrame){
-- se3::framesForwardKinematics(*m_model,*m_data);
-- se3::getFrameJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
-- temp = m_model->frames.at((se3::Model::Index)jointId).name;
-+ //se3::framesForwardKinematics(*m_model,*m_data);
-+ se3::getFrameJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
-+ sotDEBUG(25) << "EndEffJacobian for "
-+ << m_model->frames.at((se3::Model::Index)jointId).name
-+ <<" is "<<tmp<<std::endl;
- }
- else {
-- temp = m_model->getJointName((se3::Model::Index)jointId);
-- se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,m_output);
-+ //temp = m_model->getJointName((se3::Model::Index)jointId);
-+ se3::getJacobian<true>(*m_model,*m_data,(se3::Model::Index)jointId,tmp);
-+ sotDEBUG(25) << "EndEffJacobian for "
-+ << m_model->getJointName((se3::Model::Index)jointId)
-+ <<" is "<<tmp<<std::endl;
- }
-- res = m_output;
-- sotDEBUG(25) << "EndEffJacobian for "<<temp<<" is "<<m_output<<std::endl;
-+ res = tmp;
-+
- sotDEBUGOUT(25);
- return res;
- }
-
- MatrixHomogeneous& DynamicPinocchio::
--computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int time)
-+computeGenericPosition(const bool isFrame, const int jointId, MatrixHomogeneous& res, const int& time)
- {
- sotDEBUGIN(25);
- assert(m_data);
-- newtonEulerSINTERN(time);
- std::string temp;
-+ forwardKinematicsSINTERN(time);
- if(isFrame){
- se3::framesForwardKinematics(*m_model,*m_data);
- res.matrix()= m_data->oMf[jointId].toHomogeneousMatrix();
-@@ -751,42 +808,40 @@
- }
-
- dg::Vector& DynamicPinocchio::
--computeGenericVelocity( int jointId, dg::Vector& res,int time )
-+computeGenericVelocity( const int jointId, dg::Vector& res,const int& time )
- {
- sotDEBUGIN(25);
- assert(m_data);
-- newtonEulerSINTERN(time);
- res.resize(6);
-- se3::Motion aRV = m_data->v[jointId];
-+ forwardKinematicsSINTERN(time);
-+ const se3::Motion& aRV = m_data->v[jointId];
- res<<aRV.linear(),aRV.angular();
- sotDEBUGOUT(25);
- return res;
- }
-
- dg::Vector& DynamicPinocchio::
--computeGenericAcceleration( int jointId ,dg::Vector& res,int time )
-+computeGenericAcceleration( const int jointId ,dg::Vector& res,const int& time )
- {
- sotDEBUGIN(25);
- assert(m_data);
-- newtonEulerSINTERN(time);
- res.resize(6);
-- se3::Motion aRA = m_data->a[jointId];
-+ forwardKinematicsSINTERN(time);
-+ const se3::Motion& aRA = m_data->a[jointId];
- res<<aRA.linear(),aRA.angular();
- sotDEBUGOUT(25);
- return res;
- }
-
- int& DynamicPinocchio::
--computeNewtonEuler( int& dummy,int time )
-+computeNewtonEuler(int& dummy,const int& time )
- {
- sotDEBUGIN(15);
- assert(m_model);
- assert(m_data);
--
--
-- const Eigen::VectorXd q=getPinocchioPos(time);
-- const Eigen::VectorXd v=getPinocchioVel(time);
-- const Eigen::VectorXd a=getPinocchioAcc(time);
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
-+ const Eigen::VectorXd& v = pinocchioVelSINTERN.access(time);
-+ const Eigen::VectorXd& a = pinocchioAccSINTERN.access(time);
- se3::rnea(*m_model,*m_data,q,v,a);
-
- sotDEBUG(1)<< "pos = " <<q <<std::endl;
-@@ -798,13 +853,12 @@
- }
-
- dg::Matrix& DynamicPinocchio::
--computeJcom( dg::Matrix& Jcom,int time )
-+computeJcom( dg::Matrix& Jcom,const int& time )
- {
-
- sotDEBUGIN(25);
-- newtonEulerSINTERN(time);
-- const Eigen::VectorXd q=getPinocchioPos(time);
-
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
- Jcom = se3::jacobianCenterOfMass(*m_model, *m_data,
- q, false);
- sotDEBUGOUT(25);
-@@ -812,33 +866,31 @@
- }
-
- dg::Vector& DynamicPinocchio::
--computeCom( dg::Vector& com,int time )
-+computeCom( dg::Vector& com,const int& time )
- {
-
- sotDEBUGIN(25);
-- newtonEulerSINTERN(time);
-- const Eigen::VectorXd q=getPinocchioPos(time);
-- com = se3::centerOfMass(*m_model,*m_data,q,false,true);
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
-+ forwardKinematicsSINTERN(time);
-+ com = se3::centerOfMass(*m_model,*m_data,q,false,false);
- sotDEBUGOUT(25);
- return com;
- }
-
- dg::Matrix& DynamicPinocchio::
--computeInertia( dg::Matrix& res,int time )
-+computeInertia( dg::Matrix& res,const int& time )
- {
- sotDEBUGIN(25);
-- newtonEulerSINTERN(time);
-- //TODO: USE CCRBA
-- dg::Matrix upperInertiaMatrix = se3::crba(*m_model,
-- *m_data,this->getPinocchioPos(time));
-- res = upperInertiaMatrix;
-- res.triangularView<Eigen::StrictlyLower>() = upperInertiaMatrix.transpose().triangularView<Eigen::StrictlyLower>();
-+ const Eigen::VectorXd& q = pinocchioPosSINTERN.access(time);
-+ res = se3::crba(*m_model, *m_data, q);
-+ res.triangularView<Eigen::StrictlyLower>() =
-+ res.transpose().triangularView<Eigen::StrictlyLower>();
- sotDEBUGOUT(25);
- return res;
- }
-
- dg::Matrix& DynamicPinocchio::
--computeInertiaReal( dg::Matrix& res,int time )
-+computeInertiaReal( dg::Matrix& res,const int& time )
- {
- sotDEBUGIN(25);
-
-@@ -855,12 +907,11 @@
- }
-
- double& DynamicPinocchio::
--computeFootHeight (double &res , int time)
-+computeFootHeight (double &res , const int& time)
- {
- //Ankle position in local foot frame
- //TODO: Confirm that it is in the foot frame
- sotDEBUGIN(25);
-- newtonEulerSINTERN(time);
- if(!m_model->existJointName("r_sole_joint")) {
- SOT_THROW ExceptionDynamic(ExceptionDynamic::GENERIC,
- "Robot has no joint corresponding to rigthFoot");
-@@ -874,7 +925,7 @@
- }
-
- dg::Vector& DynamicPinocchio::
--computeTorqueDrift( dg::Vector& tauDrift,const int &time )
-+computeTorqueDrift( dg::Vector& tauDrift,const int& time )
- {
- sotDEBUGIN(25);
- newtonEulerSINTERN(time);
-@@ -884,10 +935,10 @@
- }
-
- dg::Vector& DynamicPinocchio::
--computeMomenta(dg::Vector & Momenta, int time)
-+computeMomenta(dg::Vector & Momenta, const int& time)
- {
- sotDEBUGIN(25);
-- inertiaSOUT(time);
-+ ccrbaSINTERN(time);
- if (Momenta.size()!=6)
- Momenta.resize(6);
-
-@@ -898,10 +949,10 @@
- }
-
- dg::Vector& DynamicPinocchio::
--computeAngularMomentum(dg::Vector & Momenta, int time)
-+computeAngularMomentum(dg::Vector & Momenta, const int& time)
- {
- sotDEBUGIN(25);
-- inertiaSOUT(time);
-+ ccrbaSINTERN(time);
-
- if (Momenta.size()!=3)
- Momenta.resize(3);
diff --git a/sot-dynamic-pinocchio-v3/patches/patch-ab b/sot-dynamic-pinocchio-v3/patches/patch-ab
deleted file mode 100644
index 1a861fd21f8b5b626cfc02822001f5d99fa0355c..0000000000000000000000000000000000000000
--- a/sot-dynamic-pinocchio-v3/patches/patch-ab
+++ /dev/null
@@ -1,11 +0,0 @@
---- src/dynamic_graph/sot/dynamics_pinocchio/humanoid_robot.py 2017-01-04 21:24:28.000000000 +0100
-+++ src/dynamic_graph/sot/dynamics_pinocchio/humanoid_robot.py 2017-04-12 17:40:52.627677686 +0200
-@@ -209,7 +209,7 @@
-
- def initializeOpPoints(self):
- for op in self.OperationalPoints:
-- self.dynamic.createOpPoint(self.OperationalPointsMap[op], self.OperationalPointsMap[op])
-+ self.dynamic.createOpPoint(op, self.OperationalPointsMap[op])
-
- def createFrame(self, frameName, transformation, operationalPoint):
- frame = OpPointModifier(frameName)