5 #ifndef CNOID_BALANCER_PLUGIN_WAIST_BALANCER_H_INCLUDED 6 #define CNOID_BALANCER_PLUGIN_WAIST_BALANCER_H_INCLUDED 8 #include <cnoid/BodyMotion> 11 #include <cnoid/LinkTraverse> 12 #include <cnoid/CompositeIK> 13 #include <boost/optional.hpp> 14 #include <boost/function.hpp> 38 void setTimeMargins(
double timeToStartBalancer,
double preInitialDuration,
double postFinalDuration);
74 std::vector<double> q0;
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;
92 bool isCalculatingInitialWaistTrajectory;
94 std::vector< boost::optional<double> > jointPositions;
99 int numFilteredFrames;
100 int frameToStartBalancer;
123 std::vector<Coeff> coeffSeq;
125 std::vector<Vector3> totalCmTranslations;
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;
138 bool isBoundaryCmAdjustmentEnabled;
139 double boundaryCmAdjustmentTransitionTime;
140 double boundaryCmAdjustmentTransitionHalfLength;
141 bool doBoundaryCmAdjustment;
142 std::vector<Vector3> boundaryCmAdjustmentTranslations;
145 bool isWaistHeightRelaxationEnabled;
146 bool doStoreOriginalWaistFeetPositionsForWaistHeightRelaxation;
147 bool doWaistHeightRelaxation;
148 struct WaistFeetPos {
154 std::vector<WaistFeetPos> waistFeetPosSeq;
155 std::vector<double> waistDeltaZseq;
157 Link* rightKneePitchJoint;
158 Link* leftKneePitchJoint;
162 std::ostream& os() {
return *os_; }
164 bool apply2(
BodyMotion& motion,
bool putAllLinkPositions);
165 void calcBoundaryCmAdjustmentTrajectory();
166 bool calcBoundaryCmAdjustmentTrajectorySub(
int begin,
int direction);
167 bool calcWaistTranslationWithCmAboveZmp(
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);
180 inline double timeOfFrame(
int frame) {
181 return std::max(targetBeginningTime, std::min(targetEndingTime, (frame / frameRate)));
Definition: WaistBalancer.h:45
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
Definition: WaistBalancer.h:57
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
Definition: WaistBalancer.h:46
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
Definition: WaistBalancer.h:56
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