Choreonoid  1.1
BodyMotionPoseProvider.h
説明を見る。
1 
6 #ifndef CNOID_BODY_BODY_MOTION_POSE_PROVIDER_H_INCLUDED
7 #define CNOID_BODY_BODY_MOTION_POSE_PROVIDER_H_INCLUDED
8 
9 #include "Body.h"
10 #include "Link.h"
11 #include "PoseProvider.h"
12 #include "BodyMotion.h"
13 #include "exportdecl.h"
14 
15 namespace cnoid {
16 
18  {
19  public:
22 
23  void initialize(BodyPtr body, BodyMotionPtr motion);
24 
25  bool updateMotion();
26 
27  virtual Body* body() const;
28  virtual double beginningTime() const;
29  virtual double endingTime() const;
30  virtual bool seek(double time);
31  virtual bool seek(double time, int waistLinkIndex, const Vector3& waistTranslation);
32  virtual int baseLinkIndex() const;
33  virtual bool getBaseLinkPosition(Vector3& out_p, Matrix3& out_R) const;
34  virtual void getJointPositions(std::vector< boost::optional<double> >& out_q) const;
35  virtual boost::optional<Vector3> zmp() const;
36 
37  private:
38  BodyPtr body_;
39  BodyMotionPtr motion;
40  int minNumJoints;
41  std::vector<Link*> footLinks;
42  std::vector<JointPathPtr> ikPaths;
43  MultiAffine3SeqPtr footLinkPositions;
44  std::vector<double> qTranslated;
45  Vector3 p_waist;
46  Matrix3 R_waist;
47  Vector3 zmp_;
48 
49  bool seek(double time, int waistLinkIndex, const Vector3& waistTranslation, bool applyWaistTranslation);
50  };
51 }
52 
53 #endif