Commit df81d01d authored by mnaveau's avatar mnaveau
Browse files

TestNaveau compiling with pinocchio

parent 2728085b
......@@ -44,15 +44,6 @@ ENDIF(SYS_TIME_H)
# Required dependencies
ADD_REQUIRED_DEPENDENCY("jrl-mal >= 1.9.0")
ADD_REQUIRED_DEPENDENCY("pinocchio >= 1.1.0")
#ADD_REQUIRED_DEPENDENCY("abstract-robot-dynamics >= 1.15")
#ADD_REQUIRED_DEPENDENCY("jrl-dynamics >= 1.19.1")
################################################################
#ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6")
#ADD_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.0.1")
#
#ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
#################################################################
# Search for qpOases
LIST(APPEND CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_SOURCE_DIR}/private_cmake)
......@@ -123,6 +114,7 @@ ENDIF(CMAKE_COMPILER_IS_GNUCXX)
SET(${PROJECT_NAME}_HEADERS
include/jrl/walkgen/patterngeneratorinterface.hh
include/jrl/walkgen/pgtypes.hh
include/jrl/walkgen/pinocchiorobot.hh
)
# Define subdirectories to explore for cmake
......
......@@ -35,7 +35,7 @@
#define _PATTERN_GENERATOR_INTERFACE_H_
#include <deque>
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
#include <jrl/walkgen/pgtypes.hh>
namespace PatternGeneratorJRL
......
......@@ -41,31 +41,18 @@ namespace PatternGeneratorJRL
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);
// overload the new[] eigen operator
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// Constructor and Destructor
PinocchioRobot();
~PinocchioRobot();
/// Functions computing kinematics and dynamics
void computeInverseDynamics();
......@@ -102,19 +89,17 @@ namespace PatternGeneratorJRL
vectorN &q,
int side);
void DetectAutomaticallyShoulders();
void DetectAutomaticallyOneShoulder(PRHand aHand,
void DetectAutomaticallyOneShoulder(se3::JointIndex aWrist,
se3::JointIndex & aShoulder);
public :
/// Getters
/// ///////
inline se3::Data * Data()
{return m_robotData;}
inline se3::Data * DataInInitialePose()
{return &m_robotDataInInitialePose;}
{return m_robotDataInInitialePose;}
inline se3::Model * Model()
{return m_robotModel;}
......@@ -123,10 +108,10 @@ namespace PatternGeneratorJRL
inline PRFoot * rightFoot()
{return &m_rightFoot;}
inline PRHand * leftHand()
{return &m_leftHand;}
inline PRHand * rightHand()
{return &m_rightHand;}
inline se3::JointIndex leftWrist()
{return m_leftWrist;}
inline se3::JointIndex rightWrist()
{return m_rightWrist;}
inline se3::JointIndex chest()
{return m_chest;}
......@@ -176,26 +161,46 @@ namespace PatternGeneratorJRL
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
};
}
/// Initialization functions
/// ////////////////////////
inline bool isInitialized()
{
return m_boolModel && m_boolData && m_boolLeftFoot && m_boolRightFoot;
}
bool checkModel(se3::Model * robotModel);
bool initializeRobotModelAndData(se3::Model * robotModel,
se3::Data * robotData);
bool initializeLeftFoot(PRFoot leftFoot);
bool initializeRightFoot(PRFoot rightFoot);
/// Attributes
/// //////////
private :
se3::Model * m_robotModel ;
se3::Data * m_robotDataInInitialePose ; // internal variable
se3::Data * m_robotData ;
PRFoot m_leftFoot , m_rightFoot ;
double m_mass ;
se3::JointIndex m_chest, m_waist, m_leftShoulder, m_rightShoulder ;
se3::JointIndex m_leftWrist , m_rightWrist ;
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::Quaterniond m_quat ;
Eigen::Vector3d m_f,m_n; // external forces and torques
Eigen::Vector3d m_com; // multibody CoM
bool m_boolModel ;
bool m_boolData ;
bool m_boolLeftFoot ;
bool m_boolRightFoot ;
}; //PinocchioRobot
}// namespace PatternGeneratorJRL
#endif // PinocchioRobot_HH
......@@ -15,8 +15,6 @@
# Make sure to find Debug.h
INCLUDE_DIRECTORIES(BEFORE ${PROJECT_SOURCE_DIR}/src)
INCLUDE_DIRECTORIES(BEFORE ${HOME}/devel/metapod/_bin/include/)
INCLUDE_DIRECTORIES(BEFORE ${HOME}/devel/metapod/_bin/include/metapod/)
# Add Boost path to include directories.
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
......@@ -32,7 +30,6 @@ ADD_DEFINITIONS("/DLSSOL_FOUND")
ENDIF(USE_LSSOL)
SET(INCLUDES
RobotDynamics/PinocchioRobot.hh
PreviewControl/rigid-body.hh
PreviewControl/OptimalControllerSolver.hh
PreviewControl/rigid-body-system.hh
......@@ -113,7 +110,7 @@ ENDIF(${qpOASES_FOUND} STREQUAL "TRUE")
SET(SOURCES
${INCLUDES}
RobotDynamics/PinocchioRobot.cpp
RobotDynamics/pinocchiorobot.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationAbstract.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationStandard.cpp
FootTrajectoryGeneration/FootTrajectoryGenerationMultiple.cpp
......@@ -199,30 +196,18 @@ IF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
ADDPREFIX(${PROJECT_NAME}_ABSOLUTE_HEADERS "${CMAKE_SOURCE_DIR}/" ${PROJECT_NAME}_HEADERS)
ENDIF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
# define and search for metapod library
SET(WITH_METAPODFROMURDF TRUE)
SET(model_name "hrp2_14")
# Define the simple humanoid model for the metapod algorithm
ADD_SAMPLEURDFMODEL(${model_name})
ADD_LIBRARY(jrl-walkgen SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS})
SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES SOVERSION ${PROJECT_VERSION})
# Define dependencies
#TARGET_LINK_LIBRARIES(jrl-walkgen metapod_${model_name} ${Boost_LIBRARIES})
# Define dependencies
SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
INSTALL(TARGETS jrl-walkgen DESTINATION ${CMAKE_INSTALL_LIBDIR})
#PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen abstract-robot-dynamics)
PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen jrl-mal)
#PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen jrl-dynamics)
PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen pinocchio)
IF(USE_LSSOL)
PKG_CONFIG_USE_DEPENDENCY(jrl-walkgen lssol)
ENDIF(USE_LSSOL)
IF(${qpOASES_FOUND} STREQUAL "TRUE")
TARGET_LINK_LIBRARIES(jrl-walkgen ${qpOASES_LIBRARIES})
ENDIF(${qpOASES_FOUND} STREQUAL "TRUE")
INSTALL(TARGETS jrl-walkgen DESTINATION ${CMAKE_INSTALL_LIBDIR})
SET_TARGET_PROPERTIES(jrl-walkgen PROPERTIES SOVERSION ${PROJECT_VERSION})
......@@ -39,7 +39,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/* dynamics pinocchio related inclusions */
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
/* Walking pattern generation related inclusions */
......
......@@ -33,7 +33,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/* abstractRobotDynamics inclusion */
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
/* Walking Pattern Generator inclusion */
......
......@@ -33,7 +33,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
// Dynamics
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
// PG
#include <jrl/walkgen/pgtypes.hh>
......
......@@ -38,8 +38,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
#include <jrl/walkgen/pgtypes.hh>
#include <Mathematics/ConvexHull.hh>
......
......@@ -34,7 +34,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
#include <jrl/walkgen/pgtypes.hh>
......
......@@ -29,7 +29,7 @@
#ifndef _COM_AND_FOOT_REALIZATION_H_
#define _COM_AND_FOOT_REALIZATION_H_
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
#include <SimplePlugin.hh>
#include <StepStackHandler.hh>
......
......@@ -130,21 +130,20 @@ InitializationMaps(std::vector<se3::Index> &FromRootToJoint,
}
void ComAndFootRealizationByGeometry::
InitializeMapsForAHand(PRHand * aHand,
InitializeMapsForAHand(se3::JointIndex aWrist,
se3::JointModelVector & ActuatedJoints,
vector<int> & IndexesInConfiguration,
se3::JointIndex & associateShoulder)
{
if (aHand==0)
if (aWrist==0)
return;
// Find back the path from the shoulder to the left hand.
// CjrlHand *LeftHand = getHumanoidDynamicRobot()->leftHand();
se3::JointIndex Chest = getPinocchioRobot()->chest();
if (Chest==0)
return;
const se3::JointIndex associatedWrist = aHand->associatedWrist;
const se3::JointIndex associatedWrist = aWrist;
if (associatedWrist==0)
return;
......@@ -267,13 +266,13 @@ void ComAndFootRealizationByGeometry::
m_LeftLegIndexinConfiguration);
// Create maps for the left hand.
InitializeMapsForAHand(getPinocchioRobot()->leftHand(),
InitializeMapsForAHand(getPinocchioRobot()->leftWrist(),
ActuatedJoints,
m_LeftArmIndexinConfiguration,
m_LeftShoulder);
// Create maps for the right hand.
InitializeMapsForAHand(getPinocchioRobot()->rightHand(),
InitializeMapsForAHand(getPinocchioRobot()->rightWrist(),
ActuatedJoints,
m_RightArmIndexinConfiguration,
m_RightShoulder);
......@@ -1251,7 +1250,7 @@ void ComAndFootRealizationByGeometry::
getPinocchioRobot()->ComputeSpecializedInverseKinematics(
m_LeftShoulder,
getPinocchioRobot()->leftHand()->associatedWrist,
getPinocchioRobot()->leftWrist(),
jointRootPosition,
jointEndPosition,
qArml);
......@@ -1264,7 +1263,7 @@ void ComAndFootRealizationByGeometry::
getPinocchioRobot()->ComputeSpecializedInverseKinematics(
m_RightShoulder,
getPinocchioRobot()->rightHand()->associatedWrist,
getPinocchioRobot()->rightWrist(),
jointRootPosition,
jointEndPosition,
qArmr);
......
......@@ -317,7 +317,7 @@ namespace PatternGeneratorJRL
\param[out] associateShoulder: The shoulder extracted from
the kinematic chain.
*/
void InitializeMapsForAHand(PRHand * aHand,
void InitializeMapsForAHand(se3::JointIndex aWrist,
se3::JointModelVector & ActuatedJoints,
vector<int> & IndexesInConfiguration,
se3::JointIndex & associateShoulder);
......
......@@ -44,7 +44,7 @@
#include <jrl/mal/matrixabstractlayer.hh>
/*! Abstract Interface for dynamic robot. */
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
/*! Humanoid Walking Pattern Generator */
......
......@@ -30,7 +30,7 @@
#include <privatepgtypes.hh>
#include <PreviewControl/SupportFSM.hh>
#include <FootTrajectoryGeneration/OnLineFootTrajectoryGeneration.h>
#include <RobotDynamics/PinocchioRobot.hh>
#include <jrl/walkgen/pinocchiorobot.hh>
namespace PatternGeneratorJRL
{
......
#include "RobotDynamics/PinocchioRobot.hh"
#include <jrl/walkgen/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)
PinocchioRobot::PinocchioRobot()
{
m_robotModel = aModel ;
m_robotData = aData ;
// all the pointor are set to 0
m_robotModel = 0 ;
m_robotData = 0 ;
m_robotDataInInitialePose = 0 ;
// resize by default
m_q.resize(50,0.0);
m_v.resize(50,0.0);
m_a.resize(50,0.0);
MAL_VECTOR_RESIZE(m_qmal,50);
MAL_VECTOR_RESIZE(m_vmal,50);
MAL_VECTOR_RESIZE(m_amal,50);
MAL_VECTOR_FILL(m_qmal,0.0);
MAL_VECTOR_FILL(m_vmal,0.0);
MAL_VECTOR_FILL(m_amal,0.0);
m_quat = Eigen::Quaterniond(
Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) *
Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) ) ;
m_f = Eigen::Vector3d::Zero();
m_n = Eigen::Vector3d::Zero();
m_com = Eigen::Vector3d::Zero();
m_boolModel = false ;
m_boolData = false ;
m_boolLeftFoot = false ;
m_boolRightFoot = false ;
m_chest = 0 ;
m_waist = 0 ;
m_leftShoulder = 0 ;
m_rightShoulder = 0 ;
m_leftWrist = 0 ;
m_rightWrist = 0;
m_mass = 0.0 ;
memset(&m_leftFoot,0,sizeof(m_leftFoot));
memset(&m_rightFoot,0,sizeof(m_rightFoot));
}
PinocchioRobot::~PinocchioRobot()
{
if (m_robotDataInInitialePose != 0)
{
delete m_robotDataInInitialePose ;
m_robotDataInInitialePose = 0 ;
}
}
bool PinocchioRobot::checkModel(se3::Model * robotModel)
{
if(!m_robotModel->existBodyName("r_ankle"))
{
m_boolModel=false;
const std::string exception_message ("r_ankle is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
if(!m_robotModel->existBodyName("l_ankle"))
{
m_boolModel=false;
const std::string exception_message ("l_ankle is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
if(!m_robotModel->existBodyName("BODY"))
{
m_boolModel=false;
const std::string exception_message ("BODY is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
if(!m_robotModel->existBodyName("torso"))
{
m_boolModel=false;
const std::string exception_message ("torso is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
if(!m_robotModel->existBodyName("r_wrist"))
{
m_boolModel=false;
const std::string exception_message ("r_wrist is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
if(!m_robotModel->existBodyName("l_wrist"))
{
const std::string exception_message ("l_wrist is not a valid body name");
throw std::invalid_argument(exception_message);
return false ;
}
return true ;
}
bool PinocchioRobot::initializeRobotModelAndData(se3::Model * robotModel,
se3::Data * robotData)
{
m_boolModel=checkModel(robotModel);
if(!m_boolModel)
return false ;
// initialize the model
///////////////////////
m_robotModel = robotModel;
// initialize the short cut for the joint ids
m_chest = m_robotModel->getBodyId("torso");
m_waist = m_robotModel->getBodyId("WAIST");
m_leftFoot.associatedAnkle = m_robotModel->getBodyId("l_ankle");
m_rightFoot.associatedAnkle = m_robotModel->getBodyId("r_ankle");
m_leftWrist = m_robotModel->getBodyId("l_ankle");
m_rightWrist = m_robotModel->getBodyId("r_ankle");
DetectAutomaticallyShoulders();
// intialize the "initial pose" (q=[0]) data
m_robotDataInInitialePose = new se3::Data(*m_robotModel);
m_robotDataInInitialePose->v[0] = se3::Motion::Zero();
m_robotDataInInitialePose->a[0] = -m_robotModel->gravity;
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);
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;
// compute the global mass of the robot
m_mass=0.0;
for(unsigned i=0; m_robotModel->inertias.size() ; ++i)
{
m_mass += m_robotModel->inertias[i].mass();
}
// initialize the data
//////////////////////
if (robotData==0)
{
m_boolData = false ;
return false;
}
else
m_boolData=true;
m_robotData = robotData;
m_robotData->v[0] = se3::Motion::Zero();
m_robotData->a[0] = -m_robotModel->gravity;
return true ;
}
void PinocchioRobot::InitializePinocchioRobot(se3::Model * aModel, se3::Data * aData)
bool PinocchioRobot::initializeLeftFoot(PRFoot leftFoot)
{
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);
m_leftFoot = leftFoot ;
m_boolLeftFoot = true ;
return true ;
}
bool PinocchioRobot::initializeRightFoot(PRFoot rightFoot)
{
m_rightFoot = rightFoot ;
m_boolRightFoot = true ;
return true ;
}
void PinocchioRobot::computeForwardKinematics()
{
......@@ -151,7 +282,7 @@ std::vector<se3::JointIndex> PinocchioRobot::jointsBetween
return out ;
}
/////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
bool PinocchioRobot::
ComputeSpecializedInverseKinematics(
const se3::JointIndex &jointRoot,
......@@ -198,7 +329,7 @@ ComputeSpecializedInverseKinematics(
{
int Side;
bool ok=false;
if (jointEnd==m_leftHand.associatedWrist)
if (jointEnd==m_leftWrist)
{
Side = 1;
ok=true;
......@@ -217,7 +348,7 @@ ComputeSpecializedInverseKinematics(
int Side;
bool ok=false;
if (jointEnd==m_rightHand.associatedWrist)
if (jointEnd==m_rightWrist)
{
Side = -1;
ok=true;
......@@ -276,7 +407,8 @@ void PinocchioRobot::getWaistFootKinematics(const matrix4d & jointRootPosition,
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
// if Dt(1)<0.0 then Opp=1.0 else Opp=-1.0
double OppSignOfDtY = Dt(1) < 0.0 ? 1.0 : -1.0;
vector3d d2,d3;
d2 = Body_P + Body_R * Dt;
......@@ -401,7 +533,7 @@ void PinocchioRobot::getShoulderWristKinematics(const matrix4d & jointRootPositi
double Xmax = ComputeXmax(Z);
X = X*Xmax;