Commit fda13dee authored by Olivier Stasse's avatar Olivier Stasse
Browse files

[pg] Add contact phase to Pattern Generator entity

+ Fix the logic of contact phase
+ Clean up the code
parent d386ce8e
......@@ -62,6 +62,13 @@ namespace pg = PatternGeneratorJRL;
namespace dynamicgraph {
namespace sot {
/// Define the support phase of the robot
enum SupportPhase {
DOUBLE_SUPPORT_PHASE = 0,
LEFT_SUPPORT_PHASE = 1,
RIGHT_SUPPORT_PHASE = -1,
};
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
......@@ -168,8 +175,8 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
/*! \brief Time step */
double m_TimeStep;
/*! \brief Double support phase detected. */
bool m_DoubleSupportPhaseState;
/*! \brief Current support/contact phase defined by enum: leftFoot=1, rightFoot=-1, doubleSupport=0. */
SupportPhase m_ContactPhase;
int m_DSStartingTime;
/*! \brief iteration time. */
......@@ -527,6 +534,8 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
the feet. */
bool &getLeftFootContact(bool &res, int time);
bool &getRightFootContact(bool &res, int time);
/*! \brief Internal method to get the information of contact phase leftFoot=1, rightFoot=-1, doubleSupport=0. */
int &getContactPhase(int &res, int time);
public:
/*! \name External signals
......@@ -653,6 +662,9 @@ class PatternGenerator_EXPORT PatternGenerator : public Entity {
SignalTimeDependent<bool, int> rightFootContactSOUT;
/*! @} */
/*! \brief Int Vector of support phase: leftFoot=1, rightFoot=-1, doubleSupport=0 (see enum). */
SignalTimeDependent<int, int> contactPhaseSOUT;
/*! \name Reimplement the interface of the plugin.
@{ */
......
......@@ -28,6 +28,7 @@
//#define VP_DEBUG_MODE 45
#include <pinocchio/fwd.hpp>
#include <sstream>
#include <stdexcept>
#include <boost/property_tree/ptree.hpp>
#include <boost/property_tree/xml_parser.hpp>
......@@ -311,6 +312,11 @@ PatternGenerator::PatternGenerator(const std::string &name)
boost::bind(&PatternGenerator::getRightFootContact, this, _1, _2),
OneStepOfControlS,
"PatternGenerator(" + name + ")::output(bool)::rightfootcontact")
,
contactPhaseSOUT(
boost::bind(&PatternGenerator::getContactPhase, this, _1, _2),
OneStepOfControlS,
"PatternGenerator(" + name + ")::output(int)::contactphase")
{
m_MotionSinceInstanciationToThisSequence.setIdentity();
......@@ -318,7 +324,7 @@ PatternGenerator::PatternGenerator(const std::string &name)
m_LocalTime = 0;
m_count = 0;
m_TimeStep = 0.005;
m_DoubleSupportPhaseState = false;
m_ContactPhase = DOUBLE_SUPPORT_PHASE;
m_forceFeedBack = false;
m_feedBackControl = false;
......@@ -419,42 +425,6 @@ PatternGenerator::PatternGenerator(const std::string &name)
// For debug, register OSOC (not relevant for normal use).
signalRegistration(OneStepOfControlS);
#if 0
signalRegistration( jointPositionSIN <<
motorControlJointPositionSIN <<
ZMPPreviousControllerSIN <<
ZMPRefSOUT <<
CoMRefSOUT <<
dCoMRefSOUT);
signalRegistration( dataInProcessSOUT <<
LeftFootCurrentPosSIN <<
RightFootCurrentPosSIN <<
LeftFootRefSOUT <<
RightFootRefSOUT);
signalRegistration( SupportFootSOUT <<
jointWalkingErrorPositionSOUT <<
waistattitudeSOUT <<
waistpositionSOUT <<
waistattitudeabsoluteSOUT <<
waistpositionabsoluteSOUT);
signalRegistration( comattitudeSOUT <<
dcomattitudeSOUT );
signalRegistration( dotLeftFootRefSOUT <<
dotRightFootRefSOUT);
signalRegistration( InitZMPRefSOUT <<
InitCoMRefSOUT <<
InitWaistPosRefSOUT <<
InitWaistAttRefSOUT <<
InitLeftFootRefSOUT <<
InitRightFootRefSOUT <<
comSIN <<
velocitydesSIN);
#else
signalRegistration(dataInProcessSOUT);
signalRegistration(jointPositionSIN << motorControlJointPositionSIN
......@@ -483,9 +453,8 @@ PatternGenerator::PatternGenerator(const std::string &name)
<< InitLeftFootRefSOUT
<< InitRightFootRefSOUT);
signalRegistration(leftFootContactSOUT << rightFootContactSOUT);
signalRegistration(leftFootContactSOUT << rightFootContactSOUT << contactPhaseSOUT);
#endif
initCommands();
// init filter for force signals "to be removed"
......@@ -656,7 +625,6 @@ bool PatternGenerator::buildReducedModel(void) {
// Search for the robot util related to robot_name.
sot::RobotUtilShrPtr aRobotUtil = sot::getRobotUtil(model_name);
std::cout << "PG:buildReducedModel:" << aRobotUtil.get() << std::endl;
// If does not exist then it is created.
if (aRobotUtil.get() == sot::RefVoidRobotUtil().get())
{
......@@ -1094,6 +1062,14 @@ bool &PatternGenerator ::getRightFootContact(bool &res, int time) {
return res;
}
int &PatternGenerator ::getContactPhase(int &res, int time) {
sotDEBUGIN(25);
OneStepOfControlS(time);
res = m_ContactPhase;
sotDEBUGOUT(25);
return res;
}
int &PatternGenerator::InitOneStepOfControl(int &dummy, int /*time*/) {
sotDEBUGIN(15);
// TODO: modified first to avoid the loop.
......@@ -1598,11 +1574,26 @@ int &PatternGenerator::OneStepOfControl(int &dummy, int time) {
// Find the support foot feet.
m_leftFootContact = true;
m_rightFootContact = true;
// If stepType = -1 -> single support phase on the dedicated foot
// If stepType = 10 -> double support phase (both feet should have stepType=10)
// If stepType = 11 -> double support phase between 2 single support phases for Kajita Algorithm
if ((lLeftFootPosition.stepType == 10) || (lRightFootPosition.stepType == 10) ||
(lLeftFootPosition.stepType == 11) || (lRightFootPosition.stepType == 11)){
m_leftFootContact = true;
m_rightFootContact = true;
m_ContactPhase = DOUBLE_SUPPORT_PHASE;
}
if (lLeftFootPosition.stepType == -1) {
lSupportFoot = 1;
m_leftFootContact = true;
if (lRightFootPosition.stepType != -1) m_rightFootContact = false;
m_DoubleSupportPhaseState = 0;
// It is almost certain that when there is single support on a foot
// the other one cannot be in simple support (neither in double support)
// This if is certainly always true -> to be checked
if (lRightFootPosition.stepType != -1){
m_rightFootContact = false;
m_ContactPhase = LEFT_SUPPORT_PHASE;
}
} else if (lRightFootPosition.stepType == -1) {
lSupportFoot = 0;
m_rightFootContact = true;
......@@ -2037,7 +2028,6 @@ void PatternGenerator::setSoleParameters(const double &inSoleLength,
const double &inSoleWidth) {
m_soleLength = inSoleLength;
m_soleWidth = inSoleWidth;
}}
// namespace sot
}
} // namespace sot
} // namespace dynamicgraph
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