private int filterTrustedFeetBasedOnContactForces(int numberOfEndEffectorsTrusted) { double totalForceZ = 0.0; for (int i = 0; i < feet.size(); i++) { RigidBody foot = feet.get(i); Wrench footWrench = footWrenches.get(foot); footSwitches.get(foot).computeAndPackFootWrench(footWrench); totalForceZ += footWrench.getLinearPartZ(); } for (int i = 0; i < feet.size(); i++) { RigidBody foot = feet.get(i); Wrench footWrench = footWrenches.get(foot); footForcesZInPercentOfTotalForce.get(foot).set(footWrench.getLinearPartZ() / totalForceZ); if (footForcesZInPercentOfTotalForce.get(foot).getDoubleValue() < forceZInPercentThresholdToFilterFoot.getDoubleValue()) { numberOfEndEffectorsTrusted--; areFeetTrusted.get(foot).set(false); return numberOfEndEffectorsTrusted; } } return numberOfEndEffectorsTrusted; }
@Override public void getTauMatrix(DenseMatrix64F matrix) { matrix.set(0, 0, successorWrench.getAngularPartY()); matrix.set(1, 0, successorWrench.getLinearPartX()); matrix.set(2, 0, successorWrench.getLinearPartZ()); }
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); } }
public void getWrench(Wrench wrenchToPack) { wrenchToPack.setToZero(successorWrench.getBodyFrame(), successorWrench.getExpressedInFrame()); wrenchToPack.setAngularPartY(successorWrench.getAngularPartY()); wrenchToPack.setLinearPartX(successorWrench.getLinearPartX()); wrenchToPack.setLinearPartZ(successorWrench.getLinearPartZ()); }
protected void getYoValuesFromWrench() { linearPart.set(wrench.getExpressedInFrame(), wrench.getLinearPartX(), wrench.getLinearPartY(), wrench.getLinearPartZ()); angularPart.set(wrench.getExpressedInFrame(), wrench.getAngularPartX(), wrench.getAngularPartY(), wrench.getAngularPartZ()); }
public void update(Wrench sensorWrench) { sensorWrench.changeFrame(world); yoSensorForce.set(sensorWrench.getExpressedInFrame(), sensorWrench.getLinearPartX(), sensorWrench.getLinearPartY(), sensorWrench.getLinearPartZ()); yoSensorTorque.set(sensorWrench.getExpressedInFrame(), sensorWrench.getAngularPartX(), sensorWrench.getAngularPartY(), sensorWrench.getAngularPartZ()); if (addSimulatedSensorNoise.getBooleanValue()) { double amp = 0.1; double bias = 0.25; yoSensorForce.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); yoSensorTorque.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); } updateSensorPosition(); updateCenterOfMass(); yoSensorToDistalCoMvectorInWorld.sub(distalCoMInWorld, yoSensorPositionInWorld); distalMassWrench.setToZero(world); distalMassWrench.setUsingArm(world, distalMassForceInWorld.getFrameTuple().getVector(), yoSensorToDistalCoMvectorInWorld.getFrameTuple().getVector()); yoSensorForceFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getLinearPartX(), distalMassWrench.getLinearPartY(), distalMassWrench.getLinearPartZ()); yoSensorTorqueFromDistalMass.set(distalMassWrench.getExpressedInFrame(), distalMassWrench.getAngularPartX(), distalMassWrench.getAngularPartY(), distalMassWrench.getAngularPartZ()); yoSensorForceMassCompensated.sub(yoSensorForce, yoSensorForceFromDistalMass); yoSensorTorqueMassCompensated.sub(yoSensorTorque, yoSensorTorqueFromDistalMass); }
public void setWrench(Wrench jointWrench) { successorWrench.getBodyFrame().checkReferenceFrameMatch(jointWrench.getBodyFrame()); successorWrench.getExpressedInFrame().checkReferenceFrameMatch(jointWrench.getExpressedInFrame()); successorWrench.setAngularPartY(jointWrench.getAngularPartY()); successorWrench.setLinearPartX(jointWrench.getLinearPartX()); successorWrench.setLinearPartZ(jointWrench.getLinearPartZ()); }
public void updateCoP() { readSensorData(footWrench); if (Math.abs(footWrench.getLinearPartZ()) < MIN_FORCE_TO_COMPUTE_COP) { yoResolvedCoP.setToNaN(); resolvedCoP3d.setToNaN(ReferenceFrame.getWorldFrame()); resolvedCoP.setToNaN(); } else { copResolver.resolveCenterOfPressureAndNormalTorque(resolvedCoP, footWrench, contactablePlaneBody.getSoleFrame()); yoResolvedCoP.set(resolvedCoP); resolvedCoP3d.setToZero(resolvedCoP.getReferenceFrame()); resolvedCoP3d.setXY(resolvedCoP); resolvedCoP3d.changeFrame(ReferenceFrame.getWorldFrame()); } }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.setToZero(successor.getBodyFixedFrame(), successorWrench.getExpressedInFrame()); jointWrench.setAngularPartY(successorWrench.getAngularPartY()); jointWrench.setLinearPartX(successorWrench.getLinearPartX()); jointWrench.setLinearPartZ(successorWrench.getLinearPartZ()); jointWrench.changeFrame(successor.getBodyFixedFrame()); }
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); }
public void control(SpatialAccelerationVector handSpatialAccelerationVector, Wrench toolWrench) { if (!hasBeenInitialized) { update(); initialize(); } update(); toolAcceleration.set(handSpatialAccelerationVector); toolAcceleration.changeFrameNoRelativeMotion(toolJoint.getFrameAfterJoint()); // TODO: Take relative acceleration between uTorsoCoM and elevator in account toolAcceleration.changeBaseFrameNoRelativeAcceleration(elevatorFrame); toolAcceleration.changeBodyFrameNoRelativeAcceleration(toolJoint.getFrameAfterJoint()); toolJoint.setDesiredAcceleration(toolAcceleration); inverseDynamicsCalculator.compute(); inverseDynamicsCalculator.getJointWrench(toolJoint, toolWrench); toolWrench.negate(); toolWrench.changeFrame(handFixedFrame); toolWrench.changeBodyFrameAttachedToSameBody(handFixedFrame); // Visualization temporaryVector.setIncludingFrame(handFixedFrame, toolWrench.getLinearPartX(), toolWrench.getLinearPartY(), toolWrench.getLinearPartZ()); temporaryVector.changeFrame(ReferenceFrame.getWorldFrame()); temporaryVector.scale(0.01); objectForceInWorld.set(temporaryVector); }