/** * * @param array ArrayList * @return ArrayList */ public static ArrayList<FrameVector> diff(ArrayList<FrameVector> array) { ArrayList<FrameVector> ret = new ArrayList<FrameVector>(); for (int i = 1; i < array.size(); i++) { FrameVector diffedVector = new FrameVector(array.get(i)); diffedVector.sub(array.get(i - 1)); ret.add(diffedVector); } return ret; }
public int compare(FramePoint o1, FramePoint o2) { differenceVector.setIncludingFrame(o1); differenceVector.sub(o2); double dotProduct = searchDirection.dot(differenceVector); return Double.compare(dotProduct, 0.0); } }
private FrameVector getWaypointToEndpoint(int i) { FrameVector waypointToEndpoint = allPositions[endpointIndices[i]].getFrameVectorCopy(); waypointToEndpoint.sub(allPositions[waypointIndices[i]].getFrameVectorCopy()); return waypointToEndpoint; }
private FrameVector getOppositeWaypointToEndpoint(int i) { FrameVector oppositeWaypointToEndpoint = allPositions[endpointIndices[i]].getFrameVectorCopy(); oppositeWaypointToEndpoint.sub(allPositions[oppositeWaypointIndices[i]].getFrameVectorCopy()); return oppositeWaypointToEndpoint; }
public FrameVector getTranslationDueToRotationAboutFrame(FramePose otherPose) { checkReferenceFrameMatch(otherPose); FrameVector ret = getTranslationToOtherPoseTotal(otherPose); ret.sub(getTranslationNOTDueToRotationAboutFrame(otherPose)); return ret; }
private void computeRpd(FrameVector rPdToPack, TwistCalculator twistCalculator, FramePoint rP, FrameVector rd) { // T_{p}^{p,w} twistCalculator.getTwistOfBody(twistOfEstimationLink, estimationLink); twistOfEstimationLink.changeFrame(estimationFrame); // \dot{r}^{p} = R_{w}^{p} \dot{r} - \tilde{\omega}r^{p} - v_{p}^{p,w} rPdToPack.setIncludingFrame(rd); rPdToPack.changeFrame(estimationFrame); twistOfEstimationLink.getAngularPart(tempFrameVector); tempFrameVector.cross(tempFrameVector, rP); rPdToPack.sub(tempFrameVector); twistOfEstimationLink.getLinearPart(tempFrameVector); rPdToPack.sub(tempFrameVector); }
public FrameVector getTranslationToOtherPoseTotal(FramePose otherPose) { checkReferenceFrameMatch(otherPose); FrameVector ret = new FrameVector(referenceFrame); ret.sub(otherPose.pose.getPoint(), this.pose.getPoint()); return ret; }
private void updateTrustedFootPosition(RigidBody trustedFoot) { YoFramePoint footPositionInWorld = footPositionsInWorld.get(trustedFoot); AlphaFilteredYoFrameVector footToRootJointPosition = footToRootJointPositions.get(trustedFoot); if (trustCoPAsNonSlippingContactPoint.getBooleanValue()) { footToRootJointPosition.getFrameTupleIncludingFrame(tempPosition); rootJointPosition.getFrameTupleIncludingFrame(tempFramePoint); tempFramePoint.sub(tempPosition); // New foot position footPositionInWorld.getFrameTuple(tempPosition); // Previous foot position tempFrameVector.sub(tempFramePoint, tempPosition); // Delta from previous to new foot position copPositionsInWorld.get(trustedFoot).add(tempFrameVector); // New CoP position } footPositionInWorld.set(rootJointPosition); footPositionInWorld.sub(footToRootJointPosition); }
private void computeUnbiasedEstimatedMeasurement(SpatialAccelerationCalculator spatialAccelerationCalculator, FrameVector estimatedMeasurement) { tempFramePoint.setToZero(measurementFrame); spatialAccelerationCalculator.getLinearAccelerationOfBodyFixedPoint(estimatedMeasurement, measurementLink, tempFramePoint); estimatedMeasurement.changeFrame(gravitationalAcceleration.getReferenceFrame()); estimatedMeasurement.sub(gravitationalAcceleration); estimatedMeasurement.changeFrame(measurementFrame); } }
private void setVectorFromPoseToPose(YoFrameVector frameVectorToPack, FramePose fromPose, FramePose toPose) { frameVectorToPack.set(toPose.getFramePointCopy()); FrameVector frameTuple = frameVectorToPack.getFrameTuple(); frameTuple.sub(fromPose.getFramePointCopy()); frameVectorToPack.setWithoutChecks(frameTuple); }
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()); }
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()); }
public DenseMatrix64F computeResidual() { computeVelocityOfStationaryPoint(tempFrameVector); tempFrameVector.changeFrame(ReferenceFrame.getWorldFrame()); tempFrameVector2.setIncludingFrame(ReferenceFrame.getWorldFrame(), pointVelocityMeasurementInputPort.getData().getVelocityOfMeasurementPointInWorldFrame()); residualVector.setIncludingFrame(tempFrameVector2); residualVector.sub(tempFrameVector); MatrixTools.insertTuple3dIntoEJMLVector(residualVector.getVector(), residual, 0); return residual; }
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 computeDerivativeTerm(FrameVector desiredVelocity, FrameVector currentVelocity) { desiredVelocity.changeFrame(bodyFrame); currentVelocity.changeFrame(bodyFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); // 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); tempMatrix.set(derivativeGainMatrix); tangentialDampingCalculator.compute(positionError, tempMatrix); tempMatrix.transform(derivativeTerm.getVector()); }
private void computeDerivativeTerm(FrameVector desiredVelocity, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); endEffectorTwist.changeFrame(baseFrame); bodyFixedPoint.setToZero(frameAtBodyFixedPoint); bodyFixedPoint.changeFrame(baseFrame); endEffectorTwist.getLinearVelocityOfPointFixedInBodyFrame(currentVelocity, bodyFixedPoint); desiredVelocity.changeFrame(baseFrame); currentVelocity.changeFrame(baseFrame); derivativeTerm.setToZero(baseFrame); derivativeTerm.sub(desiredVelocity, currentVelocity); velocityError.setAndMatchFrame(derivativeTerm); derivativeGainMatrix.transform(derivativeTerm.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); }
private void computeAngularAccelerationBlock(Matrix3d rotationFromEstimationToMeasurement) { // r tempFramePoint.setIncludingFrame(centerOfMassPositionPort.getData()); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); tempFrameVector.setIncludingFrame(tempFramePoint); // r - p_{i}^{w} tempFramePoint.setToZero(measurementFrame); tempFramePoint.changeFrame(ReferenceFrame.getWorldFrame()); tempFrameVector.sub(tempFramePoint); // R_{w}^{p} (r - p_{i}^{w}) tempFrameVector.changeFrame(estimationFrame); // \tilde{r^{p} - p_{i}^{p}} MatrixTools.toTildeForm(tempMatrix, tempFrameVector.getVector()); // R_{p}^{i} \tilde{r^{p} - p_{i}^{p}} tempMatrix.mul(rotationFromEstimationToMeasurement, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(angularAccelerationPort)); }
public DenseMatrix64F computeResidual() { Vector3d measuredAngularVelocityVector3d = angularVelocityMeasurementInputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructureInputPort.getData().getTwistCalculator(); twistCalculator.getRelativeTwist(tempTwist, orientationEstimationLink, measurementLink); tempTwist.getAngularPart(relativeAngularVelocity); relativeAngularVelocity.changeFrame(measurementFrame); predictedAngularVelocityMeasurementTemp.setIncludingFrame(angularVelocityStatePort.getData()); predictedAngularVelocityMeasurementTemp.changeFrame(measurementFrame); predictedAngularVelocityMeasurementTemp.add(relativeAngularVelocity); predictedAngularVelocityMeasurementTemp.add(biasStatePort.getData()); angularVelocityResidual.setIncludingFrame(measurementFrame, measuredAngularVelocityVector3d); angularVelocityResidual.sub(predictedAngularVelocityMeasurementTemp); MatrixTools.insertTuple3dIntoEJMLVector(angularVelocityResidual.getVector(), residual, 0); return residual; }
public void initialize(FramePoint initialPosition, FramePoint intermediatePosition, FramePoint finalPosition, double intermediateParameter) { initialPosition.changeFrame(referenceFrame); intermediatePosition.changeFrame(referenceFrame); finalPosition.changeFrame(referenceFrame); final double q = intermediateParameter; MathTools.checkIfInRange(q, 0.0, 1.0); c0.set(initialPosition); c2.set(intermediatePosition); c2.sub(initialPosition); tempInitialize.set(finalPosition); tempInitialize.sub(initialPosition); tempInitialize.scale(q); c2.sub(tempInitialize); c2.scale(1.0 / (MathTools.square(q) - q)); c1.set(finalPosition); c1.sub(initialPosition); c1.sub(c2); }