From b68e324d213d82dbb605286bf80a7d05506f0377 Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Mon, 27 Oct 2014 13:52:37 +0100
Subject: [PATCH] cleaning code
---
.../DynamicFilter.cpp | 224 +-----------------
.../DynamicFilter.hh | 9 -
tests/TestMorisawa2007.cpp | 82 +------
3 files changed, 16 insertions(+), 299 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 42dc93d7..b04444a0 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -101,7 +101,7 @@ void DynamicFilter::setRobotUpperPart(MAL_VECTOR_TYPE(double) & configuration,
return ;
}
-/// \brief Initialse all objects, to be called just after the constructor
+/// \brief Initialise all objects, to be called just after the constructor
void DynamicFilter::init(
double currentTime,
double controlPeriod,
@@ -210,6 +210,7 @@ int DynamicFilter::OffLinefilter(
deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
+ // TODO implement the filter in one single function for offline walking
return 0;
}
@@ -223,6 +224,7 @@ int DynamicFilter::OnLinefilter(
const deque<FootAbsolutePosition> & inputRightFootTraj_deq_,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
+ // TODO implement the filter in one single function for online walking
return 0 ;
}
@@ -412,51 +414,10 @@ void DynamicFilter::ComputeZMPMB(
return ;
}
-void DynamicFilter::ComputeZMPMB(
- MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration,
- vector<double> & ZMPMB)
-{
- // Copy the angular trajectory data from "Boost" to "Eigen"
- for(unsigned int j = 0 ; j < configuration.size() ; j++ )
- {
- q_(j,0) = configuration(j) ;
- dq_(j,0) = velocity(j) ;
- ddq_(j,0) = acceleration(j) ;
- }
-
- // Apply the RNEA on the robot model
- metapod::rnea< Robot_Model, true >::run(robot_, q_, dq_, ddq_);
-
- RootNode & node = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
- m_force = node.body.iX0.applyInv (node.joint.f);
-
- ZMPMB.resize(2);
- ZMPMB[0] = - m_force.n()[1] / m_force.f()[2] ;
- ZMPMB[1] = m_force.n()[0] / m_force.f()[2] ;
-
- return ;
-}
-
int DynamicFilter::OptimalControl(
deque<ZMPPosition> & inputdeltaZMP_deq,
deque<COMState> & outputDeltaCOMTraj_deq_)
{
- /// --------------------
- ofstream aof;
- string aFileName;
- ostringstream oss(std::ostringstream::ate);
- /// --------------------
- oss.str("ZMPDiscretisationErr.dat");
- aFileName = oss.str();
- aof.open(aFileName.c_str(),ofstream::out);
- aof.close();
- ///----
- aof.open(aFileName.c_str(),ofstream::app);
- aof.precision(8);
- aof.setf(ios::scientific, ios::floatfield);
-
if(!PC_->IsCoherent())
PC_->ComputeOptimalWeights(OptimalControllerSolver::MODE_WITH_INITIALPOS);
@@ -464,10 +425,6 @@ int DynamicFilter::OptimalControl(
// calcul of the preview control along the "deltaZMP_deq_"
for (unsigned i = 0 ; i < NCtrl_ ; i++ )
{
- aof << i*controlPeriod_ << " " // 1
- << sxzmp_ << " " // 2
- << syzmp_ << " " // 3
- << endl ;
PC_->OneIterationOfPreview(deltax_,deltay_,
sxzmp_,syzmp_,
inputdeltaZMP_deq,i,
@@ -479,8 +436,7 @@ int DynamicFilter::OptimalControl(
}
}
- aof.close();
-
+ // test to verify if the Kajita PC diverged
for (unsigned int i = 0 ; i < NCtrl_ ; i++)
{
for(int j=0;j<3;j++)
@@ -496,175 +452,3 @@ int DynamicFilter::OptimalControl(
}
return 0 ;
}
-
-
-void DynamicFilter::computeWaist(const FootAbsolutePosition & inputLeftFoot)
-{
- Eigen::Matrix< LocalFloatType, 6, 1 > waist_speed, waist_acc ;
- Eigen::Matrix< LocalFloatType, 3, 1 > waist_theta ;
- // compute the speed and acceleration of the waist in the world frame
- if (PreviousSupportFoot_)
- {
- Jacobian_LF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0), jacobian_lf_);
- waist_speed = jacobian_lf_ * prev_dq_ ;
- waist_acc = jacobian_lf_ * prev_ddq_ /* + d_jacobian_lf_ * prev_dq_*/ ;
- }else
- {
- Jacobian_RF::run(robot_, prev_q_, Vector3dTpl<LocalFloatType>::Type(0,0,0), jacobian_rf_);
- waist_speed = jacobian_rf_ * prev_dq_ ;
- waist_acc = jacobian_rf_ * prev_ddq_ /*+ d_jacobian_rf_ * prev_dq_*/ ;
- }
- for (unsigned int i = 0 ; i < 6 ; ++i)
- {
- dq_(i,0) = waist_speed(i,0);
- ddq_(i,0) = waist_acc(i,0);
- }
- // compute the position of the waist in the world frame
- RootNode & node_waist = boost::fusion::at_c<Robot_Model::BODY>(robot_.nodes);
- waist_theta(0,0) = prev_q_(3,0) ;
- waist_theta(1,0) = prev_dq_(4,0) ;
- waist_theta(2,0) = prev_ddq_(5,0) ;
- q_(0,0) = node_waist.body.iX0.inverse().r()(0,0) ;
- q_(1,0) = node_waist.body.iX0.inverse().r()(1,0) ;
- q_(2,0) = node_waist.body.iX0.inverse().r()(2,0) ;
- q_(3,0) = waist_theta(0,0) ;
- q_(4,0) = waist_theta(1,0) ;
- q_(5,0) = waist_theta(2,0) ;
-
- if (inputLeftFoot.stepType<0)
- {
- PreviousSupportFoot_ = true ; // left foot is supporting
- }
- else
- {
- PreviousSupportFoot_ = false ; // right foot is supporting
- }
- prev_q_ = q_ ;
- prev_dq_ = dq_ ;
- prev_ddq_ = ddq_ ;
-
- // Robot_Model::confVector q, dq, ddq;
- // for(unsigned int j = 0 ; j < 6 ; j++ )
- // {
- // q(j,0) = 0 ;
- // dq(j,0) = 0 ;
- // ddq(j,0) = 0 ;
- // }
- // for(unsigned int j = 6 ; j < ZMPMBConfiguration_.size() ; j++ )
- // {
- // q(j,0) = ZMPMBConfiguration_(j) ;
- // dq(j,0) = ZMPMBVelocity_(j) ;
- // ddq(j,0) = ZMPMBAcceleration_(j) ;
- // }
- //
- // metapod::rnea< Robot_Model, true >::run(robot_2, q, dq, ddq);
- // LankleNode & node_lankle = boost::fusion::at_c<Robot_Model::l_ankle>(robot_2.nodes);
- // RankleNode & node_rankle = boost::fusion::at_c<Robot_Model::r_ankle>(robot_2.nodes);
- //
- // CWx = node_waist.body.iX0.r()(0,0) - inputCoMState.x[0] ;
- // CWy = node_waist.body.iX0.r()(1,0) - inputCoMState.y[0] ;
- //
- // // Debug Purpose
- // // -------------
- // ofstream aof;
- // string aFileName;
- // ostringstream oss(std::ostringstream::ate);
- // static int it = 0;
- //
- // // --------------------
- // oss.str("DynamicFilterMetapodAccWaistSupportFoot.dat");
- // aFileName = oss.str();
- // if(it == 0)
- // {
- // aof.open(aFileName.c_str(),ofstream::out);
- // aof.close();
- // }
- // ///----
- // aof.open(aFileName.c_str(),ofstream::app);
- // aof.precision(8);
- // aof.setf(ios::scientific, ios::floatfield);
- // aof << filterprecision( it*samplingPeriod) << " " ; // 1
- //
- // if (inputLeftFoot.stepType < 0)
- // {
- // aof << filterprecision( node_lankle.body.ai.v()(0,0) ) << " " // 2
- // << filterprecision( node_lankle.body.ai.v()(1,0) ) << " " // 3
- // << filterprecision( node_lankle.body.ai.v()(2,0) ) << " " // 4
- // << filterprecision( node_lankle.body.ai.w()(0,0) ) << " " // 5
- // << filterprecision( node_lankle.body.ai.w()(1,0) ) << " " // 6
- // << filterprecision( node_lankle.body.ai.w()(2,0) ) << " "; // 7
- // }else
- // {
- // aof << filterprecision( node_rankle.body.ai.v()(0,0) ) << " " // 2
- // << filterprecision( node_rankle.body.ai.v()(1,0) ) << " " // 3
- // << filterprecision( node_rankle.body.ai.v()(2,0) ) << " " // 4
- // << filterprecision( node_rankle.body.ai.w()(0,0) ) << " " // 5
- // << filterprecision( node_rankle.body.ai.w()(1,0) ) << " " // 6
- // << filterprecision( node_rankle.body.ai.w()(2,0) ) << " " ;// 7
- // }
- //
- // aof << filterprecision( inputCoMState.x[2] ) << " " // 8
- // << filterprecision( inputCoMState.y[2] ) << " " // 9
- // << filterprecision( inputCoMState.z[2] ) << " " // 10
- // << filterprecision( inputCoMState.roll[2] ) << " " // 11
- // << filterprecision( inputCoMState.pitch[2] ) << " " // 12
- // << filterprecision( inputCoMState.yaw[2] ) << " " ; // 13
- //
- // if (inputLeftFoot.stepType < 0)
- // {
- // aof << filterprecision( node_lankle.body.vi.v()(0,0) ) << " " // 14
- // << filterprecision( node_lankle.body.vi.v()(1,0) ) << " " // 15
- // << filterprecision( node_lankle.body.vi.v()(2,0) ) << " " // 16
- // << filterprecision( node_lankle.body.vi.w()(0,0) ) << " " // 17
- // << filterprecision( node_lankle.body.vi.w()(1,0) ) << " " // 18
- // << filterprecision( node_lankle.body.vi.w()(2,0) ) << " " ;// 19
- // }else
- // {
- // aof << filterprecision( node_rankle.body.vi.v()(0,0) ) << " " // 14
- // << filterprecision( node_rankle.body.vi.v()(1,0) ) << " " // 15
- // << filterprecision( node_rankle.body.vi.v()(2,0) ) << " " // 16
- // << filterprecision( node_rankle.body.vi.w()(0,0) ) << " " // 17
- // << filterprecision( node_rankle.body.vi.w()(1,0) ) << " " // 18
- // << filterprecision( node_rankle.body.vi.w()(2,0) ) << " "; // 19
- // }
- //
- // aof << filterprecision( inputCoMState.x[1] ) << " " // 20
- // << filterprecision( inputCoMState.y[1] ) << " " // 21
- // << filterprecision( inputCoMState.z[1] ) << " " // 22
- // << filterprecision( inputCoMState.roll[1] ) << " " // 23
- // << filterprecision( inputCoMState.pitch[1] ) << " " // 24
- // << filterprecision( inputCoMState.yaw[1] ) << " " ; // 25
- //
- // aof << filterprecision( node_waist.joint.vj.v()(0,0) ) << " " // 26
- // << filterprecision( node_waist.joint.vj.v()(1,0) ) << " " // 27
- // << filterprecision( node_waist.joint.vj.v()(2,0) ) << " " // 28
- // << filterprecision( node_waist.joint.vj.w()(0,0) ) << " " // 29
- // << filterprecision( node_waist.joint.vj.w()(1,0) ) << " " // 30
- // << filterprecision( node_waist.joint.vj.w()(2,0) ) << " "; // 31
- //
- // aof << filterprecision( inputCoMState.x[0] ) << " " // 32
- // << filterprecision( inputCoMState.y[0] ) << " " // 33
- // << filterprecision( inputCoMState.z[0] ) << " " // 34
- // << filterprecision( inputCoMState.roll[0] ) << " " // 35
- // << filterprecision( inputCoMState.pitch[0] ) << " " // 36
- // << filterprecision( inputCoMState.yaw[0] ) << " " ; // 37
- //
- // aof << filterprecision( node_waist.body.iX0.r()(0,0) ) << " " // 38
- // << filterprecision( node_waist.body.iX0.r()(1,0) ) << " " // 39
- // << filterprecision( node_waist.body.iX0.r()(2,0) ) << " ";// 40
- //
- //
- // for(unsigned int j = 0 ; j < ZMPMBConfiguration_.size() ; j++ )//41
- // aof << filterprecision( ZMPMBConfiguration_(j) ) << " " ;
- // for(unsigned int j = 0 ; j < ZMPMBVelocity_.size() ; j++ ) //77
- // aof << filterprecision( ZMPMBVelocity_(j) ) << " " ;
- // for(unsigned int j = 0 ; j < ZMPMBAcceleration_.size() ; j++ )//113
- // aof << filterprecision( ZMPMBAcceleration_(j) ) << " " ;
- //
- //
- // aof << endl ;
- // aof.close();
- // ++it;
-
- return ;
-}
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
index 5febd42b..20fe6bfa 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.hh
@@ -123,15 +123,6 @@ namespace PatternGeneratorJRL
/// given by the function "filter"
void InverseDynamics(deque<ZMPPosition> inputZMPTraj_deq);
- /// \brief Compute the ZMPMB according to a configuration
- void ComputeZMPMB(
- MAL_VECTOR_TYPE(double) & configuration,
- MAL_VECTOR_TYPE(double) & velocity,
- MAL_VECTOR_TYPE(double) & acceleration,
- vector<double> & ZMPMB);
-
- void computeWaist(const FootAbsolutePosition & inputLeftFoot);
-
// -------------------------------------------------------------------
public: // The accessors
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 1581bc3f..99a82f05 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -706,78 +706,20 @@ protected:
{
istringstream strm2(":stepstairseq\
0.0 -0.105 0.0 0.0\
- 0.2 0.19 0.0174977327 0.0\
- 0.2 -0.19 0.0174977327 0.0\
- 0.2 0.19 0.0174977327 0.0\
- 0.2 -0.19 0.0174977327 0.0\
- 0.2 0.19 0.0174977327 0.0\
- 0.2 -0.19 0.0174977327 0.0\
- 0.2 0.19 0.0174977327 0.0\
- 0.0 -0.19 0.0 0.0\
- ");
-
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.174532925\
- // 0.0 0.19 0.0 0.174532925\
- // 0.0 -0.19 0.0 0.0\
- // 0.2 0.19 0.0 0.0\
- // 0.2 -0.19 0.0 0.0\
- // 0.2 0.19 0.0 0.0\
- // 0.2 -0.19 0.0 0.0\
- // 0.2 0.19 0.0 0.0\
- // 0.2 -0.19 0.0 0.0\
- // 0.2 0.19 0.0 0.0\
- // 0.2 -0.19 0.0 0.0\
- // 0.0 0.19 0.0 0.0\
-
-// istringstream strm2(":stepstairseq\
-// 0.0 -0.105 0.0 0.0\
-// 0.1 0.19 0.0 0.0\
-// 0.1 -0.19 0.0 0.0\
-// 0.1 0.19 0.0 0.0\
-// 0.1 -0.19 0.0 0.0\
-// 0.1 0.19 0.0 0.0\
-// 0.1 -0.19 0.0 0.0\
-// 0.1 0.19 0.0 0.0\
-// 0.1 -0.19 0.0 0.0\
-// 0.1 0.19 0.0 0.0\
-// 0.1 -0.19 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// ");
-
-// istringstream strm2(":stepstairseq\
-// 0.0 -0.105 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// 0.0 -0.19 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// 0.0 -0.19 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// 0.0 -0.19 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// 0.0 -0.19 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// 0.0 -0.19 0.0 0.0\
-// 0.0 0.19 0.0 0.0\
-// ");
-
+ 0.1 0.19 0.0 0.0\
+ 0.1 -0.19 0.0 0.0\
+ 0.1 0.19 0.0 0.0\
+ 0.1 -0.19 0.0 0.0\
+ 0.1 0.19 0.0 0.0\
+ 0.1 -0.19 0.0 0.0\
+ 0.1 0.19 0.0 0.0\
+ 0.1 -0.19 0.0 0.0\
+ 0.1 0.19 0.0 0.0\
+ 0.1 -0.19 0.0 0.0\
+ 0.0 0.19 0.0 0.0"
+ );
aPGI.ParseCmd(strm2);
}
-
}
void AnalyticalClimbingStairs(PatternGeneratorInterface &aPGI)
--
GitLab