private void stopArmIfHandCollisionIsDetected(double time) { estimateStiffnessOfConstraintsActingUponWrist(); yoWristSensorForceMagnitudeBias.update(yoWristSensorForceMagnitude.getDoubleValue()); yoWristSensorForceMagnitudeBandPassFiltered.update(yoWristSensorForceMagnitude.getDoubleValue()); yoForceLimitExceeded.set(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > yoImpactForceThreshold_N.getDoubleValue()); double forceToForceLimitRatio = yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() / yoImpactForceThreshold_N.getDoubleValue(); yoCollisionSeverityLevelOneToThree.set(MathTools.clipToMinMax((int) Math.round(forceToForceLimitRatio), 1, 3)); // yoForceLimitExceeded.set( taskspaceStiffnessCalc.getForceAlongDirectionOfMotion() > yoImpactForceThreshold_N.getDoubleValue() ); // yoStiffnessLimitExceeded.set(taskspaceStiffnessCalc.getStiffnessAlongDirectionOfMotion() > yoImpactStiffnessThreshold_NperM.getDoubleValue()); if (yoForceLimitExceeded.getBooleanValue()) { if (!yoImpactDetected.getBooleanValue() && time > 1.0 || yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > maxFilteredForce) { yoImpactDetected.set(true); yoImpactTime.set(time); controllerCommunicator.send(new HandCollisionDetectedPacket(robotSide, yoCollisionSeverityLevelOneToThree.getIntegerValue())); if (DEBUG) PrintTools.debug(this, "Sending Collision Detected Packet. FilteredForce = " + yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); } } if (Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()) > maxFilteredForce) { maxFilteredForce = Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); if (DEBUG) PrintTools.debug(this, "maxFilteredForce = " + maxFilteredForce); } }
private void stopArmIfHandCollisionIsDetected(double time) { estimateStiffnessOfConstraintsActingUponWrist(); yoWristSensorForceMagnitudeBias.update(yoWristSensorForceMagnitude.getDoubleValue()); yoWristSensorForceMagnitudeBandPassFiltered.update(yoWristSensorForceMagnitude.getDoubleValue()); yoForceLimitExceeded.set(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > yoImpactForceThreshold_N.getDoubleValue()); double forceToForceLimitRatio = yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() / yoImpactForceThreshold_N.getDoubleValue(); yoCollisionSeverityLevelOneToThree.set(MathTools.clamp((int) Math.round(forceToForceLimitRatio), 1, 3)); // yoForceLimitExceeded.set( taskspaceStiffnessCalc.getForceAlongDirectionOfMotion() > yoImpactForceThreshold_N.getDoubleValue() ); // yoStiffnessLimitExceeded.set(taskspaceStiffnessCalc.getStiffnessAlongDirectionOfMotion() > yoImpactStiffnessThreshold_NperM.getDoubleValue()); if (yoForceLimitExceeded.getBooleanValue()) { if (!yoImpactDetected.getBooleanValue() && time > 1.0 || yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue() > maxFilteredForce) { yoImpactDetected.set(true); yoImpactTime.set(time); publisher.publish(HumanoidMessageTools.createHandCollisionDetectedPacket(robotSide, yoCollisionSeverityLevelOneToThree.getIntegerValue())); if (DEBUG) PrintTools.debug(this, "Sending Collision Detected Packet. FilteredForce = " + yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); } } if (Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()) > maxFilteredForce) { maxFilteredForce = Math.abs(yoWristSensorForceMagnitudeBandPassFiltered.getDoubleValue()); if (DEBUG) PrintTools.debug(this, "maxFilteredForce = " + maxFilteredForce); } }