/** * updates the position of a swinging foot * @param swingingFoot a foot in swing * @param pelvisPosition the current pelvis position */ private void updateUntrustedFootPosition(RigidBody swingingFoot, FramePoint pelvisPosition) { YoFramePoint footPositionInWorld = footPositionsInWorld.get(swingingFoot); footPositionInWorld.set(pelvisPosition); footPositionInWorld.sub(footToRootJointPositions.get(swingingFoot)); copPositionsInWorld.get(swingingFoot).set(footPositionInWorld); copsRawInFootFrame.get(swingingFoot).setToZero(); copsFilteredInFootFrame.get(swingingFoot).setToZero(); }
/** * Assuming the CoP is not moving, the foot position can be updated. That way we can see if the foot is on the edge. * @param plantedFoot */ private void correctFootPositionsUsingCoP(RigidBody plantedFoot) { if (!useControllerDesiredCoP.getBooleanValue()) return; AlphaFilteredYoFramePoint2d copFilteredInFootFrame = copsFilteredInFootFrame.get(plantedFoot); tempCoPOffset.setIncludingFrame(copFilteredInFootFrame.getReferenceFrame(), copFilteredInFootFrame.getX(), copFilteredInFootFrame.getY(), 0.0); tempCoPOffset.changeFrame(worldFrame); YoFramePoint footPositionInWorld = footPositionsInWorld.get(plantedFoot); footPositionInWorld.set(copPositionsInWorld.get(plantedFoot)); footPositionInWorld.sub(tempCoPOffset); }
private void updateTrustedFootPosition(RigidBody trustedFoot) { YoFramePoint footPositionInWorld = footPositionsInWorld.get(trustedFoot); AlphaFilteredYoFrameVector footToRootJointPosition = footToRootJointPositions.get(trustedFoot); if (trustCoPAsNonSlippingContactPoint.getBooleanValue()) { footToRootJointPosition.getFrameTupleIncludingFrame(tempPosition); rootJointPosition.getFrameTupleIncludingFrame(tempFramePoint); tempFramePoint.sub(tempPosition); // New foot position footPositionInWorld.getFrameTuple(tempPosition); // Previous foot position tempFrameVector.sub(tempFramePoint, tempPosition); // Delta from previous to new foot position copPositionsInWorld.get(trustedFoot).add(tempFrameVector); // New CoP position } footPositionInWorld.set(rootJointPosition); footPositionInWorld.sub(footToRootJointPosition); }