From 7c2379f8cb0df7970589c16cc05b182e76f7d20f Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Tue, 11 Feb 2014 20:45:16 +0100
Subject: [PATCH] Major changing in the ZMPMB computation
---
.../ZMPVelocityReferencedQP.cpp | 31 +++++++++----------
1 file changed, 14 insertions(+), 17 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 59c04e17..cf7d4526 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -721,7 +721,7 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
/// \brief rnea, calculation of the multi body ZMP
/// ----------------------------------------------
- double _1_mg = 1/(RobotMass_*9.81);
+ vector< vector<double> > ZMPMB ( N , vector<double> (2) );
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++ )
@@ -734,16 +734,13 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
getTorques(m_robot, m_torques);
m_allTorques[i] = m_torques ;
- Node & node = boost::fusion::at_c<0>(Robot_Model.nodes);
+ Node & node = boost::fusion::at_c<0>(m_robot.nodes);
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] ) ;
+ ZMPMB[i][0] = - aforce.n()[1] / aforce.f()[2];
+ ZMPMB[i][1] = aforce.n()[0] / aforce.f()[2] ;
+ m_deltaZMPMBPositions[i].px = ZMPPositions[currentIndex+i].px - ( - aforce.n()[1] / aforce.f()[2] ) ;
+ m_deltaZMPMBPositions[i].py = ZMPPositions[currentIndex+i].py - ( aforce.n()[0] / aforce.f()[2] ) ;
m_deltaZMPMBPositions[i].pz = 0.0 ;
m_deltaZMPMBPositions[i].theta = 0.0 ;
m_deltaZMPMBPositions[i].time = m_CurrentTime + i * m_SamplingPeriod ;
@@ -769,14 +766,14 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
aof5.precision(8);
aof5.setf(ios::scientific, ios::floatfield);
for(unsigned int i = 0 ; i < m_deltaZMPMBPositions.size(); i++){
- aof5 << filterprecision(ZMPPositions[currentIndex+i].px ) << " " // 1
- << filterprecision(ZMPPositions[currentIndex+i].py ) << " " // 2
- << filterprecision(- m_allTorques[i](4,0) * _1_mg + FinalCOMTraj_deq[currentIndex+i].x[0] ) << " " // 1
- << filterprecision( m_allTorques[i](3,0) * _1_mg + FinalCOMTraj_deq[currentIndex+i].y[0] ) << " " // 2
+ aof5 << filterprecision( ZMPPositions[currentIndex+i].px ) << " " // 1
+ << filterprecision( ZMPPositions[currentIndex+i].py ) << " " // 2
+ << filterprecision( ZMPMB[i][0] ) << " " // 1
+ << filterprecision( ZMPMB[i][1] ) << " " // 2
<< filterprecision( FinalCOMTraj_deq[currentIndex+i].x[0] ) << " " // 1
<< filterprecision( FinalCOMTraj_deq[currentIndex+i].y[0] ) << " " // 2
- << filterprecision(m_deltaZMPMBPositions[i].px ) << " " // 2
- << filterprecision(m_deltaZMPMBPositions[i].py ) << " " // 3
+ << filterprecision( m_deltaZMPMBPositions[i].px ) << " " // 2
+ << filterprecision( m_deltaZMPMBPositions[i].py ) << " " // 3
<< endl ;
}
aof5.close();
@@ -797,8 +794,8 @@ int ZMPVelocityReferencedQP::DynamicFilter(std::deque<ZMPPosition> & ZMPPosition
aof5.setf(ios::scientific, ios::floatfield);
aof5 << filterprecision(ZMPPositions[currentIndex+N-1].px ) << " " // 1
<< filterprecision(ZMPPositions[currentIndex+N-1].py ) << " " // 2
- << filterprecision(- m_allTorques[N-1](4,0) * _1_mg + FinalCOMTraj_deq[currentIndex+N-1].x[0] ) << " " // 1
- << filterprecision( m_allTorques[N-1](3,0) * _1_mg + FinalCOMTraj_deq[currentIndex+N-1].y[0] ) << " " // 2
+ << filterprecision(ZMPMB[0][0] ) << " " // 1
+ << filterprecision(ZMPMB[0][1]) << " " // 2
<< endl ;
aof5.close();
--
GitLab