rigidBody.getCoMOffset(curChildCoMScaledByMass); curChildCoMScaledByMass.changeFrame(rootFrame); double massToScale = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0);
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); } }
public void update() { FramePoint tempCoMPosition = new FramePoint(worldFrame); for (RigidBodyVisualizationData comData : centerOfMassData) { comData.rigidBody.getCoMOffset(tempCoMPosition); tempCoMPosition.changeFrame(worldFrame); tempCoMPosition.add(inertiaEllipsoidGhostOffset.getFrameVectorCopy()); FrameOrientation orientation = new FrameOrientation(comData.rigidBody.getBodyFixedFrame()); orientation.changeFrame(worldFrame); comData.position.set(tempCoMPosition); comData.orientation.set(orientation); } }
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; }
rigidBody.getCoMOffset(originalCoMOffset); final YoFramePoint rigidBodyCoMOffset = new YoFramePoint(name + "CoMOffset", rigidBody.getParentJoint().getFrameAfterJoint(), registry); rigidBodyCoMOffset.setAndMatchFrame(originalCoMOffset);