From 69d63622823abd9f88ebb8af6ad58c70633206fe Mon Sep 17 00:00:00 2001
From: mnaveau <maximilien.naveau@laas.fr>
Date: Wed, 27 Apr 2016 18:33:15 +0200
Subject: [PATCH] debugging the dynamic filter
---
.../ComAndFootRealizationByGeometry.cpp | 5 +-
src/RobotDynamics/pinocchiorobot.cpp | 10 +--
.../DynamicFilter.cpp | 74 +++++++++++++------
.../ZMPVelocityReferencedSQP.cpp | 7 +-
.../nmpc_generator.cpp | 16 ++--
5 files changed, 70 insertions(+), 42 deletions(-)
diff --git a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
index e6c4e76c..22208e86 100644
--- a/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
+++ b/src/MotionGeneration/ComAndFootRealizationByGeometry.cpp
@@ -492,6 +492,7 @@ bool ComAndFootRealizationByGeometry::
{
m_TranslationToTheLeftHip(i) =
m_StaticToTheLeftHip(i) + m_DiffBetweenComAndWaist[i];
+ cout << m_StaticToTheLeftHip(i) << endl ;
m_TranslationToTheRightHip(i) =
m_StaticToTheRightHip(i) + m_DiffBetweenComAndWaist[i];
}
@@ -735,7 +736,8 @@ bool ComAndFootRealizationByGeometry::
MAL_S4x4_MATRIX_ACCESS_I_J(BodyPose,i,3) = MAL_S3_VECTOR_ACCESS(Body_P,i);
MAL_S4x4_MATRIX_ACCESS_I_J(FootPose,i,3) = MAL_S3_VECTOR_ACCESS(Foot_P,i);
}
-
+ MAL_S4x4_MATRIX_ACCESS_I_J(BodyPose,3,3) = 1.0 ;
+ MAL_S4x4_MATRIX_ACCESS_I_J(FootPose,3,3) = 1.0 ;
se3::JointIndex Waist = getPinocchioRobot()->waist();
@@ -842,7 +844,6 @@ bool ComAndFootRealizationByGeometry::
ODEBUG4("* Left Lego *","DebugDataIK.dat");
KinematicsForOneLeg(Body_R,Body_P,aLeftFoot,m_DtLeft,aCoMPosition,ToTheHip,1,ql,Stage);
-
// Kinematics for the right leg.
MAL_S3x3_C_eq_A_by_B(ToTheHip, Body_R, m_TranslationToTheRightHip);
Body_P(0) = aCoMPosition(0) + ToTheHip(0);
diff --git a/src/RobotDynamics/pinocchiorobot.cpp b/src/RobotDynamics/pinocchiorobot.cpp
index 2a4062c2..4633cb5b 100644
--- a/src/RobotDynamics/pinocchiorobot.cpp
+++ b/src/RobotDynamics/pinocchiorobot.cpp
@@ -224,10 +224,10 @@ void PinocchioRobot::computeInverseDynamics(MAL_VECTOR_TYPE(double) & q,
MAL_VECTOR_TYPE(double) & a)
{
// euler to quaternion :
- m_quat = Eigen::Quaternion<double>(
- Eigen::AngleAxisd((double)q(5), Eigen::Vector3d::UnitZ()) *
- Eigen::AngleAxisd((double)q(4), Eigen::Vector3d::UnitY()) *
- Eigen::AngleAxisd((double)q(3), Eigen::Vector3d::UnitX()) ) ;
+ m_quat = Eigen::Quaterniond(
+ Eigen::AngleAxisd(q(5), Eigen::Vector3d::UnitZ()) *
+ Eigen::AngleAxisd(q(4), Eigen::Vector3d::UnitY()) *
+ Eigen::AngleAxisd(q(3), Eigen::Vector3d::UnitX()) ) ;
// fill up m_q following the pinocchio standard : [pos quarternion DoFs]
for(unsigned i=0; i<3 ; ++i)
@@ -553,7 +553,7 @@ void PinocchioRobot::getShoulderWristKinematics(const matrix4d & jointRootPositi
double Xmax = ComputeXmax(Z);
X = X*Xmax;
- double A=0.25, B=0.25; //UpperArmLegth ForeArmLength
+ double A=0.25, B=0.25; //UpperArmLength ForeArmLength
double C=0.0,Gamma=0.0,Theta=0.0;
C = sqrt(X*X+Z*Z);
diff --git a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
index 631f756a..fe61238b 100644
--- a/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
+++ b/src/ZMPRefTrajectoryGeneration/DynamicFilter.cpp
@@ -216,9 +216,9 @@ int DynamicFilter::OnLinefilter(
ZMPMB_vec_[0][0]=inputZMPTraj_deq_[0].px;
ZMPMB_vec_[0][1]=inputZMPTraj_deq_[0].py;
ZMPMB_vec_[0][2]=0.0;
- ZMPMB_vec_[0][0]=inputZMPTraj_deq_[0].px;
- ZMPMB_vec_[0][1]=inputZMPTraj_deq_[0].py;
- ZMPMB_vec_[0][2]=0.0;
+ ZMPMB_vec_[1][0]=inputZMPTraj_deq_[1].px;
+ ZMPMB_vec_[1][1]=inputZMPTraj_deq_[1].py;
+ ZMPMB_vec_[1][2]=0.0;
unsigned int N1 = (ZMPMB_vec_.size()-1)*inc +1 ;
if(false)
@@ -381,8 +381,10 @@ void DynamicFilter::InverseDynamics(
void DynamicFilter::stage0INstage1()
{
- comAndFootRealization_->SetPreviousConfigurationStage1(comAndFootRealization_->GetPreviousConfigurationStage0());
- comAndFootRealization_->SetPreviousVelocityStage1(comAndFootRealization_->GetPreviousVelocityStage0());
+ comAndFootRealization_->SetPreviousConfigurationStage1(
+ comAndFootRealization_->GetPreviousConfigurationStage0());
+ comAndFootRealization_->SetPreviousVelocityStage1(
+ comAndFootRealization_->GetPreviousVelocityStage0());
return ;
}
@@ -398,9 +400,33 @@ void DynamicFilter::ComputeZMPMB(double samplingPeriod,
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
samplingPeriod, stage, iteration) ;
- InverseDynamics(ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_);
+ ofstream aof ;
+ static bool few=false;
+ if(!few)
+ {
+ aof.open("/tmp/legs.dat",ofstream::out);
+ aof.close();
+ few=true;
+ }
+ aof.open("/tmp/legs.dat",ofstream::app);
+ aof << inputLeftFoot.x << " " ;
+ aof << inputLeftFoot.y << " " ;
+ aof << inputLeftFoot.z << " " ;
+ aof << inputRightFoot.x << " " ;
+ aof << inputRightFoot.y << " " ;
+ aof << inputRightFoot.z << " " ;
+ aof << inputCoMState.x[0] << " " ;
+ aof << inputCoMState.y[0] << " " ;
+ aof << inputCoMState.z[0] << " " ;
+ aof << endl ;
+ aof.close();
+
+ if(iteration>1)
+ {
+ InverseDynamics(ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_);
- PR_->zeroMomentumPoint(ZMPMB);
+ PR_->zeroMomentumPoint(ZMPMB);
+ }
return ;
}
@@ -649,7 +675,7 @@ void DynamicFilter::Debug(const deque<COMState> & ctrlCoMState,
vector< MAL_S3_VECTOR_TYPE(double) > zmpmb_corr (Nctrl);
for(int i = 0 ; i < Nctrl ; ++i)
{
- InverseKinematics( CoM_tmp[i], ctrlLeftFoot[i], ctrlRightFoot[i],
+ InverseKinematics( ctrlCoMState[i], ctrlLeftFoot[i], ctrlRightFoot[i],
ZMPMBConfiguration_, ZMPMBVelocity_, ZMPMBAcceleration_,
controlPeriod_, 2, 20) ;
@@ -773,20 +799,22 @@ void DynamicFilter::Debug(const deque<COMState> & ctrlCoMState,
aof.setf(ios::scientific, ios::floatfield);
for (int i = 0 ; i < Nctrl ; ++i)
{
- aof << zmpmb_corr[i][0] << " " ;
- aof << zmpmb_corr[i][1] << " " ;
- aof << outputDeltaCOMTraj_deq_[i].x[0] << " " ;
- aof << outputDeltaCOMTraj_deq_[i].y[0] << " " ;
- aof << outputDeltaCOMTraj_deq_[i].x[1] << " " ;
- aof << outputDeltaCOMTraj_deq_[i].y[1] << " " ;
- aof << outputDeltaCOMTraj_deq_[i].x[2] << " " ;
- aof << outputDeltaCOMTraj_deq_[i].y[2] << " " ;
- aof << deltaZMPx_[i] << " " ;
- aof << deltaZMPy_[i] << " " ;
- aof << sxzmp_[i] << " " ;
- aof << syzmp_[i] << " " ;
- aof << deltaZMP_deq_[i].px << " " ;
- aof << deltaZMP_deq_[i].py << " " ;
+// aof << zmpmb_corr[i][0] << " " ;
+// aof << zmpmb_corr[i][1] << " " ;
+// aof << outputDeltaCOMTraj_deq_[i].x[0] << " " ;
+// aof << outputDeltaCOMTraj_deq_[i].y[0] << " " ;
+// aof << outputDeltaCOMTraj_deq_[i].x[1] << " " ;
+// aof << outputDeltaCOMTraj_deq_[i].y[1] << " " ;
+// aof << outputDeltaCOMTraj_deq_[i].x[2] << " " ;
+// aof << outputDeltaCOMTraj_deq_[i].y[2] << " " ;
+// aof << deltaZMPx_[i] << " " ;
+// aof << deltaZMPy_[i] << " " ;
+// aof << sxzmp_[i] << " " ;
+// aof << syzmp_[i] << " " ;
+// aof << deltaZMP_deq_[i].px << " " ;
+// aof << deltaZMP_deq_[i].py << " " ;
+ for(unsigned j=0 ; j<MAL_VECTOR_SIZE(conf[i]) ; ++j)
+ aof << conf[i][j] << " " ;
aof << endl ;
}
aof.close();
@@ -860,8 +888,6 @@ void DynamicFilter::Debug(const deque<COMState> & ctrlCoMState,
aof << deltaZMP_deq_[i].px << " " ; // 41
aof << deltaZMP_deq_[i].py << " " ; // 42
-
-
aof << endl ;
}
aof.close();
diff --git a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
index 90cdaa74..845b088b 100644
--- a/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
+++ b/src/ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
@@ -398,7 +398,8 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
time,
initLeftFoot_ ,
initRightFoot_,
- initCOM_,
+ itCOM_,
+ //initCOM_,
VelRef_);
// SOLVE PROBLEM:
@@ -433,7 +434,7 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
FinalRightFootTraj_deq[i] = RightFootTraj_deq_ctrl_[i] ;
}
- bool filterOn_ = false ;
+ bool filterOn_ = true ;
if(filterOn_)
{
@@ -441,7 +442,7 @@ void ZMPVelocityReferencedSQP::OnLine(double time,
LeftFootTraj_deq_,
RightFootTraj_deq_,
deltaCOMTraj_deq_);
-// #define DEBUG
+ #define DEBUG
#ifdef DEBUG
dynamicFilter_->Debug(COMTraj_deq_ctrl_,
LeftFootTraj_deq_ctrl_,
diff --git a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
index 324f7164..dc9c3ba5 100644
--- a/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
+++ b/src/ZMPRefTrajectoryGeneration/nmpc_generator.cpp
@@ -29,7 +29,7 @@
#include <cmath>
#include <Debug.hh>
-//#define DEBUG
+#define DEBUG
//#define DEBUG_COUT
#ifdef DEBUG
@@ -1311,16 +1311,16 @@ void NMPCgenerator::updateObstacleConstraint()
*(obstacles_[obs].r+obstacles_[obs].margin) ;
MAL_VECTOR_FILL(UBobs_[obs],1e+8);
}
-#ifdef DEBUG
+#ifdef DEBUG_COUT
cout << "prepare the obstacle : " << obstacles_[obs] << endl ;
#endif
}
#ifdef DEBUG
- DumpMatrix("Hobs_0" ,Hobs_ [0][0]);
- DumpVector("Aobs_0" ,Aobs_ [0][0]);
- DumpMatrix("Hobs_1" ,Hobs_ [0][1]);
- DumpVector("Aobs_1" ,Aobs_ [0][1]);
- DumpVector("LBobs_",LBobs_[0]);
+// DumpMatrix("Hobs_0" ,Hobs_ [0][0]);
+// DumpVector("Aobs_0" ,Aobs_ [0][0]);
+// DumpMatrix("Hobs_1" ,Hobs_ [0][1]);
+// DumpVector("Aobs_1" ,Aobs_ [0][1]);
+// DumpVector("LBobs_",LBobs_[0]);
#endif
return ;
@@ -1500,7 +1500,7 @@ void NMPCgenerator::updateCostFunction()
// p_xy_Y = 0.5 * a * Pvu^T * ( Pvs * c_k_y - dY^ref )
// + 0.5 * b * Pzu^T * ( Pzs * c_k_y - v_kp1 * f_k_y )
// p_xy_Fy = - 0.5 * b * V_kp1^T * ( Pzs * c_k_y - v_kp1 * f_k_y )
-#ifdef DEBUG
+#ifdef DEBUG_COUT
cout << vel_ref_.Global.X << " "
<< vel_ref_.Global.Y << endl;
#endif
--
GitLab