/** * Estimates the pelvis position and linear velocity using the leg kinematics * @param trustedFoot is the foot used to estimates the pelvis state * @param numberOfTrustedSides is only one or both legs used to estimate the pelvis state */ private void updatePelvisWithKinematics(RigidBody trustedFoot, int numberOfTrustedFeet) { double scaleFactor = 1.0 / numberOfTrustedFeet; footToRootJointPositions.get(trustedFoot).getFrameTuple(tempPosition); tempPosition.scale(scaleFactor); rootJointPosition.add(tempPosition); footPositionsInWorld.get(trustedFoot).getFrameTuple(tempPosition); tempPosition.scale(scaleFactor); rootJointPosition.add(tempPosition); YoFramePoint rootJointPositionPerFoot = rootJointPositionsPerFoot.get(trustedFoot); rootJointPositionPerFoot.set(footPositionsInWorld.get(trustedFoot)); rootJointPositionPerFoot.add(footToRootJointPositions.get(trustedFoot)); footVelocitiesInWorld.get(trustedFoot).getFrameTupleIncludingFrame(tempFrameVector); tempFrameVector.scale(scaleFactor * alphaRootJointLinearVelocityNewTwist.getDoubleValue()); rootJointLinearVelocityNewTwist.sub(tempFrameVector); }
public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint) { filteredYoAngularMomentum.getFrameTuple(angularMomentum); ReferenceFrame comFrame = angularMomentum.getReferenceFrame(); localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint); localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame); double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity); adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX()); adjustedDesiredCapturePoint.scale(scaleFactor); adjustedDesiredCapturePoint.add(localDesiredCapturePoint); adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame()); }