4 #include <cap_gait/contrib/Vec2f.h>
5 #include <cap_gait/contrib/Vec3f.h>
7 namespace margait_contrib
29 JointConfig(
double x,
double y,
double z,
double compliance = 1.0)
31 setPos(x, y, z, compliance);
34 void setPos(
double x,
double y,
double z,
double compliance = 1.0)
39 this->compliance = compliance;
42 inline JointConfig operator+(
const JointConfig& v)
const {
return JointConfig(x+v.x, y+v.y, z+v.z, qMin(compliance, v.compliance));}
43 inline JointConfig operator-()
const {
return JointConfig(-x, -y, -z, compliance);}
44 inline JointConfig operator-(
const JointConfig& v)
const {
return JointConfig(x-v.x, y-v.y, z-v.z, qMin(compliance, v.compliance));}
45 inline JointConfig operator+=(
const JointConfig& v) {x+=v.x; y+=v.y; z+=v.z;
return *
this;}
46 inline JointConfig operator-=(
const JointConfig& v) {x-=v.x; y-=v.y; z-=v.z;
return *
this;}
47 inline JointConfig& operator*=(
const double scalar) {x*=scalar; y*=scalar; z*=scalar;
return *
this;}
48 inline JointConfig& operator/=(
const double scalar) {x/=scalar; y/=scalar; z/=scalar;
return *
this;}
49 inline bool operator==(
const JointConfig& v)
const {
return (x==v.x) && (y==v.y) && (z==v.z) && (velocity==v.velocity);}
50 inline bool operator!=(
const JointConfig& v)
const {
return (x!=v.x) || (y!=v.y) || (z!=v.z) || (velocity!=v.velocity);}
63 ArmPose(
const JointConfig& shoulder,
const JointConfig& elbow)
65 this->shoulder = shoulder;
69 inline ArmPose operator+(
const ArmPose& v)
const {
return ArmPose(shoulder+v.shoulder, elbow+v.elbow);}
70 inline ArmPose operator-()
const {
return ArmPose(-shoulder, -elbow);}
71 inline ArmPose operator-(
const ArmPose& v)
const {
return ArmPose(shoulder-v.shoulder, elbow-v.elbow);}
72 inline ArmPose operator+=(
const ArmPose& v) {shoulder+=v.shoulder; elbow+=v.elbow;
return *
this;}
73 inline ArmPose operator-=(
const ArmPose& v) {shoulder-=v.shoulder; elbow-=v.elbow;
return *
this;}
74 inline ArmPose& operator*=(
const double scalar) {shoulder*=scalar; elbow*=scalar;
return *
this;}
75 inline ArmPose& operator/=(
const double scalar) {shoulder/=scalar; elbow/=scalar;
return *
this;}
76 inline bool operator==(
const ArmPose& v)
const {
return (shoulder==v.shoulder) && (elbow==v.elbow);}
77 inline bool operator!=(
const ArmPose& v)
const {
return (shoulder!=v.shoulder) || (elbow!=v.elbow);}
82 struct AbstractArmPose
94 AbstractArmPose(
const Vec3f& armAngle,
double armExtension,
double compliance = 1.0)
96 this->armAngle = armAngle;
97 this->armExtension = armExtension;
98 this->compliance = compliance;
101 inline AbstractArmPose operator+(
const AbstractArmPose& v)
const {
return AbstractArmPose(armAngle+v.armAngle, armExtension+v.armExtension, qMin(compliance, v.compliance));}
102 inline AbstractArmPose operator-()
const {
return AbstractArmPose(-armAngle, -armExtension, compliance);}
103 inline AbstractArmPose operator-(
const AbstractArmPose& v)
const {
return AbstractArmPose(armAngle-v.armAngle, armExtension-v.armExtension, qMin(compliance, v.compliance));}
104 inline AbstractArmPose operator+=(
const AbstractArmPose& v) {armAngle+=v.armAngle; armExtension+=v.armExtension;
return *
this;}
105 inline AbstractArmPose operator-=(
const AbstractArmPose& v) {armAngle-=v.armAngle; armExtension-=v.armExtension;
return *
this;}
106 inline AbstractArmPose& operator*=(
const double scalar){armAngle*=scalar; armExtension*=scalar;
return *
this;}
107 inline AbstractArmPose& operator/=(
const double scalar){armAngle/=scalar; armExtension/=scalar;
return *
this;}
108 inline bool operator==(
const AbstractArmPose& v)
const {
return (armAngle==v.armAngle) && (armExtension==v.armExtension);}
109 inline bool operator!=(
const AbstractArmPose& v)
const {
return (armAngle!=v.armAngle) || (armExtension!=v.armExtension);}
113 struct InverseArmPose
123 InverseArmPose(
const Vec3f& handPosition,
double compliance = 1.0)
125 this->handPosition = handPosition;
126 this->compliance = compliance;
129 inline InverseArmPose operator+(
const InverseArmPose& v)
const {
return InverseArmPose(handPosition+v.handPosition, qMin(compliance, v.compliance));}
130 inline InverseArmPose operator-()
const {
return InverseArmPose(-handPosition, compliance);}
131 inline InverseArmPose operator-(
const InverseArmPose& v)
const {
return InverseArmPose(handPosition-v.handPosition, qMin(compliance, v.compliance));}
132 inline InverseArmPose operator+=(
const InverseArmPose& v) {handPosition+=v.handPosition;
return *
this;}
133 inline InverseArmPose operator-=(
const InverseArmPose& v) {handPosition-=v.handPosition;
return *
this;}
134 inline InverseArmPose& operator*=(
const double scalar){handPosition*=scalar;
return *
this;}
135 inline InverseArmPose& operator/=(
const double scalar){handPosition/=scalar;
return *
this;}
136 inline bool operator==(
const InverseArmPose& v)
const {
return (handPosition==v.handPosition);}
137 inline bool operator!=(
const InverseArmPose& v)
const {
return (handPosition!=v.handPosition);}
151 LegPose(
const JointConfig& hip,
const JointConfig& knee,
const JointConfig& ankle)
158 inline LegPose operator+(
const LegPose& v)
const {
return LegPose(hip+v.hip, knee+v.knee, ankle+v.ankle);}
159 inline LegPose operator-()
const {
return LegPose(-hip, -knee, -ankle);}
160 inline LegPose operator-(
const LegPose& v)
const {
return LegPose(hip-v.hip, knee-v.knee, ankle-v.ankle);}
161 inline LegPose operator+=(
const LegPose& v) {hip+=v.hip; knee+=v.knee; ankle+=v.ankle;
return *
this;}
162 inline LegPose operator-=(
const LegPose& v) {hip-=v.hip; knee-=v.knee; ankle-=v.ankle;
return *
this;}
163 inline LegPose& operator*=(
const double scalar) {hip*=scalar; knee*=scalar; ankle*=scalar;
return *
this;}
164 inline LegPose& operator/=(
const double scalar) {hip/=scalar; knee/=scalar; ankle/=scalar;
return *
this;}
165 inline bool operator==(
const LegPose& v)
const {
return (hip==v.hip) && (knee==v.knee) && (ankle==v.ankle);}
166 inline bool operator!=(
const LegPose& v)
const {
return (hip!=v.hip) || (knee!=v.knee) || (ankle!=v.ankle);}
170 struct AbstractLegPose
183 AbstractLegPose(
const Vec3f& legAngle,
double legExtension,
const Vec2f& footAngle,
double compliance = 1.0)
185 this->legAngle = legAngle;
186 this->legExtension = legExtension;
187 this->footAngle = footAngle;
188 this->compliance = compliance;
191 inline AbstractLegPose operator+(
const AbstractLegPose& v)
const {
return AbstractLegPose(legAngle+v.legAngle, legExtension+v.legExtension, footAngle+v.footAngle, qMin(compliance, v.compliance));}
192 inline AbstractLegPose operator-()
const {
return AbstractLegPose(-legAngle, -legExtension, -footAngle, compliance);}
193 inline AbstractLegPose operator-(
const AbstractLegPose& v)
const {
return AbstractLegPose(legAngle-v.legAngle, legExtension-v.legExtension, footAngle-v.footAngle, qMin(compliance, v.compliance));}
194 inline AbstractLegPose operator+=(
const AbstractLegPose& v) {legAngle+=v.legAngle; legExtension+=v.legExtension; footAngle+=v.footAngle;
return *
this;}
195 inline AbstractLegPose operator-=(
const AbstractLegPose& v) {legAngle-=v.legAngle; legExtension-=v.legExtension; footAngle-=v.footAngle;
return *
this;}
196 inline AbstractLegPose& operator*=(
const double scalar){legAngle*=scalar; legExtension*=scalar; footAngle*=scalar;
return *
this;}
197 inline AbstractLegPose& operator/=(
const double scalar){legAngle/=scalar; legExtension/=scalar; footAngle/=scalar;
return *
this;}
198 inline bool operator==(
const AbstractLegPose& v)
const {
return (legAngle==v.legAngle) && (legExtension==v.legExtension) && (footAngle==v.footAngle);}
199 inline bool operator!=(
const AbstractLegPose& v)
const {
return (legAngle!=v.legAngle) || (legExtension!=v.legExtension) || (footAngle!=v.footAngle);}
203 struct InverseLegPose
214 InverseLegPose(
const Vec3f& footPosition,
const Vec3f& footAngle,
double compliance = 1.0)
216 this->footPosition = footPosition;
217 this->footAngle = footAngle;
218 this->compliance = compliance;
221 inline InverseLegPose operator+(
const InverseLegPose& v)
const {
return InverseLegPose(footPosition+v.footPosition, footAngle+v.footAngle, qMin(compliance, v.compliance));}
222 inline InverseLegPose operator-()
const {
return InverseLegPose(-footPosition, -footAngle, compliance);}
223 inline InverseLegPose operator-(
const InverseLegPose& v)
const {
return InverseLegPose(footPosition-v.footPosition, footAngle-v.footAngle, qMin(compliance, v.compliance));}
224 inline InverseLegPose operator+=(
const InverseLegPose& v) {footPosition+=v.footPosition; footAngle+=v.footAngle;
return *
this;}
225 inline InverseLegPose operator-=(
const InverseLegPose& v) {footPosition-=v.footPosition; footAngle-=v.footAngle;
return *
this;}
226 inline InverseLegPose& operator*=(
const double scalar){footPosition*=scalar; footAngle*=scalar;
return *
this;}
227 inline InverseLegPose& operator/=(
const double scalar){footPosition/=scalar; footAngle/=scalar;
return *
this;}
228 inline bool operator==(
const InverseLegPose& v)
const {
return (footPosition==v.footPosition) && (footAngle==v.footAngle);}
229 inline bool operator!=(
const InverseLegPose& v)
const {
return (footPosition!=v.footPosition) || (footAngle!=v.footAngle);}
243 HeadPose(
const JointConfig& neck)
248 inline HeadPose operator+(
const HeadPose& v)
const {
return HeadPose(neck+v.neck);}
249 inline HeadPose operator-()
const {
return HeadPose(-neck);}
250 inline HeadPose operator-(
const HeadPose& v)
const {
return HeadPose(neck-v.neck);}
251 inline HeadPose operator+=(
const HeadPose& v) {neck+=v.neck;
return *
this;}
252 inline HeadPose operator-=(
const HeadPose& v) {neck-=v.neck;
return *
this;}
253 inline HeadPose& operator*=(
const double scalar) {neck*=scalar;
return *
this;}
254 inline HeadPose& operator/=(
const double scalar) {neck/=scalar;
return *
this;}
255 inline bool operator==(
const HeadPose& v)
const {
return (neck==v.neck);}
256 inline bool operator!=(
const HeadPose& v)
const {
return (neck!=v.neck);}
269 TrunkPose(
const JointConfig& spine)
274 inline TrunkPose operator+(
const TrunkPose& v)
const {
return TrunkPose(spine+v.spine);}
275 inline TrunkPose operator-()
const {
return TrunkPose(-spine);}
276 inline TrunkPose operator-(
const TrunkPose& v)
const {
return TrunkPose(spine-v.spine);}
277 inline TrunkPose operator+=(
const TrunkPose& v) {spine+=v.spine;
return *
this;}
278 inline TrunkPose operator-=(
const TrunkPose& v) {spine-=v.spine;
return *
this;}
279 inline TrunkPose& operator*=(
const double scalar) {spine*=scalar;
return *
this;}
280 inline TrunkPose& operator/=(
const double scalar) {spine/=scalar;
return *
this;}
281 inline bool operator==(
const TrunkPose& v)
const {
return (spine==v.spine);}
282 inline bool operator!=(
const TrunkPose& v)
const {
return (spine!=v.spine);}
291 ArmPose rightArmPose;
293 LegPose rightLegPose;
300 Pose(
const HeadPose& headPose,
const TrunkPose& trunkPose,
const ArmPose& leftArmPose,
const ArmPose& rightArmPose,
const LegPose& leftLegPose,
const LegPose& rightLegPose)
302 this->headPose = headPose;
303 this->trunkPose = trunkPose;
304 this->leftArmPose = leftArmPose;
305 this->rightArmPose = rightArmPose;
306 this->leftLegPose = leftLegPose;
307 this->rightLegPose = rightLegPose;
310 inline Pose operator+(
const Pose& v)
const {
return Pose(headPose+v.headPose, trunkPose+v.trunkPose, leftArmPose+v.leftArmPose, rightArmPose+v.rightArmPose, leftLegPose+v.leftLegPose, rightLegPose+v.rightLegPose);}
311 inline Pose operator-()
const {
return Pose(-headPose, -trunkPose, -leftArmPose, -rightArmPose, -leftLegPose, -rightLegPose);}
312 inline Pose operator-(
const Pose& v)
const {
return Pose(headPose-v.headPose, trunkPose-v.trunkPose, leftArmPose-v.leftArmPose, rightArmPose-v.rightArmPose, leftLegPose-v.leftLegPose, rightLegPose-v.rightLegPose);}
313 inline Pose operator+=(
const Pose& v) {headPose+=v.headPose; trunkPose+=v.trunkPose; leftArmPose+=v.leftArmPose; rightArmPose+=v.rightArmPose; leftLegPose+=v.leftLegPose; rightLegPose+=v.rightLegPose;
return *
this;}
314 inline Pose operator-=(
const Pose& v) {headPose-=v.headPose; trunkPose-=v.trunkPose; leftArmPose-=v.leftArmPose; rightArmPose-=v.rightArmPose; leftLegPose-=v.leftLegPose; rightLegPose-=v.rightLegPose;
return *
this;}
315 inline Pose& operator*=(
const double scalar) {headPose*=scalar; trunkPose*=scalar; leftArmPose*=scalar; rightArmPose*=scalar; leftLegPose*=scalar; rightLegPose*=scalar;
return *
this;}
316 inline Pose& operator/=(
const double scalar) {headPose/=scalar; trunkPose/=scalar; leftArmPose/=scalar; rightArmPose/=scalar; leftLegPose/=scalar; rightLegPose/=scalar;
return *
this;}
317 inline bool operator==(
const Pose& v)
const {
return (headPose==v.headPose) && (trunkPose==v.trunkPose) && (leftArmPose==v.leftArmPose) && (rightArmPose==v.rightArmPose) && (leftLegPose==v.leftLegPose) && (rightLegPose==v.rightLegPose);}
318 inline bool operator!=(
const Pose& v)
const {
return (headPose!=v.headPose) || (trunkPose!=v.trunkPose) || (leftArmPose!=v.leftArmPose) || (rightArmPose!=v.rightArmPose) || (leftLegPose!=v.leftLegPose) || (rightLegPose!=v.rightLegPose);}
326 AbstractArmPose leftArmPose;
327 AbstractArmPose rightArmPose;
328 AbstractLegPose leftLegPose;
329 AbstractLegPose rightLegPose;
336 AbstractPose(
const HeadPose& headPose,
const TrunkPose& trunkPose,
const AbstractArmPose& leftArmPose,
const AbstractArmPose& rightArmPose,
const AbstractLegPose& leftLegPose,
const AbstractLegPose& rightLegPose)
338 this->headPose = headPose;
339 this->trunkPose = trunkPose;
340 this->leftArmPose = leftArmPose;
341 this->rightArmPose = rightArmPose;
342 this->leftLegPose = leftLegPose;
343 this->rightLegPose = rightLegPose;
346 inline AbstractPose operator+(
const AbstractPose& v)
const {
return AbstractPose(headPose+v.headPose, trunkPose+v.trunkPose, leftArmPose+v.leftArmPose, rightArmPose+v.rightArmPose, leftLegPose+v.leftLegPose, rightLegPose+v.rightLegPose);}
347 inline AbstractPose operator-()
const {
return AbstractPose(-headPose, -trunkPose, -leftArmPose, -rightArmPose, -leftLegPose, -rightLegPose);}
348 inline AbstractPose operator-(
const AbstractPose& v)
const {
return AbstractPose(headPose-v.headPose, trunkPose-v.trunkPose, leftArmPose-v.leftArmPose, rightArmPose-v.rightArmPose, leftLegPose-v.leftLegPose, rightLegPose-v.rightLegPose);}
349 inline AbstractPose operator+=(
const AbstractPose& v) {headPose+=v.headPose; trunkPose+=v.trunkPose; leftArmPose+=v.leftArmPose; rightArmPose+=v.rightArmPose; leftLegPose+=v.leftLegPose; rightLegPose+=v.rightLegPose;
return *
this;}
350 inline AbstractPose operator-=(
const AbstractPose& v) {headPose-=v.headPose; trunkPose-=v.trunkPose; leftArmPose-=v.leftArmPose; rightArmPose-=v.rightArmPose; leftLegPose-=v.leftLegPose; rightLegPose-=v.rightLegPose;
return *
this;}
351 inline AbstractPose& operator*=(
const double scalar) {headPose*=scalar; trunkPose*=scalar; leftArmPose*=scalar; rightArmPose*=scalar; leftLegPose*=scalar; rightLegPose*=scalar;
return *
this;}
352 inline AbstractPose& operator/=(
const double scalar) {headPose/=scalar; trunkPose/=scalar; leftArmPose/=scalar; rightArmPose/=scalar; leftLegPose/=scalar; rightLegPose/=scalar;
return *
this;}
353 inline bool operator==(
const AbstractPose& v)
const {
return (headPose==v.headPose) && (trunkPose==v.trunkPose) && (leftArmPose==v.leftArmPose) && (rightArmPose==v.rightArmPose)&& (leftLegPose==v.leftLegPose) && (rightLegPose==v.rightLegPose);}
354 inline bool operator!=(
const AbstractPose& v)
const {
return (headPose!=v.headPose) || (trunkPose!=v.trunkPose) || (leftArmPose!=v.leftArmPose) || (rightArmPose!=v.rightArmPose)|| (leftLegPose!=v.leftLegPose) || (rightLegPose!=v.rightLegPose);}
362 InverseArmPose leftArmPose;
363 InverseArmPose rightArmPose;
364 InverseLegPose leftLegPose;
365 InverseLegPose rightLegPose;
372 InversePose(
const HeadPose& headPose,
const TrunkPose& trunkPose,
const InverseArmPose& leftArmPose,
const InverseArmPose& rightArmPose,
const InverseLegPose& leftLegPose,
const InverseLegPose& rightLegPose)
374 this->headPose = headPose;
375 this->trunkPose = trunkPose;
376 this->leftArmPose = leftArmPose;
377 this->rightArmPose = rightArmPose;
378 this->leftLegPose = leftLegPose;
379 this->rightLegPose = rightLegPose;
382 inline InversePose operator+(
const InversePose& v)
const {
return InversePose(headPose+v.headPose, trunkPose+v.trunkPose, leftArmPose+v.leftArmPose, rightArmPose+v.rightArmPose, leftLegPose+v.leftLegPose, rightLegPose+v.rightLegPose);}
383 inline InversePose operator-()
const {
return InversePose(-headPose, -trunkPose, -leftArmPose, -rightArmPose, -leftLegPose, -rightLegPose);}
384 inline InversePose operator-(
const InversePose& v)
const {
return InversePose(headPose-v.headPose, trunkPose-v.trunkPose, leftArmPose-v.leftArmPose, rightArmPose-v.rightArmPose, leftLegPose-v.leftLegPose, rightLegPose-v.rightLegPose);}
385 inline InversePose operator+=(
const InversePose& v) {headPose+=v.headPose; trunkPose+=v.trunkPose; leftArmPose+=v.leftArmPose; rightArmPose+=v.rightArmPose; leftLegPose+=v.leftLegPose; rightLegPose+=v.rightLegPose;
return *
this;}
386 inline InversePose operator-=(
const InversePose& v) {headPose-=v.headPose; trunkPose-=v.trunkPose; leftArmPose-=v.leftArmPose; rightArmPose-=v.rightArmPose; leftLegPose-=v.leftLegPose; rightLegPose-=v.rightLegPose;
return *
this;}
387 inline InversePose& operator*=(
const double scalar) {headPose*=scalar; trunkPose*=scalar; leftArmPose*=scalar; rightArmPose*=scalar; leftLegPose*=scalar; rightLegPose*=scalar;
return *
this;}
388 inline InversePose& operator/=(
const double scalar) {headPose/=scalar; trunkPose/=scalar; leftArmPose/=scalar; rightArmPose/=scalar; leftLegPose/=scalar; rightLegPose/=scalar;
return *
this;}
389 inline bool operator==(
const InversePose& v)
const {
return (headPose==v.headPose) && (trunkPose==v.trunkPose) && (leftArmPose==v.leftArmPose) && (rightArmPose==v.rightArmPose)&& (leftLegPose==v.leftLegPose) && (rightLegPose==v.rightLegPose);}
390 inline bool operator!=(
const InversePose& v)
const {
return (headPose!=v.headPose) || (trunkPose!=v.trunkPose) || (leftArmPose!=v.leftArmPose) || (rightArmPose!=v.rightArmPose)|| (leftLegPose!=v.leftLegPose) || (rightLegPose!=v.rightLegPose);}
398 AbstractPose abstractPose;
399 InversePose inversePose;