From 2728085b8b1ccfb2a1da421a0141fa9e4b04c507 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 13 Apr 2016 11:39:03 +0200
Subject: [PATCH] add a class that uses the pinocchio framework
---
.../jrl/walkgen/patterngeneratorinterface.hh | 2 +-
src/CMakeLists.txt | 4 +-
.../FootTrajectoryGenerationAbstract.hh | 2 +-
...ndRightFootTrajectoryGenerationMultiple.hh | 2 +-
.../GlobalStrategyManager.hh | 2 +-
.../FootConstraintsAsLinearSystem.hh | 2 +-
src/Mathematics/relative-feet-inequalities.hh | 2 +-
src/MotionGeneration/ComAndFootRealization.hh | 4 +-
.../ComAndFootRealizationByGeometry.cpp | 2 +-
src/MotionGeneration/StepOverPlanner.hh | 2 +-
src/PatternGeneratorInterfacePrivate.cpp | 2 +-
src/PreviewControl/rigid-body-system.hh | 2 +-
src/RobotDynamics/PinocchioRobot.cpp | 460 ++++++++++++++++++
src/RobotDynamics/PinocchioRobot.hh | 201 ++++++++
.../OrientationsPreview.hh | 2 +-
.../ZMPDiscretization.hh | 2 +-
.../generator-vel-ref.hh | 2 +-
.../nmpc_generator.hh | 2 +-
tests/TestObject.cpp | 10 +-
tests/TestObject.hh | 6 +-
20 files changed, 681 insertions(+), 32 deletions(-)
create mode 100644 src/RobotDynamics/PinocchioRobot.cpp
create mode 100644 src/RobotDynamics/PinocchioRobot.hh
diff --git a/include/jrl/walkgen/patterngeneratorinterface.hh b/include/jrl/walkgen/patterngeneratorinterface.hh
index b11ce78b..bc50ae29 100644
--- a/include/jrl/walkgen/patterngeneratorinterface.hh
+++ b/include/jrl/walkgen/patterngeneratorinterface.hh
@@ -35,7 +35,7 @@
#define _PATTERN_GENERATOR_INTERFACE_H_
#include <deque>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pgtypes.hh>
namespace PatternGeneratorJRL
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 07707360..73fb7cd5 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -32,7 +32,7 @@ ADD_DEFINITIONS("/DLSSOL_FOUND")
ENDIF(USE_LSSOL)
SET(INCLUDES
- PinocchioRobot.hh
+ RobotDynamics/PinocchioRobot.hh
PreviewControl/rigid-body.hh
PreviewControl/OptimalControllerSolver.hh
PreviewControl/rigid-body-system.hh
@@ -113,7 +113,7 @@ ENDIF(${qpOASES_FOUND} STREQUAL "TRUE")
SET(SOURCES
${INCLUDES}
- PinocchioRobot.cpp
+ RobotDynamics/PinocchioRobot.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
diff --git a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
index adb1fff1..b58441ae 100644
--- a/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
+++ b/src/FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.hh
@@ -39,7 +39,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/* dynamics pinocchio related inclusions */
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
/* Walking pattern generation related inclusions */
diff --git a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
index b8518dd8..802dec1e 100644
--- a/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
+++ b/src/FootTrajectoryGeneration/LeftAndRightFootTrajectoryGenerationMultiple.hh
@@ -33,7 +33,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/* abstractRobotDynamics inclusion */
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
/* Walking Pattern Generator inclusion */
diff --git a/src/GlobalStrategyManagers/GlobalStrategyManager.hh b/src/GlobalStrategyManagers/GlobalStrategyManager.hh
index 6e42e32c..98e9c556 100644
--- a/src/GlobalStrategyManagers/GlobalStrategyManager.hh
+++ b/src/GlobalStrategyManagers/GlobalStrategyManager.hh
@@ -33,7 +33,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
// Dynamics
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
// PG
#include <jrl/walkgen/pgtypes.hh>
diff --git a/src/Mathematics/FootConstraintsAsLinearSystem.hh b/src/Mathematics/FootConstraintsAsLinearSystem.hh
index 971d2beb..dd624c5d 100644
--- a/src/Mathematics/FootConstraintsAsLinearSystem.hh
+++ b/src/Mathematics/FootConstraintsAsLinearSystem.hh
@@ -38,7 +38,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pgtypes.hh>
diff --git a/src/Mathematics/relative-feet-inequalities.hh b/src/Mathematics/relative-feet-inequalities.hh
index 7e0c651c..339a287e 100644
--- a/src/Mathematics/relative-feet-inequalities.hh
+++ b/src/Mathematics/relative-feet-inequalities.hh
@@ -34,7 +34,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pgtypes.hh>
diff --git a/src/MotionGeneration/ComAndFootRealization.hh b/src/MotionGeneration/ComAndFootRealization.hh
index accac247..9f891e2b 100644
--- a/src/MotionGeneration/ComAndFootRealization.hh
+++ b/src/MotionGeneration/ComAndFootRealization.hh
@@ -29,7 +29,7 @@
#ifndef _COM_AND_FOOT_REALIZATION_H_
#define _COM_AND_FOOT_REALIZATION_H_
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
#include <SimplePlugin.hh>
#include <StepStackHandler.hh>
@@ -132,7 +132,7 @@ namespace PatternGeneratorJRL
/*! @param aHumanoidDynamicRobot: an object able to compute dynamic parameters
of the robot. */
- inline virtual bool setHumanoidDynamicRobot(PinocchioRobot *aPinocchioRobot)
+ inline virtual bool setPinocchioRobot(PinocchioRobot *aPinocchioRobot)
{ m_PinocchioRobot = aPinocchioRobot;
return true;}
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index 59957973..f8e2021d 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -1280,7 +1280,7 @@ void ComAndFootRealizationByGeometry::
bool ComAndFootRealizationByGeometry::
setPinocchioRobot(PinocchioRobot * aPinocchioRobot)
{
- ComAndFootRealization::setHumanoidDynamicRobot(aPinocchioRobot);
+ ComAndFootRealization::setPinocchioRobot(aPinocchioRobot);
PinocchioRobot *aPR = aPinocchioRobot;
MAL_VECTOR_RESIZE(m_prev_Configuration,aPR->numberDof());
diff --git a/src/MotionGeneration/StepOverPlanner.hh b/src/MotionGeneration/StepOverPlanner.hh
index 7461a6fb..bafb08a8 100644
--- a/src/MotionGeneration/StepOverPlanner.hh
+++ b/src/MotionGeneration/StepOverPlanner.hh
@@ -44,7 +44,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/*! Abstract Interface for dynamic robot. */
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
/*! Humanoid Walking Pattern Generator */
diff --git a/src/PatternGeneratorInterfacePrivate.cpp b/src/PatternGeneratorInterfacePrivate.cpp
index 295bce58..7bb21e93 100644
--- a/src/PatternGeneratorInterfacePrivate.cpp
+++ b/src/PatternGeneratorInterfacePrivate.cpp
@@ -338,7 +338,7 @@ namespace PatternGeneratorJRL {
// Read the robot VRML file model.
- m_ComAndFootRealization[0]->setHumanoidDynamicRobot(m_PinocchioRobot);
+ m_ComAndFootRealization[0]->setPinocchioRobot(m_PinocchioRobot);
m_ComAndFootRealization[0]->SetHeightOfTheCoM(m_PC->GetHeightOfCoM());
m_ComAndFootRealization[0]->setSamplingPeriod(m_PC->SamplingPeriod());
m_ComAndFootRealization[0]->Initialization();
diff --git a/src/PreviewControl/rigid-body-system.hh b/src/PreviewControl/rigid-body-system.hh
index 22813f1b..afee5a7a 100644
--- a/src/PreviewControl/rigid-body-system.hh
+++ b/src/PreviewControl/rigid-body-system.hh
@@ -30,7 +30,7 @@
#include <privatepgtypes.hh>
#include <PreviewControl/SupportFSM.hh>
#include <FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.h>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
namespace PatternGeneratorJRL
{
diff --git a/src/RobotDynamics/PinocchioRobot.cpp b/src/RobotDynamics/PinocchioRobot.cpp
new file mode 100644
index 00000000..48046e50
--- /dev/null
+++ b/src/RobotDynamics/PinocchioRobot.cpp
@@ -0,0 +1,460 @@
+#include "RobotDynamics/PinocchioRobot.hh"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+using namespace PatternGeneratorJRL;
+
+PinocchioRobot::PinocchioRobot(se3::Model * aModel, se3::Data * aData):
+ m_robotDataInInitialePose(*m_robotModel)
+{
+ m_robotModel = aModel ;
+ m_robotData = aData ;
+
+ m_q.resize(m_robotModel->nq,0.0);
+ m_v.resize(m_robotModel->nv,0.0);
+ m_a.resize(m_robotModel->nv,0.0);
+
+ m_robotData->v[0] = se3::Motion::Zero();
+ m_robotData->a[0] = -m_robotModel->gravity;
+ m_robotDataInInitialePose.v[0] = se3::Motion::Zero();
+ m_robotDataInInitialePose.a[0] = -m_robotModel->gravity;
+
+ m_mass=0.0;
+ for(unsigned i=0; m_robotModel->inertias.size() ; ++i)
+ {
+ m_mass += m_robotModel->inertias[i].mass();
+ }
+}
+
+void PinocchioRobot::InitializePinocchioRobot(se3::Model * aModel, se3::Data * aData)
+{
+ m_q.resize(m_robotModel->nq,0.0);
+ m_v.resize(m_robotModel->nv,0.0);
+ m_a.resize(m_robotModel->nv,0.0);
+ se3::forwardKinematics(*m_robotModel,m_robotDataInInitialePose,m_q);
+}
+
+
+void PinocchioRobot::computeForwardKinematics()
+{
+ computeForwardKinematics(m_qmal);
+}
+
+void PinocchioRobot::computeForwardKinematics(MAL_VECTOR_TYPE(double) & q)
+{
+ // euler to quaternion :
+ m_quat = Eigen::Quaternion<double>(
+ Eigen::AngleAxisd((double)q(3), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd((double)q(4), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd((double)q(5), Eigen::Vector3d::UnitX()) ) ;
+
+ // fill up m_q following the pinocchio standard : [pos quarternion DoFs]
+ for(unsigned i=0; i<3 ; ++i)
+ {
+ m_q(i) = q(i);
+ }
+ m_q(3) = m_quat.x() ;
+ m_q(4) = m_quat.y() ;
+ m_q(5) = m_quat.z() ;
+ m_q(6) = m_quat.w() ;
+ for(unsigned i=0; i<m_robotModel->nv-6 ; ++i)
+ {
+ m_q(7+i) = q(6+i);
+ }
+ se3::forwardKinematics(*m_robotModel,*m_robotData,m_q);
+}
+
+void PinocchioRobot::computeInverseDynamics()
+{
+ PinocchioRobot::computeInverseDynamics(m_qmal,m_vmal,m_amal);
+}
+
+void PinocchioRobot::computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
+ MAL_VECTOR_TYPE(double) & v,
+ MAL_VECTOR_TYPE(double) & a)
+{
+ // euler to quaternion :
+ m_quat = Eigen::Quaternion<double>(
+ Eigen::AngleAxisd((double)q(3), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd((double)q(4), Eigen::Vector3d::UnitX()) *
+ Eigen::AngleAxisd((double)q(5), Eigen::Vector3d::UnitX()) ) ;
+
+ // fill up m_q following the pinocchio standard : [pos quarternion DoFs]
+ for(unsigned i=0; i<3 ; ++i)
+ {
+ m_q(i) = q(i);
+ }
+ m_q(3) = m_quat.x() ;
+ m_q(4) = m_quat.y() ;
+ m_q(5) = m_quat.z() ;
+ m_q(6) = m_quat.w() ;
+ for(unsigned i=0; i<m_robotModel->nv-6 ; ++i)
+ {
+ m_q(7+i) = q(6+i);
+ }
+
+ // fill up the velocity and acceleration vectors
+ for(unsigned i=0; i<m_robotModel->nv ; ++i)
+ {
+ m_v(i) = v(i);
+ m_a(i) = a(i);
+ }
+
+ // performing the inverse dynamics
+ se3::rnea(*m_robotModel,*m_robotData,m_q,m_v,m_a);
+}
+
+std::vector<se3::JointIndex> PinocchioRobot::fromRootToIt(se3::JointIndex it)
+{
+ std::vector<se3::JointIndex> fromRootToIt ;
+ fromRootToIt.clear();
+ se3::JointIndex i = it ;
+ while(i!=0)
+ {
+ fromRootToIt.insert(fromRootToIt.begin(),i);
+ i = m_robotModel->parents[i];
+ }
+ return fromRootToIt ;
+}
+
+std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
+( se3::JointIndex first, se3::JointIndex second)
+{
+ std::vector<se3::JointIndex> fromRootToFirst = fromRootToIt(first);
+ std::vector<se3::JointIndex> fromRootToSecond = fromRootToIt(second);
+
+ std::vector<se3::JointIndex> out ;
+ out.clear();
+ se3::JointIndex lastCommonRank = 0 ;
+ se3::JointIndex minChainLength =
+ fromRootToFirst.size() < fromRootToSecond.size()
+ ? fromRootToFirst.size() : fromRootToSecond.size() ;
+
+ for(unsigned k=1 ; k<minChainLength ; ++k)
+ {
+ if(fromRootToFirst[k] == fromRootToSecond[k])
+ ++lastCommonRank;
+ }
+
+ for(unsigned k=fromRootToFirst.size()-1; k>lastCommonRank ; --k)
+ {
+ out.push_back(fromRootToFirst[k]);
+ }
+ if(lastCommonRank==0)
+ {
+ out.push_back(fromRootToSecond[0]);
+ }
+ for(unsigned k=lastCommonRank+1 ; k<fromRootToSecond.size() ; ++k)
+ {
+ out.push_back(fromRootToSecond[k]);
+ }
+
+ return out ;
+}
+
+/////////////////////////////////////////////////////////////////////////////////////////////
+bool PinocchioRobot::
+ComputeSpecializedInverseKinematics(
+ const se3::JointIndex &jointRoot,
+ const se3::JointIndex &jointEnd,
+ const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
+ const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
+ MAL_VECTOR_TYPE(double) &q )
+{
+
+ /*! Try to find out which kinematics chain the user
+ send to the method.*/
+ if (jointRoot==m_waist)
+ {
+ /* Consider here the legs. */
+ vector3d Dt;
+ bool ok=false;
+ if (jointEnd==m_leftFoot.associatedAnkle)
+ {
+ Dt(0)=0.0;Dt(1)=0.06;Dt(2)=0.0;
+ ok=true;
+ }
+ else if (jointEnd==m_rightFoot.associatedAnkle)
+ {
+ Dt(0)=0.0;Dt(1)=-0.06;Dt(2)=0.0;
+ ok=true;
+ }
+ if (ok)
+ {
+ getWaistFootKinematics(jointRootPosition,
+ jointEndPosition,
+ q,Dt);
+ return true;
+ }
+ }
+ else
+ {
+ if ( (m_leftShoulder==0) || (m_rightShoulder==0) )
+ {
+ DetectAutomaticallyShoulders();
+ }
+
+ /* Here consider the arms */
+ if (jointRoot==m_leftShoulder)
+ {
+ int Side;
+ bool ok=false;
+ if (jointEnd==m_leftHand.associatedWrist)
+ {
+ Side = 1;
+ ok=true;
+ }
+ if (ok)
+ {
+ getShoulderWristKinematics(jointRootPosition,
+ jointEndPosition,
+ q,Side);
+ return true;
+ }
+ }
+
+ if (jointRoot==m_rightShoulder)
+ {
+ int Side;
+ bool ok=false;
+
+ if (jointEnd==m_rightHand.associatedWrist)
+ {
+ Side = -1;
+ ok=true;
+ }
+ if (ok)
+ {
+ getShoulderWristKinematics(jointRootPosition,
+ jointEndPosition,
+ q,Side);
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
+ const matrix4d & jointEndPosition,
+ vectorN &q,
+ vector3d Dt)
+{
+ double _epsilon=1.0e-6;
+ // definition des variables relatif au design du robot
+ double A = 0.3;//m_FemurLength;
+ double B = 0.3;//m_TibiaLength;
+ double C = 0.0;
+ double c5 = 0.0;
+ double q6a = 0.0;
+
+ vector3d r;
+
+ /* Build sub-matrices */
+ matrix3d Foot_R,Body_R;
+ vector3d Foot_P,Body_P;
+ for(unsigned int i=0;i<3;i++)
+ {
+ for(unsigned int j=0;j<3;j++)
+ {
+ MAL_S3x3_MATRIX_ACCESS_I_J(Body_R,i,j) =
+ MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,i,j);
+ MAL_S3x3_MATRIX_ACCESS_I_J(Foot_R,i,j) =
+ MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,i,j);
+ }
+ Body_P(i) = MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,i,3);
+ Foot_P(i) = MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,i,3);
+ }
+
+ matrix3d Foot_Rt;
+ MAL_S3x3_TRANSPOSE_A_in_At(Foot_R,Foot_Rt);
+
+ // Initialisation of q
+ if (MAL_VECTOR_SIZE(q)!=6)
+ MAL_VECTOR_RESIZE(q,6);
+
+ for(unsigned int i=0;i<6;i++)
+ q(i)=0.0;
+
+ double OppSignOfDtY = Dt(1) < 0.0 ? 1.0 : -1.0; // if Dt(1)<0.0 then Opp=1.0 else Opp=-1.0
+
+ vector3d d2,d3;
+ d2 = Body_P + Body_R * Dt;
+ d3 = d2 - Foot_P;
+
+ double l0 = sqrt(d3(0)*d3(0)+d3(1)*d3(1)+d3(2)*d3(2) - 0.035*0.035);
+ c5 = 0.5 * (l0*l0-A*A-B*B) / (A*B);
+ if (c5 > 1.0-_epsilon)
+ {
+ q[3] = 0.0;
+ }
+ if (c5 < -1.0+_epsilon)
+ {
+ q[3] = M_PI;
+ }
+ if (c5 >= -1.0+_epsilon && c5 <= 1.0-_epsilon)
+ {
+ q[3] = acos(c5);
+ }
+
+ vector3d r3;
+ r3 = Foot_Rt * d3;
+
+ q6a = asin((A/l0)*sin(M_PI- q[3]));
+
+ double l3 = sqrt(r3(1)*r3(1) + r3(2)*r3(2));
+ double l4 = sqrt(l3*l3 - 0.035*0.035);
+
+ double phi = atan2(r3(0), l4);
+ q[4] = -phi - q6a;
+
+ double psi1 = atan2(r3(1), r3(2)) * OppSignOfDtY;
+ double psi2 = 0.5*M_PI - psi1;
+ double psi3 = atan2(l4, 0.035);
+ q[5] = (psi3 - psi2) * OppSignOfDtY;
+
+ if (q[5] > 0.5*M_PI)
+ {
+ q[5] -= M_PI;
+ }
+ else if (q[5] < -0.5*M_PI)
+ {
+ q[5] += M_PI;
+ }
+
+ matrix3d R;
+ matrix3d BRt;
+ MAL_S3x3_TRANSPOSE_A_in_At(Body_R,BRt);
+
+ matrix3d Rroll;
+ double c = cos(q[5]);
+ double s = sin(q[5]);
+
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,0,0) = 1.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,0,1) = 0.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,0,2) = 0.0;
+
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,1,0) = 0.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,1,1) = c;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,1,2) = s;
+
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,2,0) = 0.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,2,1) = -s;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rroll,2,2) = c;
+
+ matrix3d Rpitch;
+ c = cos(q[4]+q[3]);
+ s = sin(q[4]+q[3]);
+
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,0,0) = c;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,0,1) = 0.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,0,2) = -s;
+
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,1,0) = 0.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,1,1) = 1.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,1,2) = 0.0;
+
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,2,0) = s;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,2,1) = 0.0;
+ MAL_S3x3_MATRIX_ACCESS_I_J(Rpitch,2,2) = c;
+
+ R = BRt * Foot_R * Rroll * Rpitch;
+ q[0] = atan2(-R(0,1),R(1,1));
+
+ double cz = cos(q[0]);
+ double sz = sin(q[0]);
+
+ q[1] = atan2(R(2,1), -R(0,1)*sz+R(1,1)*cz);
+ q[2] = atan2( -R(2,0), R(2,2));
+}
+
+double PinocchioRobot::ComputeXmax(double & Z)
+{
+ double A=0.25,
+ B=0.25;
+ double Xmax;
+ if (Z<0.0)
+ Z = 2*A*cos(15*M_PI/180.0);
+ Xmax = sqrt(A*A - (Z - B)*(Z-B));
+ return Xmax;
+}
+
+void PinocchioRobot::getShoulderWristKinematics(const matrix4d & jointRootPosition,
+ const matrix4d & jointEndPosition,
+ vectorN &q,
+ int side)
+{
+
+ // Initialisation of q
+ if (MAL_VECTOR_SIZE(q)!=6)
+ MAL_VECTOR_RESIZE(q,6);
+
+ double Alpha,Beta;
+ for(unsigned int i=0;i<6;i++)
+ q(i)=0.0;
+
+ double X = MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,0,3)
+ - MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,0,3);
+ double Z = MAL_S4x4_MATRIX_ACCESS_I_J(jointEndPosition,2,3)
+ - MAL_S4x4_MATRIX_ACCESS_I_J(jointRootPosition,2,3);
+
+ double Xmax = ComputeXmax(Z);
+ X = X*Xmax;
+
+ double A=0.25, B=0.25;
+
+ double C=0.0,Gamma=0.0,Theta=0.0;
+ C = sqrt(X*X+Z*Z);
+
+ Beta = acos((A*A+B*B-C*C)/(2*A*B))- M_PI;
+ Gamma = asin((B*sin(M_PI+Beta))/C);
+ Theta = atan2(X,Z);
+ Alpha = Gamma - Theta;
+
+ // Fill in the joint values.
+ q(0)= Alpha;
+ q(1)= 10.0*M_PI/180.0;
+ q(2)= 0.0;
+ q(3)= Beta;
+ q(4)= 0.0;
+ q(5)= 0.0;
+
+ if (side==-1)
+ q(1) = -q(1);
+
+
+}
+
+void PinocchioRobot::DetectAutomaticallyShoulders()
+{
+ DetectAutomaticallyOneShoulder(m_leftHand,m_leftShoulder);
+ DetectAutomaticallyOneShoulder(m_rightHand,m_rightShoulder);
+}
+
+void PinocchioRobot::DetectAutomaticallyOneShoulder(
+ PRHand aHand,
+ se3::JointIndex & aShoulder)
+{
+ std::vector<se3::JointIndex>FromRootToJoint;
+
+ FromRootToJoint.clear();
+ FromRootToJoint = fromRootToIt(aHand.associatedWrist);
+
+ std::vector<se3::JointIndex>::iterator itJoint = FromRootToJoint.begin();
+ bool found=false;
+ while(itJoint!=FromRootToJoint.end())
+ {
+ std::vector<se3::JointIndex>::iterator current = itJoint;
+ if (*current==m_chest)
+ found=true;
+ else
+ {
+ if (found)
+ {
+ aShoulder = *current;
+ return;
+ }
+ }
+ itJoint++;
+ }
+}
diff --git a/src/RobotDynamics/PinocchioRobot.hh b/src/RobotDynamics/PinocchioRobot.hh
new file mode 100644
index 00000000..a43305b7
--- /dev/null
+++ b/src/RobotDynamics/PinocchioRobot.hh
@@ -0,0 +1,201 @@
+/*
+ * Copyright 2016,
+ *
+ * Maximilien Naveau
+ * Olivier Stasse
+ *
+ * JRL, CNRS/AIST
+ *
+ * This file is part of jrl-walkgen.
+ * jrl-walkgen 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.
+ *
+ * jrl-walkgen 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 jrl-walkgen. If not, see <http://www.gnu.org/licenses/>.
+ *
+ * Research carried out within the scope of the
+ * Joint Japanese-French Robotics Laboratory (JRL)
+ */
+/*! \file PinocchioRobot.hh
+ \brief This object defines a humanoid robot model based on the PinocchioRobot
+frame work */
+
+#ifndef PinocchioRobot_HH
+#define PinocchioRobot_HH
+
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include <jrl/mal/matrixabstractlayer.hh>
+#include <vector>
+namespace PatternGeneratorJRL
+{
+ struct PinocchioRobotFoot_t{
+ se3::JointIndex associatedAnkle ;
+ double soleDepth ; // z axis
+ double soleWidth ; // y axis
+ double soleHeight ;// x axis
+ vector3d anklePosition ;
+ unsigned JointNb ;
+ std::vector<unsigned> JointId ;
+ };
+ typedef PinocchioRobotFoot_t PRFoot ;
+
+ struct PinocchioRobotHand_t{
+ se3::JointIndex associatedWrist ;
+ vector3d center ;
+ vector3d thumbAxis ;
+ vector3d foreFingerAxis ;
+ vector3d palmNormal ;
+ unsigned JointNb ;
+ std::vector<unsigned> JointId ;
+ };
+ typedef PinocchioRobotHand_t PRHand ;
+
+
+
+ class PinocchioRobot
+ {
+ public:
+ /// Constructor and Destructor
+ PinocchioRobot(se3::Model *aModel, se3::Data *aData);
+ void InitializePinocchioRobot(se3::Model * aModel, se3::Data * aData);
+
+
+ /// Functions computing kinematics and dynamics
+ void computeInverseDynamics();
+ void computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
+ MAL_VECTOR_TYPE(double) & v,
+ MAL_VECTOR_TYPE(double) & a);
+
+ void computeForwardKinematics();
+ void computeForwardKinematics(MAL_VECTOR_TYPE(double) & q);
+
+ // compute POSITION (not velocity) of the joints from end effector pose
+ bool ComputeSpecializedInverseKinematics(
+ const se3::JointIndex &jointRoot,
+ const se3::JointIndex &jointEnd,
+ const MAL_S4x4_MATRIX_TYPE(double) & jointRootPosition,
+ const MAL_S4x4_MATRIX_TYPE(double) & jointEndPosition,
+ MAL_VECTOR_TYPE(double) &q);
+
+ public :
+ /// tools :
+ std::vector<se3::JointIndex> jointsBetween
+ ( se3::JointIndex first, se3::JointIndex second);
+ std::vector<se3::JointIndex> fromRootToIt (se3::JointIndex it);
+
+ private :
+ // need for the inverse geometry (ComputeSpecializedInverseKinematics)
+ void getWaistFootKinematics(const matrix4d & jointRootPosition,
+ const matrix4d & jointEndPosition,
+ vectorN &q,
+ vector3d Dt);
+ double ComputeXmax(double & Z);
+ void getShoulderWristKinematics(const matrix4d & jointRootPosition,
+ const matrix4d & jointEndPosition,
+ vectorN &q,
+ int side);
+ void DetectAutomaticallyShoulders();
+ void DetectAutomaticallyOneShoulder(PRHand aHand,
+ se3::JointIndex & aShoulder);
+
+
+
+
+ public :
+ /// Getters
+ /// ///////
+ inline se3::Data * Data()
+ {return m_robotData;}
+ inline se3::Data * DataInInitialePose()
+ {return &m_robotDataInInitialePose;}
+ inline se3::Model * Model()
+ {return m_robotModel;}
+
+ inline PRFoot * leftFoot()
+ {return &m_leftFoot;}
+ inline PRFoot * rightFoot()
+ {return &m_rightFoot;}
+
+ inline PRHand * leftHand()
+ {return &m_leftHand;}
+ inline PRHand * rightHand()
+ {return &m_rightHand;}
+
+ inline se3::JointIndex chest()
+ {return m_chest;}
+ inline se3::JointIndex waist()
+ {return m_waist;}
+
+ inline double mass()
+ {return m_mass;}
+
+ inline se3::JointModelVector & getActuatedJoints()
+ {return m_robotModel->joints;}
+
+ inline MAL_VECTOR_TYPE(double) currentConfiguration()
+ {return m_qmal;}
+ inline MAL_VECTOR_TYPE(double) currentVelocity()
+ {return m_vmal;}
+ inline MAL_VECTOR_TYPE(double) currentAcceleration()
+ {return m_amal;}
+
+ inline unsigned numberDof()
+ {return m_robotModel->nv;}
+
+ inline void zeroMomentumPoint(MAL_S3_VECTOR_TYPE(double) & zmp)
+ {
+ se3::Force externalForces = m_robotData->oMi[1].act(m_robotData->f[1]) ;
+ m_f = externalForces.linear() ;
+ m_n = externalForces.angular() ;
+ zmp(0) = -m_n(1)/m_f(2) ;
+ zmp(1) = m_n(0)/m_f(2) ;
+ zmp(2) = 0.0 ; // by default
+ }
+
+ inline void positionCenterOfMass(MAL_S3_VECTOR_TYPE(double) & com)
+ {
+ m_com = m_robotData->com[0] ;
+ com(0) = m_com(0) ;
+ com(1) = m_com(1) ;
+ com(2) = m_com(2) ;
+ }
+
+ /// SETTERS
+ /// ///////
+ inline void currentConfiguration(MAL_VECTOR_TYPE(double) conf)
+ {m_qmal=conf;}
+ inline void currentVelocity(MAL_VECTOR_TYPE(double) vel)
+ {m_vmal=vel;}
+ inline void currentAcceleration(MAL_VECTOR_TYPE(double) acc)
+ {m_amal=acc;}
+
+ private:
+ se3::Model * m_robotModel ;
+ se3::Data m_robotDataInInitialePose ; // internal variable
+ se3::Data * m_robotData ;
+ PRFoot m_leftFoot , m_rightFoot ;
+ PRHand m_leftHand , m_rightHand ;
+ double m_mass ;
+ se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
+
+ MAL_VECTOR_TYPE(double) m_qmal ;
+ MAL_VECTOR_TYPE(double) m_vmal ;
+ MAL_VECTOR_TYPE(double) m_amal ;
+ Eigen::VectorXd m_q ;
+ Eigen::VectorXd m_v ;
+ Eigen::VectorXd m_a ;
+
+ // tmp variables
+ Eigen::Quaternion<double> m_quat ;
+ Eigen::Vector3d m_f,m_n; // external forces and torques
+ Eigen::Vector3d m_com; // multibody CoM
+ };
+}
+#endif // PinocchioRobot_HH
diff --git a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
index 1c94628b..fdbd55bc 100644
--- a/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
+++ b/src/ZMPRefTrajectoryGeneration/OrientationsPreview.hh
@@ -38,7 +38,7 @@
#include <privatepgtypes.hh>
#include <jrl/walkgen/pgtypes.hh>
#include <Mathematics/PolynomeFoot.hh>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
namespace PatternGeneratorJRL
{
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.hh b/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.hh
index 717d134a..61fb51d4 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPDiscretization.hh
@@ -45,7 +45,7 @@
using namespace::std;
/*! Abstract robot dynamics includes */
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
/*! Framework includes */
#include <Mathematics/PolynomeFoot.hh>
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
index cd14fb37..7cb3ab06 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.hh
@@ -31,7 +31,7 @@
#include <ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
#include <ZMPRefTrajectoryGeneration/qp-problem.hh>
#include <PreviewControl/SupportFSM.hh>
#include <PreviewControl/LinearizedInvertedPendulum2D.hh>
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
index a24ef71a..094e2a12 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.hh
@@ -30,7 +30,7 @@
#include <jrl/walkgen/pgtypes.hh>
#include <Mathematics/relative-feet-inequalities.hh>
-#include <PinocchioRobot.hh>
+#include <RobotDynamics/PinocchioRobot.hh>
#include <iomanip>
#include <cmath>
#include <qpOASES.hpp>
diff --git a/tests/TestObject.cpp b/tests/TestObject.cpp
index 920e7ab0..fbd2a985 100644
--- a/tests/TestObject.cpp
+++ b/tests/TestObject.cpp
@@ -124,14 +124,6 @@ namespace PatternGeneratorJRL
delete m_PGI;
}
- void TestObject::SpecializedRobotConstructor( CjrlHumanoidDynamicRobot *& aHDR,
- CjrlHumanoidDynamicRobot *& aDebugHDR)
- {
- aHDR = 0;
- aDebugHDR = 0;
- }
-
-
void TestObject::CreateAndInitializeHumanoidRobot(string &RobotFileName,
string &SpecificitiesFileName,
string &LinkJointRank,
@@ -141,7 +133,7 @@ namespace PatternGeneratorJRL
PatternGeneratorInterface * & aPGI)
{
// Creating the humanoid robot.
- SpecializedRobotConstructor(aHDR,aDebugHDR);
+ PinocchioRobot
if ((aHDR==0) || (aDebugHDR==0))
{
diff --git a/tests/TestObject.hh b/tests/TestObject.hh
index 1e0185e1..e118970e 100644
--- a/tests/TestObject.hh
+++ b/tests/TestObject.hh
@@ -44,7 +44,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
#include <jrl/dynamics/dynamicsfactory.hh>
#include <jrl/walkgen/patterngeneratorinterface.hh>
-#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
+//#include <MotionGeneration/ComAndFootRealizationByGeometry.hh>
#include "CommonTools.hh"
#include "ClockCPUTime.hh"
@@ -76,10 +76,6 @@ namespace PatternGeneratorJRL
/*! \brief Perform test. */
bool doTest(std::ostream &os);
- /*! \brief Decide from which object the robot is build from. */
- virtual void SpecializedRobotConstructor(PinocchioRobot *& aPR,
- PinocchioRobot *& aDebugPR );
-
protected:
/*! \brief Choose which test to perform. */
--
GitLab