Skip to content
GitLab
Commit c724f701 authored by Olivier Stasse's avatar Olivier Stasse
Browse files

Add torques among the references trajectories.

parent c3d7e3b2
Hide whitespace changes
Inline Side-by-side
include/jrl/walkgen/pinocchiorobot.hh
View file @ c724f701
......@@ -217,6 +217,8 @@ namespace PatternGeneratorJRL
{return m_vrpy;}
inline Eigen::VectorXd & currentRPYAcceleration()
{return m_arpy;}
inline Eigen::VectorXd & currentTau()
{return m_tau;}
inline unsigned numberDof()
{return m_robotModel->nq;}
......
src/CMakeLists.txt
View file @ c724f701
......@@ -202,15 +202,22 @@ macro(ADDPREFIX newlist prefix list_name)
endmacro(ADDPREFIX)
IF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
ADDPREFIX(${PROJECT_NAME}_ABSOLUTE_HEADERS "${CMAKE_SOURCE_DIR}/" ${PROJECT_NAME}_HEADERS)
ADDPREFIX(${PROJECT_NAME}_ABSOLUTE_HEADERS "${CMAKE_SOURCE_DIR}/"
${PROJECT_NAME}_HEADERS)
ENDIF ( '${CMAKE_EXTRA_GENERATOR}' STREQUAL 'CodeBlocks' )
ADD_LIBRARY(${PROJECT_NAME} SHARED ${SOURCES} ${${PROJECT_NAME}_ABSOLUTE_HEADERS})
ADD_LIBRARY(${PROJECT_NAME}
SHARED ${SOURCES}
${${PROJECT_NAME}_ABSOLUTE_HEADERS})
TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${LAPACK_LIBRARIES})
# Define dependencies
SET_TARGET_PROPERTIES(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
SET_TARGET_PROPERTIES(${PROJECT_NAME}
PROPERTIES
COMPILE_FLAGS
"-msse -msse2 -msse3 -march=core2 -mfpmath=sse -fivopts -ftree-loop-im -fipa-pta ")
PKG_CONFIG_USE_DEPENDENCY(${PROJECT_NAME} pinocchio)
IF(USE_LSSOL)
......
src/PatternGeneratorInterfacePrivate.cpp
View file @ c724f701
......@@ -525,6 +525,7 @@ void PatternGeneratorInterfacePrivate::initOnlineHerdt() {
InitRightFootAbsPos, RelativeFootPositions,
lStartingCOMState, lStartingZMPPosition);
m_GlobalStrategyManager->Setup(m_ZMPPositions, m_COMBuffer,
m_LeftFootPositions, m_RightFootPositions);
......@@ -1755,6 +1756,9 @@ int PatternGeneratorInterfacePrivate::CreateZMPReferences(
m_ZMPPositions, m_COMBuffer, lRelativeFootPositions,
m_LeftFootPositions, m_RightFootPositions, m_Xmax, lStartingCOMState,
lStartingZMPPosition, InitLeftFootAbsPos, InitRightFootAbsPos);
std::cout << "m_ZMPPositions.size() :" << m_ZMPPositions.size() << std::endl;
std::cout << "m_COMBuffer.size() :" << m_COMBuffer.size() << std::endl;
// m_COMBuffer.clear();
// m_COMBuffer.resize(m_RightFootPositions.size());
} else if (m_AlgorithmforZMPCOM == ZMPCOM_MORISAWA_2007) {
......
src/PreviewControl/ZMPPreviewControlWithMultiBodyZMP.cpp
View file @ c724f701
......@@ -68,7 +68,7 @@ ZMPPreviewControlWithMultiBodyZMP::ZMPPreviewControlWithMultiBodyZMP(
RESETDEBUG4("DebugConfSO.dat");
RESETDEBUG4("DebugConfSV.dat");
RESETDEBUG4("DebugConfSA.dat");
RESETDEBUG4("DebugDataCheckZMP1.txt");
RESETDEBUG5("DebugDataCheckZMP1.txt");
RESETDEBUG4("2ndStage.dat");
RESETDEBUG4("ZMPPCWMZOGSOC.dat");
// Sampling period.DMB
......@@ -286,13 +286,14 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
LeftFootPosition = m_FIFOLeftFootPosition[0];
RightFootPosition = m_FIFORightFootPosition[0];
#if 0
// Preview control on delta ZMP.
if ((m_StageStrategy == ZMPCOM_TRAJECTORY_SECOND_STAGE_ONLY) ||
(m_StageStrategy == ZMPCOM_TRAJECTORY_FULL)) {
ODEBUG2(m_FIFODeltaZMPPositions[0].px << " "
<< m_FIFODeltaZMPPositions[0].py);
<< m_FIFODeltaZMPPositions[0].py);
ODEBUG2("Second Stage Size of FIFODeltaZMPPositions: "
ODEBUG("Second Stage Size of FIFODeltaZMPPositions: "
<< m_FIFODeltaZMPPositions.size() << " " << m_Deltax << " "
<< m_Deltay << " " << m_sxDeltazmp << " " << m_syDeltazmp << " "
<< Deltazmpx2 << " " << Deltazmpy2);
......@@ -308,6 +309,7 @@ int ZMPPreviewControlWithMultiBodyZMP::SecondStageOfControl(
aCOMState.y[i] += m_Deltay(i, 0);
}
}
#endif
ODEBUG2("Delta :" << m_Deltax(0, 0) << " " << m_Deltay(0, 0) << " "
<< aCOMState.x[0] << " " << aCOMState.y[0]);
......@@ -398,7 +400,7 @@ int ZMPPreviewControlWithMultiBodyZMP::EvaluateMultiBodyZMP(
// compute the ZMP related to the motion found by CoMAndZMPRealization.
Eigen::Vector3d ZMPmultibody;
m_PinocchioRobot->zeroMomentumPoint(ZMPmultibody);
ODEBUG4(ZMPmultibody[0] << " " << ZMPmultibody[1] << " "
ODEBUG5(ZMPmultibody[0] << " " << ZMPmultibody[1] << " "
<< m_FIFOZMPRefPositions[0].px << " "
<< m_FIFOZMPRefPositions[0].py,
"DebugDataCheckZMP1.txt");
......@@ -451,7 +453,7 @@ int ZMPPreviewControlWithMultiBodyZMP::Setup(
int ZMPPreviewControlWithMultiBodyZMP::SetupFirstPhase(
deque<ZMPPosition> &ZMPRefPositions,
deque<COMState> &, // COMStates,
deque<COMState> & COMStates,
deque<FootAbsolutePosition> &LeftFootPositions,
deque<FootAbsolutePosition> &RightFootPositions) {
ODEBUG6("Beginning of Setup 0 ", "DebugData.txt");
......
src/ZMPRefTrajectoryGeneration/ZMPDiscretization.cpp
View file @ c724f701
......@@ -431,14 +431,6 @@ std::size_t ZMPDiscretization::InitOnLine(
// The first foot when walking dynamically
// does not leave the soil, but needs to be treated for the first phase.
m_RelativeFootPositions.push_back(RelativeFootPositions[0]);
if (1) {
ofstream dbg_aof("DebugZMPRefPos.dat", ofstream::out);
for (unsigned int i = 0; i < ZMPPositions.size(); i++) {
dbg_aof << ZMPPositions[i].px << " " << ZMPPositions[i].py << endl;
}
dbg_aof.close();
}
FilterOutValues(ZMPPositions, FinalZMPPositions, true);
ODEBUG5("InitOnLine", "DebugDataRFPos.txt");
......
tests/CommonTools.hh
View file @ c724f701
......@@ -70,6 +70,7 @@ namespace PatternGeneratorJRL
FootAbsolutePosition m_RightFootPosition;
Eigen::VectorXd m_ZMPTarget;
unsigned long int m_NbOfIt;
Eigen::VectorXd m_Tau;
/// Vector of data to dump
std::vector<double> m_DebugVector;
......@@ -78,8 +79,10 @@ namespace PatternGeneratorJRL
/// TestName
std::string m_TestName;
/// Pinocchio Robot
PinocchioRobot * m_PR ;
/// Pinocchio Robot for the Pattern generator
PinocchioRobot * m_PR;
/// Pointer towards the debugging robot.
PinocchioRobot * m_DebugPR;
OneStep();
......
tests/DumpReferencesObjects.cpp
View file @ c724f701
......@@ -33,11 +33,11 @@ DumpReferencesObjects::DumpReferencesObjects() {
m_prevZMPRef.resize(3);
m_TimeOption = false;
} else if (m_InternalFormat == 1) {
m_prevCoMp.resize(9);
m_prevWaistOrien.resize(9);
m_prevZMPlocal.resize(9);
m_prevLeftAnklePos.resize(18);
m_prevRightAnklePos.resize(18);
m_prevCoMpF2.resize(9);
m_prevWaistOrienF2.resize(9);
m_prevZMPlocalF2.resize(9);
m_prevLeftAnklePosF2.resize(18);
m_prevRightAnklePosF2.resize(18);
}
}
......@@ -59,27 +59,33 @@ void DumpReferencesObjects::prepareFile(ofstream &aof, string &prefix,
aof.setf(ios::scientific, ios::floatfield);
}
void DumpReferencesObjects::fillFileWithSubsamplingAndClose(
ofstream &aof, std::vector<double> &prev, std::vector<double> &next,
double dt, double nb_subsampling, struct OneStep &anOneStep) {
for (double subsampling = 1.0; subsampling <= nb_subsampling;
void DumpReferencesObjects::fillFileWithSubsamplingAndClose
(FillingFileArgs_t &aSetOfFillingFileArgs,
std::vector<double> &next,
std::vector<double> &prev) {
for (double subsampling = 1.0;
subsampling <= aSetOfFillingFileArgs.nb_subsampling;
subsampling += 1.0) {
if (m_TimeOption) {
aof << filterprecision(((double)anOneStep.m_NbOfIt) * dt +
subsampling * dt / nb_subsampling)
<< " "; // 1
aSetOfFillingFileArgs.aof
<< filterprecision(((double)aSetOfFillingFileArgs.anOneStep.m_NbOfIt)
* aSetOfFillingFileArgs.dt +
subsampling * aSetOfFillingFileArgs.dt /
aSetOfFillingFileArgs.nb_subsampling)
<< " "; // 1
}
for (unsigned int i = 0; i < next.size(); i++) {
double intermediate =
prev[i] + (next[i] - prev[i]) * subsampling / nb_subsampling;
aof << intermediate;
prev[i] +(next[i] - prev[i]) *
subsampling / aSetOfFillingFileArgs.nb_subsampling;
aSetOfFillingFileArgs.aof << intermediate;
if (i < next.size() - 1)
aof << " ";
aSetOfFillingFileArgs.aof << " ";
}
aof << std::endl;
aSetOfFillingFileArgs.aof << std::endl;
}
aof.close();
prev = next;
aSetOfFillingFileArgs.aof.close();
prev =next;
}
void DumpReferencesObjects::fillInTests(
......@@ -103,6 +109,11 @@ void DumpReferencesObjects::fillInTestsFormat1(
double dt = 0.005;
double nb_sampling = 5.0;
FillingFileArgs_t aSetOfFillingFileArgs(aof, // File on which to write
dt,
nb_sampling,
anOneStep); // Compute speed ?
double localZMPx = anOneStep.m_ZMPTarget(0) * cos(aCurrentConfiguration(5)) -
anOneStep.m_ZMPTarget(1) * sin(aCurrentConfiguration(5)) +
aCurrentConfiguration(0);
......@@ -116,8 +127,7 @@ void DumpReferencesObjects::fillInTestsFormat1(
vec_db[0] = filterprecision(anOneStep.m_finalCOMPosition.x[0]);
vec_db[1] = filterprecision(anOneStep.m_finalCOMPosition.y[0]);
vec_db[2] = filterprecision(anOneStep.m_finalCOMPosition.z[0]);
fillFileWithSubsamplingAndClose(aof, m_prevCoMp, vec_db, dt, nb_sampling,
anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevCoMp);
/// CoM velocity
prefix = aTestName + "dCoM";
......@@ -125,8 +135,7 @@ void DumpReferencesObjects::fillInTestsFormat1(
vec_db[0] = filterprecision(anOneStep.m_finalCOMPosition.x[1]); // 6
vec_db[1] = filterprecision(anOneStep.m_finalCOMPosition.y[1]); // 7
vec_db[2] = filterprecision(anOneStep.m_finalCOMPosition.z[1]); // 8
fillFileWithSubsamplingAndClose(aof, m_prevdCoMp, vec_db, dt, nb_sampling,
anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevdCoMp);
/// CoM acceleration
prefix = aTestName + "ddCoM";
......@@ -134,8 +143,7 @@ void DumpReferencesObjects::fillInTestsFormat1(
vec_db[0] = filterprecision(anOneStep.m_finalCOMPosition.x[2]); // 10
vec_db[1] = filterprecision(anOneStep.m_finalCOMPosition.y[2]); // 11
vec_db[2] = filterprecision(anOneStep.m_finalCOMPosition.z[2]); // 12
fillFileWithSubsamplingAndClose(aof, m_prevddCoMp, vec_db, dt, nb_sampling,
anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevddCoMp);
/// Waist Orientation
prefix = aTestName + "WaistOrientation";
......@@ -143,8 +151,7 @@ void DumpReferencesObjects::fillInTestsFormat1(
vec_db[0] = filterprecision(anOneStep.m_finalCOMPosition.yaw[0]); // 5
vec_db[1] = filterprecision(anOneStep.m_finalCOMPosition.yaw[1]); // 9
vec_db[2] = filterprecision(anOneStep.m_finalCOMPosition.yaw[2]); // 13
fillFileWithSubsamplingAndClose(aof, m_prevWaistOrien, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevWaistOrien);
/// ZMP Ref
prefix = aTestName + "ZMPRef";
......@@ -152,8 +159,7 @@ void DumpReferencesObjects::fillInTestsFormat1(
vec_db[0] = filterprecision(anOneStep.m_ZMPTarget(0)); // 14
vec_db[1] = filterprecision(anOneStep.m_ZMPTarget(1)); // 15
vec_db[2] = filterprecision(anOneStep.m_ZMPTarget(2)); // 16
fillFileWithSubsamplingAndClose(aof, m_prevZMPRef, vec_db, dt, nb_sampling,
anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevZMPlocal);
prefix = aTestName + "LeftAnklePos";
prepareFile(aof, prefix, anOneStep);
......@@ -163,32 +169,28 @@ void DumpReferencesObjects::fillInTestsFormat1(
m_AnklePositionLeft[1]); // 18
vec_db[2] = filterprecision(anOneStep.m_LeftFootPosition.z +
m_AnklePositionLeft[2]); // 19
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnklePos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnklePos);
prefix = aTestName + "LeftAnkledPos";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_LeftFootPosition.dx); // 20
vec_db[1] = filterprecision(anOneStep.m_LeftFootPosition.dy); // 21
vec_db[2] = filterprecision(anOneStep.m_LeftFootPosition.dz); // 22
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnkledPos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnkledPos);
prefix = aTestName + "LeftAnkleddPos";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_LeftFootPosition.ddx); // 23
vec_db[1] = filterprecision(anOneStep.m_LeftFootPosition.ddy); // 24
vec_db[2] = filterprecision(anOneStep.m_LeftFootPosition.ddz); // 25
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnkleddPos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnkleddPos);
prefix = aTestName + "LeftAnkleOrientation";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_LeftFootPosition.theta); // 26
vec_db[1] = filterprecision(anOneStep.m_LeftFootPosition.omega); // 29
vec_db[2] = filterprecision(anOneStep.m_LeftFootPosition.omega2); // 30
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnkleOrientation, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnkleOrientation);
prefix = aTestName + "RightAnklePos";
prepareFile(aof, prefix, anOneStep);
......@@ -198,72 +200,64 @@ void DumpReferencesObjects::fillInTestsFormat1(
m_AnklePositionRight[1]);
vec_db[2] = filterprecision(anOneStep.m_RightFootPosition.z +
m_AnklePositionRight[2]);
fillFileWithSubsamplingAndClose(aof, m_prevRightAnklePos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnklePos);
prefix = aTestName + "RightAnkledPos";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_RightFootPosition.dx);
vec_db[1] = filterprecision(anOneStep.m_RightFootPosition.dy);
vec_db[2] = filterprecision(anOneStep.m_RightFootPosition.dz);
fillFileWithSubsamplingAndClose(aof, m_prevRightAnkledPos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnkledPos);
prefix = aTestName + "RightAnkleddPos";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_RightFootPosition.ddx);
vec_db[1] = filterprecision(anOneStep.m_RightFootPosition.ddy);
vec_db[2] = filterprecision(anOneStep.m_RightFootPosition.ddz);
fillFileWithSubsamplingAndClose(aof, m_prevRightAnkleddPos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnkleddPos);
prefix = aTestName + "RightAnkleOrientation";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_RightFootPosition.theta);
vec_db[1] = filterprecision(anOneStep.m_RightFootPosition.omega);
vec_db[2] = filterprecision(anOneStep.m_RightFootPosition.omega2);
fillFileWithSubsamplingAndClose(aof, m_prevRightAnkleOrientation, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnkleOrientation);
prefix = aTestName + "LeftAnkledOrientation";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_LeftFootPosition.dtheta);
vec_db[1] = 0.0;
vec_db[2] = 0.0; // 27
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnkledOrientation, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnkledOrientation);
prefix = aTestName + "LeftAnkleddOrientation";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_LeftFootPosition.ddtheta);
vec_db[1] = 0.0;
vec_db[2] = 0.0; // 27
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnkleddOrientation, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnkleddOrientation);
prefix = aTestName + "RightAnkledOrientation";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_RightFootPosition.dtheta);
vec_db[1] = 0.0;
vec_db[2] = 0.0;
fillFileWithSubsamplingAndClose(aof, m_prevRightAnkledOrientation, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnkledOrientation);
prefix = aTestName + "RightAnkleddOrientation";
prepareFile(aof, prefix, anOneStep);
vec_db[0] = filterprecision(anOneStep.m_RightFootPosition.ddtheta);
vec_db[1] = 0.0;
vec_db[2] = 0.0;
fillFileWithSubsamplingAndClose(aof, m_prevRightAnkleddOrientation, vec_db,
dt, nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnkleddOrientation);
prefix = aTestName + "ZMPWorldRef";
prepareFile(aof, prefix, anOneStep);
vec_db.resize(2);
vec_db[0] = filterprecision(localZMPx);
vec_db[1] = filterprecision(localZMPy);
fillFileWithSubsamplingAndClose(aof, m_prevZMPlocal, vec_db, dt, nb_sampling,
anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevZMPlocal);
}
void DumpReferencesObjects::fillInTestsFormat2(
......@@ -296,8 +290,10 @@ void DumpReferencesObjects::fillInTestsFormat2(
vec_db[6] = filterprecision(anOneStep.m_finalCOMPosition.x[2]);
vec_db[7] = filterprecision(anOneStep.m_finalCOMPosition.y[2]);
vec_db[8] = filterprecision(anOneStep.m_finalCOMPosition.z[2]);
fillFileWithSubsamplingAndClose(aof, m_prevCoMp, vec_db, dt, nb_sampling,
anOneStep);
TestSuite::FillingFileArgs_t
aSetOfFillingFileArgs(aof,dt,nb_sampling,anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevCoMpF2);
/// Waist Orientation
prefix = aTestName + "WaistOrientation";
......@@ -312,8 +308,7 @@ void DumpReferencesObjects::fillInTestsFormat2(
vec_db[6] = filterprecision(anOneStep.m_finalCOMPosition.yaw[2]);
vec_db[7] = 0.0;
vec_db[8] = 0.0;
fillFileWithSubsamplingAndClose(aof, m_prevWaistOrien, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevWaistOrienF2);
prefix = aTestName + "ZMPWorldRef";
prepareFile(aof, prefix, anOneStep);
......@@ -322,8 +317,8 @@ void DumpReferencesObjects::fillInTestsFormat2(
vec_db[2] = 0.0;
for (unsigned int j = 3; j < 9; j++)
vec_db[j] = 0.0;
fillFileWithSubsamplingAndClose(aof, m_prevZMPlocal, vec_db, dt, nb_sampling,
anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevZMPlocalF2);
prefix = aTestName + "LeftFoot";
prepareFile(aof, prefix, anOneStep);
......@@ -350,8 +345,8 @@ void DumpReferencesObjects::fillInTestsFormat2(
vec_db[15] = filterprecision(anOneStep.m_LeftFootPosition.ddtheta);
vec_db[16] = 0.0;
vec_db[17] = 0.0;
fillFileWithSubsamplingAndClose(aof, m_prevLeftAnklePos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevLeftAnklePosF2);
prefix = aTestName + "RightFoot";
prepareFile(aof, prefix, anOneStep);
......@@ -378,6 +373,19 @@ void DumpReferencesObjects::fillInTestsFormat2(
vec_db[15] = filterprecision(anOneStep.m_RightFootPosition.ddtheta);
vec_db[16] = 0.0;
vec_db[17] = 0.0;
fillFileWithSubsamplingAndClose(aof, m_prevRightAnklePos, vec_db, dt,
nb_sampling, anOneStep);
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevRightAnklePosF2);
prefix = aTestName + "Torques";
prepareFile(aof, prefix, anOneStep);
Eigen::VectorXd & aTauVec = anOneStep.m_DebugPR->currentTau();
vec_db.resize(aTauVec.size());
if (m_prevTorquesF2.size()!=aTauVec.size())
m_prevTorquesF2.resize(aTauVec.size());
for( Eigen::VectorXd::Index i=0;i<aTauVec.size();i++)
vec_db[i] = aTauVec[i];
fillFileWithSubsamplingAndClose(aSetOfFillingFileArgs,vec_db,m_prevTorquesF2);
}
tests/DumpReferencesObjects.hh
View file @ c724f701
......@@ -12,6 +12,23 @@ namespace PatternGeneratorJRL
{
namespace TestSuite
{
class FillingFileArgs_t
{
public:
std::ofstream &aof;
double dt;
double nb_subsampling;
struct OneStep &anOneStep;
FillingFileArgs_t (std::ofstream &laof,
double ldt,
double lnb_subsampling,
struct OneStep &lOneStep):
aof(laof),
dt(ldt), nb_subsampling(lnb_subsampling),
anOneStep(lOneStep)
{};
};
class DumpReferencesObjects
{
......@@ -47,20 +64,18 @@ namespace PatternGeneratorJRL
Eigen::VectorXd &aCurrentConfiguration);
virtual void fillFileWithSubsamplingAndClose
(std::ofstream &aof,
std::vector<double> &prev,
(FillingFileArgs_t &aSetOfFillingFileArgs,
std::vector<double> &next,
double dt,
double nb_subsampling,
struct OneStep &anOneStep
);
std::vector<double> &prev);
std::vector<double> m_prevCoMp;
std::vector<double> m_prevCoMp,m_prevCoMpF2;
std::vector<double> m_prevdCoMp;
std::vector<double> m_prevddCoMp;
std::vector<double> m_prevWaistOrien;
std::vector<double> m_prevWaistOrien,
m_prevWaistOrienF2;
std::vector<double> m_prevLeftAnklePos,
m_prevLeftAnklePosF2,
m_prevLeftAnkledPos,
m_prevLeftAnkleddPos,
m_prevLeftAnkleOrientation,
......@@ -68,13 +83,17 @@ namespace PatternGeneratorJRL
m_prevLeftAnkleddOrientation;
std::vector<double> m_prevRightAnklePos,
m_prevRightAnklePosF2,
m_prevRightAnkledPos,
m_prevRightAnkleddPos,
m_prevRightAnkleOrientation,
m_prevRightAnkledOrientation,
m_prevRightAnkleddOrientation;
std::vector<double> m_prevZMPlocal, m_prevZMPRef;
std::vector<double> m_prevZMPlocal,
m_prevZMPlocalF2,
m_prevZMPRef,
m_prevTorquesF2;
Eigen::Vector3d m_AnklePositionRight,
m_AnklePositionLeft;
......
tests/TestKajita2003.cpp
View file @ c724f701
......@@ -55,6 +55,8 @@ protected:
if (m_DebugFGPI)
m_OneStep.fillInDebugFile();
TestObject::fillInDebugFilesFull();
/// \brief Debug Purpose
/// --------------------
ofstream aof;
......@@ -143,23 +145,34 @@ protected:
aPGI.ParseCmd(strm2);
}
// {
// istringstream strm2(":stepseq 0.0 -0.09 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.15 0.18 0.0 0.0 \
// 0.15 -0.18 0.0 0.0 \
// 0.0 0.18 0.0 0.0");
// aPGI.ParseCmd(strm2);
// }
{ istringstream strm2(":singlesupporttime 0.9");
aPGI.ParseCmd(strm2);
strm2.str(string(":doublesupporttime 0.115"));
aPGI.ParseCmd(strm2); }
{
istringstream strm2(":stepseq 0.0 -0.09 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.15 0.18 0.0 0.0 \
0.15 -0.18 0.0 0.0 \
0.0 0.18 0.0 0.0");
0.1 0.18 0.0 0.0 \
0.0 -0.18 0.0 0.0");
aPGI.ParseCmd(strm2);
}
......
tests/TestObject.cpp
View file @ c724f701
......@@ -63,7 +63,7 @@ TestObject::TestObject(int argc, char *argv[], string &aTestName,
/*! default debug output */
m_DebugFGPI = true;
m_DebugFGPIFull = false;
m_DebugFGPIFull = true;
m_DebugZMP2 = false;
m_TestProfile = 0;
......@@ -132,6 +132,8 @@ bool TestObject::init() {
// Create Pattern Generator Interface
m_OneStep.m_PR = m_PR;
m_OneStep.m_DebugPR = m_DebugPR;
m_PGI = patternGeneratorInterfaceFactory(m_PR);
m_PGI->SetCurrentJointValues(m_HalfSitting);
......@@ -464,6 +466,10 @@ void TestObject::fillInDebugFilesFull() {
Eigen::Vector3d zmpmb;
m_DebugPR->zeroMomentumPoint(zmpmb);
Eigen::VectorXd tau;
tau = m_DebugPR->currentTau();
ofstream aof;