@Override public void update() { if (position == null) { throw new NullPointerException(getClass().getSimpleName() + " must be constructed with a non null " + "position variable to call update(), otherwise use update(double)"); } update(position); }
public void update(FrameTuple3DReadOnly unfilteredFrameTuple3D) { checkReferenceFrameMatch(unfilteredFrameTuple3D); update((Tuple3DReadOnly) unfilteredFrameTuple3D); }
public void update(Tuple3DReadOnly unfilteredTuple3D) { update(unfilteredTuple3D.getX(), unfilteredTuple3D.getY(), unfilteredTuple3D.getZ()); }
private void computeAngularMomentum() { robotMomentum.setToZero(centerOfMassFrame); momentumCalculator.computeAndPack(robotMomentum); robotMomentum.getAngularPartIncludingFrame(angularMomentum); yoAngularMomentum.set(angularMomentum); filteredYoAngularMomentum.update(); }
/** * Updates the different kinematics related stuff that is used to estimate the pelvis state */ public void updateKinematics() { reset(); updateKinematicsNewTwist(); twistCalculator.compute(); for (int i = 0; i < feetRigidBodies.size(); i++) { RigidBody foot = feetRigidBodies.get(i); tempFramePoint.setToZero(rootJointFrame); tempFramePoint.changeFrame(soleFrames.get(foot)); tempFrameVector.setIncludingFrame(tempFramePoint); tempFrameVector.changeFrame(worldFrame); footToRootJointPositions.get(foot).update(tempFrameVector); } kinematicsIsUpToDate.set(true); }
private void computeDesiredSpatialVelocityToSolveFor(DenseMatrix64F spatialDesiredVelocityToPack, DenseMatrix64F spatialVelocityFromError, Twist desiredControlFrameTwist) { DenseMatrix64F selectionMatrix = inverseJacobianSolver.getSelectionMatrix(); // Clip to maximum velocity clipSpatialVector(spatialVelocityFromError, selectionMatrix, maximumTaskspaceAngularVelocityMagnitude.getDoubleValue(), maximumTaskspaceLinearVelocityMagnitude.getDoubleValue()); // Update YoVariables for the velocity getAngularAndLinearPartsFromSpatialVector(yoAngularVelocityFromError, yoLinearVelocityFromError, spatialVelocityFromError); filteredAngularVelocityFromError.update(); filteredLinearVelocityFromError.update(); setSpatialVectorFromAngularAndLinearParts(spatialVelocityFromError, filteredAngularVelocityFromError, filteredLinearVelocityFromError); desiredControlFrameTwist.getMatrix(spatialDesiredVelocityToPack, 0); CommonOps.add(spatialVelocityFromError, spatialDesiredVelocityToPack, spatialDesiredVelocityToPack); }
private void updateWristMeasuredWrench(FrameVector measuredForceToPack, FrameVector measuredTorqueToPack) { momentumBasedController.getWristMeasuredWrenchHandWeightCancelled(measuredWrench, robotSide); measuredWrench.getLinearPartIncludingFrame(tempForceVector); measuredWrench.getAngularPartIncludingFrame(tempTorqueVector); deadzoneMeasuredForce.update(tempForceVector); deadzoneMeasuredTorque.update(tempTorqueVector); filteredMeasuredForce.update(); filteredMeasuredTorque.update(); filteredMeasuredForce.getFrameTupleIncludingFrame(tempForceVector); filteredMeasuredTorque.getFrameTupleIncludingFrame(tempTorqueVector); measuredWrench.setLinearPart(tempForceVector); measuredWrench.setAngularPart(tempTorqueVector); measuredWrench.changeFrame(controlFrame); measuredWrench.getLinearPartIncludingFrame(measuredForceToPack); measuredWrench.getAngularPartIncludingFrame(measuredTorqueToPack); }
private void updateFootSensorRawMeasurement() { for (RobotSide robotSide : RobotSide.values) { FootSwitchInterface footSwitch = footSwitches.get(robotSide); tempWrench.setToZero(footSwitch.getMeasurementFrame(), footSwitch.getMeasurementFrame()); tempFrameVector.setToZero(footSwitch.getMeasurementFrame()); footSwitch.computeAndPackFootWrench(tempWrench); tempWrench.getLinearPart(tempFrameVector); footForcesRaw.get(robotSide).set(tempFrameVector); tempWrench.getAngularPart(tempFrameVector); footTorquesRaw.get(robotSide).set(tempFrameVector); footForcesRawFiltered.get(robotSide).update(); footTorquesRawFiltered.get(robotSide).update(); } }
private void updateFootSensorRawMeasurement() { for (RobotSide robotSide : RobotSide.values) { FootSwitchInterface footSwitch = footSwitches.get(robotSide); tempWrench.setToZero(footSwitch.getMeasurementFrame(), footSwitch.getMeasurementFrame()); tempFrameVector.setToZero(footSwitch.getMeasurementFrame()); footSwitch.computeAndPackFootWrench(tempWrench); tempFrameVector.set(tempWrench.getLinearPart()); footForcesRaw.get(robotSide).set(tempFrameVector); tempFrameVector.set(tempWrench.getAngularPart()); footTorquesRaw.get(robotSide).set(tempFrameVector); footForcesRawFiltered.get(robotSide).update(); footTorquesRawFiltered.get(robotSide).update(); } }
filteredVector.update(unfilteredVector); xFiltered.update(unfilteredVector.getX()); yFiltered.update(unfilteredVector.getY());