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; }
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); } }
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; }
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 double computeMass(Iterable<RigidBody> rigidBodies) { double ret = 0.0; for (RigidBody rigidBody : rigidBodies) { ret += rigidBody.getInertia().getMass(); } 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; } }
public static double computeSubTreeMass(RigidBody rootBody) { RigidBodyInertia inertia = rootBody.getInertia(); double ret = inertia == null ? 0.0 : inertia.getMass(); for (InverseDynamicsJoint childJoint : rootBody.getChildrenJoints()) { ret += computeSubTreeMass(childJoint.getSuccessor()); } return ret; }
rigidBody.getCoMOffset(curChildCoMScaledByMass); curChildCoMScaledByMass.changeFrame(rootFrame); double massToScale = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); curChildCoMScaledByMass.scale(massToScale);
public void compute() { centerOfMass.setToZero(desiredFrame); totalMass = 0.0; for (RigidBody rigidBody : rigidBodies) { rigidBody.getCoMOffset(tempPoint); double mass = rigidBody.getInertia().getMass(); tempPoint.changeFrame(desiredFrame); tempPoint.scale(mass); centerOfMass.add(tempPoint); totalMass += mass; } centerOfMass.scale(1.0 / totalMass); }
public void getCoMAcceleration(FrameVector comAccelerationToPack) { boolean firstIteration = true; double totalMass = 0.0; for (RigidBody rigidBody : rigidBodies) { double mass = rigidBody.getInertia().getMass(); rigidBody.getCoMOffset(comLocation); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(linkLinearMomentumDot, base, rigidBody, comLocation); linkLinearMomentumDot.scale(mass); if (firstIteration) comAccelerationToPack.setIncludingFrame(linkLinearMomentumDot); else comAccelerationToPack.add(linkLinearMomentumDot); totalMass += mass; firstIteration = false; } comAccelerationToPack.scale(1.0 / totalMass); } }
massVariable.set(rigidBody.getInertia().getMass());
private void findMinimumAndMaximumMassOfRigidBodies(RigidBody body) { RigidBodyInertia inertia = body.getInertia(); if (inertia != null) { double mass = body.getInertia().getMass(); if (mass < minimumMassOfRigidBodies.getDoubleValue() && mass > 1e-3) minimumMassOfRigidBodies.set(mass); if (mass > maximumMassOfRigidBodies.getDoubleValue()) maximumMassOfRigidBodies.set(mass); } if (body.hasChildrenJoints()) { List<InverseDynamicsJoint> childJoints = body.getChildrenJoints(); for (InverseDynamicsJoint joint : childJoints) { RigidBody nextBody = joint.getSuccessor(); if (nextBody != null) findMinimumAndMaximumMassOfRigidBodies(nextBody); } } }
RigidBody controlledBody = joint.getSuccessor(); double torque = acceleration * controlledBody.getInertia().getMass(); joint.getJointAxis(tempFrameVector); tempFrameVector.scale(torque);