private void clipToVectorMagnitude(double maximumMagnitude, FrameVector frameVectorToClip) { double magnitude = frameVectorToClip.length(); if (magnitude > maximumMagnitude) { frameVectorToClip.scale(maximumMagnitude / magnitude); } }
public void getAcceleration(FrameVector accelerationToPack) { accelerationToPack.setToZero(referenceFrame); // 2 * c2 c2.getFrameTuple(accelerationToPack); accelerationToPack.scale(2.0); }
@Override public void update(double time) { int i = 0; for (int j = 0; j < contactStates.size(); j++) { PlaneContactState contactState = contactStates.get(j); contactState.getContactNormalFrameVector(tempFrameVector); tempFrameVector.changeFrame(worldFrame); tempFrameVector.scale(normalVectorScale); List<? extends ContactPointInterface> contactPoints = contactState.getContactPoints(); for (int k = 0; k < contactPoints.size(); k++) { updateContactPointDynamicGraphicObjects(i++, contactPoints.get(k)); } } }
public void propagateState(double dt) { if (centerOfMassAccelerationPort != null) { centerOfMassVelocityDelta.set(centerOfMassAccelerationPort.getData()); centerOfMassVelocityDelta.scale(dt); updateCenterOfMassVelocity(centerOfMassVelocityDelta); } }
public void propagateState(double dt) { centerOfMassVelocityDelta.set(centerOfMassAccelerationPort.getData()); centerOfMassVelocityDelta.scale(dt); updateCenterOfMassVelocity(centerOfMassVelocityDelta); }
private void computeLinearVelocityFromMergingMeasurements() { // TODO Check out AlphaFusedYoVariable to that imuBasedLinearStateCalculator.updateIMUAndRootJointLinearVelocity(pelvisVelocityIMUPart); kinematicsBasedLinearStateCalculator.getPelvisVelocity(pelvisVelocityKinPart); pelvisVelocityIMUPart.scale(alphaIMUAgainstKinematicsForVelocity.getDoubleValue()); pelvisVelocityKinPart.scale(1.0 - alphaIMUAgainstKinematicsForVelocity.getDoubleValue()); rootJointVelocity.add(pelvisVelocityIMUPart, pelvisVelocityKinPart); yoRootJointVelocity.set(rootJointVelocity); imuBasedLinearStateCalculator.correctIMULinearVelocity(rootJointVelocity); }
public void getAcceleration(FrameVector accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getAcceleration(accelerationToPack); accelerationToPack.scale(minimumJerkTrajectory.getVelocity() * minimumJerkTrajectory.getVelocity()); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); tempVector.scale(minimumJerkTrajectory.getAcceleration()); accelerationToPack.add(tempVector); }
public void propagateState(double dt) { FrameVector centerOfMassVelocity = centerOfMassVelocityPort.getData(); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPositionDelta.set(centerOfMassVelocity); centerOfMassPositionDelta.scale(dt); updateCenterOfMassVelocity(centerOfMassPositionDelta); }
public void getAcceleration(FrameVector accelerationToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getAcceleration(accelerationToPack); accelerationToPack.scale(minimumJerkTrajectory.getVelocity() * minimumJerkTrajectory.getVelocity()); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); tempVector.scale(minimumJerkTrajectory.getAcceleration()); accelerationToPack.add(tempVector); }
public void propagateState(double dt) { FrameVector centerOfMassVelocity = centerOfMassVelocityPort.getData(); centerOfMassVelocity.changeFrame(worldFrame); centerOfMassPositionDelta.set(centerOfMassVelocity); centerOfMassPositionDelta.scale(dt); updateCenterOfMassPosition(centerOfMassPositionDelta); }
public void getVelocity(FrameVector velocityToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); velocityToPack.setIncludingFrame(tempVector); velocityToPack.scale(minimumJerkTrajectory.getVelocity()); }
public void getVelocity(FrameVector velocityToPack, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); velocityToPack.setToZero(referenceFrame); // 2 * c2 * q c2.getFrameTuple(velocityToPack); velocityToPack.scale(2.0 * q); // c1 c1.getFrameTuple(tempPackVelocity); velocityToPack.add(tempPackVelocity); }
public static void computeForce(FrameVector forceToPack, FramePoint centerOfMass, FramePoint cmp, double fZ) { cmp.changeFrame(centerOfMass.getReferenceFrame()); forceToPack.setIncludingFrame(centerOfMass); forceToPack.sub(cmp); forceToPack.scale(fZ / forceToPack.getZ()); }
public void getVelocity(FrameVector velocityToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); velocityToPack.setIncludingFrame(tempVector); velocityToPack.scale(minimumJerkTrajectory.getVelocity()); }
private void computeProportionalTerm(FramePoint desiredPosition) { desiredPosition.changeFrame(bodyFrame); positionError.set(desiredPosition); // Limit the maximum position error considered for control action double maximumError = gains.getMaximumProportionalError(); double errorMagnitude = positionError.length(); positionError.getFrameTuple(proportionalTerm); if (errorMagnitude > maximumError) { proportionalTerm.scale(maximumError / errorMagnitude); } proportionalGainMatrix.transform(proportionalTerm.getVector()); }
private void computeDerivativeTerm(FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity) { desiredAngularVelocity.changeFrame(bodyFrame); currentAngularVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredAngularVelocity, currentAngularVelocity); // Limit the maximum velocity error considered for control action double maximumVelocityError = gains.getMaximumDerivativeError(); double velocityErrorMagnitude = derivativeTerm.length(); if (velocityErrorMagnitude > maximumVelocityError) { derivativeTerm.scale(maximumVelocityError / velocityErrorMagnitude); } velocityError.set(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.getVector()); }
private void setAccelerationEndpointPositions() { for (int i : new int[] {0, 1}) { FrameVector waypointToEndpoint = getWaypointToEndpoint(i); FrameVector oppositeWaypointToEndpoint = getOppositeWaypointToEndpoint(i); double scaleFactor = waypointToEndpoint.dot(oppositeWaypointToEndpoint) / oppositeWaypointToEndpoint.length() * linearSplineLengthFactor.getDoubleValue(); oppositeWaypointToEndpoint.normalize(); oppositeWaypointToEndpoint.scale(scaleFactor); allPositions[accelerationEndpointIndices[i]].set(allPositions[waypointIndices[i]].getFramePointCopy()); allPositions[accelerationEndpointIndices[i]].add(oppositeWaypointToEndpoint); } }
private void computeOrientationStateOutputBlock(Matrix3d rotationFromPelvisToWorld) { computeVelocityOfStationaryPoint(tempFrameVector); tempFrameVector.changeFrame(estimationFrame); tempCenterOfMassVelocity.setIncludingFrame(centerOfMassVelocityPort.getData()); tempCenterOfMassVelocity.changeFrame(estimationFrame); tempFrameVector.sub(tempCenterOfMassVelocity); tempFrameVector.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFrameVector.getVector()); tempMatrix.mul(rotationFromPelvisToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort)); }
private void computeProportionalTerm(FrameOrientation desiredOrientation) { desiredOrientation.changeFrame(bodyFrame); desiredOrientation.getQuaternion(errorQuaternion); errorAngleAxis.set(errorQuaternion); errorAngleAxis.setAngle(AngleTools.trimAngleMinusPiToPi(errorAngleAxis.getAngle())); // Limit the maximum position error considered for control action double maximumError = gains.getMaximumProportionalError(); if (errorAngleAxis.getAngle() > maximumError) { errorAngleAxis.setAngle(Math.signum(errorAngleAxis.getAngle()) * maximumError); } proportionalTerm.set(errorAngleAxis.getX(), errorAngleAxis.getY(), errorAngleAxis.getZ()); proportionalTerm.scale(errorAngleAxis.getAngle()); rotationErrorInBody.set(proportionalTerm); proportionalGainMatrix.transform(proportionalTerm.getVector()); }
public void updateIMUAndRootJointLinearVelocity(FrameVector rootJointVelocityToPack) { updateLinearAccelerationMeasurement(); yoLinearAccelerationMeasurementInWorld.getFrameTupleIncludingFrame(linearAccelerationMeasurement); linearAccelerationMeasurement.scale(estimatorDT); yoMeasurementFrameLinearVelocityInWorld.add(linearAccelerationMeasurement); yoMeasurementFrameLinearVelocityInWorld.getFrameTupleIncludingFrame(rootJointVelocityToPack); getCorrectionVelocityForMeasurementFrameOffset(correctionVelocityForMeasurementFrameOffset); correctionVelocityForMeasurementFrameOffset.changeFrame(worldFrame); rootJointVelocityToPack.sub(correctionVelocityForMeasurementFrameOffset); yoRootJointIMUBasedLinearVelocityInWorld.set(rootJointVelocityToPack); imuLinearVelocityIMUOnly.add(linearAccelerationMeasurement); imuLinearVelocityIMUOnly.scale(alphaLeakIMUOnly.getDoubleValue()); rootJointLinearVelocityIMUOnly.set(imuLinearVelocityIMUOnly); rootJointLinearVelocityIMUOnly.sub(correctionVelocityForMeasurementFrameOffset); }