BodyDynamicsProviderDelegate::BodyDynamicsProviderDelegate(const MassCalibrationBH& theMassCalibrationBH,
const TorsoMatrixBH& theTorsoMatrixBH,
const RobotDimensionsBH& theRobotDimensionsBH,
const FrameInfoBH& theFrameInfoBH)
: supportFootInGround(RotationMatrixBH(), Vector3BH<>(0.0f, 0.0f, -theRobotDimensions.heightLeg5Joint), true),
theMassCalibrationBH(theMassCalibrationBH), theTorsoMatrixBH(theTorsoMatrixBH),
theRobotDimensionsBH(theRobotDimensionsBH), theFrameInfoBH(theFrameInfoBH)
{
init();
}
void WalkingEngineKickPlayer::applyFoot(Pose3DBH& leftOriginToFoot, Pose3DBH& rightOriginToFoot)
{
ASSERT(kick);
float sign = mirrored ? -1.f : 1.f;
Vector3BH<> additionalFootRotation;
Vector3BH<> additionFootTranslation;
for(int i = 0; i < 3; ++i)
{
additionFootTranslation[i] = getValue(WalkingEngineKick::Track(WalkingEngineKick::footTranslationX + i), 0.f);
additionalFootRotation[i] = getValue(WalkingEngineKick::Track(WalkingEngineKick::footRotationX + i), 0.f);
}
additionalFootRotation.x = additionalFootRotation.x * sign;
additionalFootRotation.z = additionalFootRotation.z * sign;
additionFootTranslation.y = additionFootTranslation.y * sign;
(mirrored ? rightOriginToFoot : leftOriginToFoot).conc(Pose3DBH(RotationMatrixBH(additionalFootRotation), additionFootTranslation));
}
void BodyDynamicsProviderDelegate::footForceDerivativeUpwards(const int jointOffset,
SpatialVector<>& fcByqDoubleDotFoot,
SpatialVector<>& fcByqDotFoot,
SpatialVector<>& fcByqFoot,
const BodyDynamics& bodyDynamics)
{
const bool leftSupport = bodyDynamics.supportLeg == IndykickRequest::left;
const MassCalibrationBH::Limb pelvis = leftSupport ? MassCalibrationBH::pelvisLeft // Support pelvis
: MassCalibrationBH::pelvisRight;
/* In a kinematic tree rootet at the torso, the natural body would be the successor body of the joint. */
const MassCalibrationBH::Limb naturalBodyLimb = static_cast<MassCalibrationBH::Limb>(pelvis + jointOffset);
/* In a kinematic tree rooted at the torso, the natural predecessor would be the predecessor body of the joint. */
const MassCalibrationBH::Limb naturalPredecessorLimb = !jointOffset ? MassCalibrationBH::torso
: static_cast<MassCalibrationBH::Limb>(pelvis + jointOffset - 1);
const Body& naturalBody = bodyDynamics.limbs[naturalBodyLimb];
const Body& naturalPredecessor = bodyDynamics.limbs[naturalPredecessorLimb];
/* The joint at 'jointOffset' connects the natural body to the natural predecessor, since the base body is at the
* end of the chain, i.e. one could say the natural leaf body. The reference coordinate system is now on the side
* of the natural predecessor of the joint, but still there where the joint is located, i.e. the physical position
* is the same as the natual body's position, but the rotation of the coordinate system is different. */
const SpatialTransform& nPredInNBody = naturalBody.X;
const SpatialTransform& nBodyInNPred = naturalBody.Xinv;
const SpatialTransform nBodyInRef(nBodyInNPred.rot, Vector3BH<>(), true);
const SpatialTransform nPredInRef(RotationMatrixBH(), -nPredInNBody.pos, true);
/* Symbols indexed with i mean the things stored in the natural predecessor, since i means the target of the joint.
* Symbols indexed with (i-1) mean the things stored in the natural body, since (i-1) means the source of the joint. */
/* === Equation 41. === */
/* First, transform the axis s_i to the reference system. This is easy, since it is the axis of rotation,
* it does not change by the rotation transform. Only the sign flips. The axis is the exception of the index rule.
* Since it connected the bodies 'natural body' and 'natural predecessor' and 'natural body' has the 'naturally' higher
* index, it is stored in the 'natural body' object. */
const SpatialVector<> axis = -naturalBody.mode;
/* Transform the aggregate spatial inertia to the reference coordinate system. */
const SpatialInertia Ic = nPredInRef.transform(naturalPredecessor.Ic);
/* Calculate the actual equation. */
const SpatialInertiaDerivative IcByq = Ic.derive(axis);
/* === Equation 42. === */
const SpatialVector<> pcByqDot = Ic * axis;
/* === Equation 43. === */
/* Transform the velocity of the natural body into the reference system. */
const SpatialVector<> velocityBody = nBodyInRef * naturalBody.v;
/* Transform the aggregate momentum of the natural predecessor into the reference system. */
const SpatialVector<> pc = nPredInRef * naturalPredecessor.pc;
/* Calculate the actual equation. */
const SpatialVector<> pcByq = IcByq * velocityBody + (axis ^ pc);
/* === Equation 44. === */
const SpatialVector<>& fcByqDoubleDot = pcByqDot;
/* === Equation 45. === */
/* Transform the time derivative of the aggregate spatial inertia to the reference system. */
const SpatialInertiaDerivative dIc = nPredInRef.transform(naturalPredecessor.dIc);
/* Transform the velocity ot the natural predecessor into the reference system. */
const SpatialVector<> velocityPred = nPredInRef * naturalPredecessor.v;
/* Calculate the actual equation. */
const SpatialVector<> fcByqDot = pcByq + dIc * axis + (velocityPred ^ pcByqDot);
/* === Equation 46. === */
/* Transform the acceleration of the natural body into the reference system. */
const SpatialVector<> accelBody = nBodyInRef * naturalBody.a;
/* Transform the aggregate force of the natural predecessor into the reference system. */
const SpatialVector<> fc = nPredInRef * naturalPredecessor.fc;
/* Calculate the derivative by q of the time derivative of the aggregate spatial inertia. */
const SpatialInertiaDerivative dIcByq = dIc.derive(axis);
/* Calculate the actual equation. */
const SpatialVector<> fcByq = (axis ^ fc)
+ (IcByq * accelBody)
+ (velocityBody ^ ((axis ^ pc) + (IcByq * velocityBody)))
+ (dIcByq * velocityBody);
/* In principle all the derivatives have been calculated by now. They only have to be transformed
* from the reference coordinate system into the coordinate system of the support foot. */
/* Get the transformation from the natural body into the support foot. */
const SpatialTransform& nBodyInFoot = getBodyInSupportFoot(naturalBodyLimb, bodyDynamics);
/* Calculate the transform from reference system into the natural body. */
const SpatialTransform& refInNBody = nBodyInRef.inverse();
/* Calculate the transform from the reference system into the support foot system. */
const SpatialTransform refInFoot = nBodyInFoot * refInNBody;
/* Do the transformations. */
fcByqDoubleDotFoot = refInFoot * fcByqDoubleDot;
fcByqDotFoot = refInFoot * fcByqDot;
fcByqFoot = refInFoot * fcByq;
}
RotationMatrixBH RotationMatrixBH::fromRotationZ(const float angle)
{
const float c = cos(angle), s = sin(angle);
return RotationMatrixBH(Vector3BH<>(c, s, 0.f), Vector3BH<>(-s, c, 0.f), Vector3BH<>(0.f, 0.f, 1.f));
}
RotationMatrixBH RotationMatrixBH::fromRotationX(const float angle)
{
const float c = cos(angle), s = sin(angle);
return RotationMatrixBH(Vector3BH<>(1.f, 0.f, 0.f), Vector3BH<>(0.f, c, s), Vector3BH<>(0.f, -s, c));
}
