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 void setFullRobotModelRootJointVelocityAndAngularVelocityToMatchRobot(FloatingJointBasics sixDoFJoint, FloatingJoint floatingJoint) { FrameVector3D angularVelocityFrameVector = new FrameVector3D(); FrameVector3D linearVelocityFrameVector = new FrameVector3D(); ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); floatingJoint.getVelocity(linearVelocityFrameVector); linearVelocityFrameVector.changeFrame(bodyFrame); floatingJoint.getAngularVelocity(angularVelocityFrameVector, bodyFrame); Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, angularVelocityFrameVector, linearVelocityFrameVector); sixDoFJoint.setJointTwist(bodyTwist); }
public void updateJointVelocities_SCS_to_ID() { if (scsFloatingJoint != null) { ReferenceFrame elevatorFrame = idFloatingJoint.getFrameBeforeJoint(); ReferenceFrame rootBodyFrame = idFloatingJoint.getFrameAfterJoint(); scsFloatingJoint.getVelocity(linearVelocity); linearVelocity.changeFrame(rootBodyFrame); scsFloatingJoint.getAngularVelocity(angularVelocity, rootBodyFrame); rootJointTwist.setIncludingFrame(rootBodyFrame, elevatorFrame, rootBodyFrame, angularVelocity, linearVelocity); idFloatingJoint.setJointTwist(rootJointTwist); } for (OneDegreeOfFreedomJoint scsJoint : allSCSOneDoFJoints) { OneDoFJointBasics idJoint = scsToIDJointMap.get(scsJoint); idJoint.setQd(scsJoint.getQDYoVariable().getDoubleValue()); } }
private void readAndUpdateRootJointAngularAndLinearVelocity() { ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint(); // Update base frames without updating all frames to transform velocity into pelvis elevatorFrame.update(); pelvisFrame.update(); FrameVector3D linearVelocity = robot.getRootJointVelocity(); linearVelocity.changeFrame(pelvisFrame); FrameVector3D angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame); angularVelocity.changeFrame(pelvisFrame); Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, angularVelocity, linearVelocity); rootJoint.setJointTwist(bodyTwist); }
public void setSixDoFJointAccelerationRandomly(FloatingJointBasics sixDoFJoint, Random random, double maxRootJointLinearAcceleration, double maxRootJointAngularAcceleration) { // 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, RandomGeometry.nextVector3D(random, maxRootJointLinearAcceleration)); FrameVector3D angularAcceleration = new FrameVector3D(bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointAngularAcceleration)); // originAcceleration.changeFrame(elevatorFrame); SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); sixDoFJoint.setJointAcceleration(spatialAccelerationVector); }
public void copyAccelerationFromForwardToInverseBroken(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint) { ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = sixDoFJoint.getFrameAfterJoint(); FrameVector3D angularAccelerationInBody = new FrameVector3D(); floatingJoint.getAngularAcceleration(angularAccelerationInBody, bodyFrame); FrameVector3D linearAccelerationInBody = new FrameVector3D(); floatingJoint.getLinearAcceleration(linearAccelerationInBody); linearAccelerationInBody.changeFrame(bodyFrame); SpatialAcceleration jointAcceleration = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); jointAcceleration.getLinearPart().set(linearAccelerationInBody); jointAcceleration.getAngularPart().set(angularAccelerationInBody); sixDoFJoint.setJointAcceleration(jointAcceleration); }
public void copyAccelerationFromForwardToInverse(FloatingJoint floatingJoint, FloatingJointBasics sixDoFJoint) { // 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); SpatialAcceleration spatialAccelerationVector = new SpatialAcceleration(bodyFrame, elevatorFrame, bodyFrame); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); sixDoFJoint.setJointAcceleration(spatialAccelerationVector); }
public void setFullRobotModelStateRandomly(Random random, double maxJointVelocity, double maxRootJointLinearAndAngularVelocity) { FloatingJointBasics rootJoint = fullRobotModel.getRootJoint(); ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame bodyFrame = rootJoint.getFrameAfterJoint(); Twist bodyTwist = new Twist(bodyFrame, elevatorFrame, bodyFrame, RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity), RandomGeometry.nextVector3D(random, maxRootJointLinearAndAngularVelocity)); rootJoint.setJointTwist(bodyTwist); rootJoint.setJointPosition(RandomGeometry.nextVector3D(random)); double yaw = RandomNumbers.nextDouble(random, Math.PI / 20.0); double pitch = RandomNumbers.nextDouble(random, Math.PI / 20.0); double roll = RandomNumbers.nextDouble(random, Math.PI / 20.0); rootJoint.getJointPose().setOrientationYawPitchRoll(yaw, pitch, roll); ArrayList<OneDoFJointBasics> oneDoFJoints = new ArrayList<OneDoFJointBasics>(); fullRobotModel.getOneDoFJoints(oneDoFJoints); for (OneDoFJointBasics oneDoFJoint : oneDoFJoints) { double lowerLimit = oneDoFJoint.getJointLimitLower(); double upperLimit = oneDoFJoint.getJointLimitUpper(); double delta = upperLimit - lowerLimit; lowerLimit = lowerLimit + 0.05 * delta; upperLimit = upperLimit - 0.05 * delta; oneDoFJoint.setQ(RandomNumbers.nextDouble(random, lowerLimit, upperLimit)); oneDoFJoint.setQd(RandomNumbers.nextDouble(random, maxJointVelocity)); } }
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); }
ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint();
frameLinearVelocity.changeFrame(rootJoint.getFrameAfterJoint()); frameAngularVelocity.changeFrame(rootJoint.getFrameAfterJoint()); rootJointTwist.setIncludingFrame(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), frameAngularVelocity, frameLinearVelocity); rootJoint.setJointTwist(rootJointTwist);