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); } }
centerOfPressureDataHolderFromController.getCenterOfPressure(tempCoP2d, trustedFoot); else footSwitches.get(trustedFoot).computeAndPackCoP(tempCoP2d);
@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); }
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); }
if (partialFootholdControlModule != null && recoverTimeHasPassed) footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop); footSwitch.computeAndPackCoP(cop2d); if (cop2d.containsNaN()) cop2d.setToZero(contactableFoot.getSoleFrame());
@Override public void doSpecificAction() footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP);
footSwitch.computeAndPackCoP(copActual);
footSwitch.computeAndPackCoP(cop); momentumBasedController.getDesiredCenterOfPressure(contactableFoot, desiredCoP); partialFootholdControlModule.compute(desiredCoP, cop);