diff --git a/include/sot-dynamic-pinocchio/dynamic-pinocchio.h b/include/sot-dynamic-pinocchio/dynamic-pinocchio.h
index 7a11113915d7ae1a4d58408b2e87d09edd044456..e7800865acafb23cead4467e7c550572c936d6d3 100644
--- a/include/sot-dynamic-pinocchio/dynamic-pinocchio.h
+++ b/include/sot-dynamic-pinocchio/dynamic-pinocchio.h
@@ -137,9 +137,19 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
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 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
///
/// \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 @@ class SOTDYNAMIC_EXPORT DynamicPinocchio
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;
diff --git a/src/sot-dynamic-pinocchio.cpp b/src/sot-dynamic-pinocchio.cpp
index aca5f58ca6b7f245ca15cf3738c07a770b7c5530..51cb88dbca2b2284b20e42fa0b717de79c4d653c 100644
--- a/src/sot-dynamic-pinocchio.cpp
+++ b/src/sot-dynamic-pinocchio.cpp
@@ -54,28 +54,43 @@ DynamicPinocchio( const std::string & name)
,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,14 +113,14 @@ DynamicPinocchio( const std::string & name)
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,
- "sotDynamic("+name+")::output(vector)::dynamicDrift" )
+ newtonEulerSINTERN,
+ "sotDynamic("+name+")::output(vector)::dynamicDrift" )
{
sotDEBUGIN(5);
@@ -245,7 +260,7 @@ DynamicPinocchio::setData(se3::Data* dataPtr){
/*--------------------------------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 @@ getLowerPositionLimits(dg::Vector& res, const int&)
}
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 @@ getUpperPositionLimits(dg::Vector& res, const int&)
}
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 @@ getUpperVelocityLimits(dg::Vector& res, const int&)
}
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 @@ getMaxEffortLimits(dg::Vector& res, const int&)
/* ---------------- 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 @@ dg::Vector DynamicPinocchio::getPinocchioPos(int time)
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 @@ createJacobianSignal( const std::string& signame, const std::string& jointName )
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 @@ createEndeffJacobianSignal( const std::string& signame, const std::string& joint
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 @@ createPositionSignal( const std::string& signame, const std::string& jointName)
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 @@ createVelocitySignal( const std::string& signame,const std::string& jointName )
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 @@ createAccelerationSignal( const std::string& signame, const std::string& jointNa
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 @@ createAccelerationSignal( const std::string& signame, const std::string& jointNa
return *sig;
}
-
-
-
void DynamicPinocchio::
destroyJacobianSignal( const std::string& signame )
{
@@ -658,7 +676,7 @@ destroyAccelerationSignal( const std::string& signame )
/* --------------------- 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 @@ dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,int time )
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 @@ dg::Vector& DynamicPinocchio::computeZmp( dg::Vector& res,int time )
}
//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);
- res.resize(6,m_model->nv);
- se3::computeJacobians(*m_model,*m_data,this->getPinocchioPos(time));
+ if(res.rows()!=6 && res.cols()!=m_model->nv)
+ res.resize(6,m_model->nv);
+ jacobiansSINTERN(time);
- se3::Data::Matrix6x m_output = Eigen::MatrixXd::Zero(6,m_model->nv);
- //Computes Jacobian in world coordinates.
+ //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);
- 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;
+ if(res.rows()!=6 && res.cols()!=m_model->nv)
+ res.resize(6,m_model->nv);
+ 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 @@ computeGenericPosition(bool isFrame, int jointId, MatrixHomogeneous& res, int ti
}
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 @@ computeNewtonEuler( int& dummy,int time )
}
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 @@ computeJcom( dg::Matrix& Jcom,int time )
}
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 @@ computeInertiaReal( dg::Matrix& res,int time )
}
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 @@ computeFootHeight (double &res , int time)
}
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 @@ computeTorqueDrift( dg::Vector& tauDrift,const int &time )
}
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 @@ computeMomenta(dg::Vector & Momenta, int time)
}
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);