public RigidBodyInertia getInertiaCopy() { return new RigidBodyInertia(getInertia()); }
private void computeNetWrenches() { for (int bodyIndex = 0; bodyIndex < allBodiesExceptRoot.size(); bodyIndex++) { RigidBody body = allBodiesExceptRoot.get(bodyIndex); Wrench netWrench = netWrenches.get(body); twistCalculator.getTwistOfBody(tempTwist, body); if (!doVelocityTerms) tempTwist.setToZero(); spatialAccelerationCalculator.getAccelerationOfBody(tempAcceleration, body); body.getInertia().computeDynamicWrenchInBodyCoordinates(tempAcceleration, tempTwist, netWrench); } }
public static double computeMass(RigidBody[] rigidBodies) { double ret = 0.0; for (int i = 0; i < rigidBodies.length; i++) { ret += rigidBodies[i].getInertia().getMass(); } return ret; }
public RigidBody(String name, RigidBodyInertia inertia, InverseDynamicsJoint parentJoint) { nameBasedHashCode = NameBasedHashCodeTools.combineHashCodes(name, parentJoint); inertia.getBodyFrame().checkReferenceFrameMatch(inertia.getExpressedInFrame()); // inertia should be expressed in body frame, otherwise it will change this.name = name; this.inertia = inertia; this.bodyFixedFrame = inertia.getBodyFrame(); this.parentJoint = parentJoint; this.parentJoint.setSuccessor(this); }
private void printLinkInformation(RigidBody link, StringBuffer buffer) { RigidBodyInertia inertia = link.getInertia(); InverseDynamicsJoint parentJoint = link.getParentJoint(); if (inertia != null) { double mass = inertia.getMass(); Vector3d comOffset = new Vector3d(); RigidBodyTransform comOffsetTransform = link.getBodyFixedFrame().getTransformToDesiredFrame(parentJoint.getFrameAfterJoint()); comOffsetTransform.getTranslation(comOffset); Matrix3d momentOfInertia = inertia.getMassMomentOfInertiaPartCopy(); buffer.append("Mass = " + mass + "\n"); buffer.append("comOffset = " + comOffset + "\n"); buffer.append("momentOfInertia = \n" + momentOfInertia + "\n"); } List<InverseDynamicsJoint> childrenJoints = link.getChildrenJoints(); for (InverseDynamicsJoint childJoint : childrenJoints) { String parentJointName; if (parentJoint != null) parentJointName = parentJoint.getName(); else parentJointName = "root joint"; buffer.append("Found Child Joint of " + parentJointName + ".\n"); printJointInformation(childJoint, buffer); } }
tempTwist.changeFrame(inertia.getExpressedInFrame()); inertia.changeFrame(tempSpatialAcceleration.getExpressedInFrame()); // easier to change the frame of inertia than the spatial acceleration DenseMatrix64F inertiaTimesSpatialAccel = MatrixTools.mult(inertia.toMatrix(), tempSpatialAcceleration.toMatrix());
/** * Precomputes Ad^{T} * I since it is used twice in Adot*v and once in computing A. */ private void precomputeAdjointTimesInertia() { tempAdjoint.zero(); for(int i = 0; i<jointList.length; i++) { rigidBodies[i].getInertia().getExpressedInFrame().getTransformToDesiredFrame(tempTransform, centerOfMassFrame); tempTransform.get(tempMatrix3d,tempVector); set3By3MatrixBlock(tempAdjoint, 0, 0, tempMatrix3d); set3By3MatrixBlock(tempAdjoint, 3, 3, tempMatrix3d); MatrixTools.toTildeForm(tempPTilde, tempVector); tempPTilde.mul(tempMatrix3d); set3By3MatrixBlock(tempAdjoint, 0, 3, tempPTilde); rigidBodies[i].getInertia().getMatrix(tempInertiaMatrix); CommonOps.mult(tempAdjoint, tempInertiaMatrix, denseAdjTimesI[i]); } }
public void computeDynamicWrenchInBodyCoordinates(SpatialAccelerationVector acceleration, Twist twist, Wrench dynamicWrenchToPack) // TODO: write test { checkExpressedInBodyFixedFrame(); checkIsCrossPartZero(); // otherwise this operation would be a lot less efficient acceleration.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); acceleration.getBaseFrame().checkIsWorldFrame(); acceleration.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); twist.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); twist.getBaseFrame().checkIsWorldFrame(); twist.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); dynamicWrenchToPack.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); dynamicWrenchToPack.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); massMomentOfInertiaPart.transform(acceleration.getAngularPart(), tempVector); // J * omegad dynamicWrenchToPack.setAngularPart(tempVector); // [J * omegad; 0] massMomentOfInertiaPart.transform(twist.getAngularPart(), tempVector); // J * omega tempVector.cross(twist.getAngularPart(), tempVector); // omega x J * omega dynamicWrenchToPack.addAngularPart(tempVector); // [J * omegad + omega x J * omega; 0] tempVector.set(acceleration.getLinearPart()); // vd tempVector.scale(mass); // m * vd dynamicWrenchToPack.setLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd] tempVector.set(twist.getLinearPart()); // v tempVector.scale(mass); // m * v tempVector.cross(twist.getAngularPart(), tempVector); // omega x m * v dynamicWrenchToPack.addLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd + omega x m * v] }
private void setOffDiagonalTerms(int i) { int parentIndex; int j = i; while (isValidParentIndex(parentIndex = parentMap[j])) { ReferenceFrame parentFrame = allRigidBodiesInOrder[parentIndex].getInertia().getExpressedInFrame(); changeFrameOfMomenta(parentFrame); j = parentIndex; GeometricJacobian motionSubspace = allRigidBodiesInOrder[j].getParentJoint().getMotionSubspace(); setMassMatrixPart(i, j, motionSubspace); } }
public void getCoMOffset(FramePoint comOffsetToPack) { inertia.getCenterOfMassOffset(comOffsetToPack); }
private static RigidBody cloneRigidBody(RigidBody original, String cloneSuffix, InverseDynamicsJoint parentJointOfClone) { FramePoint comOffset = new FramePoint(); original.getCoMOffset(comOffset); comOffset.changeFrame(original.getParentJoint().getFrameAfterJoint()); String nameOriginal = original.getName(); Matrix3d massMomentOfInertiaPartCopy = original.getInertia().getMassMomentOfInertiaPartCopy(); double mass = original.getInertia().getMass(); Vector3d comOffsetCopy = comOffset.getVectorCopy(); RigidBody clone = ScrewTools.addRigidBody(nameOriginal + cloneSuffix, parentJointOfClone, massMomentOfInertiaPartCopy, mass, comOffsetCopy); return clone; }
RigidBodyInertia inertia = rowRigidBody.getInertia(); tempTwist.set(columnJoint.getMotionSubspace().getAllUnitTwists().get(k)); tempTwist.changeFrame(inertia.getExpressedInFrame()); tempMomentum.compute(inertia, tempTwist); tempMomentum.changeFrame(centerOfMassFrame);
Matrix3d inertiaMatrix = inertia.getMassMomentOfInertiaPartCopy(); double mass = inertia.getMass(); centerOfMassData.add(comData); AppearanceDefinition appearance = YoAppearance.Color(getColor(currentRigidBody.getInertia().getMass())); appearance.setTransparency(0.6);
public static double computeMass(List<RigidBody> rigidBodies) { double ret = 0.0; for (int i = 0; i < rigidBodies.size(); i++) { ret += rigidBodies.get(i).getInertia().getMass(); } return ret; } }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, Vector3d centerOfMassOffset) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), centerOfMassOffset, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
private void handleSpatialAccelerationCommand(SpatialAccelerationCommand command) { RigidBody controlledBody = command.getEndEffector(); SpatialAccelerationVector accelerationVector = command.getSpatialAcceleration(); accelerationVector.changeBaseFrameNoRelativeAcceleration(ReferenceFrame.getWorldFrame()); twistCalculator.getTwistOfBody(tmpTwist, controlledBody); tmpWrench.setToZero(accelerationVector.getBodyFrame(), accelerationVector.getExpressedInFrame()); conversionInertias.get(controlledBody).computeDynamicWrenchInBodyCoordinates(accelerationVector, tmpTwist, tmpWrench); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(controlledBody, tmpWrench, command.getSelectionMatrix()); virtualWrenchCommandList.addCommand(virtualWrenchCommand); if (controlledBody == controlRootBody) { tmpExternalWrench.set(tmpWrench); tmpExternalWrench.negate(); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }
tempTwist.changeFrame(rigidBodies[i].getInertia().getExpressedInFrame()); tempTwist.changeFrame(jointList[j].getSuccessor().getInertia().getExpressedInFrame());
public static double computeMass(Iterable<RigidBody> rigidBodies) { double ret = 0.0; for (RigidBody rigidBody : rigidBodies) { ret += rigidBody.getInertia().getMass(); } return ret; }
public static RigidBody addRigidBody(String name, InverseDynamicsJoint parentJoint, Matrix3d momentOfInertia, double mass, RigidBodyTransform inertiaPose) { String comFrameName = name + "CoM"; ReferenceFrame comFrame = createOffsetFrame(parentJoint.getFrameAfterJoint(), inertiaPose, comFrameName); RigidBodyInertia inertia = new RigidBodyInertia(comFrame, momentOfInertia, mass); RigidBody ret = new RigidBody(name, inertia, parentJoint); return ret; }
private double getSubTreeMass(RigidBody rigidBody) { if (!subTreeMassMap.containsKey(rigidBody)) subTreeMassMap.put(rigidBody, new MutableDouble(-1.0)); MutableDouble subTreeMass = subTreeMassMap.get(rigidBody); if (subTreeMass.doubleValue() > 0.0) { return subTreeMass.doubleValue(); } else { double curSubTreeMass = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); List<InverseDynamicsJoint> childrenJoints = rigidBody.getChildrenJoints(); for (int i = 0; i < childrenJoints.size(); i++) { double childSubTreeMass = getSubTreeMass(childrenJoints.get(i).getSuccessor()); curSubTreeMass = curSubTreeMass + childSubTreeMass; } subTreeMass.setValue(curSubTreeMass); return curSubTreeMass; } }