From 73c826f7edabd34d920e6abcac365df8086dd54e Mon Sep 17 00:00:00 2001 From: Operator Computer Date: Thu, 7 Sep 2017 16:06:05 -0500 Subject: [PATCH 01/27] Renamed subsprojects to kebab-case. Moved all src and test directories in their respective maven directory structure --- .../filters/RateLimitedYoSpatialVector.java | 81 +++++++ .../DifferentialIDMassMatrixCalculator.java | 129 +++++++++++ ...amicallyConsistentNullspaceCalculator.java | 22 ++ ...oatingBaseRigidBodyDynamicsCalculator.java | 137 ++++++++++++ ...oriolisExternalWrenchMatrixCalculator.java | 209 ++++++++++++++++++ .../screwTheory/MassMatrixCalculator.java | 15 ++ .../screwTheory/MomentumCalculator.java | 36 +++ 7 files changed, 629 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java new file mode 100644 index 0000000..bc8e7bc --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java @@ -0,0 +1,81 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.robotics.math.frames.YoFrameVector; +import us.ihmc.robotics.math.frames.YoSpatialVector; + +public class RateLimitedYoSpatialVector extends YoSpatialVector +{ + private final RateLimitedYoFrameVector rateLimitedLinearPart; + private final RateLimitedYoFrameVector rateLimitedAngularPart; + + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maximumLinearRate, + YoDouble maximumAngularRate, double dt, YoFrameVector rawLinearPart, YoFrameVector rawAngularPart) + { + super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart), + new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart)); + this.rateLimitedLinearPart = (RateLimitedYoFrameVector) linearPart; + this.rateLimitedAngularPart = (RateLimitedYoFrameVector) angularPart; + } + + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maximumLinearRate, + YoDouble maximumAngularRate, double dt, YoSpatialVector rawSpatialVector) + { + super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getYoLinearPart()), + new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getYoAngularPart())); + this.rateLimitedLinearPart = (RateLimitedYoFrameVector) linearPart; + this.rateLimitedAngularPart = (RateLimitedYoFrameVector) angularPart; + } + + public RateLimitedYoSpatialVector(RateLimitedYoFrameVector yoLinearPart, RateLimitedYoFrameVector yoAngularPart) + { + super(yoLinearPart, yoAngularPart); + this.rateLimitedLinearPart = yoLinearPart; + this.rateLimitedAngularPart = yoAngularPart; + } + + public void setMaxRate(double maximumLinearRate, double maximumAngularRate) + { + rateLimitedLinearPart.setMaxRate(maximumLinearRate); + rateLimitedAngularPart.setMaxRate(maximumAngularRate); + } + + public void reset() + { + rateLimitedLinearPart.reset(); + rateLimitedAngularPart.reset(); + } + + public void update() + { + rateLimitedLinearPart.update(); + rateLimitedAngularPart.update(); + } + + public void update(YoFrameVector rawLinearPart, YoFrameVector rawAngularPart) + { + rateLimitedLinearPart.update(rawLinearPart); + rateLimitedAngularPart.update(rawAngularPart); + } + + public void update(FrameVector3D rawLinearPart, FrameVector3D rawAngularPart) + { + rateLimitedLinearPart.update(rawLinearPart); + rateLimitedAngularPart.update(rawAngularPart); + } + + public void update(Vector3DReadOnly rawLinearPart, Vector3DReadOnly rawAngularPart) + { + rateLimitedLinearPart.update(rawLinearPart); + rateLimitedAngularPart.update(rawAngularPart); + } + + public void update(double rawLinearX, double rawLinearY, double rawLinearZ, double rawAngularX, double rawAngularY, double rawAngularZ) + { + rateLimitedLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); + rateLimitedAngularPart.update(rawAngularX, rawAngularY, rawAngularZ); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java new file mode 100644 index 0000000..83aa834 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -0,0 +1,129 @@ +package us.ihmc.robotics.screwTheory; + +import java.util.ArrayList; +import java.util.LinkedHashMap; + +import org.ejml.data.DenseMatrix64F; +import org.ejml.ops.CommonOps; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.robotics.linearAlgebra.MatrixTools; + +/** + * Very inefficient. Should only be used for verification of better methods in unit tests + */ +public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator +{ + private final InverseDynamicsCalculator idCalculator; + private final InverseDynamicsJoint[] jointsInOrder; + private final DenseMatrix64F massMatrix; + private final DenseMatrix64F storedJointDesiredAccelerations; + private final DenseMatrix64F tmpDesiredJointAccelerationsMatrix; + private final DenseMatrix64F tmpTauMatrix; + private final LinkedHashMap storedJointWrenches = new LinkedHashMap(); + private final DenseMatrix64F storedJointVelocities; + + private final int totalNumberOfDoFs; + + public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) + { + LinkedHashMap zeroExternalWrench = new LinkedHashMap(); + ArrayList zeroJointToIgnore = new ArrayList(); + SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); + + idCalculator = new InverseDynamicsCalculator(rootBody, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true); + jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); + totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); + massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); + + storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); + storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); + tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); + tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); + + for (InverseDynamicsJoint joint : jointsInOrder) + { + ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); + Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); + storedJointWrenches.put(joint, jointWrench); + } + } + + @Override + public void compute() + { + storeJointState(); + setDesiredAccelerationsToZero(); + + int column = 0; + + for (int i = 0 ; i < totalNumberOfDoFs; i++) + { + tmpDesiredJointAccelerationsMatrix.set(i, 0, 1.0); + ScrewTools.setDesiredAccelerations(jointsInOrder, tmpDesiredJointAccelerationsMatrix); + + idCalculator.compute(); + tmpTauMatrix.set(ScrewTools.getTauMatrix(jointsInOrder)); + MatrixTools.setMatrixBlock(massMatrix, 0, column, tmpTauMatrix, 0, 0, totalNumberOfDoFs, 1, 1.0); + column++; + + tmpDesiredJointAccelerationsMatrix.set(i, 0, 0.0); + } + + restoreJointState(); + } + + private void setDesiredAccelerationsToZero() + { + for (InverseDynamicsJoint joint : jointsInOrder) + { + joint.setDesiredAccelerationToZero(); + joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0); + } + } + + private void storeJointState() + { + ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); + ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); + for (InverseDynamicsJoint joint : jointsInOrder) + { + DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); + joint.getTauMatrix(tauMatrix); + DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); + CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); + Wrench jointWrench = storedJointWrenches.get(joint); + jointWrench.set(joint.getFrameAfterJoint(), spatialForce); + jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); + } + } + + private void restoreJointState() + { + ScrewTools.setDesiredAccelerations(jointsInOrder, storedJointDesiredAccelerations); + ScrewTools.setVelocities(jointsInOrder, storedJointVelocities); + + for (InverseDynamicsJoint joint : jointsInOrder) + { + joint.setTorqueFromWrench(storedJointWrenches.get(joint)); + } + } + + @Override + public DenseMatrix64F getMassMatrix() + { + return massMatrix; + } + + @Override + public void getMassMatrix(DenseMatrix64F massMatrixToPack) + { + massMatrixToPack.set(massMatrix); + } + + @Override + public InverseDynamicsJoint[] getJointsInOrder() + { + return jointsInOrder; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java new file mode 100644 index 0000000..7570a14 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java @@ -0,0 +1,22 @@ +package us.ihmc.robotics.screwTheory; + +import org.ejml.data.DenseMatrix64F; + +/** + * @author twan + * Date: 4/17/13 + */ +public interface DynamicallyConsistentNullspaceCalculator +{ + void reset(); + + void addConstraint(RigidBody body, DenseMatrix64F selectionMatrix); + + void addActuatedJoint(InverseDynamicsJoint joint); + + void compute(); + + DenseMatrix64F getDynamicallyConsistentNullspace(); + + DenseMatrix64F getSNsBar(); +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java new file mode 100644 index 0000000..1aa72d6 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java @@ -0,0 +1,137 @@ +package us.ihmc.robotics.screwTheory; + +import org.ejml.data.DenseMatrix64F; +import org.ejml.factory.LinearSolverFactory; +import org.ejml.interfaces.linsol.LinearSolver; +import org.ejml.ops.CommonOps; +import us.ihmc.robotics.linearAlgebra.MatrixTools; + +public class FloatingBaseRigidBodyDynamicsCalculator +{ + private static final int large = 1000; + private static final double tolerance = 0.0001; + + private static final LinearSolver pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); + + private final DenseMatrix64F matrixTranspose = new DenseMatrix64F(large, large); + private final DenseMatrix64F localVector = new DenseMatrix64F(large, 1); + + public FloatingBaseRigidBodyDynamicsCalculator() + {} + + + public void computeQddotGivenRho(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, + DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F qddotToPack, DenseMatrix64F rho) + { + computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); + + CommonOps.scale(-1.0, floatingBaseCoriolisMatrix); + CommonOps.multAdd(matrixTranspose, rho, floatingBaseCoriolisMatrix); + + pseudoInverseSolver.setA(floatingBaseMassMatrix); + pseudoInverseSolver.solve(floatingBaseCoriolisMatrix, qddotToPack); + } + + public void computeRhoGivenQddot(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, + DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F rhoToPack) + { + computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); + + CommonOps.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); + + pseudoInverseSolver.setA(matrixTranspose); + pseudoInverseSolver.solve(floatingBaseCoriolisMatrix, rhoToPack); + } + + public void computeTauGivenRhoAndQddot(DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, + DenseMatrix64F qddot, DenseMatrix64F rho, DenseMatrix64F tauToPack) + { + computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); + + CommonOps.mult(bodyMassMatrix, qddot, tauToPack); + CommonOps.multAdd(-1.0, matrixTranspose, rho, tauToPack); + CommonOps.addEquals(tauToPack, bodyCoriolisMatrix); + } + + public void computeTauGivenRho(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, DenseMatrix64F floatingBaseContactForceJacobianMatrix, + DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F rho, DenseMatrix64F tauToPack) + { + computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); + + localVector.reshape(bodyMassMatrix.getNumCols(), 1); + computeQddotGivenRho(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, localVector, rho); + computeTauGivenRhoAndQddot(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, localVector, rho, tauToPack); + } + + public void computeTauGivenQddot(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, DenseMatrix64F floatingBaseContactForceJacobianMatrix, + DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F tauToPack) + { + computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); + + localVector.reshape(bodyContactForceJacobianMatrix.getNumRows(), 1); + computeRhoGivenQddot(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, qddot, localVector); + computeTauGivenRhoAndQddot(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, qddot, localVector, tauToPack); + } + + public boolean areFloatingBaseRigidBodyDynamicsSatisfied(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, + DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, + DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F tau, DenseMatrix64F rho) + { + if (!areFloatingBaseDynamicsSatisfied(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, qddot, rho)) + return false; + + return areRigidBodyDynamicsSatisfied(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, qddot, tau, rho); + } + + public boolean areFloatingBaseDynamicsSatisfied(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, + DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F rho) + { + computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); + + CommonOps.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); + CommonOps.multAdd(-1.0, matrixTranspose, rho, floatingBaseCoriolisMatrix); + + return equalsZero(floatingBaseCoriolisMatrix, tolerance); + } + + public boolean areRigidBodyDynamicsSatisfied(DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, + DenseMatrix64F qddot, DenseMatrix64F tau, DenseMatrix64F rho) + { + computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); + + CommonOps.multAdd(bodyMassMatrix, qddot, bodyCoriolisMatrix); + CommonOps.multAdd(-1.0, matrixTranspose, rho, bodyCoriolisMatrix); + CommonOps.subtractEquals(bodyCoriolisMatrix, tau); + + return equalsZero(bodyCoriolisMatrix, tolerance); + } + + private void computeJacobianTranspose(DenseMatrix64F jacobian, DenseMatrix64F jacobianTransposeToPack) + { + jacobianTransposeToPack.reshape(jacobian.getNumCols(), jacobian.getNumRows()); + jacobianTransposeToPack.zero(); + CommonOps.transpose(jacobian, jacobianTransposeToPack); + } + + private static boolean equalsZero(DenseMatrix64F matrix, double tolerance) + { + for (int rowIndex = 0; rowIndex < matrix.getNumRows(); rowIndex++) + { + for (int colIndex = 0; colIndex < matrix.getNumCols(); colIndex++) + { + if (!equals(matrix.get(rowIndex, colIndex), 0.0, tolerance)) + return false; + } + } + + return true; + } + + private static boolean equals(double a, double b, double tolerance) + { + if (Math.abs(a - b) > tolerance) + return false; + + return true; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java new file mode 100644 index 0000000..77fe145 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -0,0 +1,209 @@ +package us.ihmc.robotics.screwTheory; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.LinkedHashMap; +import java.util.List; + +import org.ejml.data.DenseMatrix64F; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; + +public class GravityCoriolisExternalWrenchMatrixCalculator +{ + private final RigidBody rootBody; + + private final ArrayList jointsToIgnore; + private final ArrayList allJoints = new ArrayList<>(); + private final ArrayList allBodiesExceptRoot = new ArrayList<>(); + private final ArrayList listOfBodiesWithExternalWrenches = new ArrayList<>(); + + private final LinkedHashMap externalWrenches; + private final LinkedHashMap netWrenches = new LinkedHashMap<>(); + private final LinkedHashMap jointWrenches = new LinkedHashMap<>(); + private final LinkedHashMap coriolisWrenches = new LinkedHashMap<>(); + + private final SpatialAccelerationCalculator spatialAccelerationCalculator; + + private final SpatialAccelerationVector tempAcceleration = new SpatialAccelerationVector(); + private final Twist tempTwist = new Twist(); + + private final boolean doVelocityTerms; + + private static final boolean DEFAULT_DO_VELOCITY_TERMS = true; + private static final boolean DO_ACCELERATION_TERMS = false; + private static final boolean USE_DESIRED_ACCELERATIONS = true; + + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBody body, ArrayList jointsToIgnore, double gravity) + { + this(body,ScrewTools.createGravitationalSpatialAcceleration(ScrewTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, + DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); + } + + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBody body, SpatialAccelerationVector rootAcceleration, HashMap externalWrenches, + ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) + { + this(externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms, + USE_DESIRED_ACCELERATIONS)); + } + + //// TODO: 12/31/16 remove explicit dependency on the spatial acceleration calculator + public GravityCoriolisExternalWrenchMatrixCalculator(HashMap externalWrenches, List jointsToIgnore, + SpatialAccelerationCalculator spatialAccelerationCalculator) + { + this.rootBody = spatialAccelerationCalculator.getRootBody(); + this.externalWrenches = new LinkedHashMap<>(externalWrenches); + this.jointsToIgnore = new ArrayList<>(jointsToIgnore); + this.spatialAccelerationCalculator = spatialAccelerationCalculator; + + this.doVelocityTerms = spatialAccelerationCalculator.areVelocitiesConsidered(); + + populateMapsAndLists(); + } + + public void setRootAcceleration(SpatialAccelerationVector newRootAcceleration) + { + spatialAccelerationCalculator.setRootAcceleration(newRootAcceleration); + } + + public void compute() + { + computeTwistsAndSpatialAccelerations(); + computeNetWrenches(); + computeJointWrenchesAndTorques(); + } + + public void setExternalWrench(RigidBody rigidBody, Wrench externalWrench) + { + externalWrenches.get(rigidBody).set(externalWrench); + } + + public SpatialAccelerationCalculator getSpatialAccelerationCalculator() + { + return spatialAccelerationCalculator; + } + + private void computeTwistsAndSpatialAccelerations() + { + spatialAccelerationCalculator.compute(); + } + + private void computeNetWrenches() + { + for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) + { + RigidBody body = allBodiesExceptRoot.get(bodyIndex); + Wrench netWrench = netWrenches.get(body); + body.getBodyFixedFrame().getTwistOfFrame(tempTwist); + if (!doVelocityTerms) + tempTwist.setToZero(); + spatialAccelerationCalculator.getAccelerationOfBody(body, tempAcceleration); + body.getInertia().computeDynamicWrenchInBodyCoordinates(tempAcceleration, tempTwist, netWrench); + } + } + + private final Wrench wrenchExertedByChild = new Wrench(); + private void computeJointWrenchesAndTorques() + { + for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) + { + InverseDynamicsJoint joint = allJoints.get(jointIndex); + + RigidBody successor = joint.getSuccessor(); + + Wrench jointWrench = jointWrenches.get(joint); + jointWrench.set(netWrenches.get(successor)); + + Wrench externalWrench = externalWrenches.get(successor); + jointWrench.sub(externalWrench); + + List childrenJoints = successor.getChildrenJoints(); + + for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) + { + InverseDynamicsJoint child = childrenJoints.get(childIndex); + if (!jointsToIgnore.contains(child)) + { + Wrench wrenchExertedOnChild = jointWrenches.get(child); + ReferenceFrame successorFrame = successor.getBodyFixedFrame(); + + wrenchExertedByChild.set(wrenchExertedOnChild); + wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame); + wrenchExertedByChild.scale(-1.0); // Action = -reaction + wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame()); + jointWrench.sub(wrenchExertedByChild); + } + } + + joint.setTorqueFromWrench(jointWrench); + + DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); + coriolisWrench.zero(); + joint.getTauMatrix(coriolisWrench); + } + } + + private void populateMapsAndLists() + { + ArrayList morgue = new ArrayList(); + morgue.add(rootBody); + + while (!morgue.isEmpty()) + { + RigidBody currentBody = morgue.get(0); + + ReferenceFrame bodyFixedFrame = currentBody.getBodyFixedFrame(); + + if (!currentBody.isRootBody()) + { + allBodiesExceptRoot.add(currentBody); + netWrenches.put(currentBody, new Wrench(bodyFixedFrame, bodyFixedFrame)); + if (externalWrenches.get(currentBody) == null) + { + listOfBodiesWithExternalWrenches.add(currentBody); + externalWrenches.put(currentBody, new Wrench(bodyFixedFrame, bodyFixedFrame)); + } + } + + if (currentBody.hasChildrenJoints()) + { + List childrenJoints = currentBody.getChildrenJoints(); + for (InverseDynamicsJoint joint : childrenJoints) + { + if (!jointsToIgnore.contains(joint)) + { + RigidBody successor = joint.getSuccessor(); + if (successor != null) + { + if (allBodiesExceptRoot.contains(successor)) + { + throw new RuntimeException("This algorithm doesn't do loops."); + } + + allJoints.add(joint); + jointWrenches.put(joint, new Wrench()); + coriolisWrenches.put(joint, new DenseMatrix64F(joint.getDegreesOfFreedom(), 1)); + morgue.add(successor); + } + } + } + } + + morgue.remove(currentBody); + } + } + + public void reset() + { + for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) + { + Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); + externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getExpressedInFrame()); + } + } + + public void getJointCoriolisMatrix(InverseDynamicsJoint joint, DenseMatrix64F jointCoriolisMatrixToPack) + { + jointCoriolisMatrixToPack.set(coriolisWrenches.get(joint)); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java new file mode 100644 index 0000000..d97cc3c --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java @@ -0,0 +1,15 @@ +package us.ihmc.robotics.screwTheory; + +import org.ejml.data.DenseMatrix64F; + +public interface MassMatrixCalculator +{ + + public abstract void compute(); + + public abstract DenseMatrix64F getMassMatrix(); + + public abstract void getMassMatrix(DenseMatrix64F massMatrixToPack); + + public abstract InverseDynamicsJoint[] getJointsInOrder(); +} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java new file mode 100644 index 0000000..a08a9fe --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java @@ -0,0 +1,36 @@ +package us.ihmc.robotics.screwTheory; + +import us.ihmc.euclid.tuple3D.Vector3D; + +public class MomentumCalculator +{ + private final Twist tempTwist = new Twist(); + private final Momentum tempMomentum = new Momentum(); + private final Vector3D zero = new Vector3D(); + private final RigidBody[] rigidBodiesInOrders; + + public MomentumCalculator(RigidBody... rigidBodies) + { + rigidBodiesInOrders = rigidBodies; + } + + public MomentumCalculator(RigidBody rootBody) + { + this(ScrewTools.computeSupportAndSubtreeSuccessors(rootBody)); + } + + public void computeAndPack(Momentum momentum) + { + momentum.setAngularPart(zero); + momentum.setLinearPart(zero); + + for (RigidBody rigidBody : rigidBodiesInOrders) + { + RigidBodyInertia inertia = rigidBody.getInertia(); + rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist); + tempMomentum.compute(inertia, tempTwist); + tempMomentum.changeFrame(momentum.getExpressedInFrame()); + momentum.add(tempMomentum); + } + } +} From 4a97e250554d327d9d59b8b34922c4b957d60b38 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 30 Jan 2018 15:59:07 -0600 Subject: [PATCH 02/27] Minor in cleanup the InverseDynamicsCalculator. Removed the useless externalWrenches argument in some of the constructors. Switched to use super types for some of the fields. --- .../screwTheory/DifferentialIDMassMatrixCalculator.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index 83aa834..cc2cdd7 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -27,11 +27,10 @@ public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { - LinkedHashMap zeroExternalWrench = new LinkedHashMap(); ArrayList zeroJointToIgnore = new ArrayList(); SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); - idCalculator = new InverseDynamicsCalculator(rootBody, zeroRootAcceleration, zeroExternalWrench, zeroJointToIgnore, false, true); + idCalculator = new InverseDynamicsCalculator(rootBody, zeroRootAcceleration, zeroJointToIgnore, false, true); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); From c9ac4942326ff7e6b922f389bf54b461a0f1eef9 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 31 Jan 2018 18:29:11 -0600 Subject: [PATCH 03/27] Cleaned up some the rate limited yo-variables. Also changed RateLimitedYoFramePose to use a YoFrameQuaternion instead of a YoFrameOrientation. Implemented new option for adding filters on error velocity. --- .../filters/AlphaFilteredYoSpatialVector.java | 68 +++++++++++++++++++ .../filters/RateLimitedYoSpatialVector.java | 29 +++----- 2 files changed, 76 insertions(+), 21 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java new file mode 100644 index 0000000..952f4f0 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java @@ -0,0 +1,68 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.robotics.math.frames.YoSpatialVector; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; + +public class AlphaFilteredYoSpatialVector extends YoSpatialVector +{ + private final AlphaFilteredYoFrameVector alphaFilteredLinearPart; + private final AlphaFilteredYoFrameVector alphaFilteredAngularPart; + + public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaLinearPart, + DoubleProvider alphaAngularPart, FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) + { + super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawLinearPart), + new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawAngularPart)); + this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) linearPart; + this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) angularPart; + } + + public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaLinearPart, + DoubleProvider alphaAngularPart, YoSpatialVector rawSpatialVector) + { + super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawSpatialVector.getYoLinearPart()), + new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawSpatialVector.getYoAngularPart())); + this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) linearPart; + this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) angularPart; + } + + public AlphaFilteredYoSpatialVector(AlphaFilteredYoFrameVector yoLinearPart, AlphaFilteredYoFrameVector yoAngularPart) + { + super(yoLinearPart, yoAngularPart); + this.alphaFilteredLinearPart = yoLinearPart; + this.alphaFilteredAngularPart = yoAngularPart; + } + + public void reset() + { + alphaFilteredLinearPart.reset(); + alphaFilteredAngularPart.reset(); + } + + public void update() + { + alphaFilteredLinearPart.update(); + alphaFilteredAngularPart.update(); + } + + public void update(FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) + { + alphaFilteredLinearPart.update(rawLinearPart); + alphaFilteredAngularPart.update(rawAngularPart); + } + + public void update(Vector3DReadOnly rawLinearPart, Vector3DReadOnly rawAngularPart) + { + alphaFilteredLinearPart.update(rawLinearPart); + alphaFilteredAngularPart.update(rawAngularPart); + } + + public void update(double rawLinearX, double rawLinearY, double rawLinearZ, double rawAngularX, double rawAngularY, double rawAngularZ) + { + alphaFilteredLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); + alphaFilteredAngularPart.update(rawAngularX, rawAngularY, rawAngularZ); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java index bc8e7bc..202a9df 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java @@ -1,19 +1,18 @@ package us.ihmc.robotics.math.filters; -import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.robotics.math.frames.YoFrameVector; import us.ihmc.robotics.math.frames.YoSpatialVector; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; public class RateLimitedYoSpatialVector extends YoSpatialVector { private final RateLimitedYoFrameVector rateLimitedLinearPart; private final RateLimitedYoFrameVector rateLimitedAngularPart; - public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maximumLinearRate, - YoDouble maximumAngularRate, double dt, YoFrameVector rawLinearPart, YoFrameVector rawAngularPart) + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumLinearRate, + DoubleProvider maximumAngularRate, double dt, FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) { super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart), new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart)); @@ -21,8 +20,8 @@ public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariab this.rateLimitedAngularPart = (RateLimitedYoFrameVector) angularPart; } - public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, YoDouble maximumLinearRate, - YoDouble maximumAngularRate, double dt, YoSpatialVector rawSpatialVector) + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumLinearRate, + DoubleProvider maximumAngularRate, double dt, YoSpatialVector rawSpatialVector) { super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getYoLinearPart()), new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getYoAngularPart())); @@ -37,12 +36,6 @@ public RateLimitedYoSpatialVector(RateLimitedYoFrameVector yoLinearPart, RateLim this.rateLimitedAngularPart = yoAngularPart; } - public void setMaxRate(double maximumLinearRate, double maximumAngularRate) - { - rateLimitedLinearPart.setMaxRate(maximumLinearRate); - rateLimitedAngularPart.setMaxRate(maximumAngularRate); - } - public void reset() { rateLimitedLinearPart.reset(); @@ -55,13 +48,7 @@ public void update() rateLimitedAngularPart.update(); } - public void update(YoFrameVector rawLinearPart, YoFrameVector rawAngularPart) - { - rateLimitedLinearPart.update(rawLinearPart); - rateLimitedAngularPart.update(rawAngularPart); - } - - public void update(FrameVector3D rawLinearPart, FrameVector3D rawAngularPart) + public void update(FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) { rateLimitedLinearPart.update(rawLinearPart); rateLimitedAngularPart.update(rawAngularPart); From 24a9709d5ad72332bf9f9cacef3d8e431655ec1a Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Fri, 2 Nov 2018 15:45:45 -0500 Subject: [PATCH 04/27] Renamed getExpressedInFrame() to getReferenceFrame(). Renamed some of the change frame methods. Fixed up getters/setters for linear/angular in SpatialForceVector. --- .../DifferentialIDMassMatrixCalculator.java | 2 +- ...ravityCoriolisExternalWrenchMatrixCalculator.java | 12 ++++++------ .../robotics/screwTheory/MomentumCalculator.java | 6 +++--- 3 files changed, 10 insertions(+), 10 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index cc2cdd7..b76d174 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -92,7 +92,7 @@ private void storeJointState() DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); - jointWrench.set(joint.getFrameAfterJoint(), spatialForce); + jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 77fe145..fede1b3 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -75,7 +75,7 @@ public void compute() public void setExternalWrench(RigidBody rigidBody, Wrench externalWrench) { - externalWrenches.get(rigidBody).set(externalWrench); + externalWrenches.get(rigidBody).setIncludingFrame(externalWrench); } public SpatialAccelerationCalculator getSpatialAccelerationCalculator() @@ -112,7 +112,7 @@ private void computeJointWrenchesAndTorques() RigidBody successor = joint.getSuccessor(); Wrench jointWrench = jointWrenches.get(joint); - jointWrench.set(netWrenches.get(successor)); + jointWrench.setIncludingFrame(netWrenches.get(successor)); Wrench externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); @@ -127,10 +127,10 @@ private void computeJointWrenchesAndTorques() Wrench wrenchExertedOnChild = jointWrenches.get(child); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); - wrenchExertedByChild.set(wrenchExertedOnChild); - wrenchExertedByChild.changeBodyFrameAttachedToSameBody(successorFrame); + wrenchExertedByChild.setIncludingFrame(wrenchExertedOnChild); + wrenchExertedByChild.setBodyFrame(successorFrame); wrenchExertedByChild.scale(-1.0); // Action = -reaction - wrenchExertedByChild.changeFrame(jointWrench.getExpressedInFrame()); + wrenchExertedByChild.changeFrame(jointWrench.getReferenceFrame()); jointWrench.sub(wrenchExertedByChild); } } @@ -198,7 +198,7 @@ public void reset() for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) { Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); - externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getExpressedInFrame()); + externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getReferenceFrame()); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java index a08a9fe..9004eb6 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java @@ -21,15 +21,15 @@ public MomentumCalculator(RigidBody rootBody) public void computeAndPack(Momentum momentum) { - momentum.setAngularPart(zero); - momentum.setLinearPart(zero); + momentum.getAngularPart().set(zero); + momentum.getLinearPart().set(zero); for (RigidBody rigidBody : rigidBodiesInOrders) { RigidBodyInertia inertia = rigidBody.getInertia(); rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist); tempMomentum.compute(inertia, tempTwist); - tempMomentum.changeFrame(momentum.getExpressedInFrame()); + tempMomentum.changeFrame(momentum.getReferenceFrame()); momentum.add(tempMomentum); } } From fb12fdb236223e4b943f58b1e677951a9f05587e Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Fri, 2 Nov 2018 17:20:27 -0500 Subject: [PATCH 05/27] Renamed SpatialAccelerationVector and SpatialForceVector. Switched to Mecano's spatials. --- .../DifferentialIDMassMatrixCalculator.java | 7 +++++-- .../FloatingBaseRigidBodyDynamicsCalculator.java | 1 - ...GravityCoriolisExternalWrenchMatrixCalculator.java | 11 +++++++---- .../ihmc/robotics/screwTheory/MomentumCalculator.java | 6 +++++- 4 files changed, 17 insertions(+), 8 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index b76d174..ab3c52b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -7,6 +7,9 @@ import org.ejml.ops.CommonOps; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.spatial.SpatialAcceleration; +import us.ihmc.mecano.spatial.SpatialForce; +import us.ihmc.mecano.spatial.Wrench; import us.ihmc.robotics.linearAlgebra.MatrixTools; /** @@ -28,7 +31,7 @@ public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) { ArrayList zeroJointToIgnore = new ArrayList(); - SpatialAccelerationVector zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); + SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(rootBody, zeroRootAcceleration, zeroJointToIgnore, false, true); jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); @@ -89,7 +92,7 @@ private void storeJointState() { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); - DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForceVector.SIZE, 1); + DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForce.SIZE, 1); CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java index 1aa72d6..c7f17bb 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java @@ -4,7 +4,6 @@ import org.ejml.factory.LinearSolverFactory; import org.ejml.interfaces.linsol.LinearSolver; import org.ejml.ops.CommonOps; -import us.ihmc.robotics.linearAlgebra.MatrixTools; public class FloatingBaseRigidBodyDynamicsCalculator { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index fede1b3..5b97e3b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -8,6 +8,9 @@ import org.ejml.data.DenseMatrix64F; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.spatial.SpatialAcceleration; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.spatial.Wrench; public class GravityCoriolisExternalWrenchMatrixCalculator { @@ -25,7 +28,7 @@ public class GravityCoriolisExternalWrenchMatrixCalculator private final SpatialAccelerationCalculator spatialAccelerationCalculator; - private final SpatialAccelerationVector tempAcceleration = new SpatialAccelerationVector(); + private final SpatialAcceleration tempAcceleration = new SpatialAcceleration(); private final Twist tempTwist = new Twist(); private final boolean doVelocityTerms; @@ -40,7 +43,7 @@ public GravityCoriolisExternalWrenchMatrixCalculator(RigidBody body, ArrayList externalWrenches, + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBody body, SpatialAcceleration rootAcceleration, HashMap externalWrenches, ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) { this(externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms, @@ -61,7 +64,7 @@ public GravityCoriolisExternalWrenchMatrixCalculator(HashMap populateMapsAndLists(); } - public void setRootAcceleration(SpatialAccelerationVector newRootAcceleration) + public void setRootAcceleration(SpatialAcceleration newRootAcceleration) { spatialAccelerationCalculator.setRootAcceleration(newRootAcceleration); } @@ -98,7 +101,7 @@ private void computeNetWrenches() if (!doVelocityTerms) tempTwist.setToZero(); spatialAccelerationCalculator.getAccelerationOfBody(body, tempAcceleration); - body.getInertia().computeDynamicWrenchInBodyCoordinates(tempAcceleration, tempTwist, netWrench); + body.getInertia().computeDynamicWrench(tempAcceleration, tempTwist, netWrench); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java index 9004eb6..127b370 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java @@ -1,6 +1,9 @@ package us.ihmc.robotics.screwTheory; import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.mecano.spatial.Momentum; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.spatial.interfaces.SpatialInertiaBasics; public class MomentumCalculator { @@ -26,8 +29,9 @@ public void computeAndPack(Momentum momentum) for (RigidBody rigidBody : rigidBodiesInOrders) { - RigidBodyInertia inertia = rigidBody.getInertia(); + SpatialInertiaBasics inertia = rigidBody.getInertia(); rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist); + tempMomentum.setReferenceFrame(inertia.getReferenceFrame()); tempMomentum.compute(inertia, tempTwist); tempMomentum.changeFrame(momentum.getReferenceFrame()); momentum.add(tempMomentum); From fdee16c8df7156f0d0ba143bc895f8223a5c7cb8 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Mon, 5 Nov 2018 16:46:18 -0600 Subject: [PATCH 06/27] :arrow_up: mecano 0.0.3. Use WrenchReadOnly where possible. Use SpatialAccelerationReadOnly where possible. Switched to use the yo-spatials from mecano. Renamed InverseDynamicsJoint to JointBasics. Changed package of the joints to prepare for transition. Changed the package of RigidBody. Extracted RigidBodyBasics interface and use it where possible. --- .../filters/AlphaFilteredYoSpatialVector.java | 52 ++++++++-------- .../filters/RateLimitedYoSpatialVector.java | 52 ++++++++-------- .../DifferentialIDMassMatrixCalculator.java | 20 +++--- ...amicallyConsistentNullspaceCalculator.java | 7 ++- ...oriolisExternalWrenchMatrixCalculator.java | 62 ++++++++++--------- .../screwTheory/MassMatrixCalculator.java | 4 +- .../screwTheory/MomentumCalculator.java | 9 +-- 7 files changed, 109 insertions(+), 97 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java index 952f4f0..0b95773 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java @@ -2,67 +2,67 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.robotics.math.frames.YoSpatialVector; +import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -public class AlphaFilteredYoSpatialVector extends YoSpatialVector +public class AlphaFilteredYoSpatialVector extends YoFixedFrameSpatialVector { - private final AlphaFilteredYoFrameVector alphaFilteredLinearPart; private final AlphaFilteredYoFrameVector alphaFilteredAngularPart; + private final AlphaFilteredYoFrameVector alphaFilteredLinearPart; - public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaLinearPart, - DoubleProvider alphaAngularPart, FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) + public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + DoubleProvider alphaLinearPart, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawLinearPart), - new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawAngularPart)); - this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) linearPart; - this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) angularPart; + super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawAngularPart), + new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawLinearPart)); + this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) getAngularPart(); + this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); } - public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaLinearPart, - DoubleProvider alphaAngularPart, YoSpatialVector rawSpatialVector) + public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + DoubleProvider alphaLinearPart, YoFixedFrameSpatialVector rawSpatialVector) { - super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawSpatialVector.getYoLinearPart()), - new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawSpatialVector.getYoAngularPart())); - this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) linearPart; - this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) angularPart; + super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), + new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawSpatialVector.getLinearPart())); + this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) getAngularPart(); + this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); } - public AlphaFilteredYoSpatialVector(AlphaFilteredYoFrameVector yoLinearPart, AlphaFilteredYoFrameVector yoAngularPart) + public AlphaFilteredYoSpatialVector(AlphaFilteredYoFrameVector yoAngularPart, AlphaFilteredYoFrameVector yoLinearPart) { - super(yoLinearPart, yoAngularPart); - this.alphaFilteredLinearPart = yoLinearPart; + super(yoAngularPart, yoLinearPart); this.alphaFilteredAngularPart = yoAngularPart; + this.alphaFilteredLinearPart = yoLinearPart; } public void reset() { - alphaFilteredLinearPart.reset(); alphaFilteredAngularPart.reset(); + alphaFilteredLinearPart.reset(); } public void update() { - alphaFilteredLinearPart.update(); alphaFilteredAngularPart.update(); + alphaFilteredLinearPart.update(); } - public void update(FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) + public void update(FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - alphaFilteredLinearPart.update(rawLinearPart); alphaFilteredAngularPart.update(rawAngularPart); + alphaFilteredLinearPart.update(rawLinearPart); } - public void update(Vector3DReadOnly rawLinearPart, Vector3DReadOnly rawAngularPart) + public void update(Vector3DReadOnly rawAngularPart, Vector3DReadOnly rawLinearPart) { - alphaFilteredLinearPart.update(rawLinearPart); alphaFilteredAngularPart.update(rawAngularPart); + alphaFilteredLinearPart.update(rawLinearPart); } - public void update(double rawLinearX, double rawLinearY, double rawLinearZ, double rawAngularX, double rawAngularY, double rawAngularZ) + public void update(double rawAngularX, double rawAngularY, double rawAngularZ, double rawLinearX, double rawLinearY, double rawLinearZ) { - alphaFilteredLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); alphaFilteredAngularPart.update(rawAngularX, rawAngularY, rawAngularZ); + alphaFilteredLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java index 202a9df..6f70bab 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java @@ -2,67 +2,67 @@ import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; -import us.ihmc.robotics.math.frames.YoSpatialVector; +import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoVariableRegistry; -public class RateLimitedYoSpatialVector extends YoSpatialVector +public class RateLimitedYoSpatialVector extends YoFixedFrameSpatialVector { - private final RateLimitedYoFrameVector rateLimitedLinearPart; private final RateLimitedYoFrameVector rateLimitedAngularPart; + private final RateLimitedYoFrameVector rateLimitedLinearPart; - public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumLinearRate, - DoubleProvider maximumAngularRate, double dt, FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + DoubleProvider maximumLinearRate, double dt, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart), - new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart)); - this.rateLimitedLinearPart = (RateLimitedYoFrameVector) linearPart; - this.rateLimitedAngularPart = (RateLimitedYoFrameVector) angularPart; + super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart), + new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart)); + this.rateLimitedAngularPart = (RateLimitedYoFrameVector) getAngularPart(); + this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); } - public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumLinearRate, - DoubleProvider maximumAngularRate, double dt, YoSpatialVector rawSpatialVector) + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + DoubleProvider maximumLinearRate, double dt, YoFixedFrameSpatialVector rawSpatialVector) { - super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getYoLinearPart()), - new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getYoAngularPart())); - this.rateLimitedLinearPart = (RateLimitedYoFrameVector) linearPart; - this.rateLimitedAngularPart = (RateLimitedYoFrameVector) angularPart; + super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), + new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getLinearPart())); + this.rateLimitedAngularPart = (RateLimitedYoFrameVector) getAngularPart(); + this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); } - public RateLimitedYoSpatialVector(RateLimitedYoFrameVector yoLinearPart, RateLimitedYoFrameVector yoAngularPart) + public RateLimitedYoSpatialVector(RateLimitedYoFrameVector yoAngularPart, RateLimitedYoFrameVector yoLinearPart) { - super(yoLinearPart, yoAngularPart); - this.rateLimitedLinearPart = yoLinearPart; + super(yoAngularPart, yoLinearPart); this.rateLimitedAngularPart = yoAngularPart; + this.rateLimitedLinearPart = yoLinearPart; } public void reset() { - rateLimitedLinearPart.reset(); rateLimitedAngularPart.reset(); + rateLimitedLinearPart.reset(); } public void update() { - rateLimitedLinearPart.update(); rateLimitedAngularPart.update(); + rateLimitedLinearPart.update(); } - public void update(FrameVector3DReadOnly rawLinearPart, FrameVector3DReadOnly rawAngularPart) + public void update(FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - rateLimitedLinearPart.update(rawLinearPart); rateLimitedAngularPart.update(rawAngularPart); + rateLimitedLinearPart.update(rawLinearPart); } - public void update(Vector3DReadOnly rawLinearPart, Vector3DReadOnly rawAngularPart) + public void update(Vector3DReadOnly rawAngularPart, Vector3DReadOnly rawLinearPart) { - rateLimitedLinearPart.update(rawLinearPart); rateLimitedAngularPart.update(rawAngularPart); + rateLimitedLinearPart.update(rawLinearPart); } - public void update(double rawLinearX, double rawLinearY, double rawLinearZ, double rawAngularX, double rawAngularY, double rawAngularZ) + public void update(double rawAngularX, double rawAngularY, double rawAngularZ, double rawLinearX, double rawLinearY, double rawLinearZ) { - rateLimitedLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); rateLimitedAngularPart.update(rawAngularX, rawAngularY, rawAngularZ); + rateLimitedLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index ab3c52b..fa0f8ae 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -7,6 +7,8 @@ import org.ejml.ops.CommonOps; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.SpatialForce; import us.ihmc.mecano.spatial.Wrench; @@ -18,19 +20,19 @@ public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator { private final InverseDynamicsCalculator idCalculator; - private final InverseDynamicsJoint[] jointsInOrder; + private final JointBasics[] jointsInOrder; private final DenseMatrix64F massMatrix; private final DenseMatrix64F storedJointDesiredAccelerations; private final DenseMatrix64F tmpDesiredJointAccelerationsMatrix; private final DenseMatrix64F tmpTauMatrix; - private final LinkedHashMap storedJointWrenches = new LinkedHashMap(); + private final LinkedHashMap storedJointWrenches = new LinkedHashMap(); private final DenseMatrix64F storedJointVelocities; private final int totalNumberOfDoFs; - public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBody rootBody) + public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBodyBasics rootBody) { - ArrayList zeroJointToIgnore = new ArrayList(); + ArrayList zeroJointToIgnore = new ArrayList(); SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); idCalculator = new InverseDynamicsCalculator(rootBody, zeroRootAcceleration, zeroJointToIgnore, false, true); @@ -43,7 +45,7 @@ public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBod tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); - for (InverseDynamicsJoint joint : jointsInOrder) + for (JointBasics joint : jointsInOrder) { ReferenceFrame bodyFixedFrame = joint.getSuccessor().getBodyFixedFrame(); Wrench jointWrench = new Wrench(bodyFixedFrame, bodyFixedFrame); @@ -77,7 +79,7 @@ public void compute() private void setDesiredAccelerationsToZero() { - for (InverseDynamicsJoint joint : jointsInOrder) + for (JointBasics joint : jointsInOrder) { joint.setDesiredAccelerationToZero(); joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0); @@ -88,7 +90,7 @@ private void storeJointState() { ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); - for (InverseDynamicsJoint joint : jointsInOrder) + for (JointBasics joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); joint.getTauMatrix(tauMatrix); @@ -105,7 +107,7 @@ private void restoreJointState() ScrewTools.setDesiredAccelerations(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.setVelocities(jointsInOrder, storedJointVelocities); - for (InverseDynamicsJoint joint : jointsInOrder) + for (JointBasics joint : jointsInOrder) { joint.setTorqueFromWrench(storedJointWrenches.get(joint)); } @@ -124,7 +126,7 @@ public void getMassMatrix(DenseMatrix64F massMatrixToPack) } @Override - public InverseDynamicsJoint[] getJointsInOrder() + public JointBasics[] getJointsInOrder() { return jointsInOrder; } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java index 7570a14..ab62869 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java @@ -2,6 +2,9 @@ import org.ejml.data.DenseMatrix64F; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; + /** * @author twan * Date: 4/17/13 @@ -10,9 +13,9 @@ public interface DynamicallyConsistentNullspaceCalculator { void reset(); - void addConstraint(RigidBody body, DenseMatrix64F selectionMatrix); + void addConstraint(RigidBodyBasics body, DenseMatrix64F selectionMatrix); - void addActuatedJoint(InverseDynamicsJoint joint); + void addActuatedJoint(JointBasics joint); void compute(); diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 5b97e3b..2c9054d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -8,23 +8,27 @@ import org.ejml.data.DenseMatrix64F; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.Twist; import us.ihmc.mecano.spatial.Wrench; +import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; +import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly; public class GravityCoriolisExternalWrenchMatrixCalculator { - private final RigidBody rootBody; + private final RigidBodyBasics rootBody; - private final ArrayList jointsToIgnore; - private final ArrayList allJoints = new ArrayList<>(); - private final ArrayList allBodiesExceptRoot = new ArrayList<>(); - private final ArrayList listOfBodiesWithExternalWrenches = new ArrayList<>(); + private final ArrayList jointsToIgnore; + private final ArrayList allJoints = new ArrayList<>(); + private final ArrayList allBodiesExceptRoot = new ArrayList<>(); + private final ArrayList listOfBodiesWithExternalWrenches = new ArrayList<>(); - private final LinkedHashMap externalWrenches; - private final LinkedHashMap netWrenches = new LinkedHashMap<>(); - private final LinkedHashMap jointWrenches = new LinkedHashMap<>(); - private final LinkedHashMap coriolisWrenches = new LinkedHashMap<>(); + private final LinkedHashMap externalWrenches; + private final LinkedHashMap netWrenches = new LinkedHashMap<>(); + private final LinkedHashMap jointWrenches = new LinkedHashMap<>(); + private final LinkedHashMap coriolisWrenches = new LinkedHashMap<>(); private final SpatialAccelerationCalculator spatialAccelerationCalculator; @@ -37,21 +41,21 @@ public class GravityCoriolisExternalWrenchMatrixCalculator private static final boolean DO_ACCELERATION_TERMS = false; private static final boolean USE_DESIRED_ACCELERATIONS = true; - public GravityCoriolisExternalWrenchMatrixCalculator(RigidBody body, ArrayList jointsToIgnore, double gravity) + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, ArrayList jointsToIgnore, double gravity) { this(body,ScrewTools.createGravitationalSpatialAcceleration(ScrewTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); } - public GravityCoriolisExternalWrenchMatrixCalculator(RigidBody body, SpatialAcceleration rootAcceleration, HashMap externalWrenches, - ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, SpatialAccelerationReadOnly rootAcceleration, HashMap externalWrenches, + ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) { this(externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms, USE_DESIRED_ACCELERATIONS)); } //// TODO: 12/31/16 remove explicit dependency on the spatial acceleration calculator - public GravityCoriolisExternalWrenchMatrixCalculator(HashMap externalWrenches, List jointsToIgnore, + public GravityCoriolisExternalWrenchMatrixCalculator(HashMap externalWrenches, List jointsToIgnore, SpatialAccelerationCalculator spatialAccelerationCalculator) { this.rootBody = spatialAccelerationCalculator.getRootBody(); @@ -64,7 +68,7 @@ public GravityCoriolisExternalWrenchMatrixCalculator(HashMap populateMapsAndLists(); } - public void setRootAcceleration(SpatialAcceleration newRootAcceleration) + public void setRootAcceleration(SpatialAccelerationReadOnly newRootAcceleration) { spatialAccelerationCalculator.setRootAcceleration(newRootAcceleration); } @@ -76,7 +80,7 @@ public void compute() computeJointWrenchesAndTorques(); } - public void setExternalWrench(RigidBody rigidBody, Wrench externalWrench) + public void setExternalWrench(RigidBodyBasics rigidBody, WrenchReadOnly externalWrench) { externalWrenches.get(rigidBody).setIncludingFrame(externalWrench); } @@ -95,13 +99,13 @@ private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { - RigidBody body = allBodiesExceptRoot.get(bodyIndex); + RigidBodyBasics body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); body.getBodyFixedFrame().getTwistOfFrame(tempTwist); if (!doVelocityTerms) tempTwist.setToZero(); spatialAccelerationCalculator.getAccelerationOfBody(body, tempAcceleration); - body.getInertia().computeDynamicWrench(tempAcceleration, tempTwist, netWrench); + body.getInertia().computeDynamicWrenchFast(tempAcceleration, tempTwist, netWrench); } } @@ -110,24 +114,24 @@ private void computeJointWrenchesAndTorques() { for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) { - InverseDynamicsJoint joint = allJoints.get(jointIndex); + JointBasics joint = allJoints.get(jointIndex); - RigidBody successor = joint.getSuccessor(); + RigidBodyBasics successor = joint.getSuccessor(); Wrench jointWrench = jointWrenches.get(joint); jointWrench.setIncludingFrame(netWrenches.get(successor)); - Wrench externalWrench = externalWrenches.get(successor); + WrenchReadOnly externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); - List childrenJoints = successor.getChildrenJoints(); + List childrenJoints = successor.getChildrenJoints(); for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) { - InverseDynamicsJoint child = childrenJoints.get(childIndex); + JointBasics child = childrenJoints.get(childIndex); if (!jointsToIgnore.contains(child)) { - Wrench wrenchExertedOnChild = jointWrenches.get(child); + WrenchReadOnly wrenchExertedOnChild = jointWrenches.get(child); ReferenceFrame successorFrame = successor.getBodyFixedFrame(); wrenchExertedByChild.setIncludingFrame(wrenchExertedOnChild); @@ -148,12 +152,12 @@ private void computeJointWrenchesAndTorques() private void populateMapsAndLists() { - ArrayList morgue = new ArrayList(); + ArrayList morgue = new ArrayList(); morgue.add(rootBody); while (!morgue.isEmpty()) { - RigidBody currentBody = morgue.get(0); + RigidBodyBasics currentBody = morgue.get(0); ReferenceFrame bodyFixedFrame = currentBody.getBodyFixedFrame(); @@ -170,12 +174,12 @@ private void populateMapsAndLists() if (currentBody.hasChildrenJoints()) { - List childrenJoints = currentBody.getChildrenJoints(); - for (InverseDynamicsJoint joint : childrenJoints) + List childrenJoints = currentBody.getChildrenJoints(); + for (JointBasics joint : childrenJoints) { if (!jointsToIgnore.contains(joint)) { - RigidBody successor = joint.getSuccessor(); + RigidBodyBasics successor = joint.getSuccessor(); if (successor != null) { if (allBodiesExceptRoot.contains(successor)) @@ -205,7 +209,7 @@ public void reset() } } - public void getJointCoriolisMatrix(InverseDynamicsJoint joint, DenseMatrix64F jointCoriolisMatrixToPack) + public void getJointCoriolisMatrix(JointBasics joint, DenseMatrix64F jointCoriolisMatrixToPack) { jointCoriolisMatrixToPack.set(coriolisWrenches.get(joint)); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java index d97cc3c..b63cf5b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java @@ -2,6 +2,8 @@ import org.ejml.data.DenseMatrix64F; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; + public interface MassMatrixCalculator { @@ -11,5 +13,5 @@ public interface MassMatrixCalculator public abstract void getMassMatrix(DenseMatrix64F massMatrixToPack); - public abstract InverseDynamicsJoint[] getJointsInOrder(); + public abstract JointBasics[] getJointsInOrder(); } \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java index 127b370..ab8664d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java @@ -1,6 +1,7 @@ package us.ihmc.robotics.screwTheory; import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.Momentum; import us.ihmc.mecano.spatial.Twist; import us.ihmc.mecano.spatial.interfaces.SpatialInertiaBasics; @@ -10,14 +11,14 @@ public class MomentumCalculator private final Twist tempTwist = new Twist(); private final Momentum tempMomentum = new Momentum(); private final Vector3D zero = new Vector3D(); - private final RigidBody[] rigidBodiesInOrders; + private final RigidBodyBasics[] rigidBodiesInOrders; - public MomentumCalculator(RigidBody... rigidBodies) + public MomentumCalculator(RigidBodyBasics... rigidBodies) { rigidBodiesInOrders = rigidBodies; } - public MomentumCalculator(RigidBody rootBody) + public MomentumCalculator(RigidBodyBasics rootBody) { this(ScrewTools.computeSupportAndSubtreeSuccessors(rootBody)); } @@ -27,7 +28,7 @@ public void computeAndPack(Momentum momentum) momentum.getAngularPart().set(zero); momentum.getLinearPart().set(zero); - for (RigidBody rigidBody : rigidBodiesInOrders) + for (RigidBodyBasics rigidBody : rigidBodiesInOrders) { SpatialInertiaBasics inertia = rigidBody.getInertia(); rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist); From fbfef5b01b08382bf56bd80aa2cc8be717364094 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 6 Nov 2018 16:54:49 -0600 Subject: [PATCH 07/27] Reshaping the joints API. Changed getMotionSubspace. --- .../DifferentialIDMassMatrixCalculator.java | 10 ++++++---- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 4 ++-- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index fa0f8ae..d4a2f80 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -82,7 +82,7 @@ private void setDesiredAccelerationsToZero() for (JointBasics joint : jointsInOrder) { joint.setDesiredAccelerationToZero(); - joint.setVelocity(new DenseMatrix64F(joint.getDegreesOfFreedom(), 1), 0); + joint.setJointVelocity(0, new DenseMatrix64F(joint.getDegreesOfFreedom(), 1)); } } @@ -93,9 +93,11 @@ private void storeJointState() for (JointBasics joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); - joint.getTauMatrix(tauMatrix); + joint.getJointTau(0, tauMatrix); DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForce.SIZE, 1); - CommonOps.mult(joint.getMotionSubspace().getJacobianMatrix(), tauMatrix, spatialForce); + DenseMatrix64F motionSubspace = new DenseMatrix64F(6, joint.getDegreesOfFreedom()); + joint.getMotionSubspace(motionSubspace); + CommonOps.mult(motionSubspace, tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); @@ -109,7 +111,7 @@ private void restoreJointState() for (JointBasics joint : jointsInOrder) { - joint.setTorqueFromWrench(storedJointWrenches.get(joint)); + joint.setJointWrench(storedJointWrenches.get(joint)); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 2c9054d..e0087d2 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -142,11 +142,11 @@ private void computeJointWrenchesAndTorques() } } - joint.setTorqueFromWrench(jointWrench); + joint.setJointWrench(jointWrench); DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); coriolisWrench.zero(); - joint.getTauMatrix(coriolisWrench); + joint.getJointTau(0, coriolisWrench); } } From 3dcd268afcacc323274a7c6b1f5e12de4c4df622 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 6 Nov 2018 17:31:21 -0600 Subject: [PATCH 08/27] Removing desired accelerations, started w/ SpatialAccelCalculator. Removed Joint.getDesiredJointAcceleration(SpatialAccel). Removed setter for desired accleration. Removed getter for desired accleration. --- .../screwTheory/DifferentialIDMassMatrixCalculator.java | 8 ++++---- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 3 +-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index d4a2f80..ce06df2 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -64,7 +64,7 @@ public void compute() for (int i = 0 ; i < totalNumberOfDoFs; i++) { tmpDesiredJointAccelerationsMatrix.set(i, 0, 1.0); - ScrewTools.setDesiredAccelerations(jointsInOrder, tmpDesiredJointAccelerationsMatrix); + ScrewTools.setJointAccelerations(jointsInOrder, tmpDesiredJointAccelerationsMatrix); idCalculator.compute(); tmpTauMatrix.set(ScrewTools.getTauMatrix(jointsInOrder)); @@ -81,14 +81,14 @@ private void setDesiredAccelerationsToZero() { for (JointBasics joint : jointsInOrder) { - joint.setDesiredAccelerationToZero(); + joint.setJointAccelerationToZero(); joint.setJointVelocity(0, new DenseMatrix64F(joint.getDegreesOfFreedom(), 1)); } } private void storeJointState() { - ScrewTools.getDesiredJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); + ScrewTools.getJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); for (JointBasics joint : jointsInOrder) { @@ -106,7 +106,7 @@ private void storeJointState() private void restoreJointState() { - ScrewTools.setDesiredAccelerations(jointsInOrder, storedJointDesiredAccelerations); + ScrewTools.setJointAccelerations(jointsInOrder, storedJointDesiredAccelerations); ScrewTools.setVelocities(jointsInOrder, storedJointVelocities); for (JointBasics joint : jointsInOrder) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index e0087d2..f04870e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -50,8 +50,7 @@ public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, Array public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, SpatialAccelerationReadOnly rootAcceleration, HashMap externalWrenches, ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) { - this(externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms, - USE_DESIRED_ACCELERATIONS)); + this(externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms)); } //// TODO: 12/31/16 remove explicit dependency on the spatial acceleration calculator From 0dc7f4b57e1c984b2f81e4a95a5931cca0574464 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 6 Nov 2018 18:32:10 -0600 Subject: [PATCH 09/27] Changed the returned type for getChildrenJoints. --- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index f04870e..725a32a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -123,7 +123,7 @@ private void computeJointWrenchesAndTorques() WrenchReadOnly externalWrench = externalWrenches.get(successor); jointWrench.sub(externalWrench); - List childrenJoints = successor.getChildrenJoints(); + List childrenJoints = successor.getChildrenJoints(); for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) { @@ -173,7 +173,7 @@ private void populateMapsAndLists() if (currentBody.hasChildrenJoints()) { - List childrenJoints = currentBody.getChildrenJoints(); + List childrenJoints = currentBody.getChildrenJoints(); for (JointBasics joint : childrenJoints) { if (!jointsToIgnore.contains(joint)) From 95df1fb6a4ce3dab5b4781dc4ebb906940bce8d7 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 7 Nov 2018 08:26:23 -0600 Subject: [PATCH 10/27] Fixed a reference frame mismatch. Fixed a test --- .../screwTheory/DifferentialIDMassMatrixCalculator.java | 2 +- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 1 + 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index ce06df2..af26c2c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -100,7 +100,7 @@ private void storeJointState() CommonOps.mult(motionSubspace, tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); - jointWrench.changeFrame(joint.getSuccessor().getBodyFixedFrame()); + jointWrench.changeFrame(joint.getFrameAfterJoint()); } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 725a32a..74c30f7 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -141,6 +141,7 @@ private void computeJointWrenchesAndTorques() } } + jointWrench.changeFrame(joint.getFrameAfterJoint()); joint.setJointWrench(jointWrench); DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); From 079f64dea8d8a61e80336da160b02c44e13d3d25 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 7 Nov 2018 15:32:23 -0600 Subject: [PATCH 11/27] Switched to mecano's CoM calculator. Switched to mecano's inverse dynamics calculator. Removing methods from ScrewTools. Switched to use extract/insert joint state in MultiBodySystemTools. Removed computeDegreesOfFreedom. Removed some of the collectors. Some cleanup. Removed SpatialAccelerationCalculator. Fixed a NPE. --- .../DifferentialIDMassMatrixCalculator.java | 24 ++++++++++--------- ...oriolisExternalWrenchMatrixCalculator.java | 21 ++++++++++++---- .../screwTheory/MomentumCalculator.java | 6 +++-- 3 files changed, 33 insertions(+), 18 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index af26c2c..f9a220c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -1,17 +1,19 @@ package us.ihmc.robotics.screwTheory; -import java.util.ArrayList; import java.util.LinkedHashMap; import org.ejml.data.DenseMatrix64F; import org.ejml.ops.CommonOps; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.algorithms.InverseDynamicsCalculator; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.SpatialForce; import us.ihmc.mecano.spatial.Wrench; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemTools; import us.ihmc.robotics.linearAlgebra.MatrixTools; /** @@ -32,12 +34,12 @@ public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBodyBasics rootBody) { - ArrayList zeroJointToIgnore = new ArrayList(); SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); - idCalculator = new InverseDynamicsCalculator(rootBody, zeroRootAcceleration, zeroJointToIgnore, false, true); - jointsInOrder = ScrewTools.computeSubtreeJoints(rootBody); - totalNumberOfDoFs = ScrewTools.computeDegreesOfFreedom(jointsInOrder); + idCalculator = new InverseDynamicsCalculator(rootBody, false, true); + idCalculator.setRootAcceleration(zeroRootAcceleration); + jointsInOrder = MultiBodySystemTools.collectSubtreeJoints(rootBody); + totalNumberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); @@ -64,10 +66,10 @@ public void compute() for (int i = 0 ; i < totalNumberOfDoFs; i++) { tmpDesiredJointAccelerationsMatrix.set(i, 0, 1.0); - ScrewTools.setJointAccelerations(jointsInOrder, tmpDesiredJointAccelerationsMatrix); + MultiBodySystemTools.insertJointsState(jointsInOrder, JointStateType.ACCELERATION, tmpDesiredJointAccelerationsMatrix); idCalculator.compute(); - tmpTauMatrix.set(ScrewTools.getTauMatrix(jointsInOrder)); + tmpTauMatrix.set(idCalculator.getJointTauMatrix()); MatrixTools.setMatrixBlock(massMatrix, 0, column, tmpTauMatrix, 0, 0, totalNumberOfDoFs, 1, 1.0); column++; @@ -88,8 +90,8 @@ private void setDesiredAccelerationsToZero() private void storeJointState() { - ScrewTools.getJointAccelerationsMatrix(jointsInOrder, storedJointDesiredAccelerations); - ScrewTools.getJointVelocitiesMatrix(jointsInOrder, storedJointVelocities); + MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.ACCELERATION, storedJointDesiredAccelerations); + MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); @@ -106,8 +108,8 @@ private void storeJointState() private void restoreJointState() { - ScrewTools.setJointAccelerations(jointsInOrder, storedJointDesiredAccelerations); - ScrewTools.setVelocities(jointsInOrder, storedJointVelocities); + MultiBodySystemTools.insertJointsState(jointsInOrder, JointStateType.ACCELERATION, storedJointDesiredAccelerations); + MultiBodySystemTools.insertJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 74c30f7..a7c2634 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -8,13 +8,16 @@ import org.ejml.data.DenseMatrix64F; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly; import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.Twist; import us.ihmc.mecano.spatial.Wrench; import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly; +import us.ihmc.mecano.tools.MultiBodySystemTools; public class GravityCoriolisExternalWrenchMatrixCalculator { @@ -43,21 +46,21 @@ public class GravityCoriolisExternalWrenchMatrixCalculator public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, ArrayList jointsToIgnore, double gravity) { - this(body,ScrewTools.createGravitationalSpatialAcceleration(ScrewTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, + this(body,ScrewTools.createGravitationalSpatialAcceleration(MultiBodySystemTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); } public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, SpatialAccelerationReadOnly rootAcceleration, HashMap externalWrenches, ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) { - this(externalWrenches, jointsToIgnore, new SpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms)); + this(externalWrenches, jointsToIgnore, createSpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms)); } //// TODO: 12/31/16 remove explicit dependency on the spatial acceleration calculator public GravityCoriolisExternalWrenchMatrixCalculator(HashMap externalWrenches, List jointsToIgnore, SpatialAccelerationCalculator spatialAccelerationCalculator) { - this.rootBody = spatialAccelerationCalculator.getRootBody(); + this.rootBody = (RigidBodyBasics) spatialAccelerationCalculator.getRootBody(); this.externalWrenches = new LinkedHashMap<>(externalWrenches); this.jointsToIgnore = new ArrayList<>(jointsToIgnore); this.spatialAccelerationCalculator = spatialAccelerationCalculator; @@ -67,6 +70,14 @@ public GravityCoriolisExternalWrenchMatrixCalculator(HashMap body.getInertia() != null).toArray(RigidBodyBasics[]::new); } public MomentumCalculator(RigidBodyBasics rootBody) { - this(ScrewTools.computeSupportAndSubtreeSuccessors(rootBody)); + this(rootBody.subtreeArray()); } public void computeAndPack(Momentum momentum) From e6b1fb44023f0bfd18af96dedf33331aee4f396d Mon Sep 17 00:00:00 2001 From: Georg Wiedebach Date: Thu, 4 Apr 2019 11:07:16 -0500 Subject: [PATCH 12/27] Modified the feedback controller toolbox to provide mutable frame objects. Forgot to create mutable frame rate limited vectors. Fix a bug: forgot to update a reference frame. Added name distinction between angular and linear parts. --- .../YoMutableFrameSpatialVector.java | 86 +++++++++++++++++++ ...haFilteredYoMutableFrameSpatialVector.java | 69 +++++++++++++++ .../filters/AlphaFilteredYoSpatialVector.java | 8 +- .../RateLimitedYoMutableSpatialVector.java | 70 +++++++++++++++ 4 files changed, 229 insertions(+), 4 deletions(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java new file mode 100644 index 0000000..d2cdac4 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java @@ -0,0 +1,86 @@ +package us.ihmc.robotics.dataStructures; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics; +import us.ihmc.mecano.spatial.interfaces.SpatialVectorBasics; +import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.util.YoFrameVariableNameTools; +import us.ihmc.yoVariables.variable.YoDouble; +import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameObject; +import us.ihmc.yoVariables.variable.frameObjects.YoMutableFramePoint3D; +import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameQuaternion; +import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameVector3D; + +/** + * TODO: this should probably live in mecano? + */ +public class YoMutableFrameSpatialVector extends YoMutableFrameObject implements SpatialVectorBasics +{ + private final YoMutableFrameVector3D angularPart; + private final YoMutableFrameVector3D linearPart; + + public YoMutableFrameSpatialVector(YoMutableFrameVector3D angularPart, YoMutableFrameVector3D linearPart) + { + super(angularPart.getYoFrameIndex(), angularPart.getFrameIndexMap()); + this.angularPart = angularPart; + this.linearPart = linearPart; + checkFrameConsistency(); + } + + public YoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry) + { + super(namePrefix, nameSuffix, registry); + + YoDouble xAngular = new YoDouble(YoFrameVariableNameTools.createXName(namePrefix, "Angular" + nameSuffix), registry); + YoDouble yAngular = new YoDouble(YoFrameVariableNameTools.createYName(namePrefix, "Angular" + nameSuffix), registry); + YoDouble zAngular = new YoDouble(YoFrameVariableNameTools.createZName(namePrefix, "Angular" + nameSuffix), registry); + angularPart = new YoMutableFrameVector3D(xAngular, yAngular, zAngular, getYoFrameIndex(), getFrameIndexMap()); + + YoDouble xLinear = new YoDouble(YoFrameVariableNameTools.createXName(namePrefix, "Linear" + nameSuffix), registry); + YoDouble yLinear = new YoDouble(YoFrameVariableNameTools.createYName(namePrefix, "Linear" + nameSuffix), registry); + YoDouble zLinear = new YoDouble(YoFrameVariableNameTools.createZName(namePrefix, "Linear" + nameSuffix), registry); + linearPart = new YoMutableFrameVector3D(xLinear, yLinear, zLinear, getYoFrameIndex(), getFrameIndexMap()); + } + + @Override + public FixedFrameVector3DBasics getAngularPart() + { + checkFrameConsistency(); + return angularPart; + } + + @Override + public FixedFrameVector3DBasics getLinearPart() + { + checkFrameConsistency(); + return linearPart; + } + + @Override + public ReferenceFrame getReferenceFrame() + { + checkFrameConsistency(); + return super.getReferenceFrame(); + } + + @Override + public void setReferenceFrame(ReferenceFrame referenceFrame) + { + checkFrameConsistency(); + super.setReferenceFrame(referenceFrame); + // When constructing this with two YoMutableFrameVector3D objects the angular part is updated only by the super implementation. + linearPart.setReferenceFrame(referenceFrame); + } + + /** + * This is a check that should be called every time this object is interacted with. If this + * failes it likely means that you created this pose using + * {@link #YoMutableFramePose3D(YoMutableFramePoint3D, YoMutableFrameQuaternion)} and changed the + * reference frame of one of the passed objects without modifying the other one from outside this + * class. This will make the data structure in here inconsistent. + */ + private void checkFrameConsistency() + { + angularPart.checkReferenceFrameMatch(linearPart); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java new file mode 100644 index 0000000..d013701 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java @@ -0,0 +1,69 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; +import us.ihmc.robotics.dataStructures.YoMutableFrameSpatialVector; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; + +public class AlphaFilteredYoMutableFrameSpatialVector extends YoMutableFrameSpatialVector +{ + private final AlphaFilteredYoMutableFrameVector3D alphaFilteredAngularPart; + private final AlphaFilteredYoMutableFrameVector3D alphaFilteredLinearPart; + + public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + DoubleProvider alphaLinearPart, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) + { + super(new AlphaFilteredYoMutableFrameVector3D(namePrefix, nameSuffix, registry, alphaAngularPart, rawAngularPart), + new AlphaFilteredYoMutableFrameVector3D(namePrefix, nameSuffix, registry, alphaLinearPart, rawLinearPart)); + this.alphaFilteredAngularPart = (AlphaFilteredYoMutableFrameVector3D) getAngularPart(); + this.alphaFilteredLinearPart = (AlphaFilteredYoMutableFrameVector3D) getLinearPart(); + } + + public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + DoubleProvider alphaLinearPart, YoFixedFrameSpatialVector rawSpatialVector) + { + super(new AlphaFilteredYoMutableFrameVector3D(namePrefix, nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), + new AlphaFilteredYoMutableFrameVector3D(namePrefix, nameSuffix, registry, alphaLinearPart, rawSpatialVector.getLinearPart())); + this.alphaFilteredAngularPart = (AlphaFilteredYoMutableFrameVector3D) getAngularPart(); + this.alphaFilteredLinearPart = (AlphaFilteredYoMutableFrameVector3D) getLinearPart(); + } + + public AlphaFilteredYoMutableFrameSpatialVector(AlphaFilteredYoMutableFrameVector3D yoAngularPart, AlphaFilteredYoMutableFrameVector3D yoLinearPart) + { + super(yoAngularPart, yoLinearPart); + this.alphaFilteredAngularPart = yoAngularPart; + this.alphaFilteredLinearPart = yoLinearPart; + } + + public void reset() + { + alphaFilteredAngularPart.reset(); + alphaFilteredLinearPart.reset(); + } + + public void update() + { + alphaFilteredAngularPart.update(); + alphaFilteredLinearPart.update(); + } + + public void update(FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) + { + alphaFilteredAngularPart.update(rawAngularPart); + alphaFilteredLinearPart.update(rawLinearPart); + } + + public void update(Vector3DReadOnly rawAngularPart, Vector3DReadOnly rawLinearPart) + { + alphaFilteredAngularPart.update(rawAngularPart); + alphaFilteredLinearPart.update(rawLinearPart); + } + + public void update(double rawAngularX, double rawAngularY, double rawAngularZ, double rawLinearX, double rawLinearY, double rawLinearZ) + { + alphaFilteredAngularPart.update(rawAngularX, rawAngularY, rawAngularZ); + alphaFilteredLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java index 0b95773..7cdfb9e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java @@ -14,8 +14,8 @@ public class AlphaFilteredYoSpatialVector extends YoFixedFrameSpatialVector public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawAngularPart), - new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawLinearPart)); + super(new AlphaFilteredYoFrameVector(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawAngularPart), + new AlphaFilteredYoFrameVector(namePrefix + "LinearPart", nameSuffix, registry, alphaLinearPart, rawLinearPart)); this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) getAngularPart(); this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); } @@ -23,8 +23,8 @@ public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVari public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, YoFixedFrameSpatialVector rawSpatialVector) { - super(new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), - new AlphaFilteredYoFrameVector(namePrefix, nameSuffix, registry, alphaLinearPart, rawSpatialVector.getLinearPart())); + super(new AlphaFilteredYoFrameVector(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), + new AlphaFilteredYoFrameVector(namePrefix + "LinearPart", nameSuffix, registry, alphaLinearPart, rawSpatialVector.getLinearPart())); this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) getAngularPart(); this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java new file mode 100644 index 0000000..4052379 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java @@ -0,0 +1,70 @@ +package us.ihmc.robotics.math.filters; + +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; +import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; +import us.ihmc.robotics.dataStructures.YoMutableFrameSpatialVector; +import us.ihmc.yoVariables.providers.DoubleProvider; +import us.ihmc.yoVariables.registry.YoVariableRegistry; + +public class RateLimitedYoMutableSpatialVector extends YoMutableFrameSpatialVector +{ + private final RateLimitedYoMutableFrameVector3D rateLimitedAngularPart; + private final RateLimitedYoMutableFrameVector3D rateLimitedLinearPart; + + public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + DoubleProvider maximumLinearRate, double dt, FrameVector3DReadOnly rawAngularPart, + FrameVector3DReadOnly rawLinearPart) + { + super(new RateLimitedYoMutableFrameVector3D(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart), + new RateLimitedYoMutableFrameVector3D(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart)); + this.rateLimitedAngularPart = (RateLimitedYoMutableFrameVector3D) getAngularPart(); + this.rateLimitedLinearPart = (RateLimitedYoMutableFrameVector3D) getLinearPart(); + } + + public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + DoubleProvider maximumLinearRate, double dt, YoFixedFrameSpatialVector rawSpatialVector) + { + super(new RateLimitedYoMutableFrameVector3D(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), + new RateLimitedYoMutableFrameVector3D(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getLinearPart())); + this.rateLimitedAngularPart = (RateLimitedYoMutableFrameVector3D) getAngularPart(); + this.rateLimitedLinearPart = (RateLimitedYoMutableFrameVector3D) getLinearPart(); + } + + public RateLimitedYoMutableSpatialVector(RateLimitedYoMutableFrameVector3D yoAngularPart, RateLimitedYoMutableFrameVector3D yoLinearPart) + { + super(yoAngularPart, yoLinearPart); + this.rateLimitedAngularPart = yoAngularPart; + this.rateLimitedLinearPart = yoLinearPart; + } + + public void reset() + { + rateLimitedAngularPart.reset(); + rateLimitedLinearPart.reset(); + } + + public void update() + { + rateLimitedAngularPart.update(); + rateLimitedLinearPart.update(); + } + + public void update(FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) + { + rateLimitedAngularPart.update(rawAngularPart); + rateLimitedLinearPart.update(rawLinearPart); + } + + public void update(Vector3DReadOnly rawAngularPart, Vector3DReadOnly rawLinearPart) + { + rateLimitedAngularPart.update(rawAngularPart); + rateLimitedLinearPart.update(rawLinearPart); + } + + public void update(double rawAngularX, double rawAngularY, double rawAngularZ, double rawLinearX, double rawLinearY, double rawLinearZ) + { + rateLimitedAngularPart.update(rawAngularX, rawAngularY, rawAngularZ); + rateLimitedLinearPart.update(rawLinearX, rawLinearY, rawLinearZ); + } +} From c22d07580f2ba9a835b5a15e8ee4c7f48f0b09e4 Mon Sep 17 00:00:00 2001 From: smccrory Date: Tue, 8 Oct 2019 17:56:04 -0500 Subject: [PATCH 13/27] Adding new getter in coriolis matrix calculator --- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index a7c2634..a4d448a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -7,6 +7,7 @@ import org.ejml.data.DenseMatrix64F; +import org.ejml.ops.CommonOps; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; @@ -18,6 +19,7 @@ import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly; import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.robotics.linearAlgebra.MatrixTools; public class GravityCoriolisExternalWrenchMatrixCalculator { @@ -224,4 +226,10 @@ public void getJointCoriolisMatrix(JointBasics joint, DenseMatrix64F jointCoriol { jointCoriolisMatrixToPack.set(coriolisWrenches.get(joint)); } + + public void getJointCoriolisMatrix(JointBasics joint, DenseMatrix64F jointCoriolisMatrixToPack, int startRow) + { + DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); + CommonOps.extract(coriolisWrench, 0, coriolisWrench.getNumRows(), 0, 1, jointCoriolisMatrixToPack, startRow, 0); + } } From b291874dc642b09f7c24cd4f2ff092d9040e896a Mon Sep 17 00:00:00 2001 From: smccrory Date: Tue, 8 Oct 2019 18:40:49 -0500 Subject: [PATCH 14/27] Maintaining state of joints in GravityCoriolisExternalWrenchMatrixCalculator Changing ignoredJoints object from ArrayList to List in GravityCoriolisExternalWrenchMatrixCalculator --- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index a4d448a..10a619a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -39,6 +39,7 @@ public class GravityCoriolisExternalWrenchMatrixCalculator private final SpatialAcceleration tempAcceleration = new SpatialAcceleration(); private final Twist tempTwist = new Twist(); + private final Wrench tempWrench = new Wrench(); private final boolean doVelocityTerms; @@ -46,14 +47,14 @@ public class GravityCoriolisExternalWrenchMatrixCalculator private static final boolean DO_ACCELERATION_TERMS = false; private static final boolean USE_DESIRED_ACCELERATIONS = true; - public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, ArrayList jointsToIgnore, double gravity) + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, List jointsToIgnore, double gravity) { this(body,ScrewTools.createGravitationalSpatialAcceleration(MultiBodySystemTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); } public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, SpatialAccelerationReadOnly rootAcceleration, HashMap externalWrenches, - ArrayList jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) + List jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) { this(externalWrenches, jointsToIgnore, createSpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms)); } @@ -155,11 +156,14 @@ private void computeJointWrenchesAndTorques() } jointWrench.changeFrame(joint.getFrameAfterJoint()); + + tempWrench.setIncludingFrame(joint.getJointWrench()); joint.setJointWrench(jointWrench); DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); coriolisWrench.zero(); joint.getJointTau(0, coriolisWrench); + joint.setJointWrench(tempWrench); } } From 34db8668663eaee3ddfb770ba7b86cf05278b0b6 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 4 Dec 2019 19:44:29 -0600 Subject: [PATCH 15/27] Switched MatrixTools. --- .../screwTheory/DifferentialIDMassMatrixCalculator.java | 2 +- .../GravityCoriolisExternalWrenchMatrixCalculator.java | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index f9a220c..22faf8b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -6,6 +6,7 @@ import org.ejml.ops.CommonOps; import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.matrixlib.MatrixTools; import us.ihmc.mecano.algorithms.InverseDynamicsCalculator; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; @@ -14,7 +15,6 @@ import us.ihmc.mecano.spatial.Wrench; import us.ihmc.mecano.tools.JointStateType; import us.ihmc.mecano.tools.MultiBodySystemTools; -import us.ihmc.robotics.linearAlgebra.MatrixTools; /** * Very inefficient. Should only be used for verification of better methods in unit tests diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 10a619a..1570fc4 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -6,8 +6,8 @@ import java.util.List; import org.ejml.data.DenseMatrix64F; - import org.ejml.ops.CommonOps; + import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; @@ -19,7 +19,6 @@ import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly; import us.ihmc.mecano.tools.MultiBodySystemTools; -import us.ihmc.robotics.linearAlgebra.MatrixTools; public class GravityCoriolisExternalWrenchMatrixCalculator { From 411f1d99ad9634a7a60d5d01940ae7a14c0bee57 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Fri, 24 Jan 2020 15:57:42 -0600 Subject: [PATCH 16/27] Re-implemented the gravity/coriolis/external-wrench calculator. --- ...oriolisExternalWrenchMatrixCalculator.java | 716 ++++++++++++++---- 1 file changed, 572 insertions(+), 144 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 1570fc4..6c263a4 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -1,238 +1,666 @@ package us.ihmc.robotics.screwTheory; import java.util.ArrayList; -import java.util.HashMap; +import java.util.Collection; import java.util.LinkedHashMap; import java.util.List; +import java.util.Map; import org.ejml.data.DenseMatrix64F; import org.ejml.ops.CommonOps; -import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; +import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; +import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; +import us.ihmc.mecano.algorithms.InverseDynamicsCalculator; import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator; +import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider; +import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; -import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +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.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.FixedFrameWrenchBasics; import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; +import us.ihmc.mecano.spatial.interfaces.SpatialVectorReadOnly; +import us.ihmc.mecano.spatial.interfaces.TwistReadOnly; import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly; import us.ihmc.mecano.tools.MultiBodySystemTools; +/** + * Implements the same algorithm as {@link InverseDynamicsCalculator} but ignores joint + * accelerations. + *

+ * The resulting joint torques are thus a result of the centrifugal, Coriolis, and external wrenches + * only. + *

+ */ public class GravityCoriolisExternalWrenchMatrixCalculator { - private final RigidBodyBasics rootBody; + /** Defines the multi-body system to use with this calculator. */ + private final MultiBodySystemReadOnly input; + + /** The root of the internal recursive algorithm. */ + private final RecursionStep initialRecursionStep; + /** Map to quickly retrieve information for each rigid-body. */ + private final Map rigidBodyToRecursionStepMap = new LinkedHashMap<>(); + + /** + * The output of this algorithm: the effort matrix for all the joints to consider resulting from + * Coriolis, centrifugal, and external wrenches. + */ + private final DenseMatrix64F jointTauMatrix; + + /** Whether the effort resulting from the Coriolis and centrifugal forces should be considered. */ + private final boolean considerCoriolisAndCentrifugalForces; + /** + * Extension of this algorithm into an acceleration provider that be used instead of a + * {@link SpatialAccelerationCalculator}. + */ + private final RigidBodyAccelerationProvider coriolisAccelerationProvider; + + /** + * Creates a calculator for computing the joint efforts for all the descendants of the given + * {@code rootBody}. + *

+ * Do not forgot to set the gravitational acceleration so this calculator can properly account for + * it. + *

+ * + * @param rootBody the supporting body of the subtree to be evaluated by this calculator. Not + * modified. + */ + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyReadOnly rootBody) + { + this(rootBody, true); + } - private final ArrayList jointsToIgnore; - private final ArrayList allJoints = new ArrayList<>(); - private final ArrayList allBodiesExceptRoot = new ArrayList<>(); - private final ArrayList listOfBodiesWithExternalWrenches = new ArrayList<>(); + /** + * Creates a calculator for computing the joint efforts for all the descendants of the given + * {@code rootBody}. + *

+ * Do not forgot to set the gravitational acceleration so this calculator can properly account for + * it. + *

+ * + * @param rootBody the supporting body of the subtree to be evaluated by + * this calculator. Not modified. + * @param considerCoriolisAndCentrifugalForces whether the effort resulting from the Coriolis and + * centrifugal forces should be considered. + */ + public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyReadOnly rootBody, boolean considerCoriolisAndCentrifugalForces) + { + this(MultiBodySystemReadOnly.toMultiBodySystemInput(rootBody), considerCoriolisAndCentrifugalForces); + } - private final LinkedHashMap externalWrenches; - private final LinkedHashMap netWrenches = new LinkedHashMap<>(); - private final LinkedHashMap jointWrenches = new LinkedHashMap<>(); - private final LinkedHashMap coriolisWrenches = new LinkedHashMap<>(); + /** + * Creates a calculator for computing the joint efforts for system defined by the given + * {@code input}. + *

+ * Do not forgot to set the gravitational acceleration so this calculator can properly account for + * it. + *

+ * + * @param input the definition of the system to be evaluated by this calculator. + */ + public GravityCoriolisExternalWrenchMatrixCalculator(MultiBodySystemReadOnly input) + { + this(input, true, true); + } - private final SpatialAccelerationCalculator spatialAccelerationCalculator; + /** + * Creates a calculator for computing the joint efforts for system defined by the given + * {@code input}. + *

+ * Do not forgot to set the gravitational acceleration so this calculator can properly account for + * it. + *

+ * + * @param input the definition of the system to be evaluated by this + * calculator. + * @param considerIgnoredSubtreesInertia whether the inertia of the ignored part(s) of the given + * multi-body system should be considered. When {@code true}, + * this provides more accurate joint torques as they + * compensate for instance for the gravity acting on the + * ignored rigid-bodies, i.e. bodies which have an ancestor + * joint that is ignored as specified in the given + * {@code input}. When {@code false}, the resulting joint + * torques may be less accurate and this calculator may gain + * slight performance improvement. + */ + public GravityCoriolisExternalWrenchMatrixCalculator(MultiBodySystemReadOnly input, boolean considerIgnoredSubtreesInertia) + { + this(input, true, considerIgnoredSubtreesInertia); + } - private final SpatialAcceleration tempAcceleration = new SpatialAcceleration(); - private final Twist tempTwist = new Twist(); - private final Wrench tempWrench = new Wrench(); + /** + * Creates a calculator for computing the joint efforts for system defined by the given + * {@code input}. + *

+ * Do not forgot to set the gravitational acceleration so this calculator can properly account for + * it. + *

+ * + * @param input the definition of the system to be evaluated by this + * calculator. + * @param considerCoriolisAndCentrifugalForces whether the effort resulting from the Coriolis and + * centrifugal forces should be considered. + * @param considerIgnoredSubtreesInertia whether the inertia of the ignored part(s) of the + * given multi-body system should be considered. When + * {@code true}, this provides more accurate joint + * torques as they compensate for instance for the + * gravity acting on the ignored rigid-bodies, i.e. + * bodies which have an ancestor joint that is ignored + * as specified in the given {@code input}. When + * {@code false}, the resulting joint torques may be + * less accurate and this calculator may gain slight + * performance improvement. + */ + public GravityCoriolisExternalWrenchMatrixCalculator(MultiBodySystemReadOnly input, boolean considerCoriolisAndCentrifugalForces, + boolean considerIgnoredSubtreesInertia) + { + this.input = input; + this.considerCoriolisAndCentrifugalForces = considerCoriolisAndCentrifugalForces; - private final boolean doVelocityTerms; + RigidBodyReadOnly rootBody = input.getRootBody(); + initialRecursionStep = new RecursionStep(rootBody, null, null); + rigidBodyToRecursionStepMap.put(rootBody, initialRecursionStep); + buildMultiBodyTree(initialRecursionStep, input.getJointsToIgnore()); - private static final boolean DEFAULT_DO_VELOCITY_TERMS = true; - private static final boolean DO_ACCELERATION_TERMS = false; - private static final boolean USE_DESIRED_ACCELERATIONS = true; + if (considerIgnoredSubtreesInertia) + initialRecursionStep.includeIgnoredSubtreeInertia(); - public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, List jointsToIgnore, double gravity) - { - this(body,ScrewTools.createGravitationalSpatialAcceleration(MultiBodySystemTools.getRootBody(body), gravity), new LinkedHashMap<>(), jointsToIgnore, - DEFAULT_DO_VELOCITY_TERMS, DO_ACCELERATION_TERMS); + int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); + jointTauMatrix = new DenseMatrix64F(nDoFs, 1); + + coriolisAccelerationProvider = RigidBodyAccelerationProvider.toRigidBodyAccelerationProvider(body -> rigidBodyToRecursionStepMap.get(body).rigidBodyAcceleration, + input.getInertialFrame(), + considerCoriolisAndCentrifugalForces, + false); } - public GravityCoriolisExternalWrenchMatrixCalculator(RigidBodyBasics body, SpatialAccelerationReadOnly rootAcceleration, HashMap externalWrenches, - List jointsToIgnore, boolean doVelocityTerms, boolean doAccelerationTerms) + private void buildMultiBodyTree(RecursionStep parent, Collection jointsToIgnore) { - this(externalWrenches, jointsToIgnore, createSpatialAccelerationCalculator(body, rootAcceleration, doVelocityTerms, doAccelerationTerms)); + for (JointReadOnly childJoint : parent.rigidBody.getChildrenJoints()) + { + if (jointsToIgnore.contains(childJoint)) + continue; + + RigidBodyReadOnly childBody = childJoint.getSuccessor(); + + if (childBody != null) + { + int[] jointIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(childJoint); + RecursionStep child = new RecursionStep(childBody, parent, jointIndices); + rigidBodyToRecursionStepMap.put(childBody, child); + buildMultiBodyTree(child, jointsToIgnore); + } + } } - //// TODO: 12/31/16 remove explicit dependency on the spatial acceleration calculator - public GravityCoriolisExternalWrenchMatrixCalculator(HashMap externalWrenches, List jointsToIgnore, - SpatialAccelerationCalculator spatialAccelerationCalculator) + /** + * Set the gravitational acceleration to account for in this multi-body system. + *

+ * The acceleration of the root body is set to the opposite of the gravitational acceleration such + * that it gets naturally propagated to the whole system. + *

+ * + * @param gravity the gravitational linear acceleration, it is usually equal to + * {@code (0, 0, -9.81)}. + */ + public void setGravitionalAcceleration(FrameTuple3DReadOnly gravity) { - this.rootBody = (RigidBodyBasics) spatialAccelerationCalculator.getRootBody(); - this.externalWrenches = new LinkedHashMap<>(externalWrenches); - this.jointsToIgnore = new ArrayList<>(jointsToIgnore); - this.spatialAccelerationCalculator = spatialAccelerationCalculator; + gravity.checkReferenceFrameMatch(input.getInertialFrame()); + setGravitionalAcceleration((Tuple3DReadOnly) gravity); + } - this.doVelocityTerms = spatialAccelerationCalculator.areVelocitiesConsidered(); + /** + * Set the gravitational acceleration to account for in this multi-body system. + *

+ * The acceleration of the root body is set to the opposite of the gravitational acceleration such + * that it gets naturally propagated to the whole system. + *

+ * + * @param gravity the gravitational linear acceleration, it is usually equal to + * {@code (0, 0, -9.81)}. + */ + public void setGravitionalAcceleration(Tuple3DReadOnly gravity) + { + SpatialAcceleration rootAcceleration = initialRecursionStep.rigidBodyAcceleration; + rootAcceleration.setToZero(); + rootAcceleration.getLinearPart().setAndNegate(gravity); + } - populateMapsAndLists(); + /** + * Set the gravitational acceleration to account for in this multi-body system. + *

+ * The acceleration of the root body is set to the opposite of the gravitational acceleration such + * that it gets naturally propagated to the whole system. + *

+ * + * @param gravity the gravitational linear acceleration along the z-axis, it is usually equal to + * {@code -9.81}. + */ + public void setGravitionalAcceleration(double gravity) + { + setGravitionalAcceleration(0.0, 0.0, gravity); } - private static SpatialAccelerationCalculator createSpatialAccelerationCalculator(RigidBodyReadOnly body, SpatialAccelerationReadOnly rootAcceleration, - boolean doVelocityTerms, boolean doAccelerationTerms) + /** + * Set the gravitational acceleration to account for in this multi-body system. + *

+ * The acceleration of the root body is set to the opposite of the gravitational acceleration such + * that it gets naturally propagated to the whole system. + *

+ * + * @param gravityX the gravitational linear acceleration along the x-axis, it is usually equal to + * {@code 0}. + * @param gravityY the gravitational linear acceleration along the y-axis, it is usually equal to + * {@code 0}. + * @param gravityZ the gravitational linear acceleration along the z-axis, it is usually equal to + * {@code -9.81}. + */ + public void setGravitionalAcceleration(double gravityX, double gravityY, double gravityZ) { - SpatialAccelerationCalculator spatialAccelerationCalculator = new SpatialAccelerationCalculator(body, ReferenceFrame.getWorldFrame(), doVelocityTerms, doAccelerationTerms); - spatialAccelerationCalculator.setRootAcceleration(rootAcceleration); - return spatialAccelerationCalculator; + SpatialAcceleration rootAcceleration = initialRecursionStep.rigidBodyAcceleration; + rootAcceleration.setToZero(); + rootAcceleration.getLinearPart().set(gravityX, gravityY, gravityZ); + rootAcceleration.negate(); } + /** + * Changes the spatial acceleration of the root. Even though the root is assumed to be non-moving, + * the {@code rootAcceleration} is usually set to the opposite of the gravitational acceleration, + * such that the effect of the gravity is naturally propagated to the entire system. + * + * @param newRootAcceleration the new spatial acceleration of the root. + * @throws ReferenceFrameMismatchException if any of the reference frames of + * {@code newRootAcceleration} does not match this + * calculator's root spatial acceleration's frames. + */ public void setRootAcceleration(SpatialAccelerationReadOnly newRootAcceleration) { - spatialAccelerationCalculator.setRootAcceleration(newRootAcceleration); + initialRecursionStep.rigidBodyAcceleration.set(newRootAcceleration); } - public void compute() + /** + * Resets all the external wrenches that were added to the rigid-bodies. + */ + public void setExternalWrenchesToZero() { - computeTwistsAndSpatialAccelerations(); - computeNetWrenches(); - computeJointWrenchesAndTorques(); + initialRecursionStep.setExternalWrenchToZeroRecursive(); } - public void setExternalWrench(RigidBodyBasics rigidBody, WrenchReadOnly externalWrench) + /** + * Gets the internal reference to the external wrench associated with the given rigidBody. + *

+ * Modify the return wrench to configure the wrench to be applied on this rigid-body. + *

+ * + * @param rigidBody the query. Not modified. + * @return the wrench associated to the query. + */ + public FixedFrameWrenchBasics getExternalWrench(RigidBodyReadOnly rigidBody) { - externalWrenches.get(rigidBody).setIncludingFrame(externalWrench); + return rigidBodyToRecursionStepMap.get(rigidBody).externalWrench; } - public SpatialAccelerationCalculator getSpatialAccelerationCalculator() + /** + * Sets external wrench to apply to the given {@code rigidBody}. + * + * @param rigidBody the rigid-body to which the wrench is to applied. Not modified. + * @param externalWrench the external wrench to apply to the rigid-body. + */ + public void setExternalWrench(RigidBodyReadOnly rigidBody, WrenchReadOnly externalWrench) { - return spatialAccelerationCalculator; + getExternalWrench(rigidBody).setMatchingFrame(externalWrench); } - private void computeTwistsAndSpatialAccelerations() + public void compute() { - spatialAccelerationCalculator.reset(); + initialRecursionStep.passOne(); + initialRecursionStep.passTwo(); } - private void computeNetWrenches() + /** + * Gets the definition of the multi-body system that was used to create this calculator. + * + * @return this calculator input. + */ + public MultiBodySystemReadOnly getInput() { - for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) - { - RigidBodyBasics body = allBodiesExceptRoot.get(bodyIndex); - Wrench netWrench = netWrenches.get(body); - body.getBodyFixedFrame().getTwistOfFrame(tempTwist); - if (!doVelocityTerms) - tempTwist.setToZero(); - tempAcceleration.setIncludingFrame(spatialAccelerationCalculator.getAccelerationOfBody(body)); - body.getInertia().computeDynamicWrenchFast(tempAcceleration, tempTwist, netWrench); - } + return input; } - private final Wrench wrenchExertedByChild = new Wrench(); - private void computeJointWrenchesAndTorques() + /** + * Gets the computed joint efforts. + * + * @return this calculator output: the joint efforts. + */ + public DenseMatrix64F getJointTauMatrix() { - for (int jointIndex = allJoints.size() - 1; jointIndex >= 0; jointIndex--) - { - JointBasics joint = allJoints.get(jointIndex); + return jointTauMatrix; + } - RigidBodyBasics successor = joint.getSuccessor(); + /** + * Gets the computed wrench for the given {@code joint}. + * + * @param joint the query. Not modified. + * @return the joint wrench or {@code null} if this calculator does not consider the given joint. + */ + public WrenchReadOnly getComputedJointWrench(JointReadOnly joint) + { + RecursionStep recursionStep = rigidBodyToRecursionStepMap.get(joint.getSuccessor()); + if (recursionStep == null) + return null; + else + return recursionStep.jointWrench; + } - Wrench jointWrench = jointWrenches.get(joint); - jointWrench.setIncludingFrame(netWrenches.get(successor)); + /** + * Gets the computed N-by-1 effort vector for the given {@code joint}, where N is the number of + * degrees of freedom the joint has. + * + * @param joint the query. Not modify. + * @return the tau matrix. + */ + public DenseMatrix64F getComputedJointTau(JointReadOnly joint) + { + RecursionStep recursionStep = rigidBodyToRecursionStepMap.get(joint.getSuccessor()); - WrenchReadOnly externalWrench = externalWrenches.get(successor); - jointWrench.sub(externalWrench); + if (recursionStep == null) + return null; + else + return recursionStep.tau; + } - List childrenJoints = successor.getChildrenJoints(); + /** + * Writes the computed joint efforts into the given {@code joints}. + *

+ * Any joint that is not considered by this calculator remains unchanged. + *

+ * + * @param joints the array of joints to write the effort into. Modified. + */ + public void writeComputedJointWrenches(JointBasics[] joints) + { + for (JointBasics joint : joints) + writeComputedJointWrench(joint); + } - for (int childIndex = 0; childIndex < childrenJoints.size(); childIndex++) - { - JointBasics child = childrenJoints.get(childIndex); - if (!jointsToIgnore.contains(child)) - { - WrenchReadOnly wrenchExertedOnChild = jointWrenches.get(child); - ReferenceFrame successorFrame = successor.getBodyFixedFrame(); - - wrenchExertedByChild.setIncludingFrame(wrenchExertedOnChild); - wrenchExertedByChild.setBodyFrame(successorFrame); - wrenchExertedByChild.scale(-1.0); // Action = -reaction - wrenchExertedByChild.changeFrame(jointWrench.getReferenceFrame()); - jointWrench.sub(wrenchExertedByChild); - } - } + /** + * Writes the computed joint efforts into the given {@code joints}. + *

+ * Any joint that is not considered by this calculator remains unchanged. + *

+ * + * @param joints the list of joints to write the effort into. Modified. + */ + public void writeComputedJointWrenches(List joints) + { + for (int i = 0; i < joints.size(); i++) + writeComputedJointWrench(joints.get(i)); + } - jointWrench.changeFrame(joint.getFrameAfterJoint()); + /** + * Writes the computed effort into the given {@code joint}. + *

+ * If this calculator does not consider this joint, it remains unchanged. + *

+ * + * @param joint the joint to retrieve the acceleration of and to store it. Modified. + * @return {@code true} if the joint effort was modified, {@code false} otherwise. + */ + public boolean writeComputedJointWrench(JointBasics joint) + { + WrenchReadOnly jointWrench = getComputedJointWrench(joint); - tempWrench.setIncludingFrame(joint.getJointWrench()); - joint.setJointWrench(jointWrench); + if (jointWrench == null) + return false; - DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); - coriolisWrench.zero(); - joint.getJointTau(0, coriolisWrench); - joint.setJointWrench(tempWrench); - } + joint.setJointWrench(jointWrench); + return true; } - private void populateMapsAndLists() + /** + * Gets the rigid-body acceleration provider that uses accelerations computed in this calculator. + * + * @return the acceleration provider backed by this calculator. + */ + public RigidBodyAccelerationProvider getAccelerationProvider() { - ArrayList morgue = new ArrayList(); - morgue.add(rootBody); + return coriolisAccelerationProvider; + } - while (!morgue.isEmpty()) + /** Intermediate result used for garbage free operations. */ + private final SpatialForce jointForceFromChild = new SpatialForce(); + + /** + * Represents a single recursion step with all the intermediate variables needed. + * + * @author Sylvain Bertrand + */ + private final class RecursionStep + { + /** + * The rigid-body for which this recursion is. + */ + 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. + */ + private final SpatialInertia bodyInertia; + /** + * The recursion step holding onto the direct predecessor of this recursion step's rigid-body. + */ + private final RecursionStep parent; + /** + * The recursion steps holding onto the direct successor of this recursion step's rigid-body. + */ + private final List children = new ArrayList<>(); + /** + * Calculated joint wrench, before projection onto the joint motion subspace. + */ + private final Wrench jointWrench; + /** + * User input: external wrench to be applied to this body. + */ + private final FixedFrameWrenchBasics externalWrench; + + /** + * The rigid-body spatial acceleration. + */ + private final SpatialAcceleration rigidBodyAcceleration; + /** + * Intermediate variable for storing this joint twist. + */ + private final Twist localJointTwist = new Twist(); + /** + * S is the 6-by-N matrix representing the motion subspace of the parent joint, where N is + * the number of DoFs of the joint. + */ + private final DenseMatrix64F S; + /** + * Computed joint effort. + */ + private final DenseMatrix64F tau; + /** + * Computed joint wrench, before projection onto the joint motion subspace. + */ + private final DenseMatrix64F jointWrenchMatrix; + /** + * Joint indices for storing {@code tau} in the main matrix {@code jointTauMatrix}. + */ + private int[] jointIndices; + + public RecursionStep(RigidBodyReadOnly rigidBody, RecursionStep parent, int[] jointIndices) { - RigidBodyBasics currentBody = morgue.get(0); + this.rigidBody = rigidBody; + this.parent = parent; + this.jointIndices = jointIndices; + rigidBodyAcceleration = new SpatialAcceleration(getBodyFixedFrame(), input.getInertialFrame(), getBodyFixedFrame()); - ReferenceFrame bodyFixedFrame = currentBody.getBodyFixedFrame(); + if (isRoot()) + { + bodyInertia = null; + jointWrench = null; + externalWrench = null; + S = null; + tau = null; + jointWrenchMatrix = null; + } + else + { + parent.children.add(this); + int nDoFs = getJoint().getDegreesOfFreedom(); + + bodyInertia = new SpatialInertia(rigidBody.getInertia()); + jointWrench = new Wrench(); + externalWrench = new Wrench(getBodyFixedFrame(), getBodyFixedFrame()); + S = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, nDoFs); + tau = new DenseMatrix64F(nDoFs, 1); + jointWrenchMatrix = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, 1); + getJoint().getMotionSubspace(S); + } + } - if (!currentBody.isRootBody()) + public void includeIgnoredSubtreeInertia() + { + if (!isRoot() && children.size() != rigidBody.getChildrenJoints().size()) { - allBodiesExceptRoot.add(currentBody); - netWrenches.put(currentBody, new Wrench(bodyFixedFrame, bodyFixedFrame)); - if (externalWrenches.get(currentBody) == null) + for (JointReadOnly childJoint : rigidBody.getChildrenJoints()) { - listOfBodiesWithExternalWrenches.add(currentBody); - externalWrenches.put(currentBody, new Wrench(bodyFixedFrame, bodyFixedFrame)); + if (input.getJointsToIgnore().contains(childJoint)) + { + SpatialInertia subtreeIneria = MultiBodySystemTools.computeSubtreeInertia(childJoint); + subtreeIneria.changeFrame(getBodyFixedFrame()); + bodyInertia.add(subtreeIneria); + } } } - if (currentBody.hasChildrenJoints()) + for (int childIndex = 0; childIndex < children.size(); childIndex++) + children.get(childIndex).includeIgnoredSubtreeInertia(); + } + + /** + * First pass going from the root to the leaves. + *

+ * Here the rigid-body accelerations are updated and the net wrenches resulting from the rigid-body + * acceleration and velocity are computed. + *

+ */ + public void passOne() + { + if (!isRoot()) { - List childrenJoints = currentBody.getChildrenJoints(); - for (JointBasics joint : childrenJoints) + rigidBodyAcceleration.setIncludingFrame(parent.rigidBodyAcceleration); + + TwistReadOnly bodyTwistToUse; + + if (considerCoriolisAndCentrifugalForces) { - if (!jointsToIgnore.contains(joint)) - { - RigidBodyBasics successor = joint.getSuccessor(); - if (successor != null) - { - if (allBodiesExceptRoot.contains(successor)) - { - throw new RuntimeException("This algorithm doesn't do loops."); - } - - allJoints.add(joint); - jointWrenches.put(joint, new Wrench()); - coriolisWrenches.put(joint, new DenseMatrix64F(joint.getDegreesOfFreedom(), 1)); - morgue.add(successor); - } - } + getJoint().getPredecessorTwist(localJointTwist); + rigidBodyAcceleration.changeFrame(getBodyFixedFrame(), localJointTwist, parent.getBodyFixedFrame().getTwistOfFrame()); + bodyTwistToUse = getBodyTwist(); + } + else + { + rigidBodyAcceleration.changeFrame(getBodyFixedFrame()); + bodyTwistToUse = null; } + + rigidBodyAcceleration.setBodyFrame(getBodyFixedFrame()); + + bodyInertia.computeDynamicWrench(rigidBodyAcceleration, bodyTwistToUse, jointWrench); } - morgue.remove(currentBody); + for (int childIndex = 0; childIndex < children.size(); childIndex++) + { + children.get(childIndex).passOne(); + } } - } - public void reset() - { - for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) + /** + * Second pass going from leaves to the root. + *

+ * The net wrenches are propagated upstream and summed up at each rigid-body to compute the joint + * effort. + *

+ */ + public void passTwo() { - Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); - externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getReferenceFrame()); + for (int childIndex = 0; childIndex < children.size(); childIndex++) + { + children.get(childIndex).passTwo(); + } + + if (isRoot()) + return; + + jointWrench.sub(externalWrench); + jointWrench.changeFrame(getFrameAfterJoint()); + + for (int childIndex = 0; childIndex < children.size(); childIndex++) + { + jointForceFromChild.setIncludingFrame(children.get(childIndex).jointWrench); + jointForceFromChild.changeFrame(getFrameAfterJoint()); + jointWrench.add(jointForceFromChild); + } + + jointWrench.get(jointWrenchMatrix); + CommonOps.multTransA(S, jointWrenchMatrix, tau); + + for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) + { + jointTauMatrix.set(jointIndices[dofIndex], 0, tau.get(dofIndex, 0)); + } } - } - public void getJointCoriolisMatrix(JointBasics joint, DenseMatrix64F jointCoriolisMatrixToPack) - { - jointCoriolisMatrixToPack.set(coriolisWrenches.get(joint)); - } + /** + * Resets the external wrenches from here down the leaves recursively. + */ + public void setExternalWrenchToZeroRecursive() + { + if (!isRoot()) + externalWrench.setToZero(); - public void getJointCoriolisMatrix(JointBasics joint, DenseMatrix64F jointCoriolisMatrixToPack, int startRow) - { - DenseMatrix64F coriolisWrench = coriolisWrenches.get(joint); - CommonOps.extract(coriolisWrench, 0, coriolisWrench.getNumRows(), 0, 1, jointCoriolisMatrixToPack, startRow, 0); + for (int childIndex = 0; childIndex < children.size(); childIndex++) + { + children.get(childIndex).setExternalWrenchToZeroRecursive(); + } + } + + private MovingReferenceFrame getBodyFixedFrame() + { + return rigidBody.getBodyFixedFrame(); + } + + public MovingReferenceFrame getFrameAfterJoint() + { + return getJoint().getFrameAfterJoint(); + } + + public JointReadOnly getJoint() + { + return rigidBody.getParentJoint(); + } + + public TwistReadOnly getBodyTwist() + { + return getBodyFixedFrame().getTwistOfFrame(); + } + + private boolean isRoot() + { + return parent == null; + } } } From 4f491d83bef91084b1958fa2624ceb08e8527894 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 9 Jun 2020 12:43:57 -0500 Subject: [PATCH 17/27] :arrow_up: EJML, GeoRegression, DDogleg, BoofCV. --- .../DifferentialIDMassMatrixCalculator.java | 38 +++++----- ...amicallyConsistentNullspaceCalculator.java | 8 +- ...oatingBaseRigidBodyDynamicsCalculator.java | 76 +++++++++---------- ...oriolisExternalWrenchMatrixCalculator.java | 26 +++---- .../screwTheory/MassMatrixCalculator.java | 6 +- 5 files changed, 77 insertions(+), 77 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java index 22faf8b..291d566 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java @@ -2,8 +2,8 @@ import java.util.LinkedHashMap; -import org.ejml.data.DenseMatrix64F; -import org.ejml.ops.CommonOps; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.matrixlib.MatrixTools; @@ -23,12 +23,12 @@ public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator { private final InverseDynamicsCalculator idCalculator; private final JointBasics[] jointsInOrder; - private final DenseMatrix64F massMatrix; - private final DenseMatrix64F storedJointDesiredAccelerations; - private final DenseMatrix64F tmpDesiredJointAccelerationsMatrix; - private final DenseMatrix64F tmpTauMatrix; + private final DMatrixRMaj massMatrix; + private final DMatrixRMaj storedJointDesiredAccelerations; + private final DMatrixRMaj tmpDesiredJointAccelerationsMatrix; + private final DMatrixRMaj tmpTauMatrix; private final LinkedHashMap storedJointWrenches = new LinkedHashMap(); - private final DenseMatrix64F storedJointVelocities; + private final DMatrixRMaj storedJointVelocities; private final int totalNumberOfDoFs; @@ -40,12 +40,12 @@ public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBod idCalculator.setRootAcceleration(zeroRootAcceleration); jointsInOrder = MultiBodySystemTools.collectSubtreeJoints(rootBody); totalNumberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); - massMatrix = new DenseMatrix64F(totalNumberOfDoFs, totalNumberOfDoFs); + massMatrix = new DMatrixRMaj(totalNumberOfDoFs, totalNumberOfDoFs); - storedJointDesiredAccelerations = new DenseMatrix64F(totalNumberOfDoFs, 1); - storedJointVelocities = new DenseMatrix64F(totalNumberOfDoFs, 1); - tmpDesiredJointAccelerationsMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); - tmpTauMatrix = new DenseMatrix64F(totalNumberOfDoFs, 1); + storedJointDesiredAccelerations = new DMatrixRMaj(totalNumberOfDoFs, 1); + storedJointVelocities = new DMatrixRMaj(totalNumberOfDoFs, 1); + tmpDesiredJointAccelerationsMatrix = new DMatrixRMaj(totalNumberOfDoFs, 1); + tmpTauMatrix = new DMatrixRMaj(totalNumberOfDoFs, 1); for (JointBasics joint : jointsInOrder) { @@ -84,7 +84,7 @@ private void setDesiredAccelerationsToZero() for (JointBasics joint : jointsInOrder) { joint.setJointAccelerationToZero(); - joint.setJointVelocity(0, new DenseMatrix64F(joint.getDegreesOfFreedom(), 1)); + joint.setJointVelocity(0, new DMatrixRMaj(joint.getDegreesOfFreedom(), 1)); } } @@ -94,12 +94,12 @@ private void storeJointState() MultiBodySystemTools.extractJointsState(jointsInOrder, JointStateType.VELOCITY, storedJointVelocities); for (JointBasics joint : jointsInOrder) { - DenseMatrix64F tauMatrix = new DenseMatrix64F(joint.getDegreesOfFreedom(), 1); + DMatrixRMaj tauMatrix = new DMatrixRMaj(joint.getDegreesOfFreedom(), 1); joint.getJointTau(0, tauMatrix); - DenseMatrix64F spatialForce = new DenseMatrix64F(SpatialForce.SIZE, 1); - DenseMatrix64F motionSubspace = new DenseMatrix64F(6, joint.getDegreesOfFreedom()); + DMatrixRMaj spatialForce = new DMatrixRMaj(SpatialForce.SIZE, 1); + DMatrixRMaj motionSubspace = new DMatrixRMaj(6, joint.getDegreesOfFreedom()); joint.getMotionSubspace(motionSubspace); - CommonOps.mult(motionSubspace, tauMatrix, spatialForce); + CommonOps_DDRM.mult(motionSubspace, tauMatrix, spatialForce); Wrench jointWrench = storedJointWrenches.get(joint); jointWrench.setIncludingFrame(joint.getFrameAfterJoint(), spatialForce); jointWrench.changeFrame(joint.getFrameAfterJoint()); @@ -118,13 +118,13 @@ private void restoreJointState() } @Override - public DenseMatrix64F getMassMatrix() + public DMatrixRMaj getMassMatrix() { return massMatrix; } @Override - public void getMassMatrix(DenseMatrix64F massMatrixToPack) + public void getMassMatrix(DMatrixRMaj massMatrixToPack) { massMatrixToPack.set(massMatrix); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java index ab62869..3a50a73 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.screwTheory; -import org.ejml.data.DenseMatrix64F; +import org.ejml.data.DMatrixRMaj; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; @@ -13,13 +13,13 @@ public interface DynamicallyConsistentNullspaceCalculator { void reset(); - void addConstraint(RigidBodyBasics body, DenseMatrix64F selectionMatrix); + void addConstraint(RigidBodyBasics body, DMatrixRMaj selectionMatrix); void addActuatedJoint(JointBasics joint); void compute(); - DenseMatrix64F getDynamicallyConsistentNullspace(); + DMatrixRMaj getDynamicallyConsistentNullspace(); - DenseMatrix64F getSNsBar(); + DMatrixRMaj getSNsBar(); } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java index c7f17bb..f6afd0b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java @@ -1,59 +1,59 @@ package us.ihmc.robotics.screwTheory; -import org.ejml.data.DenseMatrix64F; -import org.ejml.factory.LinearSolverFactory; -import org.ejml.interfaces.linsol.LinearSolver; -import org.ejml.ops.CommonOps; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; +import org.ejml.interfaces.linsol.LinearSolverDense; public class FloatingBaseRigidBodyDynamicsCalculator { private static final int large = 1000; private static final double tolerance = 0.0001; - private static final LinearSolver pseudoInverseSolver = LinearSolverFactory.pseudoInverse(true); + private static final LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); - private final DenseMatrix64F matrixTranspose = new DenseMatrix64F(large, large); - private final DenseMatrix64F localVector = new DenseMatrix64F(large, 1); + private final DMatrixRMaj matrixTranspose = new DMatrixRMaj(large, large); + private final DMatrixRMaj localVector = new DMatrixRMaj(large, 1); public FloatingBaseRigidBodyDynamicsCalculator() {} - public void computeQddotGivenRho(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, - DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F qddotToPack, DenseMatrix64F rho) + public void computeQddotGivenRho(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj qddotToPack, DMatrixRMaj rho) { computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); - CommonOps.scale(-1.0, floatingBaseCoriolisMatrix); - CommonOps.multAdd(matrixTranspose, rho, floatingBaseCoriolisMatrix); + CommonOps_DDRM.scale(-1.0, floatingBaseCoriolisMatrix); + CommonOps_DDRM.multAdd(matrixTranspose, rho, floatingBaseCoriolisMatrix); pseudoInverseSolver.setA(floatingBaseMassMatrix); pseudoInverseSolver.solve(floatingBaseCoriolisMatrix, qddotToPack); } - public void computeRhoGivenQddot(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, - DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F rhoToPack) + public void computeRhoGivenQddot(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj rhoToPack) { computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); - CommonOps.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); + CommonOps_DDRM.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); pseudoInverseSolver.setA(matrixTranspose); pseudoInverseSolver.solve(floatingBaseCoriolisMatrix, rhoToPack); } - public void computeTauGivenRhoAndQddot(DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, - DenseMatrix64F qddot, DenseMatrix64F rho, DenseMatrix64F tauToPack) + public void computeTauGivenRhoAndQddot(DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, + DMatrixRMaj qddot, DMatrixRMaj rho, DMatrixRMaj tauToPack) { computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); - CommonOps.mult(bodyMassMatrix, qddot, tauToPack); - CommonOps.multAdd(-1.0, matrixTranspose, rho, tauToPack); - CommonOps.addEquals(tauToPack, bodyCoriolisMatrix); + CommonOps_DDRM.mult(bodyMassMatrix, qddot, tauToPack); + CommonOps_DDRM.multAdd(-1.0, matrixTranspose, rho, tauToPack); + CommonOps_DDRM.addEquals(tauToPack, bodyCoriolisMatrix); } - public void computeTauGivenRho(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, DenseMatrix64F floatingBaseContactForceJacobianMatrix, - DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F rho, DenseMatrix64F tauToPack) + public void computeTauGivenRho(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, DMatrixRMaj floatingBaseContactForceJacobianMatrix, + DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, DMatrixRMaj rho, DMatrixRMaj tauToPack) { computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); @@ -62,8 +62,8 @@ public void computeTauGivenRho(DenseMatrix64F floatingBaseMassMatrix, DenseMatri computeTauGivenRhoAndQddot(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, localVector, rho, tauToPack); } - public void computeTauGivenQddot(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, DenseMatrix64F floatingBaseContactForceJacobianMatrix, - DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F tauToPack) + public void computeTauGivenQddot(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, DMatrixRMaj floatingBaseContactForceJacobianMatrix, + DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj tauToPack) { computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); @@ -72,9 +72,9 @@ public void computeTauGivenQddot(DenseMatrix64F floatingBaseMassMatrix, DenseMat computeTauGivenRhoAndQddot(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, qddot, localVector, tauToPack); } - public boolean areFloatingBaseRigidBodyDynamicsSatisfied(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, - DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, - DenseMatrix64F bodyContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F tau, DenseMatrix64F rho) + public boolean areFloatingBaseRigidBodyDynamicsSatisfied(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, + DMatrixRMaj bodyContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj tau, DMatrixRMaj rho) { if (!areFloatingBaseDynamicsSatisfied(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, qddot, rho)) return false; @@ -82,37 +82,37 @@ public boolean areFloatingBaseRigidBodyDynamicsSatisfied(DenseMatrix64F floating return areRigidBodyDynamicsSatisfied(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, qddot, tau, rho); } - public boolean areFloatingBaseDynamicsSatisfied(DenseMatrix64F floatingBaseMassMatrix, DenseMatrix64F floatingBaseCoriolisMatrix, - DenseMatrix64F floatingBaseContactForceJacobianMatrix, DenseMatrix64F qddot, DenseMatrix64F rho) + public boolean areFloatingBaseDynamicsSatisfied(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj rho) { computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); - CommonOps.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); - CommonOps.multAdd(-1.0, matrixTranspose, rho, floatingBaseCoriolisMatrix); + CommonOps_DDRM.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); + CommonOps_DDRM.multAdd(-1.0, matrixTranspose, rho, floatingBaseCoriolisMatrix); return equalsZero(floatingBaseCoriolisMatrix, tolerance); } - public boolean areRigidBodyDynamicsSatisfied(DenseMatrix64F bodyMassMatrix, DenseMatrix64F bodyCoriolisMatrix, DenseMatrix64F bodyContactForceJacobianMatrix, - DenseMatrix64F qddot, DenseMatrix64F tau, DenseMatrix64F rho) + public boolean areRigidBodyDynamicsSatisfied(DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, + DMatrixRMaj qddot, DMatrixRMaj tau, DMatrixRMaj rho) { computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); - CommonOps.multAdd(bodyMassMatrix, qddot, bodyCoriolisMatrix); - CommonOps.multAdd(-1.0, matrixTranspose, rho, bodyCoriolisMatrix); - CommonOps.subtractEquals(bodyCoriolisMatrix, tau); + CommonOps_DDRM.multAdd(bodyMassMatrix, qddot, bodyCoriolisMatrix); + CommonOps_DDRM.multAdd(-1.0, matrixTranspose, rho, bodyCoriolisMatrix); + CommonOps_DDRM.subtractEquals(bodyCoriolisMatrix, tau); return equalsZero(bodyCoriolisMatrix, tolerance); } - private void computeJacobianTranspose(DenseMatrix64F jacobian, DenseMatrix64F jacobianTransposeToPack) + private void computeJacobianTranspose(DMatrixRMaj jacobian, DMatrixRMaj jacobianTransposeToPack) { jacobianTransposeToPack.reshape(jacobian.getNumCols(), jacobian.getNumRows()); jacobianTransposeToPack.zero(); - CommonOps.transpose(jacobian, jacobianTransposeToPack); + CommonOps_DDRM.transpose(jacobian, jacobianTransposeToPack); } - private static boolean equalsZero(DenseMatrix64F matrix, double tolerance) + private static boolean equalsZero(DMatrixRMaj matrix, double tolerance) { for (int rowIndex = 0; rowIndex < matrix.getNumRows(); rowIndex++) { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index 6c263a4..ff21564 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -6,8 +6,8 @@ import java.util.List; import java.util.Map; -import org.ejml.data.DenseMatrix64F; -import org.ejml.ops.CommonOps; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; @@ -54,7 +54,7 @@ public class GravityCoriolisExternalWrenchMatrixCalculator * The output of this algorithm: the effort matrix for all the joints to consider resulting from * Coriolis, centrifugal, and external wrenches. */ - private final DenseMatrix64F jointTauMatrix; + private final DMatrixRMaj jointTauMatrix; /** Whether the effort resulting from the Coriolis and centrifugal forces should be considered. */ private final boolean considerCoriolisAndCentrifugalForces; @@ -176,7 +176,7 @@ public GravityCoriolisExternalWrenchMatrixCalculator(MultiBodySystemReadOnly inp initialRecursionStep.includeIgnoredSubtreeInertia(); int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(input.getJointsToConsider()); - jointTauMatrix = new DenseMatrix64F(nDoFs, 1); + jointTauMatrix = new DMatrixRMaj(nDoFs, 1); coriolisAccelerationProvider = RigidBodyAccelerationProvider.toRigidBodyAccelerationProvider(body -> rigidBodyToRecursionStepMap.get(body).rigidBodyAcceleration, input.getInertialFrame(), @@ -342,7 +342,7 @@ public MultiBodySystemReadOnly getInput() * * @return this calculator output: the joint efforts. */ - public DenseMatrix64F getJointTauMatrix() + public DMatrixRMaj getJointTauMatrix() { return jointTauMatrix; } @@ -369,7 +369,7 @@ public WrenchReadOnly getComputedJointWrench(JointReadOnly joint) * @param joint the query. Not modify. * @return the tau matrix. */ - public DenseMatrix64F getComputedJointTau(JointReadOnly joint) + public DMatrixRMaj getComputedJointTau(JointReadOnly joint) { RecursionStep recursionStep = rigidBodyToRecursionStepMap.get(joint.getSuccessor()); @@ -486,15 +486,15 @@ private final class RecursionStep * S is the 6-by-N matrix representing the motion subspace of the parent joint, where N is * the number of DoFs of the joint. */ - private final DenseMatrix64F S; + private final DMatrixRMaj S; /** * Computed joint effort. */ - private final DenseMatrix64F tau; + private final DMatrixRMaj tau; /** * Computed joint wrench, before projection onto the joint motion subspace. */ - private final DenseMatrix64F jointWrenchMatrix; + private final DMatrixRMaj jointWrenchMatrix; /** * Joint indices for storing {@code tau} in the main matrix {@code jointTauMatrix}. */ @@ -524,9 +524,9 @@ public RecursionStep(RigidBodyReadOnly rigidBody, RecursionStep parent, int[] jo bodyInertia = new SpatialInertia(rigidBody.getInertia()); jointWrench = new Wrench(); externalWrench = new Wrench(getBodyFixedFrame(), getBodyFixedFrame()); - S = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, nDoFs); - tau = new DenseMatrix64F(nDoFs, 1); - jointWrenchMatrix = new DenseMatrix64F(SpatialVectorReadOnly.SIZE, 1); + S = new DMatrixRMaj(SpatialVectorReadOnly.SIZE, nDoFs); + tau = new DMatrixRMaj(nDoFs, 1); + jointWrenchMatrix = new DMatrixRMaj(SpatialVectorReadOnly.SIZE, 1); getJoint().getMotionSubspace(S); } } @@ -616,7 +616,7 @@ public void passTwo() } jointWrench.get(jointWrenchMatrix); - CommonOps.multTransA(S, jointWrenchMatrix, tau); + CommonOps_DDRM.multTransA(S, jointWrenchMatrix, tau); for (int dofIndex = 0; dofIndex < getJoint().getDegreesOfFreedom(); dofIndex++) { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java index b63cf5b..7305a73 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java @@ -1,6 +1,6 @@ package us.ihmc.robotics.screwTheory; -import org.ejml.data.DenseMatrix64F; +import org.ejml.data.DMatrixRMaj; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; @@ -9,9 +9,9 @@ public interface MassMatrixCalculator public abstract void compute(); - public abstract DenseMatrix64F getMassMatrix(); + public abstract DMatrixRMaj getMassMatrix(); - public abstract void getMassMatrix(DenseMatrix64F massMatrixToPack); + public abstract void getMassMatrix(DMatrixRMaj massMatrixToPack); public abstract JointBasics[] getJointsInOrder(); } \ No newline at end of file From 16c26169ec0f2f48f83bda13fc6f7350e81c162c Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Sat, 18 Jul 2020 19:24:53 -0500 Subject: [PATCH 18/27] Applied cleanup. Documentation and some cleanup of the frame objects. Started cleanup and documentation of the tools. Fixed exception. Moved YoMutableFrameObject to interfaces package. --- .../YoMutableFrameSpatialVector.java | 59 ++++++++++++------- ...haFilteredYoMutableFrameSpatialVector.java | 6 +- .../filters/AlphaFilteredYoSpatialVector.java | 6 +- .../RateLimitedYoMutableSpatialVector.java | 6 +- .../filters/RateLimitedYoSpatialVector.java | 6 +- 5 files changed, 51 insertions(+), 32 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java index d2cdac4..d47ef93 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java @@ -3,45 +3,64 @@ import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics; import us.ihmc.mecano.spatial.interfaces.SpatialVectorBasics; -import us.ihmc.yoVariables.registry.YoVariableRegistry; -import us.ihmc.yoVariables.util.YoFrameVariableNameTools; +import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFramePoint3D; +import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFrameQuaternion; +import us.ihmc.yoVariables.euclid.referenceFrame.YoMutableFrameVector3D; +import us.ihmc.yoVariables.euclid.referenceFrame.interfaces.FrameIndexMap; +import us.ihmc.yoVariables.euclid.referenceFrame.interfaces.YoMutableFrameObject; +import us.ihmc.yoVariables.registry.YoRegistry; +import us.ihmc.yoVariables.tools.YoGeometryNameTools; import us.ihmc.yoVariables.variable.YoDouble; -import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameObject; -import us.ihmc.yoVariables.variable.frameObjects.YoMutableFramePoint3D; -import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameQuaternion; -import us.ihmc.yoVariables.variable.frameObjects.YoMutableFrameVector3D; +import us.ihmc.yoVariables.variable.YoLong; /** * TODO: this should probably live in mecano? */ -public class YoMutableFrameSpatialVector extends YoMutableFrameObject implements SpatialVectorBasics +public class YoMutableFrameSpatialVector implements SpatialVectorBasics, YoMutableFrameObject { + private final YoLong frameId; + private final FrameIndexMap frameIndexMap; + private final YoMutableFrameVector3D angularPart; private final YoMutableFrameVector3D linearPart; public YoMutableFrameSpatialVector(YoMutableFrameVector3D angularPart, YoMutableFrameVector3D linearPart) { - super(angularPart.getYoFrameIndex(), angularPart.getFrameIndexMap()); + this.frameId = angularPart.getYoFrameIndex(); + this.frameIndexMap = angularPart.getFrameIndexMap(); this.angularPart = angularPart; this.linearPart = linearPart; checkFrameConsistency(); } - public YoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry) + public YoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry) { - super(namePrefix, nameSuffix, registry); + frameId = new YoLong(YoGeometryNameTools.assembleName(namePrefix, "frame", nameSuffix), registry); + frameIndexMap = new FrameIndexMap.FrameIndexHashMap(); - YoDouble xAngular = new YoDouble(YoFrameVariableNameTools.createXName(namePrefix, "Angular" + nameSuffix), registry); - YoDouble yAngular = new YoDouble(YoFrameVariableNameTools.createYName(namePrefix, "Angular" + nameSuffix), registry); - YoDouble zAngular = new YoDouble(YoFrameVariableNameTools.createZName(namePrefix, "Angular" + nameSuffix), registry); + YoDouble xAngular = new YoDouble(YoGeometryNameTools.createXName(namePrefix, "Angular" + nameSuffix), registry); + YoDouble yAngular = new YoDouble(YoGeometryNameTools.createYName(namePrefix, "Angular" + nameSuffix), registry); + YoDouble zAngular = new YoDouble(YoGeometryNameTools.createZName(namePrefix, "Angular" + nameSuffix), registry); angularPart = new YoMutableFrameVector3D(xAngular, yAngular, zAngular, getYoFrameIndex(), getFrameIndexMap()); - YoDouble xLinear = new YoDouble(YoFrameVariableNameTools.createXName(namePrefix, "Linear" + nameSuffix), registry); - YoDouble yLinear = new YoDouble(YoFrameVariableNameTools.createYName(namePrefix, "Linear" + nameSuffix), registry); - YoDouble zLinear = new YoDouble(YoFrameVariableNameTools.createZName(namePrefix, "Linear" + nameSuffix), registry); + YoDouble xLinear = new YoDouble(YoGeometryNameTools.createXName(namePrefix, "Linear" + nameSuffix), registry); + YoDouble yLinear = new YoDouble(YoGeometryNameTools.createYName(namePrefix, "Linear" + nameSuffix), registry); + YoDouble zLinear = new YoDouble(YoGeometryNameTools.createZName(namePrefix, "Linear" + nameSuffix), registry); linearPart = new YoMutableFrameVector3D(xLinear, yLinear, zLinear, getYoFrameIndex(), getFrameIndexMap()); } + @Override + public FrameIndexMap getFrameIndexMap() + { + return frameIndexMap; + } + + @Override + public YoLong getYoFrameIndex() + { + return frameId; + } + @Override public FixedFrameVector3DBasics getAngularPart() { @@ -60,21 +79,21 @@ public FixedFrameVector3DBasics getLinearPart() public ReferenceFrame getReferenceFrame() { checkFrameConsistency(); - return super.getReferenceFrame(); + return YoMutableFrameObject.super.getReferenceFrame(); } @Override public void setReferenceFrame(ReferenceFrame referenceFrame) { checkFrameConsistency(); - super.setReferenceFrame(referenceFrame); + YoMutableFrameObject.super.setReferenceFrame(referenceFrame); // When constructing this with two YoMutableFrameVector3D objects the angular part is updated only by the super implementation. linearPart.setReferenceFrame(referenceFrame); } /** - * This is a check that should be called every time this object is interacted with. If this - * failes it likely means that you created this pose using + * This is a check that should be called every time this object is interacted with. If this failes + * it likely means that you created this pose using * {@link #YoMutableFramePose3D(YoMutableFramePoint3D, YoMutableFrameQuaternion)} and changed the * reference frame of one of the passed objects without modifying the other one from outside this * class. This will make the data structure in here inconsistent. diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java index d013701..60e21d0 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java @@ -5,14 +5,14 @@ import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.robotics.dataStructures.YoMutableFrameSpatialVector; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; public class AlphaFilteredYoMutableFrameSpatialVector extends YoMutableFrameSpatialVector { private final AlphaFilteredYoMutableFrameVector3D alphaFilteredAngularPart; private final AlphaFilteredYoMutableFrameVector3D alphaFilteredLinearPart; - public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { super(new AlphaFilteredYoMutableFrameVector3D(namePrefix, nameSuffix, registry, alphaAngularPart, rawAngularPart), @@ -21,7 +21,7 @@ public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSu this.alphaFilteredLinearPart = (AlphaFilteredYoMutableFrameVector3D) getLinearPart(); } - public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + public AlphaFilteredYoMutableFrameSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, YoFixedFrameSpatialVector rawSpatialVector) { super(new AlphaFilteredYoMutableFrameVector3D(namePrefix, nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java index 7cdfb9e..91e6698 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java @@ -4,14 +4,14 @@ import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; public class AlphaFilteredYoSpatialVector extends YoFixedFrameSpatialVector { private final AlphaFilteredYoFrameVector alphaFilteredAngularPart; private final AlphaFilteredYoFrameVector alphaFilteredLinearPart; - public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { super(new AlphaFilteredYoFrameVector(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawAngularPart), @@ -20,7 +20,7 @@ public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVari this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); } - public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider alphaAngularPart, + public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, YoFixedFrameSpatialVector rawSpatialVector) { super(new AlphaFilteredYoFrameVector(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java index 4052379..b3491bc 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java @@ -5,14 +5,14 @@ import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.robotics.dataStructures.YoMutableFrameSpatialVector; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; public class RateLimitedYoMutableSpatialVector extends YoMutableFrameSpatialVector { private final RateLimitedYoMutableFrameVector3D rateLimitedAngularPart; private final RateLimitedYoMutableFrameVector3D rateLimitedLinearPart; - public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { @@ -22,7 +22,7 @@ public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, Y this.rateLimitedLinearPart = (RateLimitedYoMutableFrameVector3D) getLinearPart(); } - public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + public RateLimitedYoMutableSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, YoFixedFrameSpatialVector rawSpatialVector) { super(new RateLimitedYoMutableFrameVector3D(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java index 6f70bab..4e60963 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java @@ -4,14 +4,14 @@ import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; import us.ihmc.yoVariables.providers.DoubleProvider; -import us.ihmc.yoVariables.registry.YoVariableRegistry; +import us.ihmc.yoVariables.registry.YoRegistry; public class RateLimitedYoSpatialVector extends YoFixedFrameSpatialVector { private final RateLimitedYoFrameVector rateLimitedAngularPart; private final RateLimitedYoFrameVector rateLimitedLinearPart; - public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart), @@ -20,7 +20,7 @@ public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariab this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); } - public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoVariableRegistry registry, DoubleProvider maximumAngularRate, + public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, YoFixedFrameSpatialVector rawSpatialVector) { super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), From b538f8c3a881d050a467515e7d80c950f038516a Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Fri, 15 Oct 2021 13:00:08 -0500 Subject: [PATCH 19/27] First version of momentum estimator --- .../screwTheory/MomentumCalculator.java | 29 ++++++++----------- 1 file changed, 12 insertions(+), 17 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java index a3ca854..eb363b8 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java @@ -2,40 +2,35 @@ import java.util.stream.Stream; -import us.ihmc.euclid.tuple3D.Vector3D; -import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly; import us.ihmc.mecano.spatial.Momentum; -import us.ihmc.mecano.spatial.Twist; -import us.ihmc.mecano.spatial.interfaces.SpatialInertiaBasics; +import us.ihmc.mecano.spatial.interfaces.FixedFrameMomentumBasics; +import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly; public class MomentumCalculator { - private final Twist tempTwist = new Twist(); private final Momentum tempMomentum = new Momentum(); - private final Vector3D zero = new Vector3D(); - private final RigidBodyBasics[] rigidBodiesInOrders; + private final RigidBodyReadOnly[] rigidBodiesInOrders; - public MomentumCalculator(RigidBodyBasics... rigidBodies) + public MomentumCalculator(RigidBodyReadOnly... rigidBodies) { - rigidBodiesInOrders = Stream.of(rigidBodies).filter(body -> body.getInertia() != null).toArray(RigidBodyBasics[]::new); + rigidBodiesInOrders = Stream.of(rigidBodies).filter(body -> body.getInertia() != null).toArray(RigidBodyReadOnly[]::new); } - public MomentumCalculator(RigidBodyBasics rootBody) + public MomentumCalculator(RigidBodyReadOnly rootBody) { this(rootBody.subtreeArray()); } - public void computeAndPack(Momentum momentum) + public void computeAndPack(FixedFrameMomentumBasics momentum) { - momentum.getAngularPart().set(zero); - momentum.getLinearPart().set(zero); + momentum.setToZero(); - for (RigidBodyBasics rigidBody : rigidBodiesInOrders) + for (RigidBodyReadOnly rigidBody : rigidBodiesInOrders) { - SpatialInertiaBasics inertia = rigidBody.getInertia(); - rigidBody.getBodyFixedFrame().getTwistOfFrame(tempTwist); + SpatialInertiaReadOnly inertia = rigidBody.getInertia(); tempMomentum.setReferenceFrame(inertia.getReferenceFrame()); - tempMomentum.compute(inertia, tempTwist); + tempMomentum.compute(inertia, rigidBody.getBodyFixedFrame().getTwistOfFrame()); tempMomentum.changeFrame(momentum.getReferenceFrame()); momentum.add(tempMomentum); } From f7c88096ddc782400d7b81ed6fba85cc9c54407d Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Fri, 24 Sep 2021 15:00:46 -0500 Subject: [PATCH 20/27] set up a whole-body inertia calculator Adding calculation of leg momentum Only use the joints to consider Allow to set a base frame --- .../screwTheory/MomentumCalculator.java | 48 ++++++++++++++++--- 1 file changed, 41 insertions(+), 7 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java index eb363b8..239807c 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java @@ -2,16 +2,24 @@ import java.util.stream.Stream; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +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.Twist; import us.ihmc.mecano.spatial.interfaces.FixedFrameMomentumBasics; import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly; public class MomentumCalculator { - private final Momentum tempMomentum = new Momentum(); private final RigidBodyReadOnly[] rigidBodiesInOrders; + private final Twist bodyTwist = new Twist(); + private final Momentum bodyMomentum = new Momentum(); + + private ReferenceFrame baseFrame = null; + public MomentumCalculator(RigidBodyReadOnly... rigidBodies) { rigidBodiesInOrders = Stream.of(rigidBodies).filter(body -> body.getInertia() != null).toArray(RigidBodyReadOnly[]::new); @@ -22,17 +30,43 @@ public MomentumCalculator(RigidBodyReadOnly rootBody) this(rootBody.subtreeArray()); } + public MomentumCalculator(MultiBodySystemReadOnly input) + { + rigidBodiesInOrders = input.getJointsToConsider().stream().map(JointReadOnly::getSuccessor).filter(body -> body.getInertia() != null) + .toArray(RigidBodyReadOnly[]::new); + } + + public void setBaseFrame(ReferenceFrame baseFrame) + { + this.baseFrame = baseFrame; + } + public void computeAndPack(FixedFrameMomentumBasics momentum) { momentum.setToZero(); - for (RigidBodyReadOnly rigidBody : rigidBodiesInOrders) + if (baseFrame == null) + { + for (RigidBodyReadOnly rigidBody : rigidBodiesInOrders) + { + SpatialInertiaReadOnly inertia = rigidBody.getInertia(); + bodyMomentum.setReferenceFrame(inertia.getReferenceFrame()); + bodyMomentum.compute(inertia, rigidBody.getBodyFixedFrame().getTwistOfFrame()); + bodyMomentum.changeFrame(momentum.getReferenceFrame()); + momentum.add(bodyMomentum); + } + } + else { - SpatialInertiaReadOnly inertia = rigidBody.getInertia(); - tempMomentum.setReferenceFrame(inertia.getReferenceFrame()); - tempMomentum.compute(inertia, rigidBody.getBodyFixedFrame().getTwistOfFrame()); - tempMomentum.changeFrame(momentum.getReferenceFrame()); - momentum.add(tempMomentum); + for (RigidBodyReadOnly rigidBody : rigidBodiesInOrders) + { + SpatialInertiaReadOnly inertia = rigidBody.getInertia(); + bodyMomentum.setReferenceFrame(inertia.getReferenceFrame()); + rigidBody.getBodyFixedFrame().getTwistRelativeToOther(baseFrame, bodyTwist); + bodyMomentum.compute(inertia, bodyTwist); + bodyMomentum.changeFrame(momentum.getReferenceFrame()); + momentum.add(bodyMomentum); + } } } } From 17105c538334c83193921da38ae6528cfdf7705b Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 9 Aug 2022 15:44:56 -0500 Subject: [PATCH 21/27] Addressed some frame mismatch exceptions. Cleanup UI IK solver. Get detached arm robot. Workaround bugs. WBCC compute runs without crashing. Add note. Fix four bar knees in kinematics sims. Getting somewhere. The solution appears to be somewhat trying to achieve the desired pose. Add missing multi body system tool. Add exception for mismatched oneDoFJoint array lengths. --- .../robotics/MultiBodySystemMissingTools.java | 92 +++++++++++++++++++ .../YoMutableFrameSpatialVector.java | 1 - 2 files changed, 92 insertions(+), 1 deletion(-) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java new file mode 100644 index 0000000..e6d5115 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java @@ -0,0 +1,92 @@ +package us.ihmc.robotics; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.mecano.multiBodySystem.CrossFourBarJoint; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.tools.MultiBodySystemTools; + +import java.util.ArrayList; +import java.util.List; + +public class MultiBodySystemMissingTools +{ + public static void copyOneDoFJointsConfiguration(JointBasics[] source, JointBasics[] destination) + { + OneDoFJointBasics[] oneDofJoints1 = MultiBodySystemTools.filterJoints(source, OneDoFJointBasics.class); + OneDoFJointBasics[] oneDofJoints2 = MultiBodySystemTools.filterJoints(destination, OneDoFJointBasics.class); + + if (oneDofJoints1.length != oneDofJoints2.length) + { + throw new IllegalArgumentException("The lists of joints must be the same length. %d != %d".formatted(oneDofJoints1.length, + oneDofJoints2.length)); + } + + for (int i = 0; i < oneDofJoints1.length; i++) + { + oneDofJoints2[i].setJointConfiguration(oneDofJoints1[i]); + } + } + + public static List getSubtreeJointsIncludingFourBars(RigidBodyBasics rootBody) + { + List joints = new ArrayList<>(); + + for (JointBasics joint : rootBody.childrenSubtreeIterable()) + { + if (joint instanceof CrossFourBarJoint) + { + joints.addAll(((CrossFourBarJoint) joint).getFourBarFunction().getLoopJoints()); + } + else + { + joints.add(joint); + } + } + + return joints; + } + + public static MultiBodySystemBasics createSingleBodySystem(RigidBodyBasics singleBody) + { + List allJoints = new ArrayList<>(); + List jointsToConsider = new ArrayList<>(); + List jointsToIgnore = new ArrayList<>(); + JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider); + + return new MultiBodySystemBasics() + { + @Override + public RigidBodyBasics getRootBody() + { + return singleBody; + } + + @Override + public List getAllJoints() + { + return allJoints; + } + + @Override + public List getJointsToConsider() + { + return jointsToConsider; + } + + @Override + public List getJointsToIgnore() + { + return jointsToIgnore; + } + + @Override + public JointMatrixIndexProvider getJointMatrixIndexProvider() + { + return jointMatrixIndexProvider; + } + }; + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java index d47ef93..3865585 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java @@ -85,7 +85,6 @@ public ReferenceFrame getReferenceFrame() @Override public void setReferenceFrame(ReferenceFrame referenceFrame) { - checkFrameConsistency(); YoMutableFrameObject.super.setReferenceFrame(referenceFrame); // When constructing this with two YoMutableFrameVector3D objects the angular part is updated only by the super implementation. linearPart.setReferenceFrame(referenceFrame); From 45f2cddec9ba344f33a47d27a07471f98231ce30 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Wed, 16 Aug 2023 10:05:43 -0500 Subject: [PATCH 22/27] Fixed the gravity calculator for the cross 4 bar joint. --- ...yCoriolisExternalWrenchMatrixCalculator.java | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java index ff21564..6e09641 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -478,6 +478,10 @@ private final class RecursionStep * The rigid-body spatial acceleration. */ private final SpatialAcceleration rigidBodyAcceleration; + /** + * Intermediate variable for storing this joint acceleration. + */ + private final SpatialAcceleration localJointAcceleration = new SpatialAcceleration(); /** * Intermediate variable for storing this joint twist. */ @@ -561,6 +565,9 @@ public void passOne() { if (!isRoot()) { + if (getJoint().isMotionSubspaceVariable()) + getJoint().getMotionSubspace(S); + rigidBodyAcceleration.setIncludingFrame(parent.rigidBodyAcceleration); TwistReadOnly bodyTwistToUse; @@ -570,6 +577,16 @@ public void passOne() getJoint().getPredecessorTwist(localJointTwist); rigidBodyAcceleration.changeFrame(getBodyFixedFrame(), localJointTwist, parent.getBodyFixedFrame().getTwistOfFrame()); bodyTwistToUse = getBodyTwist(); + + if (getJoint().isMotionSubspaceVariable()) + { + SpatialAccelerationReadOnly jointBiasAcceleration = getJoint().getJointBiasAcceleration(); + localJointAcceleration.setIncludingFrame(jointBiasAcceleration); + localJointAcceleration.changeFrame(getBodyFixedFrame()); + localJointAcceleration.setBodyFrame(getBodyFixedFrame()); + localJointAcceleration.setBaseFrame(parent.getBodyFixedFrame()); + rigidBodyAcceleration.add(localJointAcceleration); + } } else { From 2a5c7bccb99c8516197571d02e527ef5f619fe65 Mon Sep 17 00:00:00 2001 From: Sylvain Bertrand Date: Tue, 5 Mar 2024 10:12:14 -0600 Subject: [PATCH 23/27] New twist calculator that is more flexible Added computation of the achieved velocities for the IK mode. Last couple tweaks. --- .../screwTheory/RigidBodyTwistCalculator.java | 203 ++++++++++++++++++ 1 file changed, 203 insertions(+) create mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java new file mode 100644 index 0000000..9847652 --- /dev/null +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java @@ -0,0 +1,203 @@ +package us.ihmc.robotics.screwTheory; + +import org.ejml.data.DMatrix; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePoint3DReadOnly; +import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; +import us.ihmc.mecano.algorithms.interfaces.RigidBodyTwistProvider; +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.Twist; +import us.ihmc.mecano.spatial.interfaces.TwistReadOnly; + +import java.util.*; + +/** + * This class is used to calculate the twist of each rigid-body in a multi-body system. + *

+ * TODO This class should migrate to the Mecano library. + *

+ *

+ * Unlike {@link TwistCalculator} or {@link MovingReferenceFrame}, this class allows to use any type of data for the joint velocities through the interface {@link JointVelocityAccessor}. + *

+ */ +public class RigidBodyTwistCalculator implements RigidBodyTwistProvider +{ + + private final MultiBodySystemReadOnly input; + + private final RigidBodyTwistHolder root; + private final Map rigidBodyTwistHolders = new HashMap<>(); + + private JointVelocityAccessor jointVelocityAccessor; + private RigidBodyTwistProvider provider; + + public RigidBodyTwistCalculator(MultiBodySystemReadOnly input) + { + this.input = input; + root = new RigidBodyTwistHolder(null, input.getRootBody()); + rigidBodyTwistHolders.put(input.getRootBody(), root); + buildTree(root, input.getJointsToIgnore()); + + provider = RigidBodyTwistProvider.toRigidBodyTwistProvider(this::getTwistOfBody, getInertialFrame()); + } + + private void buildTree(RigidBodyTwistHolder parent, Collection jointsToIgnore) + { + List childrenJoints = parent.body.getChildrenJoints(); + + for (JointReadOnly childJoint : childrenJoints) + { + if (jointsToIgnore.contains(childJoint)) + continue; + + RigidBodyReadOnly childBody = childJoint.getSuccessor(); + + if (childBody == null) + continue; + + RigidBodyTwistHolder childHolder = new RigidBodyTwistHolder(parent, childBody); + parent.children.add(childHolder); + rigidBodyTwistHolders.put(childBody, childHolder); + buildTree(childHolder, jointsToIgnore); + } + } + + public void reset() + { + root.reset(); + } + + public void useJointVelocityState() + { + setJointVelocityAccessor((joint, matrixToPack) -> joint.getJointVelocity(0, matrixToPack)); + } + + public void useAllJointVelocityMatrix(DMatrix jointVelocities) + { + setJointVelocityAccessor((joint, matrixToPack) -> + { + int[] jointDoFIndices = input.getJointMatrixIndexProvider().getJointDoFIndices(joint); + for (int i = 0; i < jointDoFIndices.length; i++) + { + matrixToPack.set(i, 0, jointVelocities.get(jointDoFIndices[i], 0)); + } + }); + } + + public void setJointVelocityAccessor(JointVelocityAccessor jointVelocityAccessor) + { + root.reset(); + this.jointVelocityAccessor = jointVelocityAccessor; + } + + @Override + public TwistReadOnly getTwistOfBody(RigidBodyReadOnly body) + { + RigidBodyTwistHolder holder = rigidBodyTwistHolders.get(body); + if (holder == null) + return null; + return holder.getTwist(); + } + + @Override + public TwistReadOnly getRelativeTwist(RigidBodyReadOnly base, RigidBodyReadOnly body) + { + return provider.getRelativeTwist(base, body); + } + + @Override + public FrameVector3DReadOnly getLinearVelocityOfBodyFixedPoint(RigidBodyReadOnly base, RigidBodyReadOnly body, FramePoint3DReadOnly bodyFixedPoint) + { + return provider.getLinearVelocityOfBodyFixedPoint(base, body, bodyFixedPoint); + } + + @Override + public ReferenceFrame getInertialFrame() + { + return input.getInertialFrame(); + } + + private class RigidBodyTwistHolder + { + private final RigidBodyReadOnly body; + private final Twist twist = new Twist(); + + private final RigidBodyTwistHolder parent; + private final List children = new ArrayList<>(); + + private boolean dirty = true; + private final DMatrixRMaj jointTwistMatrix; + private final DMatrixRMaj jointVelocityMatrix; + private final DMatrixRMaj jointMotionSubspaceMatrix; + + public RigidBodyTwistHolder(RigidBodyTwistHolder parent, RigidBodyReadOnly body) + { + this.parent = parent; + this.body = body; + + if (parent == null) + { + jointTwistMatrix = null; + jointVelocityMatrix = null; + jointMotionSubspaceMatrix = null; + twist.setToZero(body.getBodyFixedFrame(), getInertialFrame(), body.getBodyFixedFrame()); + } + else + { + jointTwistMatrix = new DMatrixRMaj(Twist.SIZE, 1); + int nDoFs = body.getParentJoint().getDegreesOfFreedom(); + jointVelocityMatrix = new DMatrixRMaj(nDoFs, 1); + jointMotionSubspaceMatrix = new DMatrixRMaj(Twist.SIZE, nDoFs); + } + } + + public void reset() + { + dirty = true; + for (int i = 0; i < children.size(); i++) + { + children.get(i).reset(); + } + } + + public TwistReadOnly getTwist() + { + if (jointVelocityAccessor == null) + throw new IllegalStateException("Joint velocity accessor has not been set."); + + if (dirty) + { + if (parent == null) + { + twist.setToZero(body.getBodyFixedFrame(), getInertialFrame(), body.getBodyFixedFrame()); + } + else + { + JointReadOnly joint = body.getParentJoint(); + twist.setIncludingFrame(parent.getTwist()); + twist.changeFrame(joint.getFrameAfterJoint()); + + jointVelocityAccessor.getJointVelocity(joint, jointVelocityMatrix); + joint.getMotionSubspace(jointMotionSubspaceMatrix); + CommonOps_DDRM.mult(jointMotionSubspaceMatrix, jointVelocityMatrix, jointTwistMatrix); + twist.add(jointTwistMatrix); + + twist.changeFrame(body.getBodyFixedFrame()); + twist.setBodyFrame(body.getBodyFixedFrame()); + } + dirty = false; + } + return twist; + } + } + + public interface JointVelocityAccessor + { + void getJointVelocity(JointReadOnly joint, DMatrix velocityToPack); + } +} From cb0fb6ddb2510d6b42d2ad3d65d5e72c4ba9594f Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Tue, 6 Aug 2024 07:47:28 -0500 Subject: [PATCH 24/27] deleted minimum jerk trajectory and deprecated trapesoidal velocity trajectory and its implementations. Hexapod still depends on it. delete matrix yo variable converter tools delete some old yo variable tools delete some unused yo variable tools delete unused stiction model tools delete unused alpha to alpha functions deleted some controller classes, and left others to be reviewed deprecated some straght line tests, they need to be pushed into the striaght line pose traejctory removed straight line poisition trajectory from the pose test removed the straight line independent trajectory generators that werent used deleted the unused qp code deleted projection tools started switching away from random geometry class and the old robotics assert converted more of the tests to the right format switched getting all the current crop of moving away from random geometry working got rid of the custom random matrix generator class deleted the random geometry class, since its duplicated by other tools fixed some deletes and removed some provider structure that did nothing fixed some more deletes moved the planar landmark to the correct place deleted many other classes delete unused code deleted a whole bunch of stuff from the java toolkit deleted the simple walking controller --- .../robotics/MultiBodySystemMissingTools.java | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java index e6d5115..fa3036d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java @@ -89,4 +89,22 @@ public JointMatrixIndexProvider getJointMatrixIndexProvider() } }; } + + /** + * Computes the mass of all the links contained in the subtree of {@param rootBody} + * @param rootBody + * @return subtree mass + */ + public static double computeSubTreeMass(RigidBodyReadOnly rootBody) + { + SpatialInertiaReadOnly inertia = rootBody.getInertia(); + double ret = inertia == null ? 0.0 : inertia.getMass(); + + for (int i = 0; i < rootBody.getChildrenJoints().size(); i++) + { + ret += computeSubTreeMass(rootBody.getChildrenJoints().get(i).getSuccessor()); + } + + return ret; + } } From 611cc9aa7e7c7a64e8260c6dabd32f62177e00e2 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Tue, 1 Oct 2024 19:13:31 -0500 Subject: [PATCH 25/27] identified a bug in the FBRD Calculator for the force jacobian sign. Created some better constructors to enable testing overhauled the dynamics matrix calculator classes did a naming cleanup started using a few more common ops operations --- ...oatingBaseRigidBodyDynamicsCalculator.java | 321 +++++++++++++----- 1 file changed, 231 insertions(+), 90 deletions(-) diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java index f6afd0b..09316b1 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java +++ b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java @@ -2,135 +2,276 @@ import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.MatrixFeatures_DDRM; import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; import org.ejml.interfaces.linsol.LinearSolverDense; +import us.ihmc.robotics.MatrixMissingTools; public class FloatingBaseRigidBodyDynamicsCalculator { private static final int large = 1000; private static final double tolerance = 0.0001; - private static final LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); + private final LinearSolverDense pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); private final DMatrixRMaj matrixTranspose = new DMatrixRMaj(large, large); - private final DMatrixRMaj localVector = new DMatrixRMaj(large, 1); + private final DMatrixRMaj tempObjective = new DMatrixRMaj(large, large); public FloatingBaseRigidBodyDynamicsCalculator() - {} - - - public void computeQddotGivenRho(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, - DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj qddotToPack, DMatrixRMaj rho) { - computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); - - CommonOps_DDRM.scale(-1.0, floatingBaseCoriolisMatrix); - CommonOps_DDRM.multAdd(matrixTranspose, rho, floatingBaseCoriolisMatrix); - - pseudoInverseSolver.setA(floatingBaseMassMatrix); - pseudoInverseSolver.solve(floatingBaseCoriolisMatrix, qddotToPack); } - public void computeRhoGivenQddot(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, - DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj rhoToPack) + /** + * The rigid body dynamics are + *

+ * [0; τT]T = H(q) qDdot + C(q, qDot) + JcT ρ + *

+ *

+ * It is worth noting that C(q, qDot) can be decomposed to C(q,qDot)qDot + G(q), where the first term captures the coriolis and centrifugal forces and + * the second term captures the gravitational forces. + *

+ *

+ * Based on this, it can observed that the floating base composes its own subsystem where there is no torque. This system is underdefined. This function + * solves for the accelerations. This can be done from the following equations. + *

+ *

+ * 0 = Hbase(q) qDdot + Cbase(q,qDot) + JbaseT ρ + *

+ *

so

+ *

+ * qDdot = -Hbase+(Cbase(q,qDot) + JbaseT ρ) + *

+ *

+ * where (.)+ is the pseudo inverse operator. + *

+ * + * @param floatingBaseMassMatrix top six rows of the H(q) matrix above, which is Hbase(q). Not modified. + * @param floatingBaseGravityAndCoriolisVector top six rows of the C(q, qDot) matrix above, which is Cbase(q, qDot). Not modified. + * @param floatingBaseContactForceJacobianMatrix first six columns of the J matrix above, which is Jbase. Not modified. + * @param jointAccelerationsToPack resulting joint accelerations. ρ in the above equations. Modified. + * @param contactForces contact forces to use to compute the joint accelerations. qDdot in the above equations. Modified. + */ + public void computeJointAccelerationGivenContactForcesForFloatingSubsystem(DMatrixRMaj floatingBaseMassMatrix, + DMatrixRMaj floatingBaseGravityAndCoriolisVector, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, + DMatrixRMaj jointAccelerationsToPack, + DMatrixRMaj contactForces) { - computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); + tempObjective.set(floatingBaseGravityAndCoriolisVector); + CommonOps_DDRM.multAddTransA(floatingBaseContactForceJacobianMatrix, contactForces, tempObjective); - CommonOps_DDRM.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); + pseudoInverseSolver.setA(floatingBaseMassMatrix); + pseudoInverseSolver.solve(tempObjective, jointAccelerationsToPack); - pseudoInverseSolver.setA(matrixTranspose); - pseudoInverseSolver.solve(floatingBaseCoriolisMatrix, rhoToPack); + CommonOps_DDRM.changeSign(jointAccelerationsToPack); } - public void computeTauGivenRhoAndQddot(DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, - DMatrixRMaj qddot, DMatrixRMaj rho, DMatrixRMaj tauToPack) + /** + * The rigid body dynamics are + *

+ * [0; τT]T = H(q) qDdot + C(q, qDot) + JcT ρ + *

+ *

+ * It is worth noting that C(q, qDot) can be decomposed to C(q,qDot)qDot + G(q), where the first term captures the coriolis and centrifugal forces and + * the second term captures the gravitational forces. + *

+ *

+ * Based on this, it can observed that the floating base composes its own subsystem where there is no torque. This system is underdefined. This function + * solves for the contact forces. This can be done from the following equations. + *

+ *

+ * 0 = Hbase(q) qDdot + Cbase(q,qDot) + JbaseT ρ + *

+ *

so

+ *

+ * ρ = -(JbaseT )+ (Hbase+qDdot + Cbase(q,qDot)) + *

+ *

+ * where (.)+ is the pseudo inverse operator. + *

+ * + * @param floatingBaseMassMatrix top six rows of the Hbase(q) matrix above. Not modified. + * @param floatingBaseGravityAndCoriolisVector top six rows of the Cbase(q, qDot) matrix above. Not modified. + * @param floatingBaseContactForceJacobianMatrix first six columns of the Jbase matrix above. Not modified. + * @param jointAccelerations contact forces to compute the joint accelerations. ρ in the above equations. Not modified. + * @param contactForcesToPack resulting contact forces. qDdot in the above equations. Modified. + */ + public void computeContactForcesGivenJointAccelerationsForFloatingSubsystem(DMatrixRMaj floatingBaseMassMatrix, + DMatrixRMaj floatingBaseGravityAndCoriolisVector, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, + DMatrixRMaj jointAccelerations, + DMatrixRMaj contactForcesToPack) { - computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); + CommonOps_DDRM.transpose(floatingBaseContactForceJacobianMatrix, matrixTranspose); - CommonOps_DDRM.mult(bodyMassMatrix, qddot, tauToPack); - CommonOps_DDRM.multAdd(-1.0, matrixTranspose, rho, tauToPack); - CommonOps_DDRM.addEquals(tauToPack, bodyCoriolisMatrix); - } + tempObjective.set(floatingBaseGravityAndCoriolisVector); + CommonOps_DDRM.multAdd(floatingBaseMassMatrix, jointAccelerations, tempObjective); - public void computeTauGivenRho(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, DMatrixRMaj floatingBaseContactForceJacobianMatrix, - DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, DMatrixRMaj rho, DMatrixRMaj tauToPack) - { - computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); + pseudoInverseSolver.setA(matrixTranspose); + pseudoInverseSolver.solve(tempObjective, contactForcesToPack); - localVector.reshape(bodyMassMatrix.getNumCols(), 1); - computeQddotGivenRho(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, localVector, rho); - computeTauGivenRhoAndQddot(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, localVector, rho, tauToPack); + CommonOps_DDRM.changeSign(contactForcesToPack); } - public void computeTauGivenQddot(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, DMatrixRMaj floatingBaseContactForceJacobianMatrix, - DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj tauToPack) + /** + * The rigid body dynamics are + *

+ * [0; τT]T = H(q) qDdot + C(q, qDot) + JcT ρ + *

+ *

+ * The actuated portion of the dynamics are then: + *

+ *

+ * τ = Hbody(q) qDdot + Cbody(q,qDot) + JbodyT ρ + *

+ *

+ * This method computes the joint torques using the above equations + *

+ * + * @param bodyMassMatrix bottom N rows of the H(q) matrix above, which is Hbody(q). Not modified. + * @param bodyGravityAndCoriolisVector bottom N rows of the C(q, qDot) matrix above, which is Cbody(q, qDot). Not modified. + * @param bodyContactForceJacobianMatrix right N columns of the J matrix above, which is Jbody. Not modified. + * @param jointAccelerations joint accelerations. ρ in the above equations. Not modified. + * @param contactForces contact forces. qDdot in the above equations. Not modified. + * @param jointTorquesToPack joint torques. τ in the above equations. Modified. + */ + public static void computeJointTorquesGivenContactForcesAndJointAccelerations(DMatrixRMaj bodyMassMatrix, + DMatrixRMaj bodyGravityAndCoriolisVector, + DMatrixRMaj bodyContactForceJacobianMatrix, + DMatrixRMaj jointAccelerations, + DMatrixRMaj contactForces, + DMatrixRMaj jointTorquesToPack) { - computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); - - localVector.reshape(bodyContactForceJacobianMatrix.getNumRows(), 1); - computeRhoGivenQddot(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, qddot, localVector); - computeTauGivenRhoAndQddot(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, qddot, localVector, tauToPack); + jointTorquesToPack.set(bodyGravityAndCoriolisVector); + CommonOps_DDRM.multAdd(bodyMassMatrix, jointAccelerations, jointTorquesToPack); + CommonOps_DDRM.multAddTransA(bodyContactForceJacobianMatrix, contactForces, jointTorquesToPack); } - public boolean areFloatingBaseRigidBodyDynamicsSatisfied(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, - DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, - DMatrixRMaj bodyContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj tau, DMatrixRMaj rho) + /** + * The rigid body dynamics are + *

+ * [0; τT]T = H(q) qDdot + C(q, qDot) + JcT ρ + *

+ * The floating base portion of the dynamics are then: + *

+ *

+ * 0 = Hbase(q) qDdot + Cbase(q,qDot) + JbaseT ρ + *

+ *

+ * and the actuated portion of the dynamics are then: + *

+ *

+ * τ = Hbody(q) qDdot + Cbody(q,qDot) + JbodyT ρ + *

+ *

+ * This method performs the above calculation and checks whether the evaluation equals zero. + *

+ * + * @param floatingBaseMassMatrix top six rows of the H(q) matrix above, which is Hbase(q). Not modified. + * @param floatingBaseGravityAndCoriolisVector top six rows of the C(q, qDot) matrix above, which is Cbase(q, qDot). Not modified. + * @param floatingBaseContactForceJacobianMatrix first six columns of the J matrix above, which is Jbase. Not modified. + * @param bodyMassMatrix bottom N rows of the H(q) matrix above, which is Hbody(q). Not modified. + * @param bodyGravityAndCoriolisVector bottom N rows of the C(q, qDot) matrix above, which is Cbody(q, qDot). Not modified. + * @param bodyContactForceJacobianMatrix right N columns of the J matrix above, which is Jbody. Not modified. + * @param jointAccelerations joint accelerations. ρ in the above equations. Not modified. + * @param jointTorques joint torques. τ in the above equations. Not modified. + * @param contactForces contact forces. qDdot in the above equations. Not modified. + * @return where the defined dynamics hold. + */ + public boolean areFloatingBaseRigidBodyDynamicsSatisfied(DMatrixRMaj floatingBaseMassMatrix, + DMatrixRMaj floatingBaseGravityAndCoriolisVector, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, + DMatrixRMaj bodyMassMatrix, + DMatrixRMaj bodyGravityAndCoriolisVector, + DMatrixRMaj bodyContactForceJacobianMatrix, + DMatrixRMaj jointAccelerations, + DMatrixRMaj jointTorques, + DMatrixRMaj contactForces) { - if (!areFloatingBaseDynamicsSatisfied(floatingBaseMassMatrix, floatingBaseCoriolisMatrix, floatingBaseContactForceJacobianMatrix, qddot, rho)) + if (!areFloatingBaseDynamicsSatisfied(floatingBaseMassMatrix, + floatingBaseGravityAndCoriolisVector, + floatingBaseContactForceJacobianMatrix, + jointAccelerations, + contactForces)) return false; - return areRigidBodyDynamicsSatisfied(bodyMassMatrix, bodyCoriolisMatrix, bodyContactForceJacobianMatrix, qddot, tau, rho); - } - - public boolean areFloatingBaseDynamicsSatisfied(DMatrixRMaj floatingBaseMassMatrix, DMatrixRMaj floatingBaseCoriolisMatrix, - DMatrixRMaj floatingBaseContactForceJacobianMatrix, DMatrixRMaj qddot, DMatrixRMaj rho) - { - computeJacobianTranspose(floatingBaseContactForceJacobianMatrix, matrixTranspose); - - CommonOps_DDRM.multAdd(floatingBaseMassMatrix, qddot, floatingBaseCoriolisMatrix); - CommonOps_DDRM.multAdd(-1.0, matrixTranspose, rho, floatingBaseCoriolisMatrix); - - return equalsZero(floatingBaseCoriolisMatrix, tolerance); - } - - public boolean areRigidBodyDynamicsSatisfied(DMatrixRMaj bodyMassMatrix, DMatrixRMaj bodyCoriolisMatrix, DMatrixRMaj bodyContactForceJacobianMatrix, - DMatrixRMaj qddot, DMatrixRMaj tau, DMatrixRMaj rho) - { - computeJacobianTranspose(bodyContactForceJacobianMatrix, matrixTranspose); - - CommonOps_DDRM.multAdd(bodyMassMatrix, qddot, bodyCoriolisMatrix); - CommonOps_DDRM.multAdd(-1.0, matrixTranspose, rho, bodyCoriolisMatrix); - CommonOps_DDRM.subtractEquals(bodyCoriolisMatrix, tau); - - return equalsZero(bodyCoriolisMatrix, tolerance); + return areActuatedDynamicsSatisfied(bodyMassMatrix, + bodyGravityAndCoriolisVector, + bodyContactForceJacobianMatrix, + jointAccelerations, + jointTorques, + contactForces); } - private void computeJacobianTranspose(DMatrixRMaj jacobian, DMatrixRMaj jacobianTransposeToPack) + /** + * The rigid body dynamics are + *

+ * [0; τT]T = H(q) qDdot + C(q, qDot) + JcT ρ + *

+ *

+ * The floating base portion of the dynamics are then: + *

+ *

+ * 0 = Hbase(q) qDdot + Cbase(q,qDot) + JbaseT ρ + *

+ *

+ * This method performs the above calculation and checks whether the evaluation equals zero. + *

+ * + * @param floatingBaseMassMatrix top six rows of the H(q) matrix above, which is Hbase(q). Not modified. + * @param floatingBaseGravityAndCoriolisVector top six rows of the C(q, qDot) matrix above, which is Cbase(q, qDot). Not modified. + * @param floatingBaseContactForceJacobianMatrix first six columns of the J matrix above, which is Jbase. Not modified. + * @param jointAccelerations joint accelerations. ρ in the above equations. Not modified. + * @param contactForces contact forces. qDdot in the above equations. Not modified. + * @return whether the defined dynamics hold. + **/ + public boolean areFloatingBaseDynamicsSatisfied(DMatrixRMaj floatingBaseMassMatrix, + DMatrixRMaj floatingBaseGravityAndCoriolisVector, + DMatrixRMaj floatingBaseContactForceJacobianMatrix, + DMatrixRMaj jointAccelerations, + DMatrixRMaj contactForces) { - jacobianTransposeToPack.reshape(jacobian.getNumCols(), jacobian.getNumRows()); - jacobianTransposeToPack.zero(); - CommonOps_DDRM.transpose(jacobian, jacobianTransposeToPack); - } + tempObjective.set(floatingBaseGravityAndCoriolisVector); + CommonOps_DDRM.multAdd(floatingBaseMassMatrix, jointAccelerations, tempObjective); + CommonOps_DDRM.multAddTransA(floatingBaseContactForceJacobianMatrix, contactForces, tempObjective); - private static boolean equalsZero(DMatrixRMaj matrix, double tolerance) - { - for (int rowIndex = 0; rowIndex < matrix.getNumRows(); rowIndex++) - { - for (int colIndex = 0; colIndex < matrix.getNumCols(); colIndex++) - { - if (!equals(matrix.get(rowIndex, colIndex), 0.0, tolerance)) - return false; - } - } - - return true; + return MatrixFeatures_DDRM.isZeros(tempObjective, tolerance); } - private static boolean equals(double a, double b, double tolerance) + /** + * The rigid body dynamics are + *

+ * [0; τT]T = H(q) qDdot + C(q, qDot) + JcT ρ + *

+ *

+ * The actuated portion of the dynamics are then: + *

+ *

+ * τ = Hbody(q) qDdot + Cbody(q,qDot) + JbodyT ρ + *

+ *

+ * This method performs the above calculation and checks whether the evaluation equals zero. + *

+ * + * @param bodyMassMatrix bottom N rows of the H(q) matrix above, which is Hbody(q). Not modified. + * @param bodyGravityAndCoriolisVector bottom N rows of the C(q, qDot) matrix above, which is Cbody(q, qDot). Not modified. + * @param bodyContactForceJacobianMatrix right N columns of the J matrix above, which is Jbody. Not modified. + * @param jointAccelerations joint accelerations. ρ in the above equations. Not modified. + * @param jointTorques joint torques. τ in the above equations. Not modified. + * @param contactForces contact forces. qDdot in the above equations. Not modified. + * @return where the defined dynamics hold. + */ + public boolean areActuatedDynamicsSatisfied(DMatrixRMaj bodyMassMatrix, + DMatrixRMaj bodyGravityAndCoriolisVector, + DMatrixRMaj bodyContactForceJacobianMatrix, + DMatrixRMaj jointAccelerations, + DMatrixRMaj jointTorques, + DMatrixRMaj contactForces) { - if (Math.abs(a - b) > tolerance) - return false; + tempObjective.set(bodyGravityAndCoriolisVector); + CommonOps_DDRM.multAdd(bodyMassMatrix, jointAccelerations, tempObjective); + CommonOps_DDRM.multAddTransA(bodyContactForceJacobianMatrix, contactForces, tempObjective); - return true; + return MatrixFeatures_DDRM.isEquals(tempObjective, jointTorques, tolerance); } } From e447d79d3d4ef94138a2ecd3c614396f1f67fce1 Mon Sep 17 00:00:00 2001 From: Robert Griffin Date: Sun, 13 Oct 2024 22:50:52 -0500 Subject: [PATCH 26/27] started moving a bunch of packages to allow extracting the whole-body control core significantly overhauled and set up custom pid parameters for the whole body controller core moved a lot of the filtered yo variables into a common project that can be moved to commons moved a bunch of the polynomial classes out after getting things solidified, but need to fix the linear systems tests cleaned up a lot more of the yo variable types and moved their packages got more code moved and most of the linear algebra tests fixed removed unused code set up wbcc for extraction did considerable package movement, but solidified how the extracted repos will be structured moved out more of th e simple code into the sub-sets started refactoring some of the code to prep for the commons move set up the euclid update repo, including javadoc and pulling over the tests moved the mecano packages around set up a robotics tools project to become its own repo set up the ihmc math library more properly, and worked to add some javadoc on some of the core classes did a bunch of testing and javadoc, and cleaned up polynomial3D :arrow_up: euclid 0.22.0 :arrow_up: ihmc-commons 0.33.0 :arrow_up: ihmc-yovariables 0.13.1 incorporated all the code from open robotics :arrow_up: euclid 0.22.0 :arrow_up: ihmc-yovariables 0.13.2 --- .gitignore | 5 + build.gradle.kts | 14 +- gradle.properties | 2 +- .../robotics/MultiBodySystemMissingTools.java | 110 ------ ...amicallyConsistentNullspaceCalculator.java | 25 -- .../screwTheory/MassMatrixCalculator.java | 17 - .../DifferentialIDMassMatrixCalculator.java | 16 +- ...amicallyConsistentNullspaceCalculator.java | 225 ++++++++++++ ...oatingBaseRigidBodyDynamicsCalculator.java | 3 +- ...oriolisExternalWrenchMatrixCalculator.java | 4 +- .../MassMatrixCalculatorComparer.java | 127 +++++++ .../algorithms}/MomentumCalculator.java | 6 +- .../algorithms}/RigidBodyTwistCalculator.java | 4 +- .../interfaces/MassMatrixCalculator.java | 17 + .../mecano/tools/MultiBodySystemTools.java | 20 ++ ...ifferentialIDMassMatrixCalculatorTest.java | 24 ++ ...lisExternalWrenchMatrixCalculatorTest.java | 323 ++++++++++++++++++ .../algorithms/MassMatrixCalculatorTest.java | 172 ++++++++++ .../algorithms/MomentumCalculatorTest.java | 157 +++++++++ .../RigidBodyTwistCalculatorTest.java | 95 ++++++ .../tools/MultiBodySystemToolsTest.java | 68 ++++ ...haFilteredYoMutableFrameSpatialVector.java | 5 +- .../filters/AlphaFilteredYoSpatialVector.java | 25 +- .../RateLimitedYoMutableSpatialVector.java | 5 +- .../filters/RateLimitedYoSpatialVector.java | 25 +- .../spatial}/YoMutableFrameSpatialVector.java | 2 +- 26 files changed, 1295 insertions(+), 201 deletions(-) delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java delete mode 100644 ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory => src/main/java/us/ihmc/mecano/algorithms}/DifferentialIDMassMatrixCalculator.java (88%) create mode 100644 src/main/java/us/ihmc/mecano/algorithms/DynamicallyConsistentNullspaceCalculator.java rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory => src/main/java/us/ihmc/mecano/algorithms}/FloatingBaseRigidBodyDynamicsCalculator.java (99%) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory => src/main/java/us/ihmc/mecano/algorithms}/GravityCoriolisExternalWrenchMatrixCalculator.java (99%) create mode 100644 src/main/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorComparer.java rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory => src/main/java/us/ihmc/mecano/algorithms}/MomentumCalculator.java (91%) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory => src/main/java/us/ihmc/mecano/algorithms}/RigidBodyTwistCalculator.java (96%) create mode 100644 src/main/java/us/ihmc/mecano/algorithms/interfaces/MassMatrixCalculator.java create mode 100644 src/test/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculatorTest.java create mode 100644 src/test/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculatorTest.java create mode 100644 src/test/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorTest.java create mode 100644 src/test/java/us/ihmc/mecano/algorithms/MomentumCalculatorTest.java create mode 100644 src/test/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculatorTest.java rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math => src/yovariables-filters/java/us/ihmc/mecano/yoVariables}/filters/AlphaFilteredYoMutableFrameSpatialVector.java (94%) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math => src/yovariables-filters/java/us/ihmc/mecano/yoVariables}/filters/AlphaFilteredYoSpatialVector.java (70%) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math => src/yovariables-filters/java/us/ihmc/mecano/yoVariables}/filters/RateLimitedYoMutableSpatialVector.java (94%) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math => src/yovariables-filters/java/us/ihmc/mecano/yoVariables}/filters/RateLimitedYoSpatialVector.java (62%) rename {ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures => src/yovariables/java/us/ihmc/mecano/yoVariables/spatial}/YoMutableFrameSpatialVector.java (98%) diff --git a/.gitignore b/.gitignore index f8e3fc6..35d0922 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,9 @@ +# Top level files to ignore +gradlew +gradlew.bat + # Top level directories to ignore +/gradle /build /classes /bin diff --git a/build.gradle.kts b/build.gradle.kts index 08b5a15..4c7c8e5 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -17,9 +17,9 @@ mainDependencies { api("org.ejml:ejml-core:0.39") api("org.ejml:ejml-ddense:0.39") - api("us.ihmc:euclid:0.21.0") - api("us.ihmc:euclid-frame:0.21.0") - api("us.ihmc:euclid-geometry:0.21.0") + api("us.ihmc:euclid:0.22.0") + api("us.ihmc:euclid-frame:0.22.0") + api("us.ihmc:euclid-geometry:0.22.0") } testDependencies { @@ -47,5 +47,11 @@ graphvizDependencies { yovariablesDependencies { api(ihmc.sourceSetProject("main")) - api("us.ihmc:ihmc-yovariables:0.12.2") + api("us.ihmc:ihmc-yovariables:0.13.2") +} + +yovariablesFiltersDependencies { + api(ihmc.sourceSetProject("yovariables")) + + api("us.ihmc:ihmc-yovariables-filters:0.13.2") } diff --git a/gradle.properties b/gradle.properties index 544ca26..a4570ab 100644 --- a/gradle.properties +++ b/gradle.properties @@ -1,4 +1,4 @@ title = Mecano -extraSourceSets = ["test", "graphviz", "yovariables"] +extraSourceSets = ["test", "graphviz", "yovariables", "yovariables-filters"] compositeSearchHeight = 0 excludeFromCompositeBuild = false \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java deleted file mode 100644 index fa3036d..0000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/MultiBodySystemMissingTools.java +++ /dev/null @@ -1,110 +0,0 @@ -package us.ihmc.robotics; - -import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.euclid.transform.RigidBodyTransform; -import us.ihmc.mecano.multiBodySystem.CrossFourBarJoint; -import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; -import us.ihmc.mecano.multiBodySystem.interfaces.OneDoFJointBasics; -import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; -import us.ihmc.mecano.tools.MultiBodySystemTools; - -import java.util.ArrayList; -import java.util.List; - -public class MultiBodySystemMissingTools -{ - public static void copyOneDoFJointsConfiguration(JointBasics[] source, JointBasics[] destination) - { - OneDoFJointBasics[] oneDofJoints1 = MultiBodySystemTools.filterJoints(source, OneDoFJointBasics.class); - OneDoFJointBasics[] oneDofJoints2 = MultiBodySystemTools.filterJoints(destination, OneDoFJointBasics.class); - - if (oneDofJoints1.length != oneDofJoints2.length) - { - throw new IllegalArgumentException("The lists of joints must be the same length. %d != %d".formatted(oneDofJoints1.length, - oneDofJoints2.length)); - } - - for (int i = 0; i < oneDofJoints1.length; i++) - { - oneDofJoints2[i].setJointConfiguration(oneDofJoints1[i]); - } - } - - public static List getSubtreeJointsIncludingFourBars(RigidBodyBasics rootBody) - { - List joints = new ArrayList<>(); - - for (JointBasics joint : rootBody.childrenSubtreeIterable()) - { - if (joint instanceof CrossFourBarJoint) - { - joints.addAll(((CrossFourBarJoint) joint).getFourBarFunction().getLoopJoints()); - } - else - { - joints.add(joint); - } - } - - return joints; - } - - public static MultiBodySystemBasics createSingleBodySystem(RigidBodyBasics singleBody) - { - List allJoints = new ArrayList<>(); - List jointsToConsider = new ArrayList<>(); - List jointsToIgnore = new ArrayList<>(); - JointMatrixIndexProvider jointMatrixIndexProvider = JointMatrixIndexProvider.toIndexProvider(jointsToConsider); - - return new MultiBodySystemBasics() - { - @Override - public RigidBodyBasics getRootBody() - { - return singleBody; - } - - @Override - public List getAllJoints() - { - return allJoints; - } - - @Override - public List getJointsToConsider() - { - return jointsToConsider; - } - - @Override - public List getJointsToIgnore() - { - return jointsToIgnore; - } - - @Override - public JointMatrixIndexProvider getJointMatrixIndexProvider() - { - return jointMatrixIndexProvider; - } - }; - } - - /** - * Computes the mass of all the links contained in the subtree of {@param rootBody} - * @param rootBody - * @return subtree mass - */ - public static double computeSubTreeMass(RigidBodyReadOnly rootBody) - { - SpatialInertiaReadOnly inertia = rootBody.getInertia(); - double ret = inertia == null ? 0.0 : inertia.getMass(); - - for (int i = 0; i < rootBody.getChildrenJoints().size(); i++) - { - ret += computeSubTreeMass(rootBody.getChildrenJoints().get(i).getSuccessor()); - } - - return ret; - } -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java deleted file mode 100644 index 3a50a73..0000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DynamicallyConsistentNullspaceCalculator.java +++ /dev/null @@ -1,25 +0,0 @@ -package us.ihmc.robotics.screwTheory; - -import org.ejml.data.DMatrixRMaj; - -import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; -import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; - -/** - * @author twan - * Date: 4/17/13 - */ -public interface DynamicallyConsistentNullspaceCalculator -{ - void reset(); - - void addConstraint(RigidBodyBasics body, DMatrixRMaj selectionMatrix); - - void addActuatedJoint(JointBasics joint); - - void compute(); - - DMatrixRMaj getDynamicallyConsistentNullspace(); - - DMatrixRMaj getSNsBar(); -} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java b/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java deleted file mode 100644 index 7305a73..0000000 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MassMatrixCalculator.java +++ /dev/null @@ -1,17 +0,0 @@ -package us.ihmc.robotics.screwTheory; - -import org.ejml.data.DMatrixRMaj; - -import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; - -public interface MassMatrixCalculator -{ - - public abstract void compute(); - - public abstract DMatrixRMaj getMassMatrix(); - - public abstract void getMassMatrix(DMatrixRMaj massMatrixToPack); - - public abstract JointBasics[] getJointsInOrder(); -} \ No newline at end of file diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculator.java similarity index 88% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java rename to src/main/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculator.java index 291d566..8bedc7a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/DifferentialIDMassMatrixCalculator.java +++ b/src/main/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculator.java @@ -1,4 +1,4 @@ -package us.ihmc.robotics.screwTheory; +package us.ihmc.mecano.algorithms; import java.util.LinkedHashMap; @@ -6,14 +6,14 @@ import org.ejml.dense.row.CommonOps_DDRM; import us.ihmc.euclid.referenceFrame.ReferenceFrame; -import us.ihmc.matrixlib.MatrixTools; -import us.ihmc.mecano.algorithms.InverseDynamicsCalculator; +import us.ihmc.mecano.algorithms.interfaces.MassMatrixCalculator; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; -import us.ihmc.mecano.spatial.SpatialAcceleration; import us.ihmc.mecano.spatial.SpatialForce; import us.ihmc.mecano.spatial.Wrench; +import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MecanoFactories; import us.ihmc.mecano.tools.MultiBodySystemTools; /** @@ -34,9 +34,11 @@ public class DifferentialIDMassMatrixCalculator implements MassMatrixCalculator public DifferentialIDMassMatrixCalculator(ReferenceFrame inertialFrame, RigidBodyBasics rootBody) { - SpatialAcceleration zeroRootAcceleration = ScrewTools.createGravitationalSpatialAcceleration(rootBody, 0.0); + SpatialAccelerationReadOnly zeroRootAcceleration = MecanoFactories.newGravitationalSpatialAcceleration(rootBody, 0.0); - idCalculator = new InverseDynamicsCalculator(rootBody, false, true); + idCalculator = new InverseDynamicsCalculator(rootBody); + idCalculator.setConsiderCoriolisAndCentrifugalForces(false); + idCalculator.setConsiderJointAccelerations(true); idCalculator.setRootAcceleration(zeroRootAcceleration); jointsInOrder = MultiBodySystemTools.collectSubtreeJoints(rootBody); totalNumberOfDoFs = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); @@ -70,7 +72,7 @@ public void compute() idCalculator.compute(); tmpTauMatrix.set(idCalculator.getJointTauMatrix()); - MatrixTools.setMatrixBlock(massMatrix, 0, column, tmpTauMatrix, 0, 0, totalNumberOfDoFs, 1, 1.0); + CommonOps_DDRM.extract(tmpTauMatrix, 0, totalNumberOfDoFs, 0, 1, massMatrix, 0, column); column++; tmpDesiredJointAccelerationsMatrix.set(i, 0, 0.0); diff --git a/src/main/java/us/ihmc/mecano/algorithms/DynamicallyConsistentNullspaceCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/DynamicallyConsistentNullspaceCalculator.java new file mode 100644 index 0000000..3877a02 --- /dev/null +++ b/src/main/java/us/ihmc/mecano/algorithms/DynamicallyConsistentNullspaceCalculator.java @@ -0,0 +1,225 @@ +package us.ihmc.mecano.algorithms; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collection; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; + +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.SpecializedOps_DDRM; +import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; +import org.ejml.interfaces.linsol.LinearSolverDense; + +import us.ihmc.mecano.multiBodySystem.interfaces.FloatingJointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.tools.MultiBodySystemTools; + +/** + * + * See: L. Sentis. Synthesis and Control of Whole-Body Behaviors in Humanoid Systems (2007) + * + * @author twan + * Date: 4/15/13 + */ +public class DynamicallyConsistentNullspaceCalculator +{ + private final FloatingJointBasics rootJoint; + private final JointBasics[] jointsInOrder; + private final Map constrainedBodiesAndSelectionMatrices = new LinkedHashMap(); + private final List actuatedJoints = new ArrayList(); + private final Map> supportingBodyToJointPathMap = new LinkedHashMap>(); + private final CompositeRigidBodyMassMatrixCalculator massMatrixCalculator; + + private final boolean computeSNsBar; + + private final LinearSolverDense massMatrixSolver; + private final LinearSolverDense lambdaSolver; + private final LinearSolverDense pseudoInverseSolver; + private final DMatrixRMaj S = new DMatrixRMaj(1, 1); + private final DMatrixRMaj Js = new DMatrixRMaj(1, 1); + private final DMatrixRMaj AInverse = new DMatrixRMaj(1, 1); + private final DMatrixRMaj AInverseJSTranspose = new DMatrixRMaj(1, 1); + private final DMatrixRMaj LambdasInverse = new DMatrixRMaj(1, 1); + private final DMatrixRMaj Lambdas = new DMatrixRMaj(1, 1); + private final DMatrixRMaj JsBar = new DMatrixRMaj(1, 1); + private final DMatrixRMaj Ns = new DMatrixRMaj(1, 1); + private final DMatrixRMaj SNs = new DMatrixRMaj(1, 1); + private final DMatrixRMaj AInverseSNsTranspose = new DMatrixRMaj(1, 1); + private final DMatrixRMaj PhiStar = new DMatrixRMaj(1, 1); + private final DMatrixRMaj PhiStarInverse = new DMatrixRMaj(1, 1); + private final DMatrixRMaj SNsBar = new DMatrixRMaj(1, 1); + + private final Twist tempTwist = new Twist(); + private final DMatrixRMaj tempTwistMatrix = new DMatrixRMaj(Twist.SIZE, 1); + private final DMatrixRMaj tempJacobianPart = new DMatrixRMaj(1, 1); + + private final int nDegreesOfFreedom; + private int nConstraints; + + public DynamicallyConsistentNullspaceCalculator(FloatingJointBasics rootJoint, + boolean computeSNsBar) + { + this.rootJoint = rootJoint; + this.massMatrixCalculator = new CompositeRigidBodyMassMatrixCalculator(rootJoint.getSuccessor()); + jointsInOrder = massMatrixCalculator.getInput().getJointMatrixIndexProvider().getIndexedJointsInOrder().toArray(new JointBasics[0]); + this.nDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(jointsInOrder); + massMatrixSolver = LinearSolverFactory_DDRM.symmPosDef(nDegreesOfFreedom); + lambdaSolver = LinearSolverFactory_DDRM.symmPosDef(nDegreesOfFreedom); // size of matrix is only used to choose algorithm. nDegreesOfFreedom is an upper limit + pseudoInverseSolver = LinearSolverFactory_DDRM.pseudoInverse(true); + this.computeSNsBar = computeSNsBar; + } + + public void reset() + { + nConstraints = 0; + constrainedBodiesAndSelectionMatrices.clear(); + actuatedJoints.clear(); + } + + public void addConstraint(RigidBodyBasics body, DMatrixRMaj selectionMatrix) + { + constrainedBodiesAndSelectionMatrices.put(body, selectionMatrix); + nConstraints += selectionMatrix.getNumRows(); + JointBasics[] jointPath = MultiBodySystemTools.createJointPath(rootJoint.getPredecessor(), body); + this.supportingBodyToJointPathMap.put(body, Arrays.asList(jointPath)); + } + + public void addActuatedJoint(JointBasics joint) + { + actuatedJoints.add(joint); + } + + private static void computeS(DMatrixRMaj S, JointBasics[] jointsInOrder, Collection actuatedJoints) + { + S.zero(); + int rowStart = 0; + int columnStart = 0; + for (JointBasics inverseDynamicsJoint : jointsInOrder) + { + int degreesOfFreedom = inverseDynamicsJoint.getDegreesOfFreedom(); + + if (actuatedJoints.contains(inverseDynamicsJoint)) + { + DMatrixRMaj identity = new DMatrixRMaj(degreesOfFreedom, degreesOfFreedom); + CommonOps_DDRM.setIdentity(identity); + CommonOps_DDRM.insert(identity, S, rowStart, columnStart); + rowStart += degreesOfFreedom; + } + + columnStart += degreesOfFreedom; + } + } + + public void compute() + { + resizeMatrices(); + + computeS(S, jointsInOrder, actuatedJoints); + + computeJs(Js, supportingBodyToJointPathMap, constrainedBodiesAndSelectionMatrices); + + massMatrixCalculator.reset(); + massMatrixSolver.setA(massMatrixCalculator.getMassMatrix()); + massMatrixSolver.invert(AInverse); + CommonOps_DDRM.multTransB(AInverse, Js, AInverseJSTranspose); + CommonOps_DDRM.mult(Js, AInverseJSTranspose, LambdasInverse); + + lambdaSolver.setA(LambdasInverse); + lambdaSolver.invert(Lambdas); + CommonOps_DDRM.mult(AInverseJSTranspose, Lambdas, JsBar); + +// CommonOps_DDRM.pinv(Js, JsBar); + + CommonOps_DDRM.mult(JsBar, Js, Ns); + CommonOps_DDRM.changeSign(Ns); + SpecializedOps_DDRM.addIdentity(Ns, Ns, 1.0); + + if (computeSNsBar) + { + CommonOps_DDRM.mult(S, Ns, SNs); + CommonOps_DDRM.multTransB(AInverse, SNs, AInverseSNsTranspose); + CommonOps_DDRM.mult(SNs, AInverseSNsTranspose, PhiStar); + pseudoInverseSolver.setA(PhiStar); + pseudoInverseSolver.invert(PhiStarInverse); + CommonOps_DDRM.mult(AInverseSNsTranspose, PhiStarInverse, SNsBar); + +// CommonOps_DDRM.mult(S, Ns, SNs); +// ConfigurableSolvePseudoInverseSVD solver = new ConfigurableSolvePseudoInverseSVD(SNs.getNumRows(), SNs.getNumCols(), 0.5); +// solver.setA(SNs); +// solver.invert(SNsBar); + } + } + + private void resizeMatrices() + { + int nActuatedDegreesOfFreedom = MultiBodySystemTools.computeDegreesOfFreedom(actuatedJoints); + + S.reshape(nActuatedDegreesOfFreedom, nDegreesOfFreedom); + Js.reshape(nConstraints, nDegreesOfFreedom); + AInverse.reshape(nDegreesOfFreedom, nDegreesOfFreedom); + AInverseJSTranspose.reshape(AInverse.getNumRows(), Js.getNumRows()); + LambdasInverse.reshape(Js.getNumRows(), AInverseJSTranspose.getNumCols()); + Lambdas.reshape(LambdasInverse.getNumRows(), LambdasInverse.getNumCols()); + JsBar.reshape(AInverseJSTranspose.getNumRows(), LambdasInverse.getNumCols()); + Ns.reshape(nDegreesOfFreedom, nDegreesOfFreedom); + SNs.reshape(S.getNumRows(), Ns.getNumCols()); + AInverseSNsTranspose.reshape(AInverse.getNumRows(), SNs.getNumRows()); + PhiStar.reshape(SNs.getNumRows(), AInverseSNsTranspose.getNumCols()); + PhiStarInverse.reshape(PhiStar.getNumRows(), PhiStar.getNumCols()); + SNsBar.reshape(nDegreesOfFreedom, nActuatedDegreesOfFreedom); + } + + public DMatrixRMaj getDynamicallyConsistentNullspace() + { + return Ns; + } + + public DMatrixRMaj getSNsBar() + { + if (!computeSNsBar) + throw new RuntimeException("SNsBar not computed"); + + return SNsBar; + } + + private void computeJs(DMatrixRMaj Js, Map> supportJacobians, + Map constrainedBodiesAndSelectionMatrices) + { + Js.zero(); + + int rowStartNumber = 0; + for (RigidBodyBasics rigidBody : supportJacobians.keySet()) + { + List supportingJoints = supportJacobians.get(rigidBody); + DMatrixRMaj selectionMatrix = constrainedBodiesAndSelectionMatrices.get(rigidBody); + + int columnStartNumber = 0; + for (JointBasics inverseDynamicsJoint : jointsInOrder) + { + if (supportingJoints.contains(inverseDynamicsJoint)) + { + for (int dofIndex = 0; dofIndex < inverseDynamicsJoint.getDegreesOfFreedom(); dofIndex++) + { + tempTwist.setIncludingFrame(inverseDynamicsJoint.getUnitTwists().get(dofIndex)); + tempTwist.changeFrame(rootJoint.getFrameAfterJoint()); + tempTwist.get(0, tempTwistMatrix); + tempJacobianPart.reshape(selectionMatrix.getNumRows(), 1); + CommonOps_DDRM.mult(selectionMatrix, tempTwistMatrix, tempJacobianPart); + CommonOps_DDRM.insert(tempJacobianPart, Js, rowStartNumber, columnStartNumber++); + } + } + else + { + columnStartNumber += inverseDynamicsJoint.getDegreesOfFreedom(); + } + } + + rowStartNumber += selectionMatrix.getNumRows(); + } + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/FloatingBaseRigidBodyDynamicsCalculator.java similarity index 99% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java rename to src/main/java/us/ihmc/mecano/algorithms/FloatingBaseRigidBodyDynamicsCalculator.java index 09316b1..655342e 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/FloatingBaseRigidBodyDynamicsCalculator.java +++ b/src/main/java/us/ihmc/mecano/algorithms/FloatingBaseRigidBodyDynamicsCalculator.java @@ -1,11 +1,10 @@ -package us.ihmc.robotics.screwTheory; +package us.ihmc.mecano.algorithms; import org.ejml.data.DMatrixRMaj; import org.ejml.dense.row.CommonOps_DDRM; import org.ejml.dense.row.MatrixFeatures_DDRM; import org.ejml.dense.row.factory.LinearSolverFactory_DDRM; import org.ejml.interfaces.linsol.LinearSolverDense; -import us.ihmc.robotics.MatrixMissingTools; public class FloatingBaseRigidBodyDynamicsCalculator { diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculator.java similarity index 99% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java rename to src/main/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculator.java index 6e09641..5769e6a 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/GravityCoriolisExternalWrenchMatrixCalculator.java +++ b/src/main/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculator.java @@ -1,4 +1,4 @@ -package us.ihmc.robotics.screwTheory; +package us.ihmc.mecano.algorithms; import java.util.ArrayList; import java.util.Collection; @@ -12,8 +12,6 @@ import us.ihmc.euclid.referenceFrame.exceptions.ReferenceFrameMismatchException; import us.ihmc.euclid.referenceFrame.interfaces.FrameTuple3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Tuple3DReadOnly; -import us.ihmc.mecano.algorithms.InverseDynamicsCalculator; -import us.ihmc.mecano.algorithms.SpatialAccelerationCalculator; import us.ihmc.mecano.algorithms.interfaces.RigidBodyAccelerationProvider; import us.ihmc.mecano.frames.MovingReferenceFrame; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; diff --git a/src/main/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorComparer.java b/src/main/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorComparer.java new file mode 100644 index 0000000..15116b6 --- /dev/null +++ b/src/main/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorComparer.java @@ -0,0 +1,127 @@ +package us.ihmc.mecano.algorithms; + +import java.util.ArrayList; +import java.util.Random; + +import org.ejml.data.DMatrixRMaj; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.mecano.algorithms.interfaces.MassMatrixCalculator; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.RigidBody; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; + +public class MassMatrixCalculatorComparer +{ + private static final Vector3D X = new Vector3D(1.0, 0.0, 0.0); + private static final Vector3D Y = new Vector3D(0.0, 1.0, 0.0); + private static final Vector3D Z = new Vector3D(0.0, 0.0, 1.0); + + private final Random random = new Random(1776L); + private final ArrayList massMatrixCalculators = new ArrayList(); + private final MassMatrixCalculator diffIdMassMatricCalculator; + private final CompositeRigidBodyMassMatrixCalculator compositeMassMatricCalculator; + private final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + private final ArrayList joints; + private final RigidBodyBasics elevator; + + public MassMatrixCalculatorComparer() + { + joints = new ArrayList(); + elevator = new RigidBody("elevator", worldFrame); + Vector3D[] jointAxes = {X, Y, Z, Z, X, Z, Z, X, Y, Y}; + joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "", elevator, jointAxes)); + + + diffIdMassMatricCalculator = new DifferentialIDMassMatrixCalculator(worldFrame, elevator); + compositeMassMatricCalculator = new CompositeRigidBodyMassMatrixCalculator(elevator); + + massMatrixCalculators.add(diffIdMassMatricCalculator); + massMatrixCalculators.add(new MassMatrixCalculator() + { + @Override + public void getMassMatrix(DMatrixRMaj massMatrixToPack) + { + massMatrixToPack.set(compositeMassMatricCalculator.getMassMatrix()); + } + + @Override + public DMatrixRMaj getMassMatrix() + { + return compositeMassMatricCalculator.getMassMatrix(); + } + + @Override + public JointBasics[] getJointsInOrder() + { + return compositeMassMatricCalculator.getInput().getJointsToConsider().toArray(new JointBasics[0]); + } + + @Override + public void compute() + { + compositeMassMatricCalculator.reset(); + } + }); + } + + public void altCompare() + { + double diffIdTimeTaken = 0.0; + double compositeTimeTaken = 0.0; + + int nIterations = 10000; + for (int i = 0; i < nIterations; i++) + { + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + elevator.updateFramesRecursively(); + + long startTime = System.nanoTime(); + diffIdMassMatricCalculator.compute(); + long endTime = System.nanoTime(); + + diffIdTimeTaken += (endTime - startTime) / (1e9); + + startTime = System.nanoTime(); + compositeMassMatricCalculator.reset(); + endTime = System.nanoTime(); + + compositeTimeTaken += (endTime - startTime) / (1e9); + } + + double diffIdTimeTakenPerIteration = diffIdTimeTaken / nIterations; + double compositeTimeTakenPerIteration = compositeTimeTaken / nIterations; + + System.out.println("Diff ID time taken per iteration: " + diffIdTimeTakenPerIteration + " s"); + System.out.println("Composite RBM time taken per iteration: " + compositeTimeTakenPerIteration + " s"); + } + + public void compare() + { + for (MassMatrixCalculator massMatrixCalculator : massMatrixCalculators) + { + long startTime = System.nanoTime(); + int nIterations = 10000; + for (int i = 0; i < nIterations; i++) + { + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + elevator.updateFramesRecursively(); + massMatrixCalculator.compute(); + } + + long endTime = System.nanoTime(); + double timeTaken = (endTime - startTime) / (1e9); + double timeTakenPerIteration = timeTaken / nIterations; + System.out.println("Time taken per iteration: " + timeTakenPerIteration + " s"); + } + } + + public static void main(String[] args) + { + new MassMatrixCalculatorComparer().altCompare(); + } +} diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/MomentumCalculator.java similarity index 91% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java rename to src/main/java/us/ihmc/mecano/algorithms/MomentumCalculator.java index 239807c..6462f77 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/MomentumCalculator.java +++ b/src/main/java/us/ihmc/mecano/algorithms/MomentumCalculator.java @@ -1,4 +1,4 @@ -package us.ihmc.robotics.screwTheory; +package us.ihmc.mecano.algorithms; import java.util.stream.Stream; @@ -11,6 +11,10 @@ import us.ihmc.mecano.spatial.interfaces.FixedFrameMomentumBasics; import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly; +/** + * This class is designed to calculate the momentum of a subtree. This is useful when looking at the specific momentum contributions of independent parts of the + * rigid body system. + */ public class MomentumCalculator { private final RigidBodyReadOnly[] rigidBodiesInOrders; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculator.java similarity index 96% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java rename to src/main/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculator.java index 9847652..8c2e370 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/screwTheory/RigidBodyTwistCalculator.java +++ b/src/main/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculator.java @@ -1,4 +1,4 @@ -package us.ihmc.robotics.screwTheory; +package us.ihmc.mecano.algorithms; import org.ejml.data.DMatrix; import org.ejml.data.DMatrixRMaj; @@ -22,7 +22,7 @@ * TODO This class should migrate to the Mecano library. *

*

- * Unlike {@link TwistCalculator} or {@link MovingReferenceFrame}, this class allows to use any type of data for the joint velocities through the interface {@link JointVelocityAccessor}. + * Unlike {@link MovingReferenceFrame}, this class allows to use any type of data for the joint velocities through the interface {@link JointVelocityAccessor}. *

*/ public class RigidBodyTwistCalculator implements RigidBodyTwistProvider diff --git a/src/main/java/us/ihmc/mecano/algorithms/interfaces/MassMatrixCalculator.java b/src/main/java/us/ihmc/mecano/algorithms/interfaces/MassMatrixCalculator.java new file mode 100644 index 0000000..572f635 --- /dev/null +++ b/src/main/java/us/ihmc/mecano/algorithms/interfaces/MassMatrixCalculator.java @@ -0,0 +1,17 @@ +package us.ihmc.mecano.algorithms.interfaces; + +import org.ejml.data.DMatrixRMaj; + +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; + +public interface MassMatrixCalculator +{ + + void compute(); + + DMatrixRMaj getMassMatrix(); + + void getMassMatrix(DMatrixRMaj massMatrixToPack); + + JointBasics[] getJointsInOrder(); +} \ No newline at end of file diff --git a/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java b/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java index 9d6f61c..bdbf0d5 100644 --- a/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java +++ b/src/main/java/us/ihmc/mecano/tools/MultiBodySystemTools.java @@ -5,6 +5,7 @@ import us.ihmc.mecano.multiBodySystem.interfaces.*; import us.ihmc.mecano.multiBodySystem.iterators.SubtreeStreams; import us.ihmc.mecano.spatial.SpatialInertia; +import us.ihmc.mecano.spatial.interfaces.SpatialInertiaReadOnly; import java.lang.reflect.Array; import java.util.ArrayList; @@ -34,6 +35,24 @@ public static SpatialInertia computeSubtreeInertia(JointReadOnly joint) return computeSubtreeInertia(joint.getSuccessor()); } + /** + * Computes the mass of all the links contained in the subtree of {@param rootBody} + * @param rootBody + * @return subtree mass + */ + public static double computeSubTreeMass(RigidBodyReadOnly rootBody) + { + SpatialInertiaReadOnly inertia = rootBody.getInertia(); + double ret = inertia == null ? 0.0 : inertia.getMass(); + + for (int i = 0; i < rootBody.getChildrenJoints().size(); i++) + { + ret += computeSubTreeMass(rootBody.getChildrenJoints().get(i).getSuccessor()); + } + + return ret; + } + /** * Sums the inertia of all the rigid-bodies composing the subtree that originates at * {@code rootBody} including {@code rootBody}. @@ -1736,4 +1755,5 @@ public static List sortLoopClosureInChildrenJoints(RigidBodyReadO } return childrenJoints; } + } \ No newline at end of file diff --git a/src/test/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculatorTest.java b/src/test/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculatorTest.java new file mode 100644 index 0000000..70a4789 --- /dev/null +++ b/src/test/java/us/ihmc/mecano/algorithms/DifferentialIDMassMatrixCalculatorTest.java @@ -0,0 +1,24 @@ +package us.ihmc.mecano.algorithms; + +import static org.junit.jupiter.api.Assertions.assertEquals; + +import org.ejml.data.DMatrixRMaj; +import org.junit.jupiter.api.Test; + +public class DifferentialIDMassMatrixCalculatorTest extends MassMatrixCalculatorTest +{ + + @Test + public void testKineticEnergy() + { + setUpRandomChainRobot(); + double expectedKineticEnergy = computeKineticEnergy(joints); + + DifferentialIDMassMatrixCalculator massMatrixCalculator = new DifferentialIDMassMatrixCalculator(worldFrame, elevator); + massMatrixCalculator.compute(); + DMatrixRMaj massMatrix = massMatrixCalculator.getMassMatrix(); + double kineticEnergyFromMassMatrix = computeKineticEnergy(joints, massMatrix); + + assertEquals(expectedKineticEnergy, kineticEnergyFromMassMatrix, 1e-12); + } +} diff --git a/src/test/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculatorTest.java b/src/test/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculatorTest.java new file mode 100644 index 0000000..103b5d5 --- /dev/null +++ b/src/test/java/us/ihmc/mecano/algorithms/GravityCoriolisExternalWrenchMatrixCalculatorTest.java @@ -0,0 +1,323 @@ +package us.ihmc.mecano.algorithms; + +import static org.junit.jupiter.api.Assertions.assertNull; +import static org.junit.jupiter.api.Assertions.assertTrue; +import static us.ihmc.mecano.tools.MecanoRandomTools.nextWrench; + +import java.util.Collections; +import java.util.List; +import java.util.Map; +import java.util.Random; +import java.util.stream.Collectors; + +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.MatrixFeatures_DDRM; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.tools.EuclidCoreIOTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.mecano.multiBodySystem.Joint; +import us.ihmc.mecano.multiBodySystem.OneDoFJoint; +import us.ihmc.mecano.multiBodySystem.PrismaticJoint; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly; +import us.ihmc.mecano.multiBodySystem.interfaces.MultiBodySystemReadOnly; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyReadOnly; +import us.ihmc.mecano.spatial.SpatialAcceleration; +import us.ihmc.mecano.spatial.interfaces.SpatialAccelerationReadOnly; +import us.ihmc.mecano.spatial.interfaces.WrenchReadOnly; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MecanoTestTools; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools.RandomFloatingRevoluteJointChain; +import us.ihmc.mecano.tools.MultiBodySystemTools; + +public class GravityCoriolisExternalWrenchMatrixCalculatorTest +{ + private static final int ITERATIONS = 500; + private static final double ONE_DOF_JOINT_EPSILON = 8.0e-12; + private static final double FLOATING_JOINT_EPSILON = 2.0e-11; + private static final double ALL_JOINT_EPSILON = 1.0e-4; + + @Test + public void testPrismaticJointChain() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(50) + 1; + List joints = MultiBodySystemRandomTools.nextPrismaticJointChain(random, numberOfJoints); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ONE_DOF_JOINT_EPSILON); + } + } + + @Test + public void testPrismaticJointTree() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(50) + 1; + List joints = MultiBodySystemRandomTools.nextPrismaticJointTree(random, numberOfJoints); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ONE_DOF_JOINT_EPSILON); + } + } + + @Test + public void testRevoluteJointChain() + { + Random random = new Random(2654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(50) + 1; + List joints = MultiBodySystemRandomTools.nextRevoluteJointChain(random, numberOfJoints); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ONE_DOF_JOINT_EPSILON); + } + } + + @Test + public void testRevoluteJointTree() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + List joints = MultiBodySystemRandomTools.nextRevoluteJointTree(random, random.nextInt(50) + 1); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + joints = MultiBodySystemRandomTools.nextRevoluteJointTree(random, random.nextInt(40) + 1); + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ONE_DOF_JOINT_EPSILON); + } + } + + @Test + public void testOneDoFJointChain() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(50) + 1; + List joints = MultiBodySystemRandomTools.nextOneDoFJointChain(random, numberOfJoints); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ONE_DOF_JOINT_EPSILON); + } + } + + @Test + public void testOneDoFJointTree() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(50) + 1; + List joints = MultiBodySystemRandomTools.nextOneDoFJointTree(random, numberOfJoints); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ONE_DOF_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ONE_DOF_JOINT_EPSILON); + } + } + + @Test + public void testFloatingRevoluteJointChain() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(40) + 1; + List joints = new RandomFloatingRevoluteJointChain(random, numberOfJoints).getJoints(); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), FLOATING_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), FLOATING_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + 2.0 * FLOATING_JOINT_EPSILON); + } + } + + @Test + public void testJointChain() + { + Random random = new Random(21654); + + for (int i = 0; i < ITERATIONS; i++) + { + int numberOfJoints = random.nextInt(40) + 1; + List joints = MultiBodySystemRandomTools.nextJointChain(random, numberOfJoints); + compareAgainstInverseDynamicsCalculator(random, i, joints, Collections.emptyMap(), Collections.emptyList(), ALL_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, i, joints, nextExternalWrenches(random, joints), Collections.emptyList(), ALL_JOINT_EPSILON); + + compareAgainstInverseDynamicsCalculator(random, + i, + joints, + Collections.emptyMap(), + Collections.singletonList(joints.get(random.nextInt(joints.size()))), + ALL_JOINT_EPSILON); + } + } + + private void compareAgainstInverseDynamicsCalculator(Random random, + int iteration, + List joints, + Map externalWrenches, + List jointsToIgnore, + double epsilon) + { + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.ACCELERATION, joints); + + RigidBodyBasics rootBody = MultiBodySystemTools.getRootBody(joints.get(0).getPredecessor()); + MultiBodySystemReadOnly multiBodySystemInput = MultiBodySystemReadOnly.toMultiBodySystemInput(rootBody, jointsToIgnore); + rootBody.updateFramesRecursively(); + + double gravity = EuclidCoreRandomTools.nextDouble(random, -10.0, -1.0); + InverseDynamicsCalculator inverseDynamicsCalculator = new InverseDynamicsCalculator(multiBodySystemInput); + inverseDynamicsCalculator.setConsiderCoriolisAndCentrifugalForces(true); + inverseDynamicsCalculator.setConsiderJointAccelerations(false); + inverseDynamicsCalculator.setGravitionalAcceleration(gravity); + GravityCoriolisExternalWrenchMatrixCalculator gcewmCalculator = new GravityCoriolisExternalWrenchMatrixCalculator(multiBodySystemInput); + gcewmCalculator.setGravitionalAcceleration(gravity); + + int numberOfDoFs = joints.stream().mapToInt(JointReadOnly::getDegreesOfFreedom).sum(); + + externalWrenches.forEach(inverseDynamicsCalculator::setExternalWrench); + inverseDynamicsCalculator.compute(); + DMatrixRMaj tau_expected = inverseDynamicsCalculator.getJointTauMatrix(); + + externalWrenches.forEach(gcewmCalculator::setExternalWrench); + gcewmCalculator.compute(); + DMatrixRMaj tau_actual = gcewmCalculator.getJointTauMatrix(); + + boolean areEqual = MatrixFeatures_DDRM.isEquals(tau_expected, tau_actual, epsilon); + if (!areEqual) + { + System.out.println("iteration: " + iteration); + double maxError = 0.0; + DMatrixRMaj output = new DMatrixRMaj(numberOfDoFs, 3); + for (int row = 0; row < numberOfDoFs; row++) + { + output.set(row, 0, tau_expected.get(row, 0)); + output.set(row, 1, tau_actual.get(row, 0)); + double error = tau_expected.get(row, 0) - tau_actual.get(row, 0); + output.set(row, 2, error); + maxError = Math.max(maxError, Math.abs(error)); + } + output.print(EuclidCoreIOTools.getStringFormat(9, 6)); + System.out.println("Max error: " + maxError); + } + assertTrue(areEqual); + + List allRigidBodies = rootBody.subtreeList(); + + for (RigidBodyReadOnly rigidBody : allRigidBodies) + { + try + { + SpatialAccelerationReadOnly expectedAccelerationOfBody = inverseDynamicsCalculator.getAccelerationProvider().getAccelerationOfBody(rigidBody); + + if (expectedAccelerationOfBody == null) + { + assertNull(gcewmCalculator.getAccelerationProvider().getAccelerationOfBody(rigidBody)); + } + else + { + SpatialAcceleration actualAccelerationOfBody = new SpatialAcceleration(gcewmCalculator.getAccelerationProvider() + .getAccelerationOfBody(rigidBody)); + actualAccelerationOfBody.changeFrame(expectedAccelerationOfBody.getReferenceFrame()); + MecanoTestTools.assertSpatialAccelerationEquals(expectedAccelerationOfBody, actualAccelerationOfBody, epsilon); + + for (int i = 0; i < 5; i++) + { + RigidBodyBasics otherRigidBody = allRigidBodies.get(random.nextInt(allRigidBodies.size())); + SpatialAccelerationReadOnly expectedRelativeAcceleration = inverseDynamicsCalculator.getAccelerationProvider() + .getRelativeAcceleration(rigidBody, otherRigidBody); + if (expectedAccelerationOfBody == null) + { + assertNull(gcewmCalculator.getAccelerationProvider().getRelativeAcceleration(otherRigidBody, rigidBody)); + } + else + { + SpatialAccelerationReadOnly actualRelativeAcceleration = gcewmCalculator.getAccelerationProvider() + .getRelativeAcceleration(rigidBody, otherRigidBody); + MecanoTestTools.assertSpatialAccelerationEquals(expectedRelativeAcceleration, actualRelativeAcceleration, epsilon); + } + } + } + } + catch (NullPointerException e) + { + // TODO Remove this try-catch block when switching to Mecano 0.0.22 or later. + } + } + } + + public static Map nextExternalWrenches(Random random, List joints) + { + return joints.stream() + .filter(j -> random.nextBoolean()) + .map(j -> j.getSuccessor()) + .collect(Collectors.toMap(b -> b, b -> nextWrench(random, b.getBodyFixedFrame(), b.getBodyFixedFrame()))); + } +} diff --git a/src/test/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorTest.java b/src/test/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorTest.java new file mode 100644 index 0000000..1e7be6b --- /dev/null +++ b/src/test/java/us/ihmc/mecano/algorithms/MassMatrixCalculatorTest.java @@ -0,0 +1,172 @@ +package us.ihmc.mecano.algorithms; + + +import java.util.ArrayList; +import java.util.Random; + +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.simple.SimpleMatrix; +import org.junit.jupiter.api.AfterEach; +import org.junit.jupiter.api.BeforeEach; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.mecano.algorithms.interfaces.MassMatrixCalculator; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.RigidBody; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.SpatialInertia; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemTools; + +import static org.junit.jupiter.api.Assertions.*; + +public abstract class MassMatrixCalculatorTest +{ + protected static final Vector3D X = new Vector3D(1.0, 0.0, 0.0); + protected static final Vector3D Y = new Vector3D(0.0, 1.0, 0.0); + protected static final Vector3D Z = new Vector3D(0.0, 0.0, 1.0); + + protected final ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + protected ArrayList joints; + protected RigidBodyBasics elevator; + + private final Random random = new Random(1776L); + + @BeforeEach + public void setUp() + { + elevator = new RigidBody("elevator", worldFrame); + } + + @AfterEach + public void tearDown() + { + ReferenceFrameTools.clearWorldFrameTree(); + } + + protected void setUpRandomChainRobot() + { + Random random = new Random(); + joints = new ArrayList(); + Vector3D[] jointAxes = {X, Y, Z, X, Z, Z, X, Y, Z, X}; + joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "", elevator, jointAxes)); + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + elevator.updateFramesRecursively(); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + } + + protected double computeKineticEnergy(ArrayList joints) + { + double ret = 0.0; + ReferenceFrame elevatorFrame = elevator.getBodyFixedFrame(); + Twist twistWithRespectToWorld = new Twist(elevatorFrame, elevatorFrame, elevatorFrame); + Twist successorTwist = new Twist(); + for (RevoluteJoint joint : joints) + { + joint.getSuccessorTwist(successorTwist); + successorTwist.changeFrame(elevatorFrame); + twistWithRespectToWorld.add(successorTwist); + + SpatialInertia inertia = new SpatialInertia(joint.getSuccessor().getInertia()); + inertia.changeFrame(elevatorFrame); + + ret += inertia.computeKineticCoEnergy(twistWithRespectToWorld); + } + + return ret; + } + + protected double computeKineticEnergy(ArrayList joints, DMatrixRMaj massMatrix) + { + SimpleMatrix jointVelocities = new SimpleMatrix(joints.size(), 1); + for (int i = 0; i < joints.size(); i++) + { + jointVelocities.set(i, 0, joints.get(i).getQd()); + } + + SimpleMatrix massMatrix_ = SimpleMatrix.wrap(massMatrix); + + SimpleMatrix kineticEnergy = jointVelocities.transpose().mult(massMatrix_).mult(jointVelocities); + + return 0.5 * kineticEnergy.get(0, 0); + } + + @Test + public void compareMassMatrixCalculators() + { + double eps = 1e-10; + setUpRandomChainRobot(); + ArrayList massMatrixCalculators = new ArrayList(); + massMatrixCalculators.add(new DifferentialIDMassMatrixCalculator(worldFrame, elevator)); + massMatrixCalculators.add(new MassMatrixCalculator() + { + CompositeRigidBodyMassMatrixCalculator crbmmc = new CompositeRigidBodyMassMatrixCalculator(elevator); + + @Override + public void getMassMatrix(DMatrixRMaj massMatrixToPack) + { + massMatrixToPack.set(getMassMatrix()); + } + + @Override + public DMatrixRMaj getMassMatrix() + { + return crbmmc.getMassMatrix(); + } + + @Override + public JointBasics[] getJointsInOrder() + { + return crbmmc.getInput().getJointsToConsider().toArray(new JointBasics[0]); + } + + @Override + public void compute() + { + crbmmc.reset(); + } + }); + ArrayList massMatrices = new ArrayList(); + int nDoFs = MultiBodySystemTools.computeDegreesOfFreedom(joints); + for (int i = 0; i < massMatrixCalculators.size(); i++) + { + massMatrices.add(new DMatrixRMaj(nDoFs, nDoFs)); + } + DMatrixRMaj diffMassMatrix = new DMatrixRMaj(nDoFs, nDoFs); + + int nIterations = 10000; + for (int i = 0; i < nIterations; i++) + { + MultiBodySystemRandomTools.nextState(random, JointStateType.CONFIGURATION, -Math.PI / 2.0, Math.PI / 2.0, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.VELOCITY, joints); + MultiBodySystemRandomTools.nextState(random, JointStateType.ACCELERATION, joints); + elevator.updateFramesRecursively(); + + for (int j = 0; j < massMatrixCalculators.size(); j++) + { + massMatrixCalculators.get(j).compute(); + massMatrices.set(j, massMatrixCalculators.get(j).getMassMatrix()); + + if (j > 0) + { + CommonOps_DDRM.subtract(massMatrices.get(j), massMatrices.get(j - 1), diffMassMatrix); + + double[] data = diffMassMatrix.getData(); + for (int k = 0; k < data.length; k++) + { + assertEquals(0.0, data[k], eps); + } + } + } + } + + } + +} diff --git a/src/test/java/us/ihmc/mecano/algorithms/MomentumCalculatorTest.java b/src/test/java/us/ihmc/mecano/algorithms/MomentumCalculatorTest.java new file mode 100644 index 0000000..6715462 --- /dev/null +++ b/src/test/java/us/ihmc/mecano/algorithms/MomentumCalculatorTest.java @@ -0,0 +1,157 @@ +package us.ihmc.mecano.algorithms; + +import java.util.ArrayList; +import java.util.Random; + +import org.ejml.EjmlUnitTests; +import org.ejml.data.DMatrixRMaj; +import org.ejml.dense.row.CommonOps_DDRM; +import org.ejml.dense.row.NormOps_DDRM; +import org.junit.jupiter.api.Test; + +import us.ihmc.euclid.matrix.Matrix3D; +import us.ihmc.euclid.referenceFrame.FrameVector3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.tools.EuclidCoreTestTools; +import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.mecano.frames.CenterOfMassReferenceFrame; +import us.ihmc.mecano.multiBodySystem.PrismaticJoint; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.RigidBody; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.Momentum; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; + +import static org.junit.jupiter.api.Assertions.*; + +public class MomentumCalculatorTest +{ + private final ReferenceFrame world = ReferenceFrame.getWorldFrame(); + + @Test + public void testSingleRigidBodyTranslation() + { + Random random = new Random(1766L); + + RigidBodyBasics elevator = new RigidBody("elevator", world); + Vector3D jointAxis = EuclidCoreRandomTools.nextVector3D(random); + jointAxis.normalize(); + PrismaticJoint joint = new PrismaticJoint("joint", elevator, new Vector3D(), jointAxis); + RigidBodyBasics body = new RigidBody("body", joint, EuclidCoreRandomTools.nextDiagonalMatrix3D(random), random.nextDouble(), new Vector3D()); + + joint.setQ(random.nextDouble()); + joint.setQd(random.nextDouble()); + + Momentum momentum = computeMomentum(elevator, world); + + momentum.changeFrame(world); + FrameVector3D linearMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getLinearPart())); + FrameVector3D angularMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getAngularPart())); + + FrameVector3D linearMomentumCheck = new FrameVector3D(joint.getFrameBeforeJoint(), jointAxis); + linearMomentumCheck.scale(body.getInertia().getMass() * joint.getQd()); + FrameVector3D angularMomentumCheck = new FrameVector3D(world); + + double epsilon = 1e-9; + EuclidCoreTestTools.assertEquals(linearMomentumCheck, linearMomentum, epsilon); + EuclidCoreTestTools.assertEquals(angularMomentumCheck, angularMomentum, epsilon); + assertTrue(linearMomentum.norm() > epsilon); + } + + @Test + public void testSingleRigidBodyRotation() + { + Random random = new Random(1766L); + + RigidBodyBasics elevator = new RigidBody("elevator", world); + Vector3D jointAxis = EuclidCoreRandomTools.nextVector3D(random); + jointAxis.normalize(); + RigidBodyTransform transformToParent = new RigidBodyTransform(); + transformToParent.setIdentity(); + RevoluteJoint joint = new RevoluteJoint("joint", elevator, transformToParent, jointAxis); + RigidBodyBasics body = new RigidBody("body", joint, EuclidCoreRandomTools.nextDiagonalMatrix3D(random), random.nextDouble(), new Vector3D()); + + joint.setQ(random.nextDouble()); + joint.setQd(random.nextDouble()); + + Momentum momentum = computeMomentum(elevator, world); + + momentum.changeFrame(world); + FrameVector3D linearMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getLinearPart())); + FrameVector3D angularMomentum = new FrameVector3D(momentum.getReferenceFrame(), new Vector3D(momentum.getAngularPart())); + + FrameVector3D linearMomentumCheck = new FrameVector3D(world); + Matrix3D inertia = new Matrix3D(body.getInertia().getMomentOfInertia()); + Vector3D angularMomentumCheckVector = new Vector3D(jointAxis); + angularMomentumCheckVector.scale(joint.getQd()); + inertia.transform(angularMomentumCheckVector); + FrameVector3D angularMomentumCheck = new FrameVector3D(body.getInertia().getReferenceFrame(), angularMomentumCheckVector); + angularMomentumCheck.changeFrame(world); + + double epsilon = 1e-9; + EuclidCoreTestTools.assertEquals(linearMomentumCheck, linearMomentum, epsilon); + EuclidCoreTestTools.assertEquals(angularMomentumCheck, angularMomentum, epsilon); + assertTrue(angularMomentum.norm() > epsilon); + } + + @Test + public void testChainAgainstCentroidalMomentumMatrix() + { + Random random = new Random(17679L); + + ArrayList joints = new ArrayList(); + RigidBodyBasics elevator = new RigidBody("elevator", world); + int nJoints = 10; + Vector3D[] jointAxes = new Vector3D[nJoints]; + for (int i = 0; i < nJoints; i++) + { + Vector3D jointAxis = EuclidCoreRandomTools.nextVector3D(random); + jointAxis.normalize(); + jointAxes[i] = jointAxis; + } + + joints.addAll(MultiBodySystemRandomTools.nextRevoluteJointChain(random, "randomChain", elevator, jointAxes)); + JointBasics[] jointsArray = new RevoluteJoint[joints.size()]; + joints.toArray(jointsArray); + ReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("comFrame", world, elevator); + + for (RevoluteJoint joint : joints) + { + joint.setQ(random.nextDouble()); + joint.setQd(random.nextDouble()); + } + + Momentum momentum = computeMomentum(elevator, centerOfMassFrame); + DMatrixRMaj momentumMatrix = new DMatrixRMaj(Momentum.SIZE, 1); + momentum.get(momentumMatrix); + + CentroidalMomentumCalculator centroidalMomentumMatrix = new CentroidalMomentumCalculator(elevator, centerOfMassFrame); + centroidalMomentumMatrix.reset(); + DMatrixRMaj centroidalMomentumMatrixMatrix = centroidalMomentumMatrix.getCentroidalMomentumMatrix(); + DMatrixRMaj jointVelocitiesMatrix = new DMatrixRMaj(MultiBodySystemTools.computeDegreesOfFreedom(jointsArray), 1); + MultiBodySystemTools.extractJointsState(jointsArray, JointStateType.VELOCITY, jointVelocitiesMatrix); + DMatrixRMaj momentumFromCentroidalMomentumMatrix = new DMatrixRMaj(Momentum.SIZE, 1); + CommonOps_DDRM.mult(centroidalMomentumMatrixMatrix, jointVelocitiesMatrix, momentumFromCentroidalMomentumMatrix); + + double epsilon = 1e-9; + assertEquals(momentum.getReferenceFrame(), centerOfMassFrame); + EjmlUnitTests.assertEquals(momentumFromCentroidalMomentumMatrix, momentumMatrix, epsilon); + double norm = NormOps_DDRM.normP2(momentumMatrix); + assertTrue(norm > epsilon); + } + + private Momentum computeMomentum(RigidBodyBasics elevator, ReferenceFrame frame) + { + elevator.updateFramesRecursively(); + MomentumCalculator momentumCalculator = new MomentumCalculator(elevator); + Momentum momentum = new Momentum(frame); + momentumCalculator.computeAndPack(momentum); + + return momentum; + } +} diff --git a/src/test/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculatorTest.java b/src/test/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculatorTest.java new file mode 100644 index 0000000..e80297a --- /dev/null +++ b/src/test/java/us/ihmc/mecano/algorithms/RigidBodyTwistCalculatorTest.java @@ -0,0 +1,95 @@ +package us.ihmc.mecano.algorithms; + +import org.ejml.data.DMatrixRMaj; +import org.junit.jupiter.api.Test; +import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; +import us.ihmc.mecano.multiBodySystem.interfaces.MultiBodySystemReadOnly; +import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; +import us.ihmc.mecano.spatial.Twist; +import us.ihmc.mecano.tools.JointStateType; +import us.ihmc.mecano.tools.MecanoTestTools; +import us.ihmc.mecano.tools.MultiBodySystemRandomTools; +import us.ihmc.mecano.tools.MultiBodySystemTools; + +import java.util.List; +import java.util.Random; + +public class RigidBodyTwistCalculatorTest +{ + + public static final int ITERATIONS = 1000; + public static final double EPSILON = 1.0e-12; + + @Test + public void testAgainstMovingReferenceFrame() + { + Random random = new Random(1776L); + Twist expectedTwist = new Twist(); + + for (int i = 0; i < ITERATIONS; i++) + { + List joints = MultiBodySystemRandomTools.nextJointTree(random, 10); + RigidBodyBasics root = joints.get(0).getPredecessor(); + for (JointStateType stateType : JointStateType.values()) + { + MultiBodySystemRandomTools.nextState(random, stateType, joints); + } + + root.updateFramesRecursively(); + + RigidBodyTwistCalculator twistCalculator = new RigidBodyTwistCalculator(MultiBodySystemReadOnly.toMultiBodySystemInput(root)); + twistCalculator.setJointVelocityAccessor((joint, matrixToPack) -> joint.getJointVelocity(0, matrixToPack)); + + for (JointBasics joint : joints) + { + RigidBodyBasics body = joint.getSuccessor(); + MecanoTestTools.assertTwistEquals(body.getBodyFixedFrame().getTwistOfFrame(), twistCalculator.getTwistOfBody(body), EPSILON); + + for (JointBasics otherJoint : joints) + { + RigidBodyBasics otherBody = otherJoint.getSuccessor(); + body.getBodyFixedFrame().getTwistRelativeToOther(otherBody.getBodyFixedFrame(), expectedTwist); + MecanoTestTools.assertTwistEquals(expectedTwist, twistCalculator.getRelativeTwist(otherBody, body), EPSILON); + } + } + } + } + + @Test + public void testAgainstMovingReferenceFrameWithMatrixAccessor() + { + Random random = new Random(1776L); + Twist expectedTwist = new Twist(); + + for (int i = 0; i < ITERATIONS; i++) + { + List joints = MultiBodySystemRandomTools.nextJointTree(random, 10); + RigidBodyBasics root = joints.get(0).getPredecessor(); + for (JointStateType stateType : JointStateType.values()) + { + MultiBodySystemRandomTools.nextState(random, stateType, joints); + } + + root.updateFramesRecursively(); + + MultiBodySystemReadOnly multiBodySystemInput = MultiBodySystemReadOnly.toMultiBodySystemInput(root); + DMatrixRMaj jointVelocities = new DMatrixRMaj(multiBodySystemInput.getNumberOfDoFs(), 1); + MultiBodySystemTools.extractJointsState(multiBodySystemInput.getJointsToConsider(), JointStateType.VELOCITY, jointVelocities); + RigidBodyTwistCalculator twistCalculator = new RigidBodyTwistCalculator(multiBodySystemInput); + twistCalculator.useAllJointVelocityMatrix(jointVelocities); + + for (JointBasics joint : joints) + { + RigidBodyBasics body = joint.getSuccessor(); + MecanoTestTools.assertTwistEquals(body.getBodyFixedFrame().getTwistOfFrame(), twistCalculator.getTwistOfBody(body), EPSILON); + + for (JointBasics otherJoint : joints) + { + RigidBodyBasics otherBody = otherJoint.getSuccessor(); + body.getBodyFixedFrame().getTwistRelativeToOther(otherBody.getBodyFixedFrame(), expectedTwist); + MecanoTestTools.assertTwistEquals(expectedTwist, twistCalculator.getRelativeTwist(otherBody, body), EPSILON); + } + } + } + } +} diff --git a/src/test/java/us/ihmc/mecano/tools/MultiBodySystemToolsTest.java b/src/test/java/us/ihmc/mecano/tools/MultiBodySystemToolsTest.java index 7c1acd7..4861ae9 100644 --- a/src/test/java/us/ihmc/mecano/tools/MultiBodySystemToolsTest.java +++ b/src/test/java/us/ihmc/mecano/tools/MultiBodySystemToolsTest.java @@ -18,7 +18,14 @@ import org.junit.jupiter.api.Test; +import us.ihmc.euclid.matrix.Matrix3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.tools.ReferenceFrameTools; +import us.ihmc.euclid.tools.EuclidCoreRandomTools; +import us.ihmc.euclid.tuple3D.Vector3D; import us.ihmc.mecano.multiBodySystem.OneDoFJoint; +import us.ihmc.mecano.multiBodySystem.RevoluteJoint; +import us.ihmc.mecano.multiBodySystem.RigidBody; import us.ihmc.mecano.multiBodySystem.interfaces.JointBasics; import us.ihmc.mecano.multiBodySystem.interfaces.JointReadOnly; import us.ihmc.mecano.multiBodySystem.interfaces.RigidBodyBasics; @@ -675,4 +682,65 @@ public void testFindRigidBody() throws Exception assertTrue(expectedBody == MultiBodySystemTools.findRigidBody((RigidBodyReadOnly) rootBody, expectedBodyName.toUpperCase(), true)); } } + + @Test + public void testComputeSubTreeMass() + { + Random random = new Random(100L); + + ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); + RigidBodyBasics elevator = new RigidBody("body", worldFrame); + int numberOfJoints = 100; + double addedMass = createRandomRigidBodyTreeAndReturnTotalMass(worldFrame, elevator, numberOfJoints, random); + + assertEquals(addedMass, MultiBodySystemTools.computeSubTreeMass(elevator), 0.00001); + + ReferenceFrameTools.clearWorldFrameTree(); + } + + public static double createRandomRigidBodyTreeAndReturnTotalMass(ReferenceFrame worldFrame, RigidBodyBasics elevator, int numberOfJoints, Random random) + { + double totalMass = 0.0; + boolean rootAdded = false; + ArrayList potentialInverseDynamicsParentJoints = new ArrayList(); // synchronized with potentialParentJoints + + for (int i = 0; i < numberOfJoints; i++) + { + Vector3D jointOffset = EuclidCoreRandomTools.nextVector3D(random); + Vector3D jointAxis = new Vector3D(random.nextDouble(), random.nextDouble(), random.nextDouble()); + jointAxis.normalize(); + Matrix3D momentOfInertia = EuclidCoreRandomTools.nextDiagonalMatrix3D(random); + double mass = random.nextDouble(); + totalMass += mass; + Vector3D comOffset = EuclidCoreRandomTools.nextVector3D(random); + + + + RigidBodyBasics inverseDynamicsParentBody; + if (!rootAdded) + { + rootAdded = true; + inverseDynamicsParentBody = elevator; + } + else + { + int parentIndex = random.nextInt(potentialInverseDynamicsParentJoints.size()); + RevoluteJoint inverseDynamicsParentJoint = potentialInverseDynamicsParentJoints.get(parentIndex); + inverseDynamicsParentBody = inverseDynamicsParentJoint.getSuccessor(); + } + + RevoluteJoint currentJoint = new RevoluteJoint("jointID" + i, inverseDynamicsParentBody, jointOffset, jointAxis); + double jointPosition = random.nextDouble(); + currentJoint.setQ(jointPosition); + currentJoint.setQd(0.0); + currentJoint.setQdd(0.0); + new RigidBody("bodyID" + i, currentJoint, momentOfInertia, mass, comOffset); + + potentialInverseDynamicsParentJoints.add(currentJoint); + } + + elevator.updateFramesRecursively(); + + return totalMass; + } } diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/AlphaFilteredYoMutableFrameSpatialVector.java similarity index 94% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java rename to src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/AlphaFilteredYoMutableFrameSpatialVector.java index 60e21d0..9f01a33 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoMutableFrameSpatialVector.java +++ b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/AlphaFilteredYoMutableFrameSpatialVector.java @@ -1,9 +1,10 @@ -package us.ihmc.robotics.math.filters; +package us.ihmc.mecano.yoVariables.filters; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; -import us.ihmc.robotics.dataStructures.YoMutableFrameSpatialVector; +import us.ihmc.mecano.yoVariables.spatial.YoMutableFrameSpatialVector; +import us.ihmc.yoVariables.euclid.filters.AlphaFilteredYoMutableFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/AlphaFilteredYoSpatialVector.java similarity index 70% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java rename to src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/AlphaFilteredYoSpatialVector.java index 91e6698..0c5db2d 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/AlphaFilteredYoSpatialVector.java +++ b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/AlphaFilteredYoSpatialVector.java @@ -1,35 +1,36 @@ -package us.ihmc.robotics.math.filters; +package us.ihmc.mecano.yoVariables.filters; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; +import us.ihmc.yoVariables.euclid.filters.AlphaFilteredYoFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; public class AlphaFilteredYoSpatialVector extends YoFixedFrameSpatialVector { - private final AlphaFilteredYoFrameVector alphaFilteredAngularPart; - private final AlphaFilteredYoFrameVector alphaFilteredLinearPart; + private final AlphaFilteredYoFrameVector3D alphaFilteredAngularPart; + private final AlphaFilteredYoFrameVector3D alphaFilteredLinearPart; public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - super(new AlphaFilteredYoFrameVector(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawAngularPart), - new AlphaFilteredYoFrameVector(namePrefix + "LinearPart", nameSuffix, registry, alphaLinearPart, rawLinearPart)); - this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) getAngularPart(); - this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); + super(new AlphaFilteredYoFrameVector3D(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawAngularPart), + new AlphaFilteredYoFrameVector3D(namePrefix + "LinearPart", nameSuffix, registry, alphaLinearPart, rawLinearPart)); + this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector3D) getAngularPart(); + this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector3D) getLinearPart(); } public AlphaFilteredYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider alphaAngularPart, DoubleProvider alphaLinearPart, YoFixedFrameSpatialVector rawSpatialVector) { - super(new AlphaFilteredYoFrameVector(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), - new AlphaFilteredYoFrameVector(namePrefix + "LinearPart", nameSuffix, registry, alphaLinearPart, rawSpatialVector.getLinearPart())); - this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector) getAngularPart(); - this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector) getLinearPart(); + super(new AlphaFilteredYoFrameVector3D(namePrefix + "AngularPart", nameSuffix, registry, alphaAngularPart, rawSpatialVector.getAngularPart()), + new AlphaFilteredYoFrameVector3D(namePrefix + "LinearPart", nameSuffix, registry, alphaLinearPart, rawSpatialVector.getLinearPart())); + this.alphaFilteredAngularPart = (AlphaFilteredYoFrameVector3D) getAngularPart(); + this.alphaFilteredLinearPart = (AlphaFilteredYoFrameVector3D) getLinearPart(); } - public AlphaFilteredYoSpatialVector(AlphaFilteredYoFrameVector yoAngularPart, AlphaFilteredYoFrameVector yoLinearPart) + public AlphaFilteredYoSpatialVector(AlphaFilteredYoFrameVector3D yoAngularPart, AlphaFilteredYoFrameVector3D yoLinearPart) { super(yoAngularPart, yoLinearPart); this.alphaFilteredAngularPart = yoAngularPart; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/RateLimitedYoMutableSpatialVector.java similarity index 94% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java rename to src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/RateLimitedYoMutableSpatialVector.java index b3491bc..ca7da1b 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoMutableSpatialVector.java +++ b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/RateLimitedYoMutableSpatialVector.java @@ -1,9 +1,10 @@ -package us.ihmc.robotics.math.filters; +package us.ihmc.mecano.yoVariables.filters; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; -import us.ihmc.robotics.dataStructures.YoMutableFrameSpatialVector; +import us.ihmc.mecano.yoVariables.spatial.YoMutableFrameSpatialVector; +import us.ihmc.yoVariables.euclid.filters.RateLimitedYoMutableFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/RateLimitedYoSpatialVector.java similarity index 62% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java rename to src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/RateLimitedYoSpatialVector.java index 4e60963..8d75356 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/math/filters/RateLimitedYoSpatialVector.java +++ b/src/yovariables-filters/java/us/ihmc/mecano/yoVariables/filters/RateLimitedYoSpatialVector.java @@ -1,35 +1,36 @@ -package us.ihmc.robotics.math.filters; +package us.ihmc.mecano.yoVariables.filters; import us.ihmc.euclid.referenceFrame.interfaces.FrameVector3DReadOnly; import us.ihmc.euclid.tuple3D.interfaces.Vector3DReadOnly; import us.ihmc.mecano.yoVariables.spatial.YoFixedFrameSpatialVector; +import us.ihmc.yoVariables.euclid.filters.RateLimitedYoFrameVector3D; import us.ihmc.yoVariables.providers.DoubleProvider; import us.ihmc.yoVariables.registry.YoRegistry; public class RateLimitedYoSpatialVector extends YoFixedFrameSpatialVector { - private final RateLimitedYoFrameVector rateLimitedAngularPart; - private final RateLimitedYoFrameVector rateLimitedLinearPart; + private final RateLimitedYoFrameVector3D rateLimitedAngularPart; + private final RateLimitedYoFrameVector3D rateLimitedLinearPart; public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, FrameVector3DReadOnly rawAngularPart, FrameVector3DReadOnly rawLinearPart) { - super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart), - new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart)); - this.rateLimitedAngularPart = (RateLimitedYoFrameVector) getAngularPart(); - this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); + super(new RateLimitedYoFrameVector3D(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawAngularPart), + new RateLimitedYoFrameVector3D(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawLinearPart)); + this.rateLimitedAngularPart = (RateLimitedYoFrameVector3D) getAngularPart(); + this.rateLimitedLinearPart = (RateLimitedYoFrameVector3D) getLinearPart(); } public RateLimitedYoSpatialVector(String namePrefix, String nameSuffix, YoRegistry registry, DoubleProvider maximumAngularRate, DoubleProvider maximumLinearRate, double dt, YoFixedFrameSpatialVector rawSpatialVector) { - super(new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), - new RateLimitedYoFrameVector(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getLinearPart())); - this.rateLimitedAngularPart = (RateLimitedYoFrameVector) getAngularPart(); - this.rateLimitedLinearPart = (RateLimitedYoFrameVector) getLinearPart(); + super(new RateLimitedYoFrameVector3D(namePrefix, nameSuffix, registry, maximumAngularRate, dt, rawSpatialVector.getAngularPart()), + new RateLimitedYoFrameVector3D(namePrefix, nameSuffix, registry, maximumLinearRate, dt, rawSpatialVector.getLinearPart())); + this.rateLimitedAngularPart = (RateLimitedYoFrameVector3D) getAngularPart(); + this.rateLimitedLinearPart = (RateLimitedYoFrameVector3D) getLinearPart(); } - public RateLimitedYoSpatialVector(RateLimitedYoFrameVector yoAngularPart, RateLimitedYoFrameVector yoLinearPart) + public RateLimitedYoSpatialVector(RateLimitedYoFrameVector3D yoAngularPart, RateLimitedYoFrameVector3D yoLinearPart) { super(yoAngularPart, yoLinearPart); this.rateLimitedAngularPart = yoAngularPart; diff --git a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java b/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoMutableFrameSpatialVector.java similarity index 98% rename from ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java rename to src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoMutableFrameSpatialVector.java index 3865585..0fc8ddf 100644 --- a/ihmc-robotics-toolkit/src/main/java/us/ihmc/robotics/dataStructures/YoMutableFrameSpatialVector.java +++ b/src/yovariables/java/us/ihmc/mecano/yoVariables/spatial/YoMutableFrameSpatialVector.java @@ -1,4 +1,4 @@ -package us.ihmc.robotics.dataStructures; +package us.ihmc.mecano.yoVariables.spatial; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.referenceFrame.interfaces.FixedFrameVector3DBasics; From a47a947a28290333ea0e0a5432ddd4c8133324c8 Mon Sep 17 00:00:00 2001 From: "rgriffin@ihmc.org" Date: Tue, 29 Oct 2024 18:28:54 -0500 Subject: [PATCH 27/27] :arrow_up: ihmc-yovariables 0.13.3 :arrow_up: euclid 0.22.2 --- build.gradle.kts | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/build.gradle.kts b/build.gradle.kts index 4c7c8e5..e081d83 100644 --- a/build.gradle.kts +++ b/build.gradle.kts @@ -17,9 +17,9 @@ mainDependencies { api("org.ejml:ejml-core:0.39") api("org.ejml:ejml-ddense:0.39") - api("us.ihmc:euclid:0.22.0") - api("us.ihmc:euclid-frame:0.22.0") - api("us.ihmc:euclid-geometry:0.22.0") + api("us.ihmc:euclid:0.22.2") + api("us.ihmc:euclid-frame:0.22.2") + api("us.ihmc:euclid-geometry:0.22.2") } testDependencies { @@ -47,11 +47,11 @@ graphvizDependencies { yovariablesDependencies { api(ihmc.sourceSetProject("main")) - api("us.ihmc:ihmc-yovariables:0.13.2") + api("us.ihmc:ihmc-yovariables:0.13.3") } yovariablesFiltersDependencies { api(ihmc.sourceSetProject("yovariables")) - api("us.ihmc:ihmc-yovariables-filters:0.13.2") + api("us.ihmc:ihmc-yovariables-filters:0.13.3") }