public double length() { return getFrameTuple().length(); }
private void clipToVectorMagnitude(double maximumMagnitude, FrameVector frameVectorToClip) { double magnitude = frameVectorToClip.length(); if (magnitude > maximumMagnitude) { frameVectorToClip.scale(maximumMagnitude / magnitude); } }
public void setContactNormalVector(FrameVector normal) { double length = normal.length(); if (length < 1e-7) { contactNormal.setToNaN(); } else { contactNormal.setIncludingFrame(normal); } }
private void updateFootLinearVelocities() { for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); twistCalculator.getTwistOfBody(footTwist, foot); footTwist.getLinearPart(footLinearVelocity); currentFootLinearVelocities.get(foot).set(footLinearVelocity.length()); } }
private void updateSensorValuesFromRobot() { forceSensorData.getWrench(wristSensorWrench); sensorMassCompensator.update(forceSensorData); FrameVector sensorForceRawInWorld = sensorMassCompensator.getSensorForceRaw(world); yoWristSensorForceMagnitude.set(sensorForceRawInWorld.length()); }
public void compute(Map<RigidBody, Wrench> externalWrenches) { for (int i = 0; i < contactablePlaneBodies.size(); i++) { ContactablePlaneBody contactablePlaneBody = contactablePlaneBodies.get(i); FramePoint2d cop = cops.get(contactablePlaneBody); YoFramePoint2d yoCop = yoCops.get(contactablePlaneBody); yoCop.getFrameTuple2d(cop); Wrench wrench = externalWrenches.get(contactablePlaneBody.getRigidBody()); if (wrench != null) { wrench.getLinearPartIncludingFrame(tempForce); double normalTorque = centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(cop, wrench, contactablePlaneBody.getSoleFrame()); centersOfPressure2d.get(contactablePlaneBody).set(cop); tempCoP3d.setXYIncludingFrame(cop); centersOfPressureWorld.get(contactablePlaneBody).setAndMatchFrame(tempCoP3d); groundReactionForceMagnitudes.get(contactablePlaneBody).set(tempForce.length()); normalTorques.get(contactablePlaneBody).set(normalTorque); } else { groundReactionForceMagnitudes.get(contactablePlaneBody).set(0.0); centersOfPressureWorld.get(contactablePlaneBody).setToNaN(); cop.setToNaN(); } yoCop.set(cop); desiredCenterOfPressureDataHolder.setCenterOfPressure(cop, contactablePlaneBody.getRigidBody()); } }
scaleFactor = scaleFactor - 1.0; planarWaypointOffset.scale(scaleFactor); double offsetLength = planarWaypointOffset.length();
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(); }
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()); } }
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()); }
/** * Numerically compute the maximum speed along the trajectory and the time at which this speed occurs. * The time precision can be specified. */ public void computeMaxSpeed(double timeIncrement) { maxSpeed.set(Double.NEGATIVE_INFINITY); maxSpeedTime.set(Double.NaN); for (double time = 0.0; time <= 1.0; time += timeIncrement) { compute(time); getVelocity(tempVelocity); double speed = tempVelocity.length(); if (speed > maxSpeed.getDoubleValue()) { maxSpeed.set(speed); maxSpeedTime.set(time); } } }
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 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()); }
if (directionOfMotion.length() > 0.0)
@Override public void compute(FrameVector output, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector currentVelocity, FrameVector feedForward) { computeProportionalTerm(desiredPosition); if (currentVelocity != null) computeDerivativeTerm(desiredVelocity, currentVelocity); computeIntegralTerm(); output.setToNaN(bodyFrame); output.add(proportionalTerm, derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearActionMagnitude = output.length(); if (feedbackLinearActionMagnitude > gains.getMaximumFeedback()) { output.scale(gains.getMaximumFeedback() / feedbackLinearActionMagnitude); } feedbackLinearAction.set(output); rateLimitedFeedbackLinearAction.update(); rateLimitedFeedbackLinearAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
public void compute(FrameVector output, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector currentAngularVelocity, FrameVector feedForward) { computeProportionalTerm(desiredOrientation); if (currentAngularVelocity != null) computeDerivativeTerm(desiredAngularVelocity, currentAngularVelocity); computeIntegralTerm(); output.setToZero(proportionalTerm.getReferenceFrame()); output.add(proportionalTerm); output.add(derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackAngularActionMagnitude = output.length(); double maximumAction = gains.getMaximumFeedback(); if (feedbackAngularActionMagnitude > maximumAction) { output.scale(maximumAction / feedbackAngularActionMagnitude); } feedbackAngularAction.set(output); rateLimitedFeedbackAngularAction.update(); rateLimitedFeedbackAngularAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
public void compute(FrameVector acceleration, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector feedForward, RigidBody base) { ReferenceFrame baseFrame = base.getBodyFixedFrame(); computeProportionalTerm(desiredPosition, baseFrame); computeDerivativeTerm(desiredVelocity, base); computeIntegralTerm(baseFrame); acceleration.setIncludingFrame(proportionalTerm); acceleration.add(derivativeTerm); acceleration.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearAccelerationMagnitude = acceleration.length(); if (feedbackLinearAccelerationMagnitude > gains.getMaximumFeedback()) { acceleration.scale(gains.getMaximumFeedback() / feedbackLinearAccelerationMagnitude); } feedbackLinearAcceleration.setAndMatchFrame(acceleration); rateLimitedFeedbackLinearAcceleration.update(); rateLimitedFeedbackLinearAcceleration.getFrameTupleIncludingFrame(acceleration); acceleration.changeFrame(baseFrame); feedForward.changeFrame(baseFrame); acceleration.add(feedForward); }
private void readSensorData(Wrench footWrenchToPack) { forceSensorData.getWrench(footWrenchToPack); // First in measurement frame for all the frames... footForce.setToZero(footWrenchToPack.getExpressedInFrame()); footWrenchToPack.getLinearPart(footForce); yoFootForce.set(footForce); footTorque.setToZero(footWrenchToPack.getExpressedInFrame()); footWrenchToPack.getAngularPart(footTorque); yoFootTorque.set(footTorque); // magnitude of force part is independent of frame footForceMagnitude.set(footForce.length()); // Now change to frame after the parent joint (ankle or wrist for example): footWrenchInBodyFixedFrame.set(footWrenchToPack); footWrenchInBodyFixedFrame.changeFrame(contactablePlaneBody.getRigidBody().getBodyFixedFrame()); footForce.setToZero(footWrenchInBodyFixedFrame.getExpressedInFrame()); footWrenchInBodyFixedFrame.getLinearPart(footForce); footTorque.setToZero(footWrenchInBodyFixedFrame.getExpressedInFrame()); footWrenchInBodyFixedFrame.getAngularPart(footTorque); footForce.changeFrame(contactablePlaneBody.getFrameAfterParentJoint()); yoFootForceInFoot.set(footForce); footTorque.changeFrame(contactablePlaneBody.getFrameAfterParentJoint()); yoFootTorqueInFoot.set(footTorque); footForce.changeFrame(ReferenceFrame.getWorldFrame()); footTorque.changeFrame(ReferenceFrame.getWorldFrame()); yoFootForceInWorld.set(footForce); yoFootTorqueInWorld.set(footTorque); updateSensorVisualizer(); }