Commit 73dcb110 authored by mnaveau's avatar mnaveau
Browse files

modify the test to get another offline trajectory of the HerdtPG,

minor modification off output debug in the rest of the files
parent 585760fb
......@@ -24,7 +24,6 @@ int MultiContactHirukawa::InverseKinematicsOnLimbs(std::vector< FootAbsolutePosi
std::vector< HandAbsolutePosition > & lh,
unsigned int currentIndex)
{
cout << "q = \n" << q_ << endl ;
//metapod::jac< Robot_Model>::run(*robot_, jacobian_);
Jac_LF::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_LF);
Jac_RF::run(*robot_, q_, metapod::Vector3dTpl<LocalFloatType>::Type(0,0,0), jac_RF);
......@@ -136,7 +135,7 @@ int MultiContactHirukawa::online(vector<COMState> & comPos_,
vector<HandAbsolutePosition> & lh_)
{
DetermineContact(rf_,lf_,rh_,lh_);
// InverseKinematicsOnLimbs(rf_,lf_,rh_,lh_,0);
InverseKinematicsOnLimbs(rf_,lf_,rh_,lh_,0);
return 0 ;
}
\ No newline at end of file
......@@ -597,7 +597,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
RightFootTraj_deq_,
deltaCOMTraj_deq_);
// #define DEBUG
#define DEBUG
#ifdef DEBUG
dynamicFilter_->Debug(FinalCOMTraj_deq,
FinalLeftFootTraj_deq,
......
......@@ -590,17 +590,17 @@ protected:
aPGI.ParseCmd(strm2);
}
}
void walkForward(PatternGeneratorInterface &aPGI)
void walkForward1m_s(PatternGeneratorInterface &aPGI)
{
{
istringstream strm2(":setVelReference 0.2 0.0 0.0");
istringstream strm2(":setVelReference 0.1 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}
void walkForward1m_s(PatternGeneratorInterface &aPGI)
void walkForward2m_s(PatternGeneratorInterface &aPGI)
{
{
istringstream strm2(":setVelReference 0.1 0.0 0.0");
istringstream strm2(":setVelReference 0.2 0.0 0.0");
aPGI.ParseCmd(strm2);
}
}
......@@ -611,10 +611,10 @@ protected:
aPGI.ParseCmd(strm2);
}
}
void walkSidewards3m_s(PatternGeneratorInterface &aPGI)
void walkSidewards1m_s(PatternGeneratorInterface &aPGI)
{
{
istringstream strm2(":setVelReference 0.0 -0.3 0.0");
istringstream strm2(":setVelReference 0.0 -0.1 0.0");
aPGI.ParseCmd(strm2);
}
}
......@@ -625,17 +625,10 @@ protected:
aPGI.ParseCmd(strm2);
}
}
void walkSidewards1m_s(PatternGeneratorInterface &aPGI)
{
{
istringstream strm2(":setVelReference 0.0 -0.1 0.0");
aPGI.ParseCmd(strm2);
}
}
void walkSidewards(PatternGeneratorInterface &aPGI)
void walkSidewards3m_s(PatternGeneratorInterface &aPGI)
{
{
istringstream strm2(":setVelReference 0.0 -0.2 0.0");
istringstream strm2(":setVelReference 0.0 -0.3 0.0");
aPGI.ParseCmd(strm2);
}
}
......@@ -700,12 +693,15 @@ protected:
};
#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
{ { 5*200,&TestHerdt2010::startWalkInDiagonal3m_s},
{10*200,&TestHerdt2010::startWalkInDiagonal1m_s},
{15*200,&TestHerdt2010::walkSidewards1m_s},
{20*200,&TestHerdt2010::walkSidewards2m_s},
{25*200,&TestHerdt2010::stop},
{30*200,&TestHerdt2010::stopOnLineWalking}};
{ { 5*200,&TestHerdt2010::walkForward3m_s},
{10*200,&TestHerdt2010::walkForward2m_s},
{15*200,&TestHerdt2010::walkForward1m_s},
{20*200,&TestHerdt2010::walkSidewards1m_s},
{25*200,&TestHerdt2010::walkSidewards2m_s},
{30*200,&TestHerdt2010::walkSidewards3m_s},
{35*200,&TestHerdt2010::startTurningRightOnSpot},
{40*200,&TestHerdt2010::stop},
{45*200,&TestHerdt2010::stopOnLineWalking}};
// Test when triggering event.
for(unsigned int i=0;i<localNbOfEvents;i++)
{
......
......@@ -127,8 +127,8 @@ public:
bool doTest(ostream &os)
{
metapod::bcalc<Robot_Model>::run(robot_,q_init_);
metapod::jcalc<Robot_Model>::run(robot_,q_init_,Robot_Model::confVector::Zero());
metapod::bcalc<Robot_Model>::run(robot_,q_init_);
ofstream aof;
string aFileName;
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment