walk_interfaces: walk_interfaces: pattern-generator.hh Source File

00001 #ifndef WALK_INTERFACE_WALK_HH
00002 # define WALK_INTERFACE_WALK_HH
00003 # include <vector>
00004 
00005 # include <boost/static_assert.hpp>
00006 
00007 # include <walk_interfaces/types.hh>
00008 # include <walk_interfaces/discretized-trajectory.hh>
00009 # include <walk_interfaces/stamped-footprint.hh>
00010 
00011 namespace walk
00012 {
00013   template <typename T>
00014   class PatternGenerator;
00015 
00016   template <typename T>
00017   struct PatternGeneratorTraits
00018   {
00019     BOOST_STATIC_ASSERT(sizeof(T) == 0);
00020   };
00021 
00050   template <typename T>
00051   class PatternGenerator
00052   {
00053   public:
00054     typedef T concreteType_t;
00055 
00057     typedef typename PatternGeneratorTraits<concreteType_t>::Footprint
00058     footprint_t;
00059 
00061     typedef typename PatternGeneratorTraits<concreteType_t>::Trajectory3d
00062     Trajectory3d;
00064     typedef typename PatternGeneratorTraits<concreteType_t>::Trajectory2d
00065     Trajectory2d;
00067     typedef typename PatternGeneratorTraits<concreteType_t>::TrajectoryV2d
00068     TrajectoryV2d;
00070     typedef typename PatternGeneratorTraits<concreteType_t>::TrajectoryV3d
00071     TrajectoryV3d;
00073     typedef typename PatternGeneratorTraits<concreteType_t>::TrajectoryNd
00074     TrajectoryNd;
00075 
00077     typedef WALK_INTERFACES_EIGEN_STL_VECTOR(footprint_t) footprints_t;
00078 
00081 
00083     explicit PatternGenerator();
00085     explicit PatternGenerator(const PatternGenerator<T>&);
00087     virtual ~PatternGenerator();
00088 
00090 
00092     PatternGenerator<T>& operator=(const PatternGenerator<T>&);
00093 
00100     void setInitialRobotPosition(const HomogeneousMatrix3d& leftFoot,
00101                                  const HomogeneousMatrix3d& rightFoot,
00102                                  const Vector3d& centerOfMass,
00103                                  const Posture& posture);
00104 
00113     void setInitialRobotPosition(const HomogeneousMatrix3d& leftFoot,
00114                                  const HomogeneousMatrix3d& rightFoot,
00115                                  const Vector3d& centerOfMass,
00116                                  const Posture& posture,
00117                                  const HomogeneousMatrix3d& leftHand,
00118                                  const HomogeneousMatrix3d& rightHand);
00119 
00120 
00127     void setFinalRobotPosition(const HomogeneousMatrix3d& leftFoot,
00128                                const HomogeneousMatrix3d& rightFoot,
00129                                const Vector3d& centerOfMass,
00130                                const Posture& posture);
00131 
00140     void setFinalRobotPosition(const HomogeneousMatrix3d& leftFoot,
00141                                const HomogeneousMatrix3d& rightFoot,
00142                                const Vector3d& centerOfMass,
00143                                const Posture& posture,
00144                                const HomogeneousMatrix3d& leftHand,
00145                                const HomogeneousMatrix3d& rightHand);
00146 
00147 
00153     void setFootprints(const footprints_t&, bool startWithLeftFoot);
00154 
00156     const footprints_t& footprints() const;
00157 
00159     bool startWithLeftFoot() const;
00160 
00163 
00165     const Trajectory3d& leftFootTrajectory() const;
00167     const Trajectory3d& rightFootTrajectory() const;
00169     const TrajectoryV2d& zmpTrajectory() const;
00171     const TrajectoryV3d& centerOfMassTrajectory() const;
00173     const TrajectoryNd& postureTrajectory() const;
00174 
00176     const Trajectory3d& leftHandTrajectory() const;
00178     const Trajectory3d& rightHandTrajectory() const;
00179 
00181 
00184 
00186     const HomogeneousMatrix3d& initialLeftFootPosition() const;
00188     const HomogeneousMatrix3d& initialRightFootPosition() const;
00190     const Vector3d& initialCenterOfMassPosition() const;
00192     const Posture& initialPosture() const;
00194     const HomogeneousMatrix3d& initialLeftHandPosition() const;
00196     const HomogeneousMatrix3d& initialRightHandPosition() const;
00197 
00199 
00200 
00203 
00205     const HomogeneousMatrix3d& finalLeftFootPosition() const;
00207     const HomogeneousMatrix3d& finalRightFootPosition() const;
00209     const Vector3d& finalCenterOfMassPosition() const;
00211     const Posture& finalPosture() const;
00213     const HomogeneousMatrix3d& finalLeftHandPosition() const;
00215     const HomogeneousMatrix3d& finalRightHandPosition() const;
00216 
00218 
00219   protected:
00221     Trajectory3d& getLeftFootTrajectory();
00223     Trajectory3d& getRightFootTrajectory();
00225     TrajectoryV2d& getZmpTrajectory();
00227     TrajectoryV3d& getCenterOfMassTrajectory();
00229     TrajectoryNd& getPostureTrajectory();
00231     Trajectory3d& getLeftHandTrajectory();
00233     Trajectory3d& getRightHandTrajectory();
00234 
00236     HomogeneousMatrix3d& getInitialLeftFootPosition();
00238     HomogeneousMatrix3d& getInitialRightFootPosition();
00240     Vector3d& getInitialCenterOfMassPosition();
00242     Posture& getInitialPosture();
00244     HomogeneousMatrix3d& getInitialLeftHandPosition();
00246     HomogeneousMatrix3d& getInitialRightHandPosition();
00247 
00249     HomogeneousMatrix3d& getFinalLeftFootPosition();
00251     HomogeneousMatrix3d& getFinalRightFootPosition();
00253     Vector3d& getFinalCenterOfMassPosition();
00255     Posture& getFinalPosture();
00257     HomogeneousMatrix3d& getFinalLeftHandPosition();
00259     HomogeneousMatrix3d& getFinalRightHandPosition();
00260 
00261 
00268     virtual void computeTrajectories() = 0;
00269 
00270   private:
00272     footprints_t footprints_;
00274     bool startWithLeftFoot_;
00276     Trajectory3d leftFootTrajectory_;
00278     Trajectory3d rightFootTrajectory_;
00280     TrajectoryV3d centerOfMassTrajectory_;
00282     TrajectoryV2d zmpTrajectory_;
00284     TrajectoryNd postureTrajectory_;
00286     Trajectory3d leftHandTrajectory_;
00288     Trajectory3d rightHandTrajectory_;
00289 
00290 
00292     HomogeneousMatrix3d initialLeftFootPosition_;
00294     HomogeneousMatrix3d initialRightFootPosition_;
00296     Vector3d initialCenterOfMassPosition_;
00298     Posture initialPosture_;
00300     HomogeneousMatrix3d initialLeftHandPosition_;
00302     HomogeneousMatrix3d initialRightHandPosition_;
00303 
00305     HomogeneousMatrix3d finalLeftFootPosition_;
00307     HomogeneousMatrix3d finalRightFootPosition_;
00309     Vector3d finalCenterOfMassPosition_;
00311     Posture finalPosture_;
00313     HomogeneousMatrix3d finalLeftHandPosition_;
00315     HomogeneousMatrix3d finalRightHandPosition_;
00316   public:
00317     EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
00318   };
00319 
00324   template <typename T>
00325   std::ostream& operator<<(std::ostream& stream,
00326                            const PatternGenerator<T>& pg);
00327 
00328 
00329   class DiscretizedPatternGenerator2d;
00330   template <>
00331   struct PatternGeneratorTraits<DiscretizedPatternGenerator2d>
00332   {
00334     typedef StampedFootprint2d Footprint;
00335 
00337     typedef DiscretizedTrajectory<StampedPosition3d> Trajectory3d;
00339     typedef DiscretizedTrajectory<StampedPosition2d> Trajectory2d;
00341     typedef DiscretizedTrajectory<StampedVector2d> TrajectoryV2d;
00343     typedef DiscretizedTrajectory<StampedVector3d> TrajectoryV3d;
00345     typedef DiscretizedTrajectory<StampedVectorNd> TrajectoryNd;
00346   };
00347 
00348 
00349   class DiscretizedPatternGenerator2d
00350     : public PatternGenerator<DiscretizedPatternGenerator2d>
00351   {};
00352 
00353 } // end of namespace walk.
00354 
00355 # include <walk_interfaces/pattern-generator.hxx>
00356 #endif //! WALK_INTERFACE_WALK_HH