@Override public void doControl() { // update the linear momentum rate by numerical differentiation of the robot momentum simulatedRobot.getRootJoint().physics.recursiveComputeLinearMomentum(linearMomentum); yoLinearMomentum.set(linearMomentum); momentumChange.update(); momentumChange.get(linearMomentumRate); // get mass and COM position from the robot double totalMass = simulatedRobot.computeCenterOfMass(comPosition); comPosition2d.set(comPosition.getX(), comPosition.getY()); // now compute the COM acceleration comAcceleration.set(linearMomentumRate.getX(), linearMomentumRate.getY()); comAcceleration.scale(1.0 / totalMass); // CMP = COM - 1/omega^2 * COMAcc double omega0 = Math.sqrt(-gravity / comPosition.getZ()); cmp.set(comAcceleration); cmp.scale(- 1.0 / (omega0 * omega0)); cmp.add(comPosition2d); yoCmp.set(cmp); }
@Override public void doControl() { // update the linear momentum rate by numerical differentiation of the robot momentum simulatedRobot.getRootJoint().physics.recursiveComputeLinearMomentum(linearMomentum); yoLinearMomentum.set(linearMomentum); momentumChange.update(); momentumChange.get(linearMomentumRate); // get mass and COM position from the robot double totalMass = simulatedRobot.computeCenterOfMass(comPosition); comPosition2d.set(comPosition.getX(), comPosition.getY()); // now compute the COM acceleration comAcceleration.set(linearMomentumRate.getX(), linearMomentumRate.getY()); comAcceleration.scale(1.0 / totalMass); // CMP = COM - 1/omega^2 * COMAcc double omega0 = Math.sqrt(-gravity / comPosition.getZ()); cmp.set(comAcceleration); cmp.scale(- 1.0 / (omega0 * omega0)); cmp.add(comPosition2d); yoCmp.set(cmp); }