20 #ifndef __SOT_PATTERN_GENERATOR_H__ 21 #define __SOT_PATTERN_GENERATOR_H__ 30 #include <pinocchio/fwd.hpp> 34 #include <dynamic-graph/entity.h> 35 #include <dynamic-graph/pool.h> 36 #include <dynamic-graph/signal-ptr.h> 37 #include <dynamic-graph/signal-time-dependent.h> 38 #include <pinocchio/fwd.hpp> 40 #include <sot/core/flags.hh> 41 #include <sot/core/matrix-geometry.hh> 44 #include <jrl/walkgen/patterngeneratorinterface.hh> 45 #include <jrl/walkgen/pinocchiorobot.hh> 46 namespace pg = PatternGeneratorJRL;
53 #if defined(pg_EXPORTS) 54 #define PatternGenerator_EXPORT __declspec(dllexport) 56 #define PatternGenerator_EXPORT __declspec(dllimport) 59 #define PatternGenerator_EXPORT 80 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88 static const int WORLD_FRAME = 0;
92 static const int EGOCENTERED_FRAME = 1;
96 static const int LEFT_FOOT_CENTERED_FRAME = 2;
100 static const int WAIST_CENTERED_FRAME = 3;
107 DYNAMIC_GRAPH_ENTITY_DECL();
117 pg::PatternGeneratorInterface *
m_PGI;
194 bool buildModel(
void);
197 bool InitState(
void);
201 void setPreviewControlParametersFile(
const std::string &filename);
204 void setURDFFile(
const std::string &filename);
210 void setSRDFFile(
const std::string &filename);
213 void setXmlRankFile(
const std::string &filename);
217 void setParamPreviewFile(
const std::string &filename);
220 void setSoleParameters(
const double &inSoleLength,
const double &inSoleWidth);
223 void addJointMapping(
const std::string &link,
const std::string &repName);
253 dynamicgraph::Vector &getZMPRef(dynamicgraph::Vector &res,
int time);
256 dynamicgraph::Vector &getCoMRef(dynamicgraph::Vector &res,
int time);
259 dynamicgraph::Vector &getdCoMRef(dynamicgraph::Vector &res,
int time);
262 dynamicgraph::Vector &getddCoMRef(dynamicgraph::Vector &res,
int time);
265 dynamicgraph::Vector &getExternalForces(dynamicgraph::Vector &forces,
269 MatrixHomogeneous &getLeftFootRef(MatrixHomogeneous &res,
int time);
272 MatrixHomogeneous &getRightFootRef(MatrixHomogeneous &res,
int time);
275 MatrixHomogeneous &getdotLeftFootRef(MatrixHomogeneous &res,
int time);
278 MatrixHomogeneous &getdotRightFootRef(MatrixHomogeneous &res,
int time);
281 MatrixHomogeneous &getFlyingFootRef(MatrixHomogeneous &res,
int time);
284 dynamicgraph::Vector &getjointWalkingErrorPosition(dynamicgraph::Vector &res,
288 dynamicgraph::Vector &getdComAttitude(dynamicgraph::Vector &res,
int time);
292 dynamicgraph::Vector &getddComAttitude(dynamicgraph::Vector &res,
int time);
295 dynamicgraph::Vector &getComAttitude(dynamicgraph::Vector &res,
int time);
298 VectorRollPitchYaw &getWaistAttitude(VectorRollPitchYaw &res,
int time);
301 VectorRollPitchYaw &getWaistAttitudeAbsolute(VectorRollPitchYaw &res,
306 MatrixHomogeneous &getWaistAttitudeMatrixAbsolute(MatrixHomogeneous &res,
310 unsigned &getDataInProcess(
unsigned &res,
int time);
313 dynamicgraph::Vector &getWaistPosition(dynamicgraph::Vector &res,
int time);
316 dynamicgraph::Vector &getWaistPositionAbsolute(dynamicgraph::Vector &res,
322 unsigned int &getSupportFoot(
unsigned int &res,
int time);
325 int &InitOneStepOfControl(
int &dummy,
int time);
327 int &OneStepOfControl(
int &dummy,
int time);
340 m_PrevSamplingLeftFootAbsPos;
342 m_NextSamplingLeftFootAbsPos;
344 m_InitLeftFootAbsPos;
448 void ParseCmdFile(std::istringstream &cmdArg, std::ostream &os);
452 void FromAbsoluteFootPosToDotHomogeneous(
453 pg::FootAbsolutePosition aFootPosition, MatrixHomogeneous &aFootMH,
454 MatrixHomogeneous &adotFootMH);
458 void FromAbsoluteFootPosToHomogeneous(pg::FootAbsolutePosition aFootPosition,
459 MatrixHomogeneous &aFootMH);
463 void getAbsoluteWaistPosAttHomogeneousMatrix(MatrixHomogeneous &aWaistMH);
465 void SubsamplingFootPos(pg::FootAbsolutePosition &PrevFootPosition,
466 pg::FootAbsolutePosition &NextFootPosition,
467 MatrixHomogeneous &FootPositionOut,
468 MatrixHomogeneous &dotFootPositionOut,
469 unsigned int &count);
471 void SubsamplingVector(dynamicgraph::Vector &PrevPosition,
472 dynamicgraph::Vector &NextPosition,
473 dynamicgraph::Vector &PositionOut,
474 unsigned int &count);
476 void CopyFootPosition(pg::FootAbsolutePosition &FootPositionIn,
477 pg::FootAbsolutePosition &FootPositionOut);
481 dynamicgraph::Vector &getInitZMPRef(dynamicgraph::Vector &res,
int time);
485 dynamicgraph::Vector &getInitCoMRef(dynamicgraph::Vector &res,
int time);
489 dynamicgraph::Vector &getInitWaistPosRef(dynamicgraph::Vector &res,
int time);
493 VectorRollPitchYaw &getInitWaistAttRef(VectorRollPitchYaw &res,
int time);
496 MatrixHomogeneous &getInitLeftFootRef(MatrixHomogeneous &res,
int time);
499 MatrixHomogeneous &getInitRightFootRef(MatrixHomogeneous &res,
int time);
503 bool &getLeftFootContact(
bool &res,
int time);
504 bool &getRightFootContact(
bool &res,
int time);
529 SignalPtr<dynamicgraph::Vector, int>
comSIN;
535 SignalPtr<dynamicgraph::Vector, int>
zmpSIN;
540 SignalTimeDependent<dynamicgraph::Vector, int>
forceSOUT;
640 void initCommands(
void);
641 int stringToReferenceEnum(
const std::string &FrameReference);
642 void setReferenceFromString(
const std::string &str);
643 void addOnLineStep(
const double &x,
const double &y,
const double &th);
644 void addStep(
const double &x,
const double &y,
const double &th);
645 void pgCommandLine(
const std::string &cmdline);
646 void useFeedBackSignals(
const bool &feedBack);
647 void useDynamicFilter(
const bool &dynamicFilter);
655 #endif // #ifndef __SOT_PATTERN_GENERATOR_H__ Eigen::VectorXd m_ZMPPrevious
Definition: pg.h:637
SignalTimeDependent< unsigned int, int > dataInProcessSOUT
true iff PG is processing. Use it for synchronize.
Definition: pg.h:606
double m_AnkleSoilDistance
Distance between ankle and soil.
Definition: pg.h:166
SignalTimeDependent< dynamicgraph::Vector, int > comattitudeSOUT
Externalize the com attitude.
Definition: pg.h:581
SignalTimeDependent< MatrixHomogeneous, int > dotLeftFootRefSOUT
Externalize the left foot position reference.
Definition: pg.h:565
SignalTimeDependent< unsigned int, int > SupportFootSOUT
Externalize the support foot.
Definition: pg.h:575
dynamicgraph::Vector m_ZMPRefPos
Absolute position of the reference ZMP.
Definition: pg.h:360
SignalTimeDependent< dynamicgraph::Vector, int > dcomattitudeSOUT
Externalize the dcom attitude.
Definition: pg.h:584
MatrixHomogeneous m_RightFootPosition
Definition: pg.h:338
SignalPtr< dynamicgraph::Vector, int > jointPositionSIN
Real state position values.
Definition: pg.h:510
MatrixHomogeneous m_k_Waist_kp1
Rigit motion between two waist positions at the beginning of the walking and at the end of the walkin...
Definition: pg.h:335
PatternGeneratorJRL::FootAbsolutePosition m_NextSamplingRightFootAbsPos
Definition: pg.h:341
pg::PatternGeneratorInterface * GetPatternGeneratorInterface()
Give access directly to the pattern generator... You really have to know what your are doing...
Definition: pg.h:227
dynamicgraph::Vector m_dComAttitude
dCom Attitude: does not really exist apart when the robot is seen as an inverted pendulum ...
Definition: pg.h:369
#define PatternGenerator_EXPORT
Definition: pg.h:59
dynamicgraph::Vector m_PrevSamplingWaistAttAbs
Definition: pg.h:411
SignalTimeDependent< VectorRollPitchYaw, int > InitWaistAttRefSOUT
Externalize the initial Waist reference.
Definition: pg.h:618
dynamicgraph::Vector m_WaistAttitude
Waist Attitude.
Definition: pg.h:407
SignalTimeDependent< dynamicgraph::Vector, int > CoMRefSOUT
Externalize the CoM reference.
Definition: pg.h:522
pinocchio::Data * m_robotData
The pointor toward the robot data.
Definition: pg.h:115
MatrixHomogeneous m_dotRightFootPosition
Definition: pg.h:347
SignalPtr< dynamicgraph::Vector, int > comStateSIN
Take the current CoM state (pos, vel, acc).
Definition: pg.h:532
SignalTimeDependent< MatrixHomogeneous, int > LeftFootRefSOUT
Externalize the left foot position reference.
Definition: pg.h:559
pinocchio::Model m_robotModel
The model of the robot.
Definition: pg.h:111
dynamicgraph::Vector m_WaistPosition
Waist position.
Definition: pg.h:401
std::string m_srdfFile
Directory+Name where the SRDF file of the robot's model is located.
Definition: pg.h:131
SignalTimeDependent< bool, int > leftFootContactSOUT
Booleans for contact of the feet.
Definition: pg.h:627
dynamicgraph::Vector m_currentForces
Definition: pg.h:542
SignalTimeDependent< MatrixHomogeneous, int > InitRightFootRefSOUT
Externalize the right foot position reference.
Definition: pg.h:624
int m_SupportFoot
Integer for the support foot.
Definition: pg.h:160
bool m_trigger
trigger to start walking
Definition: pg.h:424
dynamicgraph::Vector m_dCOMRefPos
Absolute position of the reference dCoM.
Definition: pg.h:381
int Dummy
Definition: pg.h:229
SignalTimeDependent< VectorRollPitchYaw, int > waistattitudeSOUT
Externalize the waist attitude.
Definition: pg.h:590
PatternGeneratorJRL::FootAbsolutePosition m_InitRightFootAbsPos
Definition: pg.h:343
bool m_DoubleSupportPhaseState
Double support phase detected.
Definition: pg.h:172
unsigned int m_dataInProcess
true iff the pattern if dealing with data, false if pg is not working anymore/yet.
Definition: pg.h:429
dynamicgraph::Vector m_InitCOMRefPos
Initial Absolute position of the reference CoM.
Definition: pg.h:398
dynamicgraph::Vector m_PrevSamplingddCOMRefPos
Definition: pg.h:387
dynamicgraph::Vector m_WaistPositionAbsolute
Waist position Absolute.
Definition: pg.h:404
This class provides dynamically stable CoM, ZMP, feet trajectories. It wraps up the algorithms implem...
Definition: pg.h:77
bool m_feedBackControl
Booleans used to indicate if feedback signals shoul be used or not.
Definition: pg.h:433
bool m_init
Boolean variable to initialize the object by loading an object.
Definition: pg.h:152
dynamicgraph::Vector m_COMRefPos
Absolute position of the reference CoM.
Definition: pg.h:376
std::deque< dynamicgraph::Vector > m_bufferForce
Definition: pg.h:543
dynamicgraph::Vector m_NextSamplingdCOMRefPos
Definition: pg.h:383
pg::PinocchioRobot * m_PR
Pointer towards the robot model inside jrl-walkgen.
Definition: pg.h:113
SignalTimeDependent< Dummy, int > OneStepOfControlS
Internal signal to trigger one step of the algorithm.
Definition: pg.h:244
SignalTimeDependent< VectorRollPitchYaw, int > waistattitudeabsoluteSOUT
Externalize the absolute waist attitude.
Definition: pg.h:593
dynamicgraph::Vector m_PrevSamplingCOMRefPos
Definition: pg.h:377
dynamicgraph::Vector m_NextSamplingCOMRefPos
Definition: pg.h:378
SignalTimeDependent< dynamicgraph::Vector, int > ddcomattitudeSOUT
Externalize the ddcom attitude.
Definition: pg.h:587
dynamicgraph::Vector m_NextSamplingddCOMRefPos
Definition: pg.h:388
std::map< std::string, std::string > specialJoints_
Definition: pg.h:146
SignalTimeDependent< dynamicgraph::Vector, int > waistpositionSOUT
Externalize the waist position.
Definition: pg.h:600
dynamicgraph::Vector m_NextSamplingWaistAttAbs
Definition: pg.h:412
SignalTimeDependent< MatrixHomogeneous, int > dotRightFootRefSOUT
Externalize the right foot position reference.
Definition: pg.h:568
SignalPtr< MatrixHomogeneous, int > RightFootCurrentPosSIN
Take the current right foot homogeneous position.
Definition: pg.h:556
SignalPtr< dynamicgraph::Vector, int > motorControlJointPositionSIN
Motor control joint position values.
Definition: pg.h:513
std::string m_PreviewControlParametersFile
Some information related to the preview control.
Definition: pg.h:123
SignalTimeDependent< dynamicgraph::Vector, int > ddCoMRefSOUT
Definition: pg.h:527
int m_DSStartingTime
Definition: pg.h:173
SignalTimeDependent< bool, int > rightFootContactSOUT
Definition: pg.h:628
dynamicgraph::Vector m_InitZMPRefPos
Initial Absolute position of the reference ZMP.
Definition: pg.h:391
SignalTimeDependent< dynamicgraph::Vector, int > forceSOUT
Definition: pg.h:540
PatternGeneratorJRL::FootAbsolutePosition m_PrevSamplingRightFootAbsPos
Definition: pg.h:339
unsigned int m_count
count for subsampling.
Definition: pg.h:179
int m_ReferenceFrame
Keep the frame reference.
Definition: pg.h:163
SignalPtr< dynamicgraph::Vector, int > velocitydesSIN
Take the current desired velocity.
Definition: pg.h:547
MatrixHomogeneous m_FlyingFootPosition
Relative Position of the flying foot.
Definition: pg.h:357
SignalTimeDependent< dynamicgraph::Vector, int > jointWalkingErrorPositionSOUT
Externalize the joint values for walking.
Definition: pg.h:578
SignalTimeDependent< dynamicgraph::Vector, int > ZMPRefSOUT
Externalize the ZMP reference .
Definition: pg.h:519
dynamicgraph::Vector m_VelocityReference
Velocity reference for Herdt's PG.
Definition: pg.h:421
double m_soleWidth
Width of the sole.
Definition: pg.h:143
dynamicgraph::Vector m_ComAttitude
Com Attitude: does not really exist apart from when the robot is seen as an inverted pendulum...
Definition: pg.h:365
dynamicgraph::Vector m_WaistAttitudeAbsolute
Waist Attitude Absolute.
Definition: pg.h:410
SignalPtr< dynamicgraph::Vector, int > zmpSIN
Take the current zmp (x, y, z).
Definition: pg.h:535
bool m_forceFeedBack
Booleans used to indicate if force feedback signals shoul be used or not.
Definition: pg.h:437
SignalPtr< dynamicgraph::Vector, int > ZMPPreviousControllerSIN
Previous ZMP value (ZMP send by the preceding controller).
Definition: pg.h:516
SignalPtr< MatrixHomogeneous, int > LeftFootCurrentPosSIN
Take the current left foot homogeneous position.
Definition: pg.h:553
SignalTimeDependent< MatrixHomogeneous, int > InitLeftFootRefSOUT
Externalize the left foot position reference.
Definition: pg.h:621
MatrixHomogeneous m_MotionSinceInstanciationToThisSequence
Keep track of the motion between sequence of motions.
Definition: pg.h:354
SignalPtr< dynamicgraph::Vector, int > comSIN
Take the current CoM.
Definition: pg.h:529
SignalPtr< dynamicgraph::Vector, int > forceSIN
Take the current external force applied to the com (fx, fy, fz).
Definition: pg.h:539
dynamicgraph::Vector m_ddComAttitude
ddCom Attitude: does not really exist apart when the robot is seen as an inverted pendulum ...
Definition: pg.h:373
SignalTimeDependent< MatrixHomogeneous, int > FlyingFootRefSOUT
Externalize the foot which is not considered as support foot, the information are given in a relative...
Definition: pg.h:572
std::vector< unsigned > m_wrml2urdfIndex
Definition: pg.h:137
std::string m_urdfFile
Directory+Name where the URDF file of the robot's model is located.
Definition: pg.h:127
SignalTimeDependent< dynamicgraph::Vector, int > waistpositionabsoluteSOUT
Externalize the absolute waist position.
Definition: pg.h:603
bool m_InitPositionByRealState
Boolean variable to initialize the position: first through the real state of the robot, then through the motor command.
Definition: pg.h:157
unsigned int m_LocalTime
iteration time.
Definition: pg.h:176
double m_TimeStep
Time step.
Definition: pg.h:169
dynamicgraph::Vector m_JointErrorValuesForWalking
Joint values for walking.
Definition: pg.h:418
dynamicgraph::Vector m_PrevSamplingdCOMRefPos
Definition: pg.h:382
std::string m_xmlRankFile
Directory+Name where the Rank of the joints are notified.
Definition: pg.h:135
SignalTimeDependent< dynamicgraph::Vector, int > dCoMRefSOUT
Externalize the CoM reference.
Definition: pg.h:525
double m_soleLength
Lenght of the sole.
Definition: pg.h:140
dynamicgraph::Vector m_InitWaistRefPos
Initial Absolute position and attitude of the reference Waist.
Definition: pg.h:395
SignalPtr< bool, int > triggerSIN
Take the current trigger to start OneStepOfControl.
Definition: pg.h:550
SignalTimeDependent< MatrixHomogeneous, int > waistattitudematrixabsoluteSOUT
Externalize the absolute waist attitude into a homogeneous matrix.
Definition: pg.h:597
dynamicgraph::Vector m_ddCOMRefPos
Absolute position of the reference ddCoM.
Definition: pg.h:386
SignalTimeDependent< dynamicgraph::Vector, int > InitCoMRefSOUT
Externalize the initial CoM reference.
Definition: pg.h:612
SignalTimeDependent< dynamicgraph::Vector, int > InitZMPRefSOUT
Externalize the initial ZMP reference .
Definition: pg.h:609
MatrixHomogeneous m_InitRightFootPosition
Definition: pg.h:351
std::vector< double > m_filterWindow
Definition: pg.h:544
SignalTimeDependent< dynamicgraph::Vector, int > InitWaistPosRefSOUT
Externalize the initial Waist reference.
Definition: pg.h:615
MatrixHomogeneous m_WaistAttitudeMatrixAbsolute
Waist Attitude Homogeneous Matrix.
Definition: pg.h:415
dynamicgraph::Vector m_initForce
Definition: pg.h:541
SignalTimeDependent< Dummy, int > firstSINTERN
Internal signal for initialization and one shot signals.
Definition: pg.h:241
SignalTimeDependent< MatrixHomogeneous, int > RightFootRefSOUT
Externalize the right foot position reference.
Definition: pg.h:562
bool m_rightFootContact
Booleans used to indicate feet contacts.
Definition: pg.h:440
Definition: exception-pg.h:47
pg::PatternGeneratorInterface * m_PGI
Pointer towards the interface of the pattern generator.
Definition: pg.h:117