private void computeRootJointToWorldTransform(FloatingInverseDynamicsJoint rootJoint, ReferenceFrame estimationFrame, RigidBodyTransform rootJointToWorldToPack, RigidBodyTransform estimationLinkTransform) { // H_{root}^{estimation} rootJoint.getFrameAfterJoint().getTransformToDesiredFrame(tempRootJointFrameToEstimationFrame, estimationFrame); // H_{root}^{w} = H_{estimation}^{w} * H_{root}^{estimation} rootJointToWorldToPack.set(estimationLinkTransform); rootJointToWorldToPack.multiply(tempRootJointFrameToEstimationFrame); }
public PositionStateRobotModelUpdater(ControlFlowInputPort<FullInverseDynamicsStructure> inverseDynamicsStructureInputPort, ControlFlowOutputPort<FramePoint> centerOfMassPositionOutputPort, ControlFlowOutputPort<FrameVector> centerOfMassVelocityOutputPort) { this.inverseDynamicsStructureInputPort = inverseDynamicsStructureInputPort; this.centerOfMassPositionOutputPort = centerOfMassPositionOutputPort; this.centerOfMassVelocityOutputPort = centerOfMassVelocityOutputPort; FullInverseDynamicsStructure inverseDynamicsStructure = inverseDynamicsStructureInputPort.getData(); RigidBody elevator = inverseDynamicsStructure.getElevator(); FloatingInverseDynamicsJoint rootJoint = inverseDynamicsStructure.getRootJoint(); this.centerOfMassCalculator = new CenterOfMassCalculator(elevator, rootJoint.getFrameAfterJoint()); this.centerOfMassJacobianBody = new CenterOfMassJacobian(ScrewTools.computeSupportAndSubtreeSuccessors(elevator), ScrewTools.computeSubtreeJoints(rootJoint.getSuccessor()), rootJoint.getFrameAfterJoint()); }
public void updateForFrozenState() { // Keep setting the position so the localization updater works properly. yoRootJointPosition.get(tempRootJointTranslation); rootJoint.setPosition(tempRootJointTranslation); // Set the rootJoint twist to zero. rootJoint.getJointTwist(rootJointTwist); rootJointTwist.setToZero(); rootJoint.setJointTwist(rootJointTwist); rootJointFrame.update(); twistCalculator.compute(); updateCoMState(); }
public void setDesiredJointState(FloatingInverseDynamicsJoint rootJoint, OneDoFJoint[] newJointData) { if (newJointData.length != desiredJointAngles.length) throw new RuntimeException("Array size does not match"); for (int i = 0; i < desiredJointAngles.length; i++) desiredJointAngles[i] = (float) newJointData[i].getqDesired(); rootJoint.getTranslation(desiredRootTranslation); rootJoint.getRotation(desiredRootOrientation); }
@Override public void updateRootJointOrientationAndAngularVelocity() { yoRootJointFrameOrientation.getQuaternion(rootJointOrientation); rootJoint.setRotation(rootJointOrientation); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setToZero(); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); }
private void computeRootJointTwistLinearPart(FloatingInverseDynamicsJoint rootJoint, Twist rootJointTwistToPack, FrameVector rootJointLinearVelocity) { rootJoint.getJointTwist(tempRootJointTwistExisting); tempRootJointTwistExisting.checkReferenceFramesMatch(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint()); tempRootJointTwistExisting.getAngularPart(tempRootJointTwistExistingAngularPart); rootJointLinearVelocity.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointTwistToPack.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearVelocity.getVector(), tempRootJointTwistExistingAngularPart.getVector()); }
public RigidBody getRigidBody(Joint joint) { if (joint instanceof FloatingJoint) { FloatingInverseDynamicsJoint parentSixDoFJoint = floatingToSixDofToJointMap.get(joint); return parentSixDoFJoint.getSuccessor(); } else if (joint instanceof OneDegreeOfFreedomJoint) { OneDoFJoint parentOneDoFJoint = scsToOneDoFJointMap.get(joint); return parentOneDoFJoint.getSuccessor(); } else { throw new RuntimeException(); } }
private void updateFullRobotModel(RobotConfigurationData robotConfigurationData, FullRobotModel model, ForceSensorDataHolder forceSensorDataHolder) { FullRobotModelCache fullRobotModelCache = getFullRobotModelCache(model); FloatingInverseDynamicsJoint rootJoint = model.getRootJoint(); if (robotConfigurationData.jointNameHash != fullRobotModelCache.jointNameHash) { System.out.println(robotConfigurationData.jointNameHash); System.out.println(fullRobotModelCache.jointNameHash); throw new RuntimeException("Joint names do not match for RobotConfigurationData"); } float[] newJointAngles = robotConfigurationData.getJointAngles(); for (int i = 0; i < newJointAngles.length; i++) { fullRobotModelCache.allJoints[i].setQ(newJointAngles[i]); } Vector3f translation = robotConfigurationData.getPelvisTranslation(); rootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); rootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); rootJoint.getPredecessor().updateFramesRecursively(); if (forceSensorDataHolder != null) { for (int i = 0; i < forceSensorDataHolder.getForceSensorDefinitions().size(); i++) { forceSensorDataHolder.get(forceSensorDataHolder.getForceSensorDefinitions().get(i)).setWrench( robotConfigurationData.getMomentAndForceVectorForSensor(i)); } } }
sixDoFJoint.setPositionAndRotation(positionAndRotation); ReferenceFrame elevatorFrame = sixDoFJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = sixDoFJoint.getFrameAfterJoint(); sixDoFJoint.setJointTwist(bodyTwist); originAcceleration.setToZero(sixDoFJoint.getFrameBeforeJoint()); angularAcceleration.setToZero(sixDoFJoint.getFrameAfterJoint()); originAcceleration.changeFrame(sixDoFJoint.getFrameBeforeJoint()); spatialAccelerationVector.setToZero(sixDoFJoint.getFrameAfterJoint(), sixDoFJoint.getFrameBeforeJoint(), sixDoFJoint.getFrameAfterJoint()); spatialAccelerationVector.setBasedOnOriginAcceleration(angularAcceleration, originAcceleration, bodyTwist); sixDoFJoint.setDesiredAcceleration(spatialAccelerationVector); else sixDoFJoint.setAcceleration(spatialAccelerationVector);
desiredRootJoint.setPosition(translation.getX(), translation.getY(), translation.getZ()); Quat4f orientation = robotConfigurationData.getPelvisOrientation(); desiredRootJoint.setRotation(orientation.getX(), orientation.getY(), orientation.getZ(), orientation.getW()); desiredRootJoint.setVelocity(zeroVelocityMatrix, 0);
private void readAndUpdateRootJointAngularAndLinearVelocity() { ReferenceFrame elevatorFrame = rootJoint.getFrameBeforeJoint(); ReferenceFrame pelvisFrame = rootJoint.getFrameAfterJoint(); FrameVector linearVelocity = robot.getRootJointVelocity(); linearVelocity.changeFrame(pelvisFrame); FrameVector angularVelocity = robot.getRootJointAngularVelocityInRootJointFrame(pelvisFrame); angularVelocity.changeFrame(pelvisFrame); Twist bodyTwist = new Twist(pelvisFrame, elevatorFrame, pelvisFrame, linearVelocity.getVector(), angularVelocity.getVector()); rootJoint.setJointTwist(bodyTwist); }
private void updateRootJointTwistAngularPart() { // T_{rootBody}^{rootBody, measurementLink} twistCalculator.getRelativeTwist(twistRootJointFrameRelativeToMeasurementLink, measurementLink, rootJoint.getSuccessor()); // T_{rootBody}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeFrame(rootJointFrame); // T_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.changeBodyFrameNoRelativeTwist(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, measurementLink} twistRootJointFrameRelativeToMeasurementLink.getAngularPart(angularVelocityRootJointFrameRelativeToMeasurementLink); // omega_{measurementLink}^{measurementFrame, world} imuProcessedOutput.getAngularVelocityMeasurement(angularVelocityMeasurement); if (imuBiasProvider != null) { imuBiasProvider.getAngularVelocityBiasInIMUFrame(imuProcessedOutput, angularVelocityMeasurementBias); angularVelocityMeasurement.sub(angularVelocityMeasurementBias); } angularVelocityMeasurementLinkRelativeToWorld.setIncludingFrame(measurementFrame, angularVelocityMeasurement); // omega_{measurementLink}^{rootJointFrame, world} angularVelocityMeasurementLinkRelativeToWorld.changeFrame(rootJointFrame); // omega_{rootJointFrame}^{rootJointFrame, world} = omega_{rootJointFrame}^{rootJointFrame, measurementLink} + omega_{measurementLink}^{rootJointFrame, world} angularVelocityRootJointFrameRelativeToWorld.add(angularVelocityRootJointFrameRelativeToMeasurementLink, angularVelocityMeasurementLinkRelativeToWorld); rootJoint.getJointTwist(twistRootBodyRelativeToWorld); twistRootBodyRelativeToWorld.setAngularPart(angularVelocityRootJointFrameRelativeToWorld); rootJoint.setJointTwist(twistRootBodyRelativeToWorld); twistCalculator.compute(); yoRootJointAngularVelocity.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityMeasFrame.setAndMatchFrame(angularVelocityMeasurementLinkRelativeToWorld); yoRootJointAngularVelocityInWorld.setAndMatchFrame(angularVelocityRootJointFrameRelativeToWorld); }
private void updateRootJointTwistLinearPart(FrameVector centerOfMassVelocityWorld, FloatingInverseDynamicsJoint rootJoint) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); computeRootJointLinearVelocity(centerOfMassVelocityWorld, tempRootJointLinearVelocity, tempRootJointAngularVelocity, rootJoint); computeRootJointTwistLinearPart(rootJoint, tempRootJointTwist, tempRootJointLinearVelocity); rootJoint.setJointTwist(tempRootJointTwist); }
private void getCorrectionVelocityForMeasurementFrameOffset(FrameVector correctionTermToPack) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); measurementOffset.setToZero(measurementFrame); measurementOffset.changeFrame(rootJoint.getFrameAfterJoint()); correctionTermToPack.setToZero(tempRootJointAngularVelocity.getReferenceFrame()); correctionTermToPack.cross(tempRootJointAngularVelocity, measurementOffset); } }
@Override public void wasRewound() { FloatingInverseDynamicsJoint rootJoint = fullRobotModel.getRootJoint(); yoRootJointTranslation.get(rootJointTranslation); rootJoint.setPosition(rootJointTranslation); yoRootJointRotation.get(rootJointRotation); rootJoint.setRotation(rootJointRotation); } }
private void computeRootJointAcceleration(FloatingInverseDynamicsJoint rootJoint, SpatialAccelerationVector rootJointAcceleration, FrameVector rootJointAngularAcceleration, FrameVector rootJointLinearAcceleration) { rootJointAngularAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointLinearAcceleration.checkReferenceFrameMatch(rootJoint.getFrameAfterJoint()); rootJointAcceleration.set(rootJoint.getFrameAfterJoint(), rootJoint.getFrameBeforeJoint(), rootJoint.getFrameAfterJoint(), rootJointLinearAcceleration.getVector(), rootJointAngularAcceleration.getVector()); }
public static void setRandomPositionAndOrientation(FloatingInverseDynamicsJoint rootJoint, Random random) { rootJoint.setPositionAndRotation(RigidBodyTransform.generateRandomTransform(random)); }
rootJoint.getTranslation(translation); estimatedRootJointPosition.set(translation); rootJoint.getRotation(orientation); estimatedRootJointQuaternion.set(orientation); estimatedRootJointOrientation.set(orientation); rootJoint.getJointTwist(twist);
public void packEstimatorJoints(long timestamp, long sensorHeadPPSTimestamp, RobotConfigurationData jointConfigurationData) { if (jointConfigurationData == null) { return; } rootJoint.getTranslation(rootTranslation); rootJoint.getRotation(rootOrientation); rootJoint.getAngularVelocity(rootAngularVelocity); rootJoint.getLinearVelocity(rootLinearVelocity); rootJoint.getLinearAcceleration(rootLinearAcceleration); jointConfigurationData.setPelvisAngularVelocity(rootAngularVelocity); jointConfigurationData.setPelvisLinearVelocity(rootLinearVelocity); jointConfigurationData.setPelvisLinearAcceleration(rootLinearAcceleration); jointConfigurationData.setRootTranslation(rootTranslation); jointConfigurationData.setRootOrientation(rootOrientation); jointConfigurationData.setJointState(joints); jointConfigurationData.setTimestamp(timestamp); jointConfigurationData.setSensorHeadPPSTimestamp(sensorHeadPPSTimestamp); for (int sensorNumber = 0; sensorNumber < getNumberOfForceSensors(); sensorNumber++) { float[] forceAndMomentVector = jointConfigurationData.getMomentAndForceVectorForSensor(sensorNumber); forceSensorDataList.get(sensorNumber).getWrench(forceAndMomentVector); } }
private void updateRootJointRotation(FloatingInverseDynamicsJoint rootJoint, FrameOrientation estimationLinkOrientation, ReferenceFrame estimationFrame) { tempOrientationEstimatinLink.setIncludingFrame(estimationLinkOrientation); computeEstimationLinkToWorldTransform(tempEstimationLinkToWorld, tempOrientationEstimatinLink); computeRootJointToWorldTransform(rootJoint, estimationFrame, tempRootJointToWorld, tempEstimationLinkToWorld); Matrix3d rootJointRotation = new Matrix3d(); tempRootJointToWorld.getRotation(rootJointRotation); rootJoint.setRotation(rootJointRotation); }