Choreonoid  1.5
WaistBalancer.h
Go to the documentation of this file.
1 
5 #ifndef CNOID_BALANCER_PLUGIN_WAIST_BALANCER_H_INCLUDED
6 #define CNOID_BALANCER_PLUGIN_WAIST_BALANCER_H_INCLUDED
7 
8 #include <cnoid/BodyMotion>
9 #include <cnoid/Body>
10 #include <cnoid/Link>
11 #include <cnoid/LinkTraverse>
12 #include <cnoid/CompositeIK>
13 #include <boost/optional.hpp>
14 #include <boost/function.hpp>
15 #include <vector>
16 
17 namespace cnoid {
18 
19  class PoseProvider;
20 
22  {
23  public:
24  WaistBalancer();
25 
26  void setMessageOutputStream(std::ostream& os) {
27  os_ = &os;
28  }
29 
30  void setBody(const BodyPtr& body);
31  const BodyPtr& body() const;
32 
33  void setWaistLink(Link* waistLink);
34  void setNumIterations(int n);
35  int numIterations() const;
36  void setTimeRange(double lower, double upper);
37  void setFullTimeRange();
38  void setTimeMargins(double timeToStartBalancer, double preInitialDuration, double postFinalDuration);
39  void setGravity(double g);
40  void setDynamicsTimeRatio(double r);
41 
47  };
48  static const char* boundaryConditionTypeNameOf(int type);
49  static int boundaryConditionTypeOf(const std::string& name);
50  void setBoundaryConditionType(int type);
51 
58  };
59  static const char* boundarySmootherTypeNameOf(int type);
60  static int boundarySmootherTypeOf(const std::string& name);
61  void setBoundarySmoother(int type, double smoothingTime);
62 
63  void enableBoundaryCmAdjustment(bool on, double transitionTime = 1.0);
64 
66  void setInitialWaistTrajectoryMode(int mode);
67 
68  void enableWaistHeightRelaxation(bool on);
69 
70  bool apply(PoseProvider* provider, BodyMotion& motion, bool putAllLinkPositions = false);
71 
72  private:
73 
74  std::vector<double> q0;
75  Vector3 p0;
76  Matrix3 R0;
77 
78  int initialWaistTrajectoryMode;
79  double timeRangeLower;
80  double timeRangeUpper;
81  double targetBeginningTime;
82  double targetEndingTime;
83  double timeToStartBalancer;
84  double preInitialDuration;
85  double postFinalDuration;
86  double dynamicsTimeRatio;
87 
88  BodyPtr body_;
89  Link* baseLink;
90  LinkTraverse fkTraverse;
91  PoseProvider* provider;
92  bool isCalculatingInitialWaistTrajectory;
93 
94  std::vector< boost::optional<double> > jointPositions;
95 
96  int numIterations_;
97  int beginningFrame;
98  int endingFrame;
99  int numFilteredFrames;
100  int frameToStartBalancer;
101  double frameRate;
102  double timeStep; // original delta time
103  double dt; // delta time (as the dynamics)
104  double dt2; // delta time square (as the dyanmics)
105  double g;
106  double mg;
107  double m;
108  double inertial_g;
109  Vector3 cm; // center of mass
110  Vector3 P0; // prev momentum
111  Vector3 L0; // prev angular momentum
112  Vector3 dP;
113  Vector3 dL;
114  Vector3 zmp; // calculated ZMP
115  Vector3 desiredZmp;
116  Vector3 zmpDiff;
117 
118  struct Coeff {
119  double a;
120  double b;
121  Vector3 d;
122  };
123  std::vector<Coeff> coeffSeq;
124 
125  std::vector<Vector3> totalCmTranslations;
126 
127  Link* waistLink;
128  int waistLinkIndex;
129 
130  int boundaryConditionType;
131  int boundarySmootherType_;
132  bool doBoundarySmoother;
133  double boundarySmoothingTime;
134  boost::function<void(int begin, int direction)> boundarySmootherFunction;
135  int numBoundarySmoothingFrames;
136  bool doProcessFinalBoundary;
137 
138  bool isBoundaryCmAdjustmentEnabled;
139  double boundaryCmAdjustmentTransitionTime;
140  double boundaryCmAdjustmentTransitionHalfLength;
141  bool doBoundaryCmAdjustment;
142  std::vector<Vector3> boundaryCmAdjustmentTranslations;
143 
144  // for the waist height relaxation
145  bool isWaistHeightRelaxationEnabled;
146  bool doStoreOriginalWaistFeetPositionsForWaistHeightRelaxation;
147  bool doWaistHeightRelaxation;
148  struct WaistFeetPos {
149  Vector3 p_Waist;
150  Matrix3 R_Waist;
151  Vector3 p_Foot[2];
152  Matrix3 R_Foot[2];
153  };
154  std::vector<WaistFeetPos> waistFeetPosSeq;
155  std::vector<double> waistDeltaZseq;
156  CompositeIK waistFeetIK;
157  Link* rightKneePitchJoint;
158  Link* leftKneePitchJoint;
159 
160  std::ostream* os_;
161 
162  std::ostream& os() { return *os_; }
163 
164  bool apply2(BodyMotion& motion, bool putAllLinkPositions);
165  void calcBoundaryCmAdjustmentTrajectory();
166  bool calcBoundaryCmAdjustmentTrajectorySub(int begin, int direction);
167  bool calcWaistTranslationWithCmAboveZmp(
168  int frame, const Vector3& zmp, Vector3& out_translation);
169  void initBodyKinematics(int frame, const Vector3& cmTranslation);
170  void updateCmAndZmp(int frame);
171  bool updateBodyKinematics1(int frame);
172  void updateBodyKinematics2();
173  bool calcCmTranslations();
174  void initWaistHeightRelaxation();
175  void relaxWaistHeightTrajectory();
176  void applyCubicBoundarySmoother(int begin, int direction);
177  void applyQuinticBoundarySmoother(int begin, int direction);
178  bool applyCmTranslations(BodyMotion& motion, bool putAllLinkPositions);
179 
180  inline double timeOfFrame(int frame) {
181  return std::max(targetBeginningTime, std::min(targetEndingTime, (frame / frameRate)));
182  }
183  };
184 }
185 
186 #endif
Definition: WaistBalancer.h:44
Definition: WaistBalancer.h:54
WaistBalancer()
Definition: WaistBalancer.cpp:38
Definition: WaistBalancer.h:53
Definition: WaistBalancer.h:65
void setBody(const BodyPtr &body)
Definition: WaistBalancer.cpp:58
void setDynamicsTimeRatio(double r)
Definition: WaistBalancer.cpp:117
void setTimeRange(double lower, double upper)
Definition: WaistBalancer.cpp:89
BoundaryConditionType
Definition: WaistBalancer.h:42
Definition: WaistBalancer.h:55
void setMessageOutputStream(std::ostream &os)
Definition: WaistBalancer.h:26
Definition: PoseProvider.h:16
void setGravity(double g)
Definition: WaistBalancer.cpp:111
Definition: CompositeIK.h:18
const BodyPtr & body() const
Definition: WaistBalancer.cpp:65
static const char * boundaryConditionTypeNameOf(int type)
Definition: WaistBalancer.cpp:123
InitialWaistTrajectoryMode
Definition: WaistBalancer.h:65
void setBoundaryConditionType(int type)
Definition: WaistBalancer.cpp:141
void enableWaistHeightRelaxation(bool on)
Definition: WaistBalancer.cpp:198
Definition: BodyMotion.h:20
Definition: WaistBalancer.h:65
bool apply(PoseProvider *provider, BodyMotion &motion, bool putAllLinkPositions=false)
Definition: WaistBalancer.cpp:204
static const char * boundarySmootherTypeNameOf(int type)
Definition: WaistBalancer.cpp:147
Defines the minimum processing for performing pasing file for STL.
Definition: AbstractSceneLoader.h:9
BoundarySmootherType
Definition: WaistBalancer.h:52
Definition: LinkTraverse.h:17
int numIterations() const
Definition: WaistBalancer.cpp:83
void setFullTimeRange()
Definition: WaistBalancer.cpp:96
Eigen::Vector3d Vector3
Definition: EigenTypes.h:58
static int boundaryConditionTypeOf(const std::string &name)
Definition: WaistBalancer.cpp:132
static int boundarySmootherTypeOf(const std::string &name)
Definition: WaistBalancer.cpp:159
void setBoundarySmoother(int type, double smoothingTime)
Definition: WaistBalancer.cpp:170
Definition: WaistBalancer.h:21
void setNumIterations(int n)
Definition: WaistBalancer.cpp:77
void setTimeMargins(double timeToStartBalancer, double preInitialDuration, double postFinalDuration)
Definition: WaistBalancer.cpp:103
Definition: WaistBalancer.h:43
void setWaistLink(Link *waistLink)
Definition: WaistBalancer.cpp:71
void setInitialWaistTrajectoryMode(int mode)
Definition: WaistBalancer.cpp:192
Eigen::Matrix3d Matrix3
Definition: EigenTypes.h:57
void enableBoundaryCmAdjustment(bool on, double transitionTime=1.0)
Definition: WaistBalancer.cpp:185