Skip to content

Commit

Permalink
Merge pull request #15 from ihmcrobotics/feature/calculator-inertia-u…
Browse files Browse the repository at this point in the history 
…pdates

Feature/calculator inertia updates
  • Loading branch information
@james-p-foster
james-p-foster authored Jan 19, 2024
2 parents 1e608ba + a2ba51b commit 42d4660
Show file tree
Hide file tree
Showing 14 changed files with 901 additions and 198 deletions.
122 changes: 70 additions & 52 deletions src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,12 +8,14 @@
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.*;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.JointMatrixIndexProvider;
import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
import us.ihmc.mecano.multiBodySystem.interfaces.MultiBodySystemReadOnly;
import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly;
import us.ihmc.mecano.spatial.SpatialInertia;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly;
import us.ihmc.mecano.tools.JointStateType;
import us.ihmc.mecano.tools.MecanoFactories;
import us.ihmc.mecano.tools.MultiBodySystemTools;
Expand Down Expand Up @@ -214,7 +216,7 @@ protected void updateTransformToParent(RigidBodyTransform transformToParent)
recursionSteps = buildMultiBodyTree(initialRecursionStep, input.getJointsToIgnore()).toArray(new RecursionStep[0]);

if (considerIgnoredSubtreesInertia)
initialRecursionStep.includeIgnoredSubtreeInertia();
initialRecursionStep.updateIgnoredSubtreeInertia();

jacobianColumn = new FrameVector3D(this.jacobianFrame);

Expand Down Expand Up @@ -436,11 +438,15 @@ private class RecursionStep
*/
private final RigidBodyReadOnly rigidBody;
/**
* Body inertia: usually equal to {@code rigidBody.getInertial()}. However, if at least one child of
* {@code rigidBody} is ignored, it is equal to this rigid-body inertia and the subtree inertia
* attached to the ignored joint.
* Used for storing intermediate result when part of the subtree is ignored but the subtree inertia
* is still to be considered.
*/
private final SpatialInertia bodyInertia;
private SpatialInertia bodyInertia;
/**
* Combined inertia of the subtree that is ignored but inertia is still to be considered for this
* recursion step.
*/
private SpatialInertia bodySubtreeInertia;

/**
* The total mass of this rigid-body and all its descendant.
Expand Down Expand Up @@ -474,90 +480,97 @@ public RecursionStep(RigidBodyReadOnly rigidBody, int[] jointIndices)
this.jointIndices = jointIndices;

if (isRoot())
{
bodyInertia = null;
jacobianJointBlock = null;
}
else
{
bodyInertia = new SpatialInertia(rigidBody.getInertia());
jacobianJointBlock = new DMatrixRMaj(3, getJoint().getDegreesOfFreedom());
}
}

public void includeIgnoredSubtreeInertia()
private void updateIgnoredSubtreeInertia()
{
if (bodySubtreeInertia != null)
{
bodyInertia.setToZero();
bodySubtreeInertia.setToZero();
}

if (!isRoot() && children.size() != rigidBody.getChildrenJoints().size())
{
for (JointReadOnly childJoint : rigidBody.getChildrenJoints())
{
if (input.getJointsToIgnore().contains(childJoint))
{
SpatialInertia subtreeIneria = MultiBodySystemTools.computeSubtreeInertia(childJoint);
subtreeIneria.changeFrame(rigidBody.getBodyFixedFrame());
bodyInertia.add(subtreeIneria);
SpatialInertia subtreeInertia = MultiBodySystemTools.computeSubtreeInertia(childJoint);
subtreeInertia.changeFrame(getBodyFixedFrame());
if (bodySubtreeInertia == null)
{
bodyInertia = new SpatialInertia(getBodyFixedFrame(), getBodyFixedFrame());
bodySubtreeInertia = new SpatialInertia(getBodyFixedFrame(), getBodyFixedFrame());
}
bodySubtreeInertia.add(subtreeInertia);
}
}
}

for (int childIndex = 0; childIndex < children.size(); childIndex++)
children.get(childIndex).includeIgnoredSubtreeInertia();
children.get(childIndex).updateIgnoredSubtreeInertia();
}

/**
* First pass going from the leaves to the root.
* <p>
* Here the subtree mass and center of mass is computed for each rigid-body.
* Here the frame to perform operations in as well as the spatial inertia to use are determined, and then passed to an inner method
* {@link #passOneInner(ReferenceFrame, SpatialInertiaReadOnly)}. The spatial inertia to use can vary depending on whether some subtree joints of the
* given rigid body are being ignored, in which case the ignored subtree inertias are lumped in with the given rigid body's spatial inertia.
* </p>
*/
public void passOne()
{
ReferenceFrame frameToUse = isJacobianFrameAtCenterOfMass ? rootFrame : jacobianFrame;

if (isRoot())
if (bodyInertia != null)
{
// Update the total mass
subTreeMass = bodyInertia == null ? 0.0 : bodyInertia.getMass();
for (int i = 0; i < children.size(); i++)
subTreeMass += children.get(i).subTreeMass;
bodyInertia.setIncludingFrame(rigidBody.getInertia());
bodyInertia.add(bodySubtreeInertia);
passOneInner(frameToUse, bodyInertia);
}
else
{
passOneInner(frameToUse, rigidBody.getInertia());
}

// The centerOfMassTimesMass can be used to obtain the overall center of mass position.
if (bodyInertia == null)
{
centerOfMassTimesMass.setToZero(frameToUse);
}
else
{
centerOfMassTimesMass.setIncludingFrame(bodyInertia.getCenterOfMassOffset());
centerOfMassTimesMass.changeFrame(frameToUse);
centerOfMassTimesMass.scale(bodyInertia.getMass());
}
for (int i = 0; i < children.size(); i++)
centerOfMassTimesMass.add(children.get(i).centerOfMassTimesMass);

for (int i = 0; i < children.size(); i++)
{
centerOfMassTimesMass.add(children.get(i).centerOfMassTimesMass);
}
centerOfMass.setIncludingFrame(centerOfMassTimesMass);
centerOfMass.scale(1.0 / subTreeMass);
}

/**
* Inner logic of {@link #passOne()}.
* <p>
* Here the subtree mass and center of mass is computed for each rigid-body.
* </p>
* @param frameToUse the frame to use for subtree mass and center of mass calculations.
* @param inertiaToUse the spatial inertia for subtree mass and center of mass calculations.
*/
private void passOneInner(ReferenceFrame frameToUse, SpatialInertiaReadOnly inertiaToUse)
{
if (inertiaToUse == null)
{
// Update the total mass
subTreeMass = 0.0;
centerOfMassTimesMass.setToZero(frameToUse);
}
else
{
// Update the sub-tree mass
subTreeMass = bodyInertia.getMass();
for (int i = 0; i < children.size(); i++)
subTreeMass += children.get(i).subTreeMass;

// Update the sub-tree center of mass
centerOfMassTimesMass.setIncludingFrame(bodyInertia.getCenterOfMassOffset());
subTreeMass = inertiaToUse.getMass();
centerOfMassTimesMass.setIncludingFrame(inertiaToUse.getCenterOfMassOffset());
centerOfMassTimesMass.changeFrame(frameToUse);
centerOfMassTimesMass.scale(bodyInertia.getMass());

for (int i = 0; i < children.size(); i++)
{
centerOfMassTimesMass.add(children.get(i).centerOfMassTimesMass);
}
centerOfMassTimesMass.scale(inertiaToUse.getMass());
}

centerOfMass.setIncludingFrame(centerOfMassTimesMass);
centerOfMass.scale(1.0 / subTreeMass);
for (int i = 0; i < children.size(); i++)
subTreeMass += children.get(i).subTreeMass;
}

/**
Expand Down Expand Up @@ -619,5 +632,10 @@ public JointReadOnly getJoint()
{
return rigidBody.getParentJoint();
}

public MovingReferenceFrame getBodyFixedFrame()
{
return rigidBody.getBodyFixedFrame();
}
}
}
60 changes: 45 additions & 15 deletions src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -10,6 +10,7 @@
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
import us.ihmc.mecano.multiBodySystem.interfaces.JointMatrixIndexProvider;
import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
import us.ihmc.mecano.multiBodySystem.interfaces.MultiBodySystemReadOnly;
Expand Down Expand Up @@ -135,7 +136,7 @@ public CentroidalMomentumCalculator(MultiBodySystemReadOnly input, ReferenceFram
iterativeSteps = buildMultiBodyTree(initialIterativeStep, input.getJointsToIgnore()).toArray(new IterativeStep[0]);

if (considerIgnoredSubtreesInertia)
initialIterativeStep.includeIgnoredSubtreeInertia();
initialIterativeStep.updateIgnoredSubtreeInertia();

jointUnitTwist = new Twist();
unitMomentum = new Momentum(matrixFrame);
Expand Down Expand Up @@ -376,11 +377,15 @@ private class IterativeStep
*/
private final RigidBodyReadOnly rigidBody;
/**
* Body inertia: usually equal to {@code rigidBody.getInertial()}. However, if at least one child of
* {@code rigidBody} is ignored, it is equal to this rigid-body inertia and the subtree inertia
* attached to the ignored joint.
* Used for storing intermediate result when part of the subtree is ignored but the subtree inertia
* is still to be considered.
*/
private final SpatialInertia bodyInertia;
private SpatialInertia bodyInertia;
/**
* Combined inertia of the subtree that is ignored but inertia is still to be considered for this
* recursion step.
*/
private SpatialInertia bodySubtreeInertia;
/**
* Result of this recursion step: the matrix block of the centroidal momentum matrix for the parent
* joint.
Expand Down Expand Up @@ -408,35 +413,44 @@ public IterativeStep(RigidBodyReadOnly rigidBody, int[] jointIndices)

if (isRoot())
{
bodyInertia = null;
centroidalMomentumMatrixBlock = null;
matrixFrameToBodyFixedFrameTransform = null;
}
else
{
bodyInertia = new SpatialInertia(rigidBody.getInertia());
centroidalMomentumMatrixBlock = new DMatrixRMaj(6, getJoint().getDegreesOfFreedom());
matrixFrameToBodyFixedFrameTransform = new RigidBodyTransform();
}
}

public void includeIgnoredSubtreeInertia()
private void updateIgnoredSubtreeInertia()
{
if (bodySubtreeInertia != null)
{
bodyInertia.setToZero();
bodySubtreeInertia.setToZero();
}

if (!isRoot() && children.size() != rigidBody.getChildrenJoints().size())
{
for (JointReadOnly childJoint : rigidBody.getChildrenJoints())
{
if (input.getJointsToIgnore().contains(childJoint))
{
SpatialInertia subtreeIneria = MultiBodySystemTools.computeSubtreeInertia(childJoint);
subtreeIneria.changeFrame(rigidBody.getBodyFixedFrame());
bodyInertia.add(subtreeIneria);
SpatialInertia subtreeInertia = MultiBodySystemTools.computeSubtreeInertia(childJoint);
subtreeInertia.changeFrame(getBodyFixedFrame());
if (bodySubtreeInertia == null)
{
bodyInertia = new SpatialInertia(getBodyFixedFrame(), getBodyFixedFrame());
bodySubtreeInertia = new SpatialInertia(getBodyFixedFrame(), getBodyFixedFrame());
}
bodySubtreeInertia.add(subtreeInertia);
}
}
}

for (int childIndex = 0; childIndex < children.size(); childIndex++)
children.get(childIndex).includeIgnoredSubtreeInertia();
children.get(childIndex).updateIgnoredSubtreeInertia();
}

/**
Expand All @@ -450,7 +464,7 @@ public void passOne()
if (isRoot())
return;

ReferenceFrame inertiaFrame = bodyInertia.getReferenceFrame();
ReferenceFrame inertiaFrame = rigidBody.getInertia().getReferenceFrame();
matrixFrame.getTransformToDesiredFrame(matrixFrameToBodyFixedFrameTransform, inertiaFrame);
}

Expand Down Expand Up @@ -504,14 +518,25 @@ public void passTwo()
*/
private void addToUnitMomentumRecursively(TwistReadOnly ancestorUnitTwist, FixedFrameMomentumBasics unitMomentumToAddTo)
{
ReferenceFrame inertiaFrame = bodyInertia.getReferenceFrame();
ReferenceFrame inertiaFrame = rigidBody.getInertia().getReferenceFrame();

intermediateUnitTwist.setIncludingFrame(ancestorUnitTwist);
intermediateUnitTwist.applyTransform(matrixFrameToBodyFixedFrameTransform);
intermediateUnitTwist.setReferenceFrame(inertiaFrame);

intermediateMomentum.setReferenceFrame(inertiaFrame);
intermediateMomentum.compute(bodyInertia, intermediateUnitTwist);

if (bodyInertia != null)
{
bodyInertia.setIncludingFrame(rigidBody.getInertia());
bodyInertia.add(bodySubtreeInertia);
intermediateMomentum.compute(bodyInertia, intermediateUnitTwist);
}
else
{
intermediateMomentum.compute(rigidBody.getInertia(), intermediateUnitTwist);
}

intermediateMomentum.applyInverseTransform(matrixFrameToBodyFixedFrameTransform);
intermediateMomentum.setReferenceFrame(matrixFrame);

Expand All @@ -530,5 +555,10 @@ public JointReadOnly getJoint()
{
return rigidBody.getParentJoint();
}

public MovingReferenceFrame getBodyFixedFrame()
{
return rigidBody.getBodyFixedFrame();
}
}
}
Loading

0 comments on commit 42d4660

Please sign in to comment.