//==============================================================================
// CALC EQUIVALENT JOINT FORCES
//==============================================================================
// To be called from tip to base.
// Temps do not need to be initialized.
// (sherm 060727) In spatial operators, this calculates ~H*Phi*(F-(MA+b))
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcEquivalentJointForces(
const SBTreePositionCache& pc,
const SBDynamicsCache& dc,
const SpatialVec* bodyForces,
SpatialVec* allZ,
Real* jointForces) const
{
const SpatialVec& myBodyForce = bodyForces[nodeNum];
SpatialVec& z = allZ[nodeNum];
Vec<dof>& eps = toU(jointForces);
// Centrifugal forces are MA+b where M is body spatial inertia,
// A is total coriolis acceleration, and b is gyroscopic force.
z = myBodyForce - getTotalCentrifugalForces(dc);
for (unsigned i=0; i<children.size(); ++i) {
const SpatialVec& zChild = allZ[children[i]->getNodeNum()];
const PhiMatrix& phiChild = children[i]->getPhi(pc);
z += phiChild * zChild;
}
eps = ~getH(pc) * z;
}
//==============================================================================
// CALC INVERSE DYNAMICS
//==============================================================================
// This algorithm calculates
// f = M*udot + C(u) - f_applied
// in O(N) time, where C(u) are the velocity-dependent coriolis, centrifugal and
// gyroscopic forces, f_applied are the applied body and joint forces, and udot
// is given.
//
// Pass 1 is base to tip and is just a calculation of body accelerations arising
// from udot and the coriolis accelerations. It is embodied in the reusable
// calcBodyAccelerationsFromUdotOutward() method above.
//
// Pass 2 is tip to base. It takes body accelerations from pass 1 (including coriolis
// accelerations as well as hinge accelerations), and the applied forces,
// and calculates the additional hinge forces that would be necessary to produce
// the observed accelerations.
//
// Costs for inverse dynamics include both passes:
// pass1: 12*dof + 18 flops
// pass2: 12*dof + 75 flops
// -----------------
// total: 24*dof + 93 flops
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::
calcInverseDynamicsPass2Inward(
const SBTreePositionCache& pc,
const SBTreeVelocityCache& vc,
const SpatialVec* allA_GB,
const Real* jointForces,
const SpatialVec* bodyForces,
SpatialVec* allF, // temp
Real* allTau) const
{
const Vec<dof>& myJointForce = fromU(jointForces);
const SpatialVec& myBodyForce = bodyForces[nodeNum];
const SpatialVec& A_GB = allA_GB[nodeNum];
SpatialVec& F = allF[nodeNum];
Vec<dof>& tau = toU(allTau);
// Start with rigid body force from desired body acceleration and
// gyroscopic forces due to angular velocity, minus external forces
// applied directly to this body.
F = getMk_G(pc)*A_GB + getGyroscopicForce(vc) - myBodyForce; // 57 flops
// Add in forces on children, shifted to this body.
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& FChild = allF[children[i]->getNodeNum()];
F += phiChild * FChild; // 18 flops
}
// Project body forces into hinge space and subtract any hinge forces already
// being applied to get the remaining hinge forces needed.
tau = ~getH(pc)*F - myJointForce; // 12*dof flops
}
// Pass 2 of multiplyByMInv.
// Base to tip: temp allA_GB does not need to be initialized before
// beginning the iteration.
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMInvPass2Outward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
const Real* allEpsilon,
SpatialVec* allA_GB,
Real* allUDot) const
{
const Vec<dof>& eps = fromU(allEpsilon);
SpatialVec& A_GB = allA_GB[nodeNum];
Vec<dof>& udot = toU(allUDot); // pull out this node's udot
const bool isPrescribed = isUDotKnown(ic);
const HType& H = getH(pc);
const PhiMatrix& phi = getPhi(pc);
const Mat<dof,dof>& DI = getDI(abc);
const HType& G = getG(abc);
// Shift parent's acceleration outward (Ground==0). 12 flops
const SpatialVec& A_GP = allA_GB[parent->getNodeNum()];
const SpatialVec APlus = ~phi * A_GP;
// For a prescribed mobilizer, set udot==0.
if (isPrescribed) {
udot = 0;
A_GB = APlus;
} else {
udot = DI*eps - ~G*APlus; // 2dof^2 + 11 dof flops
A_GB = APlus + H*udot; // 12 dof flops
}
}
// Pass 1, to be called from tip to base.
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMInvPass1Inward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
const Real* jointForces,
SpatialVec* allZ,
SpatialVec* allZPlus,
Real* allEpsilon) const
{
const Vec<dof>& f = fromU(jointForces);
SpatialVec& z = allZ[nodeNum];
SpatialVec& zPlus = allZPlus[nodeNum];
Vec<dof>& eps = toU(allEpsilon);
const bool isPrescribed = isUDotKnown(ic);
const HType& H = getH(pc);
const HType& G = getG(abc);
z = 0;
for (unsigned i=0; i<children.size(); i++) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
z += phiChild * zPlusChild; // 18 flops
}
zPlus = z;
if (!isPrescribed) {
eps = f - ~H*z; // 12*dof flops
zPlus += G*eps; // 12*dof flops
}
}
//
// to be called from base to tip.
//
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::setVelFromSVel(
const SBTreePositionCache& pc,
const SBTreeVelocityCache& mc,
const SpatialVec& sVel,
Vector& u) const
{
toU(u) = ~getH(pc) * (sVel - (~getPhi(pc) * parent->getV_GB(mc)));
}
//==============================================================================
// CALC UDOT
//==============================================================================
// To be called from tip to base.
// Temps do not need to be initialized.
//
// First pass
// ----------
// Given previously-calculated quantities from the State
// P this body's articulated body inertia
// Phi composite body child-to-parent shift matrix
// H joint transition matrix; sense is transposed from Jain
// G P * H * DI
// and supplied arguments
// F body force applied to B
// f generalize forces applied to B's mobilities
// udot_p known udots (if mobilizer is prescribed)
// calculate
// z articulated body residual force on B
// zPlus AB residual force as felt on inboard side of mobilizer
// eps f - ~H*z gen force plus gen equiv of body forces
// For a prescribed mobilizer we have zPlus=z. Otherwise, zPlus = z + G*eps.
//
// This is the first (inward) loop of Algorithm 16.2 on page 323 of Jain's
// 2011 book, modified to include the applied body force and not including
// the auxiliary quantity "nu=DI*eps".
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass1Inward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
const SBDynamicsCache& dc,
const Real* jointForces,
const SpatialVec* bodyForces,
const Real* allUDot,
SpatialVec* allZ,
SpatialVec* allZPlus,
Real* allEpsilon) const
{
const Vec<dof>& f = fromU(jointForces);
const SpatialVec& F = bodyForces[nodeNum];
SpatialVec& z = allZ[nodeNum];
SpatialVec& zPlus = allZPlus[nodeNum];
Vec<dof>& eps = toU(allEpsilon);
const bool isPrescribed = isUDotKnown(ic);
const HType& H = getH(pc);
const ArticulatedInertia& P = getP(abc);
const HType& G = getG(abc);
// z = Pa+b - F
z = getMobilizerCentrifugalForces(dc) - F; // 6 flops
// z += P H udot_p if prescribed
if (isPrescribed && !isUDotKnownToBeZero(ic)) {
const Vec<dof>& udot = fromU(allUDot);
z += P*(H*udot); // 66+12*dof flops
}
// z += sum(Phi(child) * zPlus(child)) for all children
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& zPlusChild = allZPlus[children[i]->getNodeNum()];
z += phiChild * zPlusChild; // 18 flops
}
eps = f - ~H*z; // 12*dof flops
zPlus = z;
if (!isPrescribed)
zPlus += G*eps; // 12*dof flops
}
CString ArbI18N::Utf8HankakuToZenkaku(char const * s)
{
/* Change Hiragana to Katakana and set all characters to full-width */
BufferCharSource utf8Source(s);
StringCharSink utf8Sink;
InternalEncodingConverter unicodeSource(utf8Source,
ExternalEncoding::Unicode20UTF8(), InternalEncoding::Unicode20());
ToUppercaseTransform toU(unicodeSource);
ToKatakanaTransform toK(toU);
HankakuKatakanaToZenkakuTransform HtoZ(toK);
InternalEncodingConverterSink unicodeSink(InternalEncoding::Unicode20(),
ExternalEncoding::Unicode20UTF8(), utf8Sink);
unicodeSink.Put(HtoZ);
CString r = utf8Sink.c_str();
return r;
}
// Call tip to base after calling multiplyByMPass1Outward() for each
// rigid body.
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::multiplyByMPass2Inward(
const SBTreePositionCache& pc,
const SpatialVec* allA_GB,
SpatialVec* allF, // temp
Real* allTau) const
{
const SpatialVec& A_GB = allA_GB[nodeNum];
SpatialVec& F = allF[nodeNum];
Vec<dof>& tau = toU(allTau);
// 45 flops because Mk has a nice structure
F = getMk_G(pc)*A_GB;
for (unsigned i=0; i<children.size(); ++i) {
const PhiMatrix& phiChild = children[i]->getPhi(pc);
const SpatialVec& FChild = allF[children[i]->getNodeNum()];
F += phiChild * FChild; // 18 flops
}
tau = ~getH(pc)*F; // 11*dof flops
}
//
// Calculate acceleration in internal coordinates, based on the last set
// of forces that were reduced into epsilon (e.g., see above).
// Base to tip: temp allA_GB does not need to be initialized before
// beginning the iteration.
//
template<int dof, bool noR_FM, bool noX_MB, bool noR_PF> void
RigidBodyNodeSpec<dof, noR_FM, noX_MB, noR_PF>::calcUDotPass2Outward(
const SBInstanceCache& ic,
const SBTreePositionCache& pc,
const SBArticulatedBodyInertiaCache& abc,
const SBTreeVelocityCache& vc,
const SBDynamicsCache& dc,
const Real* allEpsilon,
SpatialVec* allA_GB,
Real* allUDot,
Real* allTau) const
{
const Vec<dof>& eps = fromU(allEpsilon);
SpatialVec& A_GB = allA_GB[nodeNum];
Vec<dof>& udot = toU(allUDot); // pull out this node's udot
const bool isPrescribed = isUDotKnown(ic);
const HType& H = getH(pc);
const PhiMatrix& phi = getPhi(pc);
const ArticulatedInertia& P = getP(abc);
const SpatialVec& a = getMobilizerCoriolisAcceleration(vc);
const Mat<dof,dof>& DI = getDI(abc);
const HType& G = getG(abc);
// Shift parent's acceleration outward (Ground==0). 12 flops
const SpatialVec& A_GP = allA_GB[parent->getNodeNum()];
const SpatialVec APlus = ~phi * A_GP;
if (isPrescribed) {
Vec<dof>& tau = updTau(ic,allTau); // pull out this node's tau
// This is f - ~H(P*APlus + z); compare Jain 16.17b. Note sign
// change since our tau is on the LHS while his is on the RHS.
tau = eps - ~H*(P*APlus); // 66 + 12*dof flops
} else {
udot = DI*eps - ~G*APlus; // 2*dof^2 + 11*dof
}
A_GB = APlus + H*udot + a;
}
