From 7434f11f8680bd21c101f6f4b13a39423ccf40ff Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Tue, 2 Sep 2014 18:11:01 +0200
Subject: [PATCH] few changes and debug things
---
.../ZMPVelocityReferencedQP.cpp | 4 ---
.../generator-vel-ref.cpp | 34 ++++++++++++++++---
tests/TestMorisawa2007.cpp | 17 +++++++---
3 files changed, 42 insertions(+), 13 deletions(-)
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
index 321a3cd1..64bdabc1 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
@@ -919,10 +919,6 @@ void ZMPVelocityReferencedQP::ControlInterpolation(
Solution_.SupportStates_deq, Solution_,
Solution_.SupportOrientations_deq,
FinalLeftFootTraj_deq, FinalRightFootTraj_deq);
-// cout << "FinalLeftFootTraj_deq.ddtheta = " << FinalLeftFootTraj_deq[CurrentIndex_-1].ddtheta << " "
-// << FinalLeftFootTraj_deq[CurrentIndex_+NbSampleControl_-1].ddtheta << endl ;
-// cout << "FinalRightFootTraj_deq.ddtheta = " << FinalRightFootTraj_deq[CurrentIndex_-1].ddtheta << " "
-// << FinalRightFootTraj_deq[CurrentIndex_+NbSampleControl_-1].ddtheta << endl ;
return ;
}
diff --git a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
index 1386f199..38de469a 100644
--- a/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
+++ b/src/ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
@@ -297,9 +297,15 @@ GeneratorVelRef::build_inequalities_cop(linear_inequality_t & Inequalities,
++prwSS_it;//Point at the first previewed instant
for( unsigned i=0; i<N_; i++ )
{
- if( prwSS_it->StateChanged )
+ if( prwSS_it->StateChanged ){
RFI_->set_vertices( CoPHull, *prwSS_it, INEQ_COP );
-
+ if( prwSS_it->Foot == LEFT )
+ cout << "LEFT \n" ;
+ else
+ cout << "RIGHT \n" ;
+ for(unsigned int k = 0 ; k < CoPHull.X_vec.size() ; ++k)
+ cout << CoPHull.X_vec[k] << " " << CoPHull.Y_vec[k] << endl ;
+ }
RFI_->compute_linear_system( CoPHull, *prwSS_it );
for( unsigned j = 0; j < nbEdges; j++ )
@@ -339,6 +345,13 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
RFI_->set_vertices( FeetHull, *prwSS_it, INEQ_FEET );
prwSS_it++;
+ if( prwSS_it->Foot == LEFT )
+ cout << "LEFT \n" ;
+ else
+ cout << "RIGHT \n" ;
+ for(unsigned int k = 0 ; k < FeetHull.X_vec.size() ; ++k)
+ cout << FeetHull.X_vec[k] << " " << FeetHull.Y_vec[k] << endl ;
+
RFI_->compute_linear_system( FeetHull, *prwSS_it );
for( unsigned j = 0; j < nbEdges; j++ )
@@ -351,7 +364,7 @@ GeneratorVelRef::build_inequalities_feet( linear_inequality_t & Inequalities,
prwSS_it++;
}
-
+ cout << "############################################################\n";
}
@@ -404,15 +417,26 @@ GeneratorVelRef::build_constraints_cop(const linear_inequality_t & IneqCoP,
compute_term ( MM_, -1.0, IneqCoP.D.Y_mat, Robot_->DynamicsCoPJerk().U );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, N_ );
+ cout << "IneqCoP.D.X_mat = " << IneqCoP.D.X_mat << endl ;
+ cout << "Robot_->DynamicsCoPJerk().U = " << Robot_->DynamicsCoPJerk().U << endl ;
+ cout << "IneqCoP.D.Y_mat = " << IneqCoP.D.Y_mat << endl ;
+ cout << "Robot_->DynamicsCoPJerk().U = " << Robot_->DynamicsCoPJerk().U << endl ;
+
// +D*V
compute_term ( MM_, 1.0, IneqCoP.D.X_mat, IntermedData_->State().V );
// + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_ );
+
+ cout << "IntermedData_->State().V = " << IntermedData_->State().V << endl ;
compute_term ( MM_, 1.0, IneqCoP.D.Y_mat, IntermedData_->State().V );
// + Robot_->LeftFoot().Dynamics(COP).U + Robot_->RightFoot().Dynamics(COP).U );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
+
+
+
+
//constant part
// +dc
Pb.add_term_to( VECTOR_DS,IneqCoP.Dc_vec, NbConstraints );
@@ -454,12 +478,12 @@ GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
const IntermedQPMat::state_variant_t & State,
int NbStepsPreviewed, QPProblem & Pb)
{
-
unsigned int NbConstraints = Pb.NbConstraints();
// -D*V_f
compute_term ( MM_, -1.0, IneqFeet.D.X_mat, State.V_f );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_ );
+
compute_term ( MM_, -1.0, IneqFeet.D.Y_mat, State.V_f );
Pb.add_term_to( MATRIX_DU, MM_, NbConstraints, 2*N_+NbStepsPreviewed );
@@ -469,9 +493,9 @@ GeneratorVelRef::build_constraints_feet(const linear_inequality_t & IneqFeet,
// D*Vc_f*FPc
compute_term ( MV_, 1.0, IneqFeet.D.X_mat, State.Vc_fX );
Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
+
compute_term ( MV_, 1.0, IneqFeet.D.Y_mat, State.Vc_fY );
Pb.add_term_to( VECTOR_DS, MV_, NbConstraints );
-
}
diff --git a/tests/TestMorisawa2007.cpp b/tests/TestMorisawa2007.cpp
index 9238ef62..55b0a36a 100644
--- a/tests/TestMorisawa2007.cpp
+++ b/tests/TestMorisawa2007.cpp
@@ -315,7 +315,7 @@ protected:
aof.open(aFileName.c_str(),ofstream::app);
aof.precision(8);
aof.setf(ios::scientific, ios::floatfield);
- aof << filterprecision(m_OneStep.NbOfIt*samplingPeriod_ ) << " " // 1
+ aof << filterprecision(m_OneStep.NbOfIt*samplingPeriod_ ) << " " // 1
<< filterprecision(m_OneStep.finalCOMPosition.x[0] ) << " " // 2
<< filterprecision(m_OneStep.finalCOMPosition.y[0] ) << " " // 3
<< filterprecision(m_OneStep.finalCOMPosition.z[0] ) << " " // 4
@@ -703,9 +703,18 @@ protected:
}
{
- istringstream strm2(":stepstairseq 0.0 -0.105 0.0 0.0\
- 0.2 0.19 0.0 0.0\
- 0.2 -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\
");
aPGI.ParseCmd(strm2);
--
GitLab