public void getCenterOfMassOffset(FramePoint centerOfMassOffsetToPack) { centerOfMassOffsetToPack.setIncludingFrame(expressedInframe, crossPart); centerOfMassOffsetToPack.scale(-1.0 / mass); // comOffset = -1/m * c }
/** * Returns the average of two 3D points. * * @param a the first 3D point. Not modified. * @param b the second 3D point. Not modified. * @param avgToPack the point in which the computed average is stored. Modified. */ public static void averagePoints(FramePoint a, FramePoint b, FramePoint avgToPack) { avgToPack.setIncludingFrame(a); avgToPack.add(b); avgToPack.scale(0.5); }
public static FramePoint average(List<? extends FramePoint> framePoints) { ReferenceFrame frame = framePoints.get(0).getReferenceFrame(); FramePoint nextPoint = new FramePoint(frame); FramePoint average = new FramePoint(framePoints.get(0)); for (int i = 1; i < framePoints.size(); i++) { nextPoint.set(framePoints.get(i)); nextPoint.changeFrame(frame); average.add(nextPoint); } average.scale(1.0 / ((double) framePoints.size())); return average; }
public void compute() { centerOfMass.setToZero(desiredFrame); totalMass = 0.0; for (RigidBody rigidBody : rigidBodies) { rigidBody.getCoMOffset(tempPoint); double mass = rigidBody.getInertia().getMass(); tempPoint.changeFrame(desiredFrame); tempPoint.scale(mass); centerOfMass.add(tempPoint); totalMass += mass; } centerOfMass.scale(1.0 / totalMass); }
/** * 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 computePositionFromMergingMeasurements() { yoRootJointPosition.getFrameTuple(rootJointPosition); imuBasedLinearStateCalculator.updatePelvisPosition(rootJointPosition, pelvisPositionIMUPart); kinematicsBasedLinearStateCalculator.getPelvisPosition(pelvisPositionKinPart); pelvisPositionIMUPart.scale(alphaIMUAgainstKinematicsForPosition.getDoubleValue()); pelvisPositionKinPart.scale(1.0 - alphaIMUAgainstKinematicsForPosition.getDoubleValue()); rootJointPosition.set(pelvisPositionIMUPart); rootJointPosition.add(pelvisPositionKinPart); yoRootJointPosition.set(rootJointPosition); }
public void getPosition(FramePoint positionToPack, double parameter) { double q = parameter; MathTools.checkIfInRange(q, 0.0, 1.0); positionToPack.setToZero(referenceFrame); // c2 * q^2 c2.getFrameTuple(positionToPack); positionToPack.scale(MathTools.square(q)); // c1 * q c1.getFrameTuple(tempPackPosition); tempPackPosition.scale(q); positionToPack.add(tempPackPosition); // c0 c0.getFrameTuple(tempPackPosition2); positionToPack.add(tempPackPosition2); }
curChildCoMScaledByMass.changeFrame(rootFrame); double massToScale = (rigidBodyList.contains(rigidBody) ? rigidBody.getInertia().getMass() : 0.0); curChildCoMScaledByMass.scale(massToScale);
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); } }
protected void checkForCloseWaypoints() { if (waypointsAreCloseTogether()) { FramePoint midpoint = allPositions[waypointIndices[0]].getFramePointCopy(); midpoint.add(allPositions[waypointIndices[1]].getFramePointCopy()); midpoint.scale(0.5); for (int i = 0; i < 2; i++) { allPositions[waypointIndices[i]].set(midpoint); } waypointsAreTheSamePoint = true; } else { waypointsAreTheSamePoint = false; } }
private void computeAngularVelocityStateOutputBlock(Matrix3d rotationFromPelvisToWorld) { tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData()); tempCenterOfMassPosition.changeFrame(estimationFrame); ReferenceFrame referenceFrame = referenceFrameNameMap.getFrameByName(pointVelocityMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointVelocityMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(estimationFrame); tempFramePoint.sub(tempCenterOfMassPosition); tempFramePoint.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint()); tempMatrix.mul(rotationFromPelvisToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(angularVelocityPort)); }
midpoint.set(endPoint); midpoint.add(startPoint); midpoint.scale(0.5); midpoint.add(0.0, 0.0, calculatorParameters.getVerticalBuffer());
private void computeOrientationStateOutputBlock() { estimationFrame.getTransformToDesiredFrame(tempTransform, ReferenceFrame.getWorldFrame()); tempTransform.getRotation(rotationFromEstimationToWorld); tempCenterOfMassPosition.setIncludingFrame(centerOfMassPositionPort.getData()); tempCenterOfMassPosition.changeFrame(estimationFrame); ReferenceFrame referenceFrame = referenceFrameMap.getFrameByName(pointPositionMeasurementInputPort.getData().getBodyFixedReferenceFrameName()); tempFramePoint.setIncludingFrame(referenceFrame, pointPositionMeasurementInputPort.getData().getMeasurementPointInBodyFrame()); tempFramePoint.changeFrame(estimationFrame); tempFramePoint.sub(tempCenterOfMassPosition); tempFramePoint.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFramePoint.getPoint()); tempMatrix.mul(rotationFromEstimationToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort)); }
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); }
adjustedWaypoints.get(1).set(originalWaypoints.get(1)); midGroundPoint.scale(0.5);