Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add the option to compute the full joint wrench. #13

Merged
merged 2 commits into from
Jan 10, 2024
Merged

Add the option to compute the full joint wrench. #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f6c9e4b
Add the option to compute the full joint wrench.
SylvainBertrand Jan 9, 2024
c0946a8
Reducing diff due to formatting.
SylvainBertrand Jan 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 9 additions & 27 deletions src/main/java/us/ihmc/mecano/algorithms/CenterOfMassJacobian.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,21 +1,12 @@
package us.ihmc.mecano.algorithms;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import org.ejml.data.DMatrix1Row;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

import us.ihmc.euclid.referenceFrame.FramePoint3D;
import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DBasics;
import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly;
import us.ihmc.euclid.referenceFrame.interfaces.ReferenceFrameHolder;
import us.ihmc.euclid.referenceFrame.interfaces.*;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.multiBodySystem.interfaces.JointMatrixIndexProvider;
import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly;
Expand All @@ -27,6 +18,10 @@
import us.ihmc.mecano.tools.MecanoFactories;
import us.ihmc.mecano.tools.MultiBodySystemTools;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

/**
* Computes the center of mass Jacobian that maps from joint velocity space to center of mass
* Cartesian velocity space.
Expand Down Expand Up @@ -190,7 +185,9 @@ public CenterOfMassJacobian(MultiBodySystemReadOnly input, ReferenceFrame jacobi
this(input, jacobianFrame, null, considerIgnoredSubtreesInertia);
}

private CenterOfMassJacobian(MultiBodySystemReadOnly input, ReferenceFrame jacobianFrame, String centerOfMassFrameName,
private CenterOfMassJacobian(MultiBodySystemReadOnly input,
ReferenceFrame jacobianFrame,
String centerOfMassFrameName,
boolean considerIgnoredSubtreesInertia)
{
this.input = input;
Expand Down Expand Up @@ -231,22 +228,7 @@ private List<RecursionStep> buildMultiBodyTree(RecursionStep parent, Collection<
List<RecursionStep> recursionSteps = new ArrayList<>();
recursionSteps.add(parent);

List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());

if (childrenJoints.size() > 1)
{ // Reorganize the joints in the children to ensure that loop closures are treated last.
List<JointReadOnly> loopClosureAncestors = new ArrayList<>();

for (int i = 0; i < childrenJoints.size();)
{
if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
loopClosureAncestors.add(childrenJoints.remove(i));
else
i++;
}

childrenJoints.addAll(loopClosureAncestors);
}
List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);

for (JointReadOnly childJoint : childrenJoints)
{
Expand Down
26 changes: 5 additions & 21 deletions src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,13 +1,8 @@
package us.ihmc.mecano.algorithms;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import org.ejml.data.DMatrix1Row;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
Expand All @@ -29,6 +24,10 @@
import us.ihmc.mecano.tools.JointStateType;
import us.ihmc.mecano.tools.MultiBodySystemTools;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

/**
* Computes the centroidal momentum matrix that maps from joint velocity space to the system linear
* and angular momentum.
Expand Down Expand Up @@ -156,22 +155,7 @@ private List<IterativeStep> buildMultiBodyTree(IterativeStep parent, Collection<
List<IterativeStep> iterativeSteps = new ArrayList<>();
iterativeSteps.add(parent);

List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());

if (childrenJoints.size() > 1)
{ // Reorganize the joints in the children to ensure that loop closures are treated last.
List<JointReadOnly> loopClosureAncestors = new ArrayList<>();

for (int i = 0; i < childrenJoints.size();)
{
if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
loopClosureAncestors.add(childrenJoints.remove(i));
else
i++;
}

childrenJoints.addAll(loopClosureAncestors);
}
List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);

for (JointReadOnly childJoint : childrenJoints)
{
Expand Down
59 changes: 13 additions & 46 deletions src/main/java/us/ihmc/mecano/algorithms/CentroidalMomentumRateCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,17 +1,8 @@
package us.ihmc.mecano.algorithms;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.Collection;
import java.util.List;
import java.util.Map;
import java.util.function.Function;
import java.util.stream.Collectors;

import org.ejml.data.DMatrix1Row;
import org.ejml.data.DMatrixRMaj;
import org.ejml.dense.row.CommonOps_DDRM;

import us.ihmc.euclid.referenceFrame.FrameVector3D;
import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics;
Expand All @@ -24,24 +15,16 @@
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.Momentum;
import us.ihmc.mecano.spatial.SpatialAcceleration;
import us.ihmc.mecano.spatial.SpatialForce;
import us.ihmc.mecano.spatial.SpatialInertia;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.Wrench;
import us.ihmc.mecano.spatial.interfaces.FixedFrameMomentumBasics;
import us.ihmc.mecano.spatial.interfaces.FixedFrameSpatialForceBasics;
import us.ihmc.mecano.spatial.interfaces.MomentumBasics;
import us.ihmc.mecano.spatial.interfaces.MomentumReadOnly;
import us.ihmc.mecano.spatial.interfaces.SpatialForceBasics;
import us.ihmc.mecano.spatial.interfaces.SpatialForceReadOnly;
import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly;
import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
import us.ihmc.mecano.spatial.*;
import us.ihmc.mecano.spatial.interfaces.*;
import us.ihmc.mecano.tools.JointStateType;
import us.ihmc.mecano.tools.MecanoTools;
import us.ihmc.mecano.tools.MultiBodySystemTools;

import java.util.*;
import java.util.function.Function;
import java.util.stream.Collectors;

/**
* Computes the centroidal momentum matrix that maps from joint velocity space to the system linear
* and angular momentum and the convective term representing the Coriolis and centrifugal wrenches
Expand Down Expand Up @@ -219,22 +202,7 @@ private List<RecursionStep> buildMultiBodyTree(RecursionStep parent, Collection<
List<RecursionStep> recursionSteps = new ArrayList<>();
recursionSteps.add(parent);

List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());

if (childrenJoints.size() > 1)
{ // Reorganize the joints in the children to ensure that loop closures are treated last.
List<JointReadOnly> loopClosureAncestors = new ArrayList<>();

for (int i = 0; i < childrenJoints.size();)
{
if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
loopClosureAncestors.add(childrenJoints.remove(i));
else
i++;
}

childrenJoints.addAll(loopClosureAncestors);
}
List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);

for (JointReadOnly childJoint : childrenJoints)
{
Expand Down Expand Up @@ -544,11 +512,11 @@ public SpatialForceReadOnly getBiasSpatialForce()

/**
* Computes the convective term while considering only a subset of the multi-body system.
*
*
* @param jointSelection the only joints that are considered.
* @param biasSpatialForceToPack the vector used to store the result. Modified.
* @return {@code true} if the convective term was successfully computed, {@code false} if not all
* the joints could be found and the result is inaccurate.
* the joints could be found and the result is inaccurate.
*/
public boolean getBiasSpatialForceMatrix(List<? extends JointReadOnly> jointSelection, SpatialForceBasics biasSpatialForceToPack)
{
Expand Down Expand Up @@ -586,11 +554,11 @@ public DMatrixRMaj getBiasSpatialForceMatrix()

/**
* Computes the convective term while considering only a subset of the multi-body system.
*
* @param jointSelection the only joints that are considered.
* @param biasSpatialForceToPack the matrix used to store the result. Modified.
*
* @param jointSelection the only joints that are considered.
* @param biasSpatialForceMatrixToPack the matrix used to store the result. Modified.
* @return {@code true} if the convective term was successfully computed, {@code false} if not all
* the joints could be found and the result is inaccurate.
* the joints could be found and the result is inaccurate.
*/
public boolean getBiasSpatialForceMatrix(List<? extends JointReadOnly> jointSelection, DMatrixRMaj biasSpatialForceMatrixToPack)
{
Expand Down Expand Up @@ -762,7 +730,6 @@ public void passTwo()
int column = jointIndices[dofIndex];
CommonOps_DDRM.extract(centroidalMomentumMatrixBlock, 0, 6, dofIndex, dofIndex + 1, centroidalMomentumMatrix, 0, column);
}

}

/**
Expand Down
39 changes: 9 additions & 30 deletions src/main/java/us/ihmc/mecano/algorithms/CompositeRigidBodyMassMatrixCalculator.java
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,29 +1,23 @@
package us.ihmc.mecano.algorithms;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

import org.ejml.data.DMatrix;
import org.ejml.data.DMatrixRMaj;

import us.ihmc.euclid.referenceFrame.ReferenceFrame;
import us.ihmc.euclid.transform.RigidBodyTransform;
import us.ihmc.mecano.frames.MovingReferenceFrame;
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.Momentum;
import us.ihmc.mecano.spatial.SpatialAcceleration;
import us.ihmc.mecano.spatial.SpatialForce;
import us.ihmc.mecano.spatial.SpatialInertia;
import us.ihmc.mecano.spatial.Twist;
import us.ihmc.mecano.spatial.Wrench;
import us.ihmc.mecano.spatial.*;
import us.ihmc.mecano.spatial.interfaces.SpatialForceReadOnly;
import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly;
import us.ihmc.mecano.spatial.interfaces.TwistReadOnly;
import us.ihmc.mecano.tools.MultiBodySystemTools;

import java.util.ArrayList;
import java.util.Collection;
import java.util.List;

/**
* This calculator computes:
* <ul>
Expand Down Expand Up @@ -242,22 +236,7 @@ private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyIne
{
List<CompositeRigidBodyInertia> inertiaList = new ArrayList<>();

List<JointReadOnly> childrenJoints = new ArrayList<>(parent.rigidBody.getChildrenJoints());

if (childrenJoints.size() > 1)
{ // Reorganize the joints in the children to ensure that loop closures are treated last.
List<JointReadOnly> loopClosureAncestors = new ArrayList<>();

for (int i = 0; i < childrenJoints.size();)
{
if (MultiBodySystemTools.doesSubtreeContainLoopClosure(childrenJoints.get(i).getSuccessor()))
loopClosureAncestors.add(childrenJoints.remove(i));
else
i++;
}

childrenJoints.addAll(loopClosureAncestors);
}
List<JointReadOnly> childrenJoints = MultiBodySystemTools.sortLoopClosureInChildrenJoints(parent.rigidBody);

for (JointReadOnly childJoint : childrenJoints)
{
Expand Down Expand Up @@ -292,7 +271,7 @@ private List<CompositeRigidBodyInertia> buildMultiBodyTree(CompositeRigidBodyIne
* Note that enabling the calculation of the Coriolis and centrifugal matrix will increase the
* computational load when updating the mass matrix.
* </p>
*
*
* @param enableCoriolisMatrixCalculation whether to enable or disable the calculation of the
* Coriolis and centrifugal matrix. Disabled by default.
*/
Expand Down Expand Up @@ -370,10 +349,10 @@ public DMatrixRMaj getMassMatrix()

/**
* Gets the Coriolis and centrifugal matrix for this multi-body system.
*
*
* @return the Coriolis and centrifugal matrix.
* @throws UnsupportedOperationException if the calculation of the Coriolis and centrifugal matrix
* was not enabled.
* was not enabled.
* @see #setEnableCoriolisMatrixCalculation(boolean)
*/
public DMatrixRMaj getCoriolisMatrix()
Expand Down
Loading