@Override public void update() { foreSensorData.getWrench(wrenchToPack); wrenchToPack.getLinearPart(vectorToPack); touchdownDetected.set(vectorToPack.length() > touchdownForceThreshold.getDoubleValue()); touchdownDetectedFiltered.update(); } }
tempWrench.getLinearPart(tempVector); tempVector.changeFrame(ReferenceFrame.getWorldFrame()); force.set(tempVector);
/** * Takes the dot product of this twist and a wrench, resulting in the (reference frame independent) instantaneous power. * @param wrench a wrench that * 1) has an 'onWhat' reference frame that is the same as this twist's 'bodyFrame' reference frame. * 2) is expressed in the same reference frame as this twist * @return the instantaneous power associated with this twist and the wrench */ public double dot(Wrench wrench) { this.bodyFrame.checkReferenceFrameMatch(wrench.getBodyFrame()); this.expressedInFrame.checkReferenceFrameMatch(wrench.getExpressedInFrame()); double power = this.angularPart.dot(wrench.getAngularPart()) + this.linearPart.dot(wrench.getLinearPart()); return power; }
private void updateFootSensorRawMeasurement() { for (RobotSide robotSide : RobotSide.values) { FootSwitchInterface footSwitch = footSwitches.get(robotSide); tempWrench.setToZero(footSwitch.getMeasurementFrame(), footSwitch.getMeasurementFrame()); tempFrameVector.setToZero(footSwitch.getMeasurementFrame()); footSwitch.computeAndPackFootWrench(tempWrench); tempWrench.getLinearPart(tempFrameVector); footForcesRaw.get(robotSide).set(tempFrameVector); tempWrench.getAngularPart(tempFrameVector); footTorquesRaw.get(robotSide).set(tempFrameVector); footForcesRawFiltered.get(robotSide).update(); footTorquesRawFiltered.get(robotSide).update(); } }
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(); }