Unverified Commit 1e26dbcb authored by olivier stasse's avatar olivier stasse Committed by GitHub
Browse files

Change waistattitudematrix to waistattitudematrixabsolute (#23)



* [pg] Changed signal waistattitudematrix in waistattitudematrixabsolute
Makes the reference frame more explicit for the signal.
Co-authored-by: default avatarLouise Scherrer <lscherrer@akasan.laas.fr>
parent 92b273e4
......@@ -303,7 +303,8 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
/*! \brief Internal method to get the absolute attitude of the waist into
an homogeneous matrix. */
MatrixHomogeneous &getWaistAttitudeMatrix(MatrixHomogeneous &res, int time);
MatrixHomogeneous &getWaistAttitudeMatrixAbsolute(MatrixHomogeneous &res,
int time);
/*! \brief Internal method to get the dataInPorcess flag */
unsigned &getDataInProcess(unsigned &res, int time);
......@@ -411,7 +412,7 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
dynamicgraph::Vector m_NextSamplingWaistAttAbs;
/*! \brief Waist Attitude Homogeneous Matrix */
MatrixHomogeneous m_WaistAttitudeMatrix;
MatrixHomogeneous m_WaistAttitudeMatrixAbsolute;
/*! \brief Joint values for walking. */
dynamicgraph::Vector m_JointErrorValuesForWalking;
......@@ -593,7 +594,7 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
/*! \brief Externalize the absolute waist attitude into a homogeneous matrix.
*/
SignalTimeDependent<MatrixHomogeneous, int> waistattitudematrixSOUT;
SignalTimeDependent<MatrixHomogeneous, int> waistattitudematrixabsoluteSOUT;
/*! \brief Externalize the waist position. */
SignalTimeDependent<dynamicgraph::Vector, int> waistpositionSOUT;
......
......@@ -229,11 +229,12 @@ PatternGenerator::PatternGenerator(const std::string &name)
")::output(vectorRPY)::waistattitudeabsolute")
,
waistattitudematrixSOUT(
boost::bind(&PatternGenerator::getWaistAttitudeMatrix, this, _1, _2),
waistattitudematrixabsoluteSOUT(
boost::bind(&PatternGenerator::getWaistAttitudeMatrixAbsolute, this,
_1, _2),
OneStepOfControlS,
"PatternGenerator(" + name +
")::output(homogeneousmatrix)::waistattitudematrix")
")::output(homogeneousmatrix)::waistattitudematrixabsolute")
,
waistpositionSOUT(
......@@ -368,7 +369,7 @@ PatternGenerator::PatternGenerator(const std::string &name)
m_WaistPositionAbsolute.resize(3);
m_WaistPositionAbsolute.fill(0);
m_WaistAttitudeMatrix.setIdentity();
m_WaistAttitudeMatrixAbsolute.setIdentity();
m_LeftFootPosition.setIdentity();
m_RightFootPosition.setIdentity();
......@@ -464,7 +465,7 @@ PatternGenerator::PatternGenerator(const std::string &name)
signalRegistration(SupportFootSOUT << jointWalkingErrorPositionSOUT
<< comattitudeSOUT << dcomattitudeSOUT
<< ddcomattitudeSOUT << waistattitudeSOUT
<< waistattitudematrixSOUT);
<< waistattitudematrixabsoluteSOUT);
signalRegistration(waistpositionSOUT << waistattitudeabsoluteSOUT
<< waistpositionabsoluteSOUT);
......@@ -1359,6 +1360,9 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
m_WaistPositionAbsolute(i) = CurrentConfiguration(i);
m_WaistAttitudeAbsolute(i) = CurrentConfiguration(i + 3);
}
getAbsoluteWaistPosAttHomogeneousMatrix(m_WaistAttitudeMatrixAbsolute);
m_COMRefPos(0) = lCOMRefState.x[0];
m_COMRefPos(1) = lCOMRefState.y[0];
m_COMRefPos(2) = lCOMRefState.z[0];
......@@ -1471,6 +1475,9 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
m_NextSamplingLeftFootAbsPos, m_LeftFootPosition,
m_dotLeftFootPosition, m_count);
getAbsoluteWaistPosAttHomogeneousMatrix(m_WaistAttitudeMatrixAbsolute);
// end added lines for test on waist
m_count++;
Eigen::Matrix<double, 4, 1> newRefPos, oldRefPos;
......@@ -1592,7 +1599,7 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
m_WaistPosition = WaistPoseAbsolute.translation();
m_WaistAttitudeMatrix = WaistPoseAbsolute;
m_WaistAttitudeMatrixAbsolute = WaistPoseAbsolute;
sotDEBUG(25) << "ComRef: " << m_COMRefPos << endl;
sotDEBUG(25) << "iPoseOrigin: " << iPoseOrigin << endl;
......@@ -1903,11 +1910,11 @@ VectorRollPitchYaw &PatternGenerator::getWaistAttitudeAbsolute(
return res;
}
MatrixHomogeneous &PatternGenerator::getWaistAttitudeMatrix(
MatrixHomogeneous &PatternGenerator::getWaistAttitudeMatrixAbsolute(
MatrixHomogeneous &res, int time) {
sotDEBUGIN(5);
OneStepOfControlS(time);
res = m_WaistAttitudeMatrix;
res = m_WaistAttitudeMatrixAbsolute;
sotDEBUGOUT(5);
return res;
}
......
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