public void update() { if (footRawCoPPositionsInWorld != null) { overallRawCoPPositionInWorld.setToZero(); double totalFootForce = 0.0; for (int i = 0; i < footList.size(); i++) { RigidBody rigidBody = footList.get(i); footSwitches.get(rigidBody).computeAndPackCoP(tempRawCoP2d); tempRawCoP.setIncludingFrame(tempRawCoP2d.getReferenceFrame(), tempRawCoP2d.getX(), tempRawCoP2d.getY(), 0.0); tempRawCoP.changeFrame(worldFrame); footRawCoPPositionsInWorld.get(rigidBody).set(tempRawCoP); footSwitches.get(rigidBody).computeAndPackFootWrench(tempWrench); double singleFootForce = tempWrench.getLinearPartZ(); totalFootForce += singleFootForce; tempRawCoP.scale(singleFootForce); overallRawCoPPositionInWorld.add(tempRawCoP); } overallRawCoPPositionInWorld.scale(1.0 / totalFootForce); } }
private boolean isFootBarelyLoaded() { return footSwitch.computeFootLoadPercentage() < footLoadThresholdToHoldPosition.getDoubleValue(); }
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(); } }
if (footSwitches.get(foot).hasFootHitGround()) haveFeetHitGroundFiltered.get(foot).update(true); else if (footSwitches.get(foot).getForceMagnitudePastThreshhold())
private int filterTrustedFeetBasedOnContactForces(int numberOfEndEffectorsTrusted) { double totalForceZ = 0.0; for (int i = 0; i < feet.size(); i++) { RigidBody foot = feet.get(i); Wrench footWrench = footWrenches.get(foot); footSwitches.get(foot).computeAndPackFootWrench(footWrench); totalForceZ += footWrench.getLinearPartZ(); } for (int i = 0; i < feet.size(); i++) { RigidBody foot = feet.get(i); Wrench footWrench = footWrenches.get(foot); footForcesZInPercentOfTotalForce.get(foot).set(footWrench.getLinearPartZ() / totalForceZ); if (footForcesZInPercentOfTotalForce.get(foot).getDoubleValue() < forceZInPercentThresholdToFilterFoot.getDoubleValue()) { numberOfEndEffectorsTrusted--; areFeetTrusted.get(foot).set(false); return numberOfEndEffectorsTrusted; } } return numberOfEndEffectorsTrusted; }
centerOfPressureDataHolderFromController.getCenterOfPressure(tempCoP2d, trustedFoot); else footSwitches.get(trustedFoot).computeAndPackCoP(tempCoP2d); if (!isCoPInsideFoot) if (footSwitches.get(trustedFoot).computeFootLoadPercentage() > 0.2)
public void compute() { for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).hasFootHitGround(); //debug footControlModules.get(robotSide).doControl(); } }
@Override public void doSpecificAction() { if (partialFootholdControlModule != null) { footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop); YoPlaneContactState contactState = momentumBasedController.getContactState(contactableFoot); boolean contactStateHasChanged = partialFootholdControlModule.applyShrunkPolygon(contactState); if (contactStateHasChanged) contactState.notifyContactStateHasChanged(); } footAcceleration.setToZero(contactableFoot.getFrameAfterParentJoint(), rootBody.getBodyFixedFrame(), contactableFoot.getFrameAfterParentJoint()); ReferenceFrame bodyFixedFrame = contactableFoot.getRigidBody().getBodyFixedFrame(); footAcceleration.changeBodyFrameNoRelativeAcceleration(bodyFixedFrame); footAcceleration.changeFrameNoRelativeMotion(bodyFixedFrame); spatialAccelerationCommand.setSpatialAcceleration(footAcceleration); }
ReferenceFrame footSensorFrame = footSwitches.get(robotSide).getMeasurementFrame();
@Override public void doTransitionIntoAction() { balanceManager.clearICPPlan(); footSwitches.get(swingSide).reset(); integrateAnkleAccelerationsOnSwingLeg(swingSide); }
public void update() { for (int i = 0; i < allRigidBodies.size(); i++) { RigidBody rigidBody = allRigidBodies.get(i); FootSwitchInterface footSwitch = footSwitches.get(rigidBody); GeometricJacobian jacobian = jacobians.get(rigidBody); footSwitch.computeAndPackFootWrench(wrench); wrench.changeFrame(rigidBody.getBodyFixedFrame()); wrench.negate(); jacobian.compute(); jacobian.computeJointTorques(wrench, jointTorquesMatrix); InverseDynamicsJoint[] joints = jacobian.getJointsInOrder(); for (int j = 0; j < joints.length; j++) { OneDoFJoint joint = (OneDoFJoint) joints[j]; jointTorques.get(joint).set(jointTorquesMatrix.get(j, 0)); } } } }
if (partialFootholdControlModule != null && recoverTimeHasPassed) footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop); footBarelyLoaded.set(footSwitch.computeFootLoadPercentage() < footLoadThreshold.getDoubleValue()); footSwitch.computeAndPackCoP(cop2d); if (cop2d.containsNaN()) cop2d.setToZero(contactableFoot.getSoleFrame());
@Override public boolean isDone() { hasMinimumTimePassed.set(hasMinimumTimePassed()); if (!hasMinimumTimePassed.getBooleanValue()) return false; return hasMinimumTimePassed.getBooleanValue() && footSwitches.get(swingSide).hasFootHitGround(); }
@Override public void doSpecificAction() footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP);
ReferenceFrame footSensorFrame = footSwitches.get(robotSide).getMeasurementFrame();
private void computeCop() { double force = 0.0; centerOfPressure.setToZero(worldFrame); for (RobotSide robotSide : RobotSide.values) { footSwitches.get(robotSide).computeAndPackCoP(tempFootCop2d); if (tempFootCop2d.containsNaN()) continue; footSwitches.get(robotSide).computeAndPackFootWrench(tempFootWrench); double footForce = tempFootWrench.getLinearPartZ(); force += footForce; tempFootCop.setIncludingFrame(tempFootCop2d.getReferenceFrame(), tempFootCop2d.getX(), tempFootCop2d.getY(), 0.0); tempFootCop.changeFrame(worldFrame); tempFootCop.scale(footForce); centerOfPressure.add(tempFootCop.getX(), tempFootCop.getY()); } centerOfPressure.scale(1.0 / force); yoCenterOfPressure.set(centerOfPressure); }
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); tempFrameVector.set(tempWrench.getLinearPart()); footForcesRaw.get(robotSide).set(tempFrameVector); tempFrameVector.set(tempWrench.getAngularPart()); footTorquesRaw.get(robotSide).set(tempFrameVector); footForcesRawFiltered.get(robotSide).update(); footTorquesRawFiltered.get(robotSide).update(); } }
private void computeTotalGroundReactionForce() { totalGroundReactionForce.setToZero(); for (int i = 0; i < feet.size(); i++) { RigidBody foot = feet.get(i); Wrench footWrench = footWrenches.get(foot); footSwitches.get(foot).computeAndPackFootWrench(footWrench); footWrench.getLinearPartIncludingFrame(tempFootForce); tempFootForce.changeFrame(worldFrame); totalGroundReactionForce.add(tempFootForce); } totalGroundReactionForce.getFrameTuple(tempCoMAcceleration); comAcceleration.set(tempCoMAcceleration); comAcceleration.setZ(comAcceleration.getZ() - robotMass.getDoubleValue() * gravitationalAcceleration); }
GlitchFilteredBooleanYoVariable isFootTrusted = areFeetTrusted.get(foot); boolean hasFootHitGround = footSwitches.get(foot).hasFootHitGround(); boolean isFootStatic = currentFootLinearVelocities.get(foot).getDoubleValue() < footLinearVelocityThreshold.getDoubleValue();
footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop);