/** * Put the constant CMP in the middle of the two given footsteps. * @param constantCMPToPack YoFramePoint that will be packed with the constant CMP location * @param firstFootstep FramePoint holding the position of the first footstep * @param secondFootstep FramePoint holding the position of the second footstep */ public static void putConstantCMPBetweenFeet(YoFramePoint constantCMPToPack, FramePoint firstFootstep, FramePoint secondFootstep) { constantCMPToPack.setAndMatchFrame(firstFootstep); constantCMPToPack.add(secondFootstep); constantCMPToPack.scale(0.5); }
/** * 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); }
private void updateCurrents() { if (numberOfFeetTrusted.getIntegerValue() == numberOfFeet) { for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); footPosition.setToZero(footSoleFrames.get(foot)); currentFootPositions.get(foot).setAndMatchFrame(footPosition); } currentAverageFootPosition.setToZero(); for (int i = 0; i < numberOfFeet; i++) { RigidBody foot = allFeet.get(i); currentAverageFootPosition.add(currentFootPositions.get(foot)); } currentAverageFootPosition.scale(1.0 / numberOfFeet); } else { for (int i = 0; i < numberOfFeet; i++) currentFootPositions.get(allFeet.get(i)).setToNaN(); currentAverageFootPosition.setToNaN(); } }
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); }
public void add(YoFramePose yoFramePose) { getPosition().add(yoFramePose.getPosition()); getOrientation().add(yoFramePose.getOrientation()); }
referenceAverageFootPosition.add(referenceFootPositions.get(foot));
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); } }
copPositionsInWorld.get(trustedFoot).add(tempCoPOffset);
private void setAccelerationEndpointPositions() { for (int i : new int[] {0, 1}) { FrameVector waypointToEndpoint = getWaypointToEndpoint(i); FrameVector oppositeWaypointToEndpoint = getOppositeWaypointToEndpoint(i); double scaleFactor = waypointToEndpoint.dot(oppositeWaypointToEndpoint) / oppositeWaypointToEndpoint.length() * linearSplineLengthFactor.getDoubleValue(); oppositeWaypointToEndpoint.normalize(); oppositeWaypointToEndpoint.scale(scaleFactor); allPositions[accelerationEndpointIndices[i]].set(allPositions[waypointIndices[i]].getFramePointCopy()); allPositions[accelerationEndpointIndices[i]].add(oppositeWaypointToEndpoint); } }
yoForcePointPosition.add(amp * 2.0 * (Math.random() - 0.5) + bias, amp * 2.0 * (Math.random() - 0.5) + bias, amp * 2.0 * (Math.random() - 0.5) + bias);