From 10dfbc4461e9c6d19c40a807a840507476f20020 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Tue, 11 Feb 2014 19:02:20 +0100
Subject: [PATCH] add the Cmake file for the use of metapod
---
.../ZMPVelocityReferencedQP.cpp | 26 +++++++++----------
.../ZMPVelocityReferencedQP.hh | 1 +
2 files changed, 13 insertions(+), 14 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 47034788..5021022a 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -719,8 +719,9 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
i);
}
- /// \brief rnea
- /// -----------
+ /// \brief rnea, calculation of the multi body ZMP
+ /// ----------------------------------------------
+ double _1_mg = 1/(RobotMass_*9.81);
for (unsigned int i = 0 ; i < N ; i++ ){
// Apply the RNEA to the metapod multibody and print the result in a log file.
for(unsigned int j = 0 ; j < m_configurationTraj[i].size() ; j++ )
@@ -732,19 +733,15 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
metapod::rnea< Robot_Model, true >::run(m_robot, m_q, m_dq, m_ddq);
getTorques(m_robot, m_torques);
m_allTorques[i] = m_torques ;
- }
- /// \brief Projection of the Torques on the ground, the result is the ZMP Multi-Body
- /// --------------------------------------------------------------------------------
- double _1_mg = 1/(RobotMass_*9.81);
-// m_deltaZMPMBPositions[0].px = 0.0 ;
-// m_deltaZMPMBPositions[0].py = 0.0 ;
-// m_deltaZMPMBPositions[0].pz = 0.0 ;
-// m_deltaZMPMBPositions[0].theta = 0.0 ;
-// m_deltaZMPMBPositions[0].time = m_CurrentTime ;
-// m_deltaZMPMBPositions[0].stepType = ZMPPositions[0].stepType ;
- for(unsigned int i = 0; i < N ; i++ )
- {
+ Node & node = boost::fusion::at_c<0>(Robot_Model.nodes);
+ metapod::Spatial::Force aforce = node.joint.f ;
+
+
+
+
+
+
m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - m_allTorques[i](4,0) * _1_mg + FinalCOMTraj_deq[currentIndex+i].x[0] ) ;
m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( m_allTorques[i](3,0) * _1_mg + FinalCOMTraj_deq[currentIndex+i].y[0] ) ;
m_deltaZMPMBPositions[i].pz = 0.0 ;
@@ -753,6 +750,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
m_deltaZMPMBPositions[i].stepType = ZMPPositions[currentIndex+i].stepType ;
}
+
/// \brief Debug Purpose
/// --------------------
ofstream aof5;
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
index 3334c5f9..88e606ce 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
@@ -49,6 +49,7 @@
#define METAPOD_TYPEDEF
typedef double LocalFloatType;
typedef metapod::hrp2_14<LocalFloatType> Robot_Model;
+typedef metapod::Nodes< Robot_Model, 0 >::type Node;
#endif
namespace PatternGeneratorJRL
--
GitLab