@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
@Override public void doControl() { double mass = robot.computeCOMMomentum(tempCenterOfMassPoint, tempCenterOfMassVelocity, tempAngularMomentum); exactCenterOfMassPosition.set(tempCenterOfMassPoint); tempCenterOfMassVelocity.scale(1.0 / mass); exactCenterOfMassVelocity.set(tempCenterOfMassVelocity); exactCenterOfMassAcceleration.update(); }
@Override public void doControl() { if(firstTick) { robot.computeCOMMomentum(rootJoint, new Point3D(), initialLinearMomentum, initialAngularMomentum); firstTick = false; } Vector3D currentLinearMomentum = new Vector3D(); Vector3D currentAngularMomentum = new Vector3D(); robot.computeCOMMomentum(rootJoint, new Point3D(), currentLinearMomentum, currentAngularMomentum); EuclidCoreTestTools.assertTuple3DEquals( "Linear momentum hasn't been conserved. p(t=0)=" + EuclidCoreIOTools.getTuple3DString("%6.8f", initialLinearMomentum) + ", p(t=" + robot .getTime() + ")=" + EuclidCoreIOTools.getTuple3DString("%6.8f", currentLinearMomentum), initialLinearMomentum, currentLinearMomentum, epsilon); EuclidCoreTestTools.assertTuple3DEquals( "Angular momentum hasn't been conserved. L(t=0)=" + EuclidCoreIOTools.getTuple3DString("%6.8f", initialAngularMomentum) + ", L(t=" + robot .getTime() + ")=" + EuclidCoreIOTools.getTuple3DString("%6.8f", currentAngularMomentum), initialAngularMomentum, currentAngularMomentum, epsilon); } }
private void computeCoMVelocityError() { Point3d comPoint = new Point3d(); Vector3d linearVelocity = new Vector3d(); Vector3d angularMomentum = new Vector3d(); double mass = robot.computeCOMMomentum(comPoint, linearVelocity, angularMomentum); linearVelocity.scale(1.0 / mass); perfectCoMVelocity.set(linearVelocity); orientationEstimator.getEstimatedCoMVelocity(estimatedCoMVelocityFrameVector); Vector3d estimatedCoMVelocity = estimatedCoMVelocityFrameVector.getVectorCopy(); estimatedCoMVelocity.sub(linearVelocity); comVelocityError.set(estimatedCoMVelocity.length()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout=300000) public void testTree() { Random random = new Random(167L); Robot robot = new Robot("robot"); ReferenceFrame worldFrame = ReferenceFrame.getWorldFrame(); LinkedHashMap<RevoluteJoint, PinJoint> jointMap = new LinkedHashMap<RevoluteJoint, PinJoint>(); RigidBodyBasics elevator = new RigidBody("elevator", worldFrame); double gravity = 0.0; int numberOfJoints = 3; InverseDynamicsCalculatorSCSTest.createRandomTreeRobotAndSetJointPositionsAndVelocities(robot, jointMap, worldFrame, elevator, numberOfJoints, gravity, true, true, random); robot.updateVelocities(); CenterOfMassReferenceFrame centerOfMassFrame = new CenterOfMassReferenceFrame("com", worldFrame, elevator); centerOfMassFrame.update(); CentroidalMomentumCalculator centroidalMomentumMatrix = new CentroidalMomentumCalculator(elevator, centerOfMassFrame); centroidalMomentumMatrix.reset(); Momentum comMomentum = computeCoMMomentum(elevator, centerOfMassFrame, centroidalMomentumMatrix); Point3D comPoint = new Point3D(); Vector3D linearMomentum = new Vector3D(); Vector3D angularMomentum = new Vector3D(); robot.computeCOMMomentum(comPoint, linearMomentum, angularMomentum); EuclidCoreTestTools.assertTuple3DEquals(linearMomentum, comMomentum.getLinearPart(), 1e-12); EuclidCoreTestTools.assertTuple3DEquals(angularMomentum, comMomentum.getAngularPart(), 1e-12); }
private void computeCoMPositionError() { Point3d comPoint = new Point3d(); Vector3d linearVelocity = new Vector3d(); Vector3d angularMomentum = new Vector3d(); robot.computeCOMMomentum(comPoint, linearVelocity, angularMomentum); perfectCoMPosition.set(comPoint); Vector3d comError = new Vector3d(); orientationEstimator.getEstimatedCoMPosition(estimatedCoMPosition); comError.set(estimatedCoMPosition.getPointCopy()); comError.sub(comPoint); comZPositionError.set(comError.getZ()); comError.setZ(0.0); comXYPositionError.set(comError.length()); }
Vector3D linearMomentum = new Vector3D(); Vector3D angularMomentum = new Vector3D(); robot.computeCOMMomentum(comPoint, linearMomentum, angularMomentum);