diff --git a/walk_interfaces/include/walk_interfaces/pattern-generator.hh b/walk_interfaces/include/walk_interfaces/pattern-generator.hh index 7428615..90d1047 100644 --- a/walk_interfaces/include/walk_interfaces/pattern-generator.hh +++ b/walk_interfaces/include/walk_interfaces/pattern-generator.hh @@ -102,6 +102,22 @@ namespace walk const Vector3d& centerOfMass, const Posture& posture); + /// \brief Define the initial robot state. + /// + /// \param leftFoot left foot initial position. + /// \param rightFoot right foot initial position. + /// \param centerOfMass center of mass initial position. + /// \param posture initial posture. + /// \param leftHand left hand initial position. + /// \param rightHand right hand initial position. + void setInitialRobotPosition(const HomogeneousMatrix3d& leftFoot, + const HomogeneousMatrix3d& rightFoot, + const Vector3d& centerOfMass, + const Posture& posture, + const HomogeneousMatrix3d& leftHand, + const HomogeneousMatrix3d& rightHand); + + /// \brief Define the final robot state. /// /// \param leftFoot left foot final position. @@ -113,6 +129,22 @@ namespace walk const Vector3d& centerOfMass, const Posture& posture); + /// \brief Define the final robot state. + /// + /// \param leftFoot left foot final position. + /// \param rightFoot right foot final position. + /// \param centerOfMass center of mass final position. + /// \param posture final posture. + /// \param leftHand left hand final position. + /// \param rightHand right hand final position. + void setFinalRobotPosition(const HomogeneousMatrix3d& leftFoot, + const HomogeneousMatrix3d& rightFoot, + const Vector3d& centerOfMass, + const Posture& posture, + const HomogeneousMatrix3d& leftHand, + const HomogeneousMatrix3d& rightHand); + + /// \brief Set the footprint sequence for the pattern generator. /// /// \param startWithLeftFoot True if the movement first flying @@ -140,6 +172,11 @@ namespace walk /// \brief Posture trajectory getter. const TrajectoryNd& postureTrajectory() const; + /// \brief Left hand trajectory getter. + const Trajectory3d& leftHandTrajectory() const; + /// \brief Right hand trajectory getter. + const Trajectory3d& rightHandTrajectory() const; + /// \} /// \name Initial configuration. @@ -153,6 +190,10 @@ namespace walk const Vector3d& initialCenterOfMassPosition() const; /// \brief Initial posture getter. const Posture& initialPosture() const; + /// \brief Initial left hand position getter. + const HomogeneousMatrix3d& initialLeftHandPosition() const; + /// \brief Initial right hand position getter. + const HomogeneousMatrix3d& initialRightHandPosition() const; /// \} @@ -168,6 +209,10 @@ namespace walk const Vector3d& finalCenterOfMassPosition() const; /// \brief Final posture getter. const Posture& finalPosture() const; + /// \brief Final left hand position getter. + const HomogeneousMatrix3d& finalLeftHandPosition() const; + /// \brief Final right hand position getter. + const HomogeneousMatrix3d& finalRightHandPosition() const; /// \} @@ -182,6 +227,10 @@ namespace walk TrajectoryV3d& getCenterOfMassTrajectory(); /// \brief Posture trajectory setter. TrajectoryNd& getPostureTrajectory(); + /// \brief Left hand trajectory setter. + Trajectory3d& getLeftHandTrajectory(); + /// \brief Right hand trajectory setter. + Trajectory3d& getRightHandTrajectory(); /// \brief Initial left foot position setter. HomogeneousMatrix3d& getInitialLeftFootPosition(); @@ -191,6 +240,10 @@ namespace walk Vector3d& getInitialCenterOfMassPosition(); /// \brief Initial posture setter. Posture& getInitialPosture(); + /// \brief Initial left hand position setter. + HomogeneousMatrix3d& getInitialLeftHandPosition(); + /// \brief Initial right hand position setter. + HomogeneousMatrix3d& getInitialRightHandPosition(); /// \brief Final left foot position setter. HomogeneousMatrix3d& getFinalLeftFootPosition(); @@ -200,6 +253,11 @@ namespace walk Vector3d& getFinalCenterOfMassPosition(); /// \brief Final posture setter. Posture& getFinalPosture(); + /// \brief Final left hand position setter. + HomogeneousMatrix3d& getFinalLeftHandPosition(); + /// \brief Final right hand position setter. + HomogeneousMatrix3d& getFinalRightHandPosition(); + /// \brief Compute the reference trajectories. /// @@ -224,6 +282,11 @@ namespace walk TrajectoryV2d zmpTrajectory_; /// \brief Posture trajectory. TrajectoryNd postureTrajectory_; + /// \brief Left hand trajectory. + Trajectory3d leftHandTrajectory_; + /// \brief Right hand trajectory. + Trajectory3d rightHandTrajectory_; + /// \brief Initial left foot position. HomogeneousMatrix3d initialLeftFootPosition_; @@ -233,6 +296,10 @@ namespace walk Vector3d initialCenterOfMassPosition_; /// \brief Initial posture. Posture initialPosture_; + /// \brief Initial left hand position. + HomogeneousMatrix3d initialLeftHandPosition_; + /// \brief Initial right hand position. + HomogeneousMatrix3d initialRightHandPosition_; /// \brief Final left foot position. HomogeneousMatrix3d finalLeftFootPosition_; @@ -242,6 +309,10 @@ namespace walk Vector3d finalCenterOfMassPosition_; /// \brief Final posture. Posture finalPosture_; + /// \brief Final left hand position. + HomogeneousMatrix3d finalLeftHandPosition_; + /// \brief Final right hand position. + HomogeneousMatrix3d finalRightHandPosition_; public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; }; @@ -274,7 +345,7 @@ namespace walk typedef DiscretizedTrajectory TrajectoryNd; }; - + class DiscretizedPatternGenerator2d : public PatternGenerator {}; diff --git a/walk_interfaces/include/walk_interfaces/pattern-generator.hxx b/walk_interfaces/include/walk_interfaces/pattern-generator.hxx index a69671a..cb87d45 100644 --- a/walk_interfaces/include/walk_interfaces/pattern-generator.hxx +++ b/walk_interfaces/include/walk_interfaces/pattern-generator.hxx @@ -13,6 +13,8 @@ namespace walk centerOfMassTrajectory_(), zmpTrajectory_(), postureTrajectory_(), + leftHandTrajectory_(), + rightHandTrajectory_(), initialLeftFootPosition_(), initialRightFootPosition_(), @@ -28,11 +30,15 @@ namespace walk initialRightFootPosition_.setIdentity(); initialCenterOfMassPosition_.setZero(); initialPosture_.setZero(); + initialLeftHandPosition_.setIdentity(); + initialRightHandPosition_.setIdentity(); finalLeftFootPosition_.setIdentity(); finalRightFootPosition_.setIdentity(); finalCenterOfMassPosition_.setZero(); finalPosture_.setZero(); + finalLeftHandPosition_.setIdentity(); + finalRightHandPosition_.setIdentity(); } template @@ -44,7 +50,9 @@ namespace walk rightFootTrajectory_(pg.rightFootTrajectory_), centerOfMassTrajectory_(pg.centerOfMassTrajectory_), zmpTrajectory_(pg.zmpTrajectory_), - postureTrajectory_(pg.postureTrajectory_) + postureTrajectory_(pg.postureTrajectory_), + leftHandTrajectory_(pg.leftHandTrajectory_), + rightHandTrajectory_(pg.rightHandTrajectory_) {} template @@ -61,6 +69,8 @@ namespace walk this->centerOfMassTrajectory_ = pg.centerOfMassTrajectory_; this->zmpTrajectory_ = pg.zmpTrajectory_; this->postureTrajectory_ = pg.postureTrajectory_; + this->leftHandTrajectory_ = pg.leftHandTrajectory_; + this->rightHandTrajectory_ = pg.rightHandTrajectory_; return *this; } @@ -78,6 +88,25 @@ namespace walk initialPosture_ = posture; } + template + void + PatternGenerator::setInitialRobotPosition + (const HomogeneousMatrix3d& leftFoot, + const HomogeneousMatrix3d& rightFoot, + const Vector3d& centerOfMass, + const Posture& posture, + const HomogeneousMatrix3d& leftHand, + const HomogeneousMatrix3d& rightHand) + { + initialLeftFootPosition_ = leftFoot; + initialRightFootPosition_ = rightFoot; + initialCenterOfMassPosition_ = centerOfMass; + initialPosture_ = posture; + initialLeftHandPosition_ = leftHand; + initialRightHandPosition_ = rightHand; + } + + template void PatternGenerator::setFinalRobotPosition @@ -92,6 +121,25 @@ namespace walk finalPosture_ = posture; } + template + void + PatternGenerator::setFinalRobotPosition + (const HomogeneousMatrix3d& leftFoot, + const HomogeneousMatrix3d& rightFoot, + const Vector3d& centerOfMass, + const Posture& posture, + const HomogeneousMatrix3d& leftHand, + const HomogeneousMatrix3d& rightHand) + { + finalLeftFootPosition_ = leftFoot; + finalRightFootPosition_ = rightFoot; + finalCenterOfMassPosition_ = centerOfMass; + finalPosture_ = posture; + finalLeftHandPosition_ = leftHand; + finalRightHandPosition_ = rightHand; + } + + template const typename PatternGenerator::footprints_t& @@ -156,6 +204,20 @@ namespace walk return postureTrajectory_; } + template + const typename PatternGenerator::Trajectory3d& + PatternGenerator::leftHandTrajectory() const + { + return leftHandTrajectory_; + } + + template + const typename PatternGenerator::Trajectory3d& + PatternGenerator::rightHandTrajectory() const + { + return rightHandTrajectory_; + } + template typename PatternGenerator::Trajectory3d& PatternGenerator::getLeftFootTrajectory() @@ -191,6 +253,20 @@ namespace walk return postureTrajectory_; } + template + typename PatternGenerator::Trajectory3d& + PatternGenerator::getLeftHandTrajectory() + { + return leftHandTrajectory_; + } + + template + typename PatternGenerator::Trajectory3d& + PatternGenerator::getRightHandTrajectory() + { + return rightHandTrajectory_; + } + template HomogeneousMatrix3d& PatternGenerator::getInitialLeftFootPosition() @@ -219,6 +295,21 @@ namespace walk return initialPosture_; } + template + HomogeneousMatrix3d& + PatternGenerator::getInitialLeftHandPosition() + { + return initialLeftHandPosition_; + } + + template + HomogeneousMatrix3d& + PatternGenerator::getInitialRightHandPosition() + { + return initialRightHandPosition_; + } + + template HomogeneousMatrix3d& PatternGenerator::getFinalLeftFootPosition() @@ -247,6 +338,21 @@ namespace walk return finalPosture_; } + template + HomogeneousMatrix3d& + PatternGenerator::getFinalLeftHandPosition() + { + return finalLeftHandPosition_; + } + + template + HomogeneousMatrix3d& + PatternGenerator::getFinalRightHandPosition() + { + return finalRightHandPosition_; + } + + template @@ -263,6 +369,21 @@ namespace walk return initialRightFootPosition_; } + template + const HomogeneousMatrix3d& + PatternGenerator::initialLeftHandPosition() const + { + return initialLeftHandPosition_; + } + + template + const HomogeneousMatrix3d& + PatternGenerator::initialRightHandPosition() const + { + return initialRightHandPosition_; + } + + template const Vector3d& PatternGenerator::initialCenterOfMassPosition() const @@ -305,6 +426,21 @@ namespace walk return finalPosture_; } + template + const HomogeneousMatrix3d& + PatternGenerator::finalLeftHandPosition() const + { + return finalLeftHandPosition_; + } + + template + const HomogeneousMatrix3d& + PatternGenerator::finalRightHandPosition() const + { + return finalRightHandPosition_; + } + + template std::ostream& operator<<(std::ostream& os, const PatternGenerator& pg) @@ -335,7 +471,11 @@ namespace walk << "zmp trajectory:\n" << pg.zmpTrajectory() << "center of mass trajectory:\n" - << pg.centerOfMassTrajectory(); + << pg.centerOfMassTrajectory() + << "left hand trajectory:\n" + << pg.leftHandTrajectory() + << "right hand trajectory:\n" + << pg.rightHandTrajectory(); return os; } } // end of namespace walk.