/** * Computes the 2D vector perpendicular to the given 2D {@code vector} such that: * <ul> * <li> {@code vector2d.dot(perpendicularVector2d) == 0.0}. * <li> {@code vector2d.angle(perpendicularVector2d) == Math.PI / 2.0}. * </ul> * <p> * WARNING: the 3D arguments are projected onto the XY-plane to perform the actual computation in 2D. * </p> * * @param vector the vector to compute in the xy-plane the perpendicular of. Not modified. * @param perpendicularVectorToPack a vector in which the x and y components of the 2D perpendicular vector are stored. Modified. */ // FIXME this is just bad. public static void getPerpendicularVector2d(FrameVector vector, FrameVector perpendicularVectorToPack) { perpendicularVectorToPack.set(-vector.getY(), vector.getX(), perpendicularVectorToPack.getZ()); }
private DenseMatrix64F computeSingleRhoCoPJacobian(FramePoint basisVectorOrigin, FrameVector basisVector) { wrenchFromRho.getLinearPartIncludingFrame(forceFromRho); forceFromRho.changeFrame(planeFrame); if (forceFromRho.getZ() > 1.0e-1) { basisVectorOrigin.changeFrame(planeFrame); basisVector.changeFrame(planeFrame); unitSpatialForceVector.setIncludingFrame(basisVector, basisVectorOrigin); singleRhoCoPJacobian.set(0, 0, -unitSpatialForceVector.getAngularPartY() / forceFromRho.getZ()); singleRhoCoPJacobian.set(1, 0, unitSpatialForceVector.getAngularPartX() / forceFromRho.getZ()); } else { singleRhoCoPJacobian.zero(); } return singleRhoCoPJacobian; }
@Override public void getDesiredAccelerationMatrix(DenseMatrix64F matrix, int rowStart) { matrix.set(rowStart + 0, 0, jointAngularAccelerationDesired.getX()); matrix.set(rowStart + 1, 0, jointAngularAccelerationDesired.getY()); matrix.set(rowStart + 2, 0, jointAngularAccelerationDesired.getZ()); }
public void update(FrameVector vectorUnfiltered) { x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
@Override public void getVelocityMatrix(DenseMatrix64F matrix, int rowStart) { matrix.set(rowStart + 0, 0, jointAngularVelocity.getX()); matrix.set(rowStart + 1, 0, jointAngularVelocity.getY()); matrix.set(rowStart + 2, 0, jointAngularVelocity.getZ()); }
@Override public void getTauMatrix(DenseMatrix64F matrix) { matrix.set(0, 0, jointTorque.getX()); matrix.set(1, 0, jointTorque.getY()); matrix.set(2, 0, jointTorque.getZ()); }
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 update(FrameVector vectorUnfiltered) { checkReferenceFrameMatch(vectorUnfiltered); x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
public void update(FrameVector vectorSource) { checkReferenceFrameMatch(vectorSource); x.update(vectorSource.getX()); y.update(vectorSource.getY()); z.update(vectorSource.getZ()); }
public void update(FrameVector vectorUnfiltered) { checkReferenceFrameMatch(vectorUnfiltered); x.update(vectorUnfiltered.getX()); y.update(vectorUnfiltered.getY()); z.update(vectorUnfiltered.getZ()); }
public double computeFootLoadPercentage() { readSensorData(footWrench); yoFootForceInFoot.getFrameTupleIncludingFrame(footForce); footForce.clipToMinMax(0.0, Double.MAX_VALUE); footForceMagnitude.set(footForce.length()); footLoadPercentage.update(footForce.getZ() / robotTotalWeight); return footLoadPercentage.getDoubleValue(); }
public static void computeWrench(Wrench groundReactionWrenchToPack, FrameVector force, FramePoint2d cop, double normalTorque) { ReferenceFrame referenceFrame = cop.getReferenceFrame(); force.changeFrame(referenceFrame); // r x f + tauN Vector3d torque = new Vector3d(); torque.setX(cop.getY() * force.getZ()); torque.setY(-cop.getX() * force.getZ()); torque.setZ(cop.getX() * force.getY() - cop.getY() * force.getX() + normalTorque); groundReactionWrenchToPack.set(referenceFrame, force.getVector(), torque); }
private void doYoVectorCrossProduct(YoFrameVector v1, YoFrameVector v2, YoFrameVector vecToPack) { temp.cross(v1.getFrameTuple(), v2.getFrameTuple()); if (temp.length() > 0) { temp.normalize(); } vecToPack.set(world, temp.getX(), temp.getY(), temp.getZ()); } }
public void compute(double time) { timeIntoStep.set(time); nominalTrajectoryGenerator.compute(time); nominalTrajectoryGenerator.getLinearData(nominalTrajectoryPosition, nominalTrajectoryVelocity, nominalTrajectoryAcceleration); xPolynomial.compute(time); yPolynomial.compute(time); desiredPosition.setX(xPolynomial.getPosition()); desiredPosition.setY(yPolynomial.getPosition()); desiredPosition.setZ(nominalTrajectoryPosition.getZ()); desiredVelocity.setX(xPolynomial.getVelocity()); desiredVelocity.setY(yPolynomial.getVelocity()); desiredVelocity.setZ(nominalTrajectoryVelocity.getZ()); desiredAcceleration.setX(xPolynomial.getAcceleration()); desiredAcceleration.setY(yPolynomial.getAcceleration()); desiredAcceleration.setZ(nominalTrajectoryAcceleration.getZ()); }
@Override public void update() { for(int i = 0; i < legOneDoFJoints.size(); i++) { OneDoFJoint oneDoFJoint = legOneDoFJoints.get(i); jointTorques.set(i, 0, oneDoFJoint.getTauMeasured()); } footJacobian.compute(); DenseMatrix64F jacobianMatrix = footJacobian.getJacobianMatrix(); CommonOps.mult(selectionMatrix, jacobianMatrix, linearPartOfJacobian); UnrolledInverseFromMinor.inv3(linearPartOfJacobian, linearJacobianInverse, 1.0); CommonOps.multTransA(linearJacobianInverse, jointTorques, footLinearForce); footForce.setToZero(footJacobian.getJacobianFrame()); footForce.set(footLinearForce.get(0), footLinearForce.get(1), footLinearForce.get(2)); footForce.changeFrame(ReferenceFrame.getWorldFrame()); measuredZForce.set(footForce.getZ() * -1.0); isInContact.set(measuredZForce.getDoubleValue() > zForceThreshold.getDoubleValue()); } }
private void convertAngularVelocityAndSetOnOutputPort() { Vector3d measuredAngularVelocityVector3d = angularVelocityInputPort.getData(); TwistCalculator twistCalculator = inverseDynamicsStructureInputPort.getData().getTwistCalculator(); twistCalculator.getRelativeTwist(tempRelativeTwistOrientationMeasFrameToEstFrame, angularVelocityMeasurementLink, estimationLink); tempRelativeTwistOrientationMeasFrameToEstFrame.getAngularPart(relativeAngularVelocity); relativeAngularVelocity.changeFrame(estimationFrame); tempAngularVelocityMeasurementLink.setIncludingFrame(angularVelocityMeasurementFrame, measuredAngularVelocityVector3d); tempAngularVelocityMeasurementLink.changeFrame(estimationFrame); relativeAngularVelocity.add(tempAngularVelocityMeasurementLink); tempAngularVelocityEstimationLink.setIncludingFrame(relativeAngularVelocity); // just a copy for clarity // Introduce simulated IMU drift tempAngularVelocityEstimationLink.setZ(tempAngularVelocityEstimationLink.getZ() + imuSimulatedDriftYawVelocity.getDoubleValue()); angularVelocityOutputPort.setData(tempAngularVelocityEstimationLink); }
public FrameVector getCurrentDesiredVelocity(double timeInMove) { FramePoint Xfh = getCurrentDesiredPoint(timeInMove + stepSizeforNumericalCalculation); FramePoint Xf2h = getCurrentDesiredPoint(timeInMove + 2.0 * stepSizeforNumericalCalculation); FramePoint Xrh = getCurrentDesiredPoint(timeInMove - stepSizeforNumericalCalculation); FramePoint Xr2h = getCurrentDesiredPoint(timeInMove - 2.0 * stepSizeforNumericalCalculation); FrameVector ret = new FrameVector(Xfh.getReferenceFrame()); ret.setX((-Xf2h.getX() + 8.0 * Xfh.getX() - 8.0 * Xrh.getX() + Xr2h.getX()) / (12.0 * stepSizeforNumericalCalculation)); ret.setY((-Xf2h.getY() + 8.0 * Xfh.getY() - 8.0 * Xrh.getY() + Xr2h.getY()) / (12.0 * stepSizeforNumericalCalculation)); ret.setZ((-Xf2h.getZ() + 8.0 * Xfh.getZ() - 8.0 * Xrh.getZ() + Xr2h.getZ()) / (12.0 * stepSizeforNumericalCalculation)); // Edge cases result in half the expected value, so multiply by 2 if ((timeInMove <= stepSizeforNumericalCalculation) || (this.moveDuration - timeInMove <= stepSizeforNumericalCalculation)) { ret.setX(ret.getX() * 2); ret.setY(ret.getY() * 2); ret.setZ(ret.getZ() * 2); } return ret; }
boolean isSteppingForwardEnough = soleToSoleFrameVector.getX() > footstepLengthThresholdToPutExitCMPOnToesSteppingDown.getDoubleValue(); soleToSoleFrameVector.changeFrame(worldFrame); boolean isSteppingDownEnough = soleToSoleFrameVector.getZ() < -footstepHeightThresholdToPutExitCMPOnToesSteppingDown.getDoubleValue();
yRotPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, copyOfInitialAngularVelocity.getY(), 0.0, tempAxisAngle.getY() * tempAxisAngle.getAngle(), copyOfFinalAngularVelocity.getY(), 0.0); zRotPolynomial.setQuintic(0.0, trajectoryTime.getDoubleValue(), 0.0, copyOfInitialAngularVelocity.getZ(), 0.0, tempAxisAngle.getZ() * tempAxisAngle.getAngle(), copyOfFinalAngularVelocity.getZ(), 0.0);