public void reset() { for (int i = 0; i < listOfBodiesWithExternalWrenches.size(); i++) { Wrench externalWrench = externalWrenches.get(listOfBodiesWithExternalWrenches.get(i)); externalWrench.setToZero(externalWrench.getBodyFrame(), externalWrench.getExpressedInFrame()); } }
public void reset() { for (int i = 0; i < contactablePlaneBodies.size(); i++) { RigidBody rigidBody = contactablePlaneBodies.get(i).getRigidBody(); externalWrenches.get(rigidBody).setToZero(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame()); } for (int i = 0; i < rigidBodiesWithWrenchToCompensateFor.size(); i++) { RigidBody rigidBody = rigidBodiesWithWrenchToCompensateFor.get(i); externalWrenches.get(rigidBody).setToZero(rigidBody.getBodyFixedFrame(), rigidBody.getBodyFixedFrame()); } }
@Override public void computeAndPackFootWrench(Wrench footWrenchToPack) { footWrenchToPack.setToZero(); if (hasFootHitGround()) footWrenchToPack.setLinearPartZ(totalRobotWeight / 4.0); }
@Override public void computeAndPackFootWrench(Wrench footWrenchToPack) { footWrenchToPack.setToZero(); if (hasFootHitGround()) footWrenchToPack.setLinearPartZ(totalRobotWeight); }
public Map<RigidBody, Wrench> computeWrenchesFromRho(DenseMatrix64F rho) { // Reinintialize wrenches for (int i = 0; i < rigidBodies.size(); i++) { RigidBody rigidBody = rigidBodies.get(i); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchesFromRho.get(rigidBody).setToZero(bodyFixedFrame, bodyFixedFrame); } int rhoStartIndex = 0; for (int i = 0; i < rigidBodies.size(); i++) { RigidBody rigidBody = rigidBodies.get(i); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); PlaneContactStateToWrenchMatrixHelper helper = planeContactStateToWrenchMatrixHelpers.get(rigidBody); helper.computeWrenchFromRho(rhoStartIndex, rho); Wrench wrenchFromRho = helper.getWrenchFromRho(); wrenchFromRho.changeFrame(bodyFixedFrame); wrenchesFromRho.get(rigidBody).add(wrenchFromRho); rhoStartIndex += helper.getRhoSize(); } return wrenchesFromRho; }
public void computeTotalWrench(Wrench totalGroundReactionWrenchToPack, Collection<Wrench> wrenches, ReferenceFrame referenceFrame) { totalGroundReactionWrenchToPack.setToZero(referenceFrame, referenceFrame); for (Wrench wrench : wrenches) { temporaryWrench.set(wrench); temporaryWrench.changeFrame(referenceFrame); temporaryWrench.changeBodyFrameAttachedToSameBody(referenceFrame); totalGroundReactionWrenchToPack.add(temporaryWrench); } } }
@Override public void computeAndPackFootWrench(Wrench footWrenchToPack) { updateMeasurement(); ReferenceFrame bodyFixedFrame = contactablePlaneBody.getRigidBody().getBodyFixedFrame(); footWrenchToPack.setToZero(bodyFixedFrame, measurementFrame); footWrenchToPack.setLinearPart(measuredForce); }
protected void putYoValuesIntoWrench() { wrench.setToZero(bodyFrame, expressedInFrame); wrench.setLinearPart(linearPart.getFrameTuple()); wrench.setAngularPart(angularPart.getFrameTuple()); }
public void getWristRawMeasuredWrench(Wrench wrenchToPack, RobotSide robotSide) { if (wristRawMeasuredForces == null || wristRawMeasuredTorques == null) return; wristRawMeasuredForces.get(robotSide).getFrameTupleIncludingFrame(tempWristForce); wristRawMeasuredTorques.get(robotSide).getFrameTupleIncludingFrame(tempWristTorque); ReferenceFrame measurementFrames = wristForceSensorMeasurementFrames.get(robotSide); wrenchToPack.setToZero(measurementFrames, measurementFrames); wrenchToPack.setLinearPart(tempWristForce); wrenchToPack.setAngularPart(tempWristTorque); }
@Override public void processDataAtControllerRate() { logDataProcessorHelper.update(); admissibleGroundReactionWrench.setToZero(centerOfMassFrame, centerOfMassFrame); admissibleDesiredGroundReactionTorque.getFrameTupleIncludingFrame(tempVector); admissibleGroundReactionWrench.setAngularPart(tempVector); admissibleDesiredGroundReactionForce.getFrameTupleIncludingFrame(tempVector); admissibleGroundReactionWrench.setLinearPart(tempVector); centerOfPressureResolver.resolveCenterOfPressureAndNormalTorque(tempCoP, admissibleGroundReactionWrench, worldFrame); admissibleDesiredCenterOfPressure.set(tempCoP); }
public void getWristMeasuredWrenchHandWeightCancelled(Wrench wrenchToPack, RobotSide robotSide) { if (wristForcesHandWeightCancelled == null || wristTorquesHandWeightCancelled == null) return; wristForcesHandWeightCancelled.get(robotSide).getFrameTupleIncludingFrame(tempWristForce); wristTorquesHandWeightCancelled.get(robotSide).getFrameTupleIncludingFrame(tempWristTorque); ReferenceFrame measurementFrames = wristForceSensorMeasurementFrames.get(robotSide); wrenchToPack.setToZero(measurementFrames, measurementFrames); wrenchToPack.setLinearPart(tempWristForce); wrenchToPack.setAngularPart(tempWristTorque); }
public void getWrench(Wrench wrenchToPack) { wrenchToPack.setToZero(successorWrench.getBodyFrame(), successorWrench.getExpressedInFrame()); wrenchToPack.setAngularPartY(successorWrench.getAngularPartY()); wrenchToPack.setLinearPartX(successorWrench.getLinearPartX()); wrenchToPack.setLinearPartZ(successorWrench.getLinearPartZ()); }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = wristsubtreeCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempForce.setIncludingFrame(worldFrame, 0.0, 0.0, -wristSubtreeMass.get(robotSide).getDoubleValue() * gravity); tempForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }
private void cancelHandWeight(RobotSide robotSide, Wrench wrenchToSubstractHandWeightTo, ReferenceFrame measurementFrame) { CenterOfMassReferenceFrame handCoMFrame = handCenterOfMassFrames.get(robotSide); handCoMFrame.update(); tempWristForce.setIncludingFrame(worldFrame, 0.0, 0.0, -handsMass.get(robotSide).getDoubleValue() * gravity); tempWristForce.changeFrame(handCoMFrame); wristWrenchDueToGravity.setToZero(measurementFrame, handCoMFrame); wristWrenchDueToGravity.setLinearPart(tempWristForce); wristWrenchDueToGravity.changeFrame(measurementFrame); wrenchToSubstractHandWeightTo.sub(wristWrenchDueToGravity); }
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); }
@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 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(); } }
public void computeWrenchFromRho(int startIndex, DenseMatrix64F allRobotRho) { CommonOps.extract(allRobotRho, startIndex, startIndex + rhoSize, 0, 1, rhoMatrix, 0, 0); yoRho.set(rhoMatrix); if (yoPlaneContactState.inContact()) { totalWrenchMatrix.zero(); for (int rhoIndex = 0; rhoIndex < rhoSize; rhoIndex++) { double rho = rhoMatrix.get(rhoIndex, 0); CommonOps.extract(rhoJacobianMatrix, 0, SpatialForceVector.SIZE, rhoIndex, rhoIndex + 1, singleRhoWrenchMatrix, 0, 0); MatrixTools.addMatrixBlock(totalWrenchMatrix, 0, 0, singleRhoWrenchMatrix, 0, 0, SpatialForceVector.SIZE, 1, rho); } RigidBody rigidBody = yoPlaneContactState.getRigidBody(); ReferenceFrame bodyFixedFrame = rigidBody.getBodyFixedFrame(); wrenchFromRho.set(bodyFixedFrame, centerOfMassFrame, totalWrenchMatrix); CommonOps.mult(copJacobianMatrix, rhoMatrix, previousCoPMatrix); previousCoP.setX(previousCoPMatrix.get(0, 0)); previousCoP.setY(previousCoPMatrix.get(1, 0)); } else { wrenchFromRho.setToZero(); } }
private void handleSpatialAccelerationCommand(SpatialAccelerationCommand command) { RigidBody controlledBody = command.getEndEffector(); SpatialAccelerationVector accelerationVector = command.getSpatialAcceleration(); accelerationVector.changeBaseFrameNoRelativeAcceleration(ReferenceFrame.getWorldFrame()); twistCalculator.getTwistOfBody(tmpTwist, controlledBody); tmpWrench.setToZero(accelerationVector.getBodyFrame(), accelerationVector.getExpressedInFrame()); conversionInertias.get(controlledBody).computeDynamicWrenchInBodyCoordinates(accelerationVector, tmpTwist, tmpWrench); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame()); VirtualWrenchCommand virtualWrenchCommand = new VirtualWrenchCommand(); virtualWrenchCommand.set(controlledBody, tmpWrench, command.getSelectionMatrix()); virtualWrenchCommandList.addCommand(virtualWrenchCommand); if (controlledBody == controlRootBody) { tmpExternalWrench.set(tmpWrench); tmpExternalWrench.negate(); tmpExternalWrench.changeFrame(controlledBody.getBodyFixedFrame()); optimizationControlModule.submitExternalWrench(controlledBody, tmpExternalWrench); } optimizationControlModule.addSelection(command.getSelectionMatrix()); }
tempFrameVector.scale(torque); tmpWrench.setToZero(tempFrameVector.getReferenceFrame(), tempFrameVector.getReferenceFrame()); tmpWrench.setAngularPart(tempFrameVector); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame());