/** {@inheritDoc} */ @Override default void setJointAccelerationToZero() { getJointAcceleration().setToZero(); }
/** {@inheritDoc} */ @Override default void setJointLinearAcceleration(Vector3DReadOnly jointLinearAcceleration) { getJointAcceleration().getLinearPart().set(jointLinearAcceleration); }
/** {@inheritDoc} */ @Override default void setJointAngularAcceleration(Vector3DReadOnly jointAngularAcceleration) { getJointAcceleration().getAngularPart().set(jointAngularAcceleration); }
/** * Integrates the given {@code joint}'s acceleration and velocity to update its velocity and * configuration. * * @param joint the joint to integrate the state of. The joint configuration is modified. */ public void doubleIntegrateFromAcceleration(FloatingJointBasics joint) { doubleIntegrate(joint.getJointAcceleration(), joint.getJointTwist(), joint.getJointPose()); }
private void compareJointAccelerations(double epsilon) { boolean areEqual = true; String errorMessage = "Joint accelerations are not equal\n"; String jointTriggerErrorMessage = ""; FloatingJoint scsFloatingJoint = (FloatingJoint) robot.getRootJoints().get(0); SpatialAcceleration expected = extractFromFloatingJoint(scsFloatingJoint, floatingJoint.getFrameBeforeJoint(), floatingJoint.getFrameAfterJoint()); if (!floatingJoint.getJointAcceleration().epsilonEquals(expected, epsilon)) { areEqual = false; jointTriggerErrorMessage = " floating-joint"; } errorMessage += "Floating joint:\nExpected: " + expected + "\nActual : " + floatingJoint.getJointAcceleration(); for (OneDoFJointBasics joint : allOneDoFJoints) { String jointName = joint.getName(); OneDegreeOfFreedomJoint scsJoint = (OneDegreeOfFreedomJoint) robot.getJoint(jointName); if (!EuclidCoreTools.epsilonEquals(scsJoint.getQDD(), joint.getQdd(), epsilon)) { areEqual = false; jointTriggerErrorMessage += " " + jointName; } errorMessage += "\n" + jointName + ",\texpected: " + String.format(FORMAT, scsJoint.getQDD()) + ",\tactual: " + String.format(FORMAT, joint.getQdd()) + ", difference: " + String.format(FORMAT, Math.abs(scsJoint.getQDD() - joint.getQdd())); } if (!areEqual) { throw new RuntimeException(errorMessage + "\nJoint(s) triggering error:" + jointTriggerErrorMessage); } }
public boolean checkFullRobotModelRootJointAccelerationmatchesRobot(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint, double epsilon) { // Note: To get the acceleration, you can't just changeFrame on the acceleration provided by SCS. Use setBasedOnOriginAcceleration instead. ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); Twist bodyTwist = new Twist(); bodyTwist.setIncludingFrame(sixDoFJoint.getJointTwist()); FrameVector3D originAcceleration = new FrameVector3D(elevatorFrame); FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame); floatingJoint.getLinearAccelerationInWorld(originAcceleration); floatingJoint.getAngularAccelerationInBody(angularAcceleration); originAcceleration.changeFrame(elevatorFrame); //TODO: These should be from the inverse dynamics to the SCS frames... originAcceleration.changeFrame(ReferenceFrame.getWorldFrame()); computedRootJointLinearAcceleration.set(originAcceleration); computedRootJointAngularAcceleration.set(angularAcceleration); SpatialAcceleration spatialAccelerationVectorOfSimulatedRootJoint = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAccelerationVectorOfSimulatedRootJoint.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); SpatialAcceleration spatialAccelerationVectorOfInverseDynamicsRootJoint = new SpatialAcceleration(); spatialAccelerationVectorOfInverseDynamicsRootJoint.setIncludingFrame(sixDoFJoint.getJointAcceleration()); return spatialAccelerationVectorOfInverseDynamicsRootJoint.epsilonEquals(spatialAccelerationVectorOfInverseDynamicsRootJoint, epsilon); }
spatialAccelerationVector.setIncludingFrame(sixDoFJoint.getJointAcceleration()); originAcceleration.setIncludingFrame(spatialAccelerationVector.getLinearPart()); originAcceleration.changeFrame(ReferenceFrame.getWorldFrame());