protected void putYoValuesIntoWrench() { wrench.setToZero(bodyFrame, expressedInFrame); wrench.setLinearPart(linearPart.getFrameTuple()); wrench.setAngularPart(angularPart.getFrameTuple()); }
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 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); }
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); }
tmpWrench.setAngularPart(tempFrameVector); tmpWrench.changeBodyFrameAttachedToSameBody(controlledBody.getBodyFixedFrame()); tmpWrench.changeFrame(ReferenceFrame.getWorldFrame());
public void computeDynamicWrenchInBodyCoordinates(SpatialAccelerationVector acceleration, Twist twist, Wrench dynamicWrenchToPack) // TODO: write test { checkExpressedInBodyFixedFrame(); checkIsCrossPartZero(); // otherwise this operation would be a lot less efficient acceleration.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); acceleration.getBaseFrame().checkIsWorldFrame(); acceleration.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); twist.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); twist.getBaseFrame().checkIsWorldFrame(); twist.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); dynamicWrenchToPack.getBodyFrame().checkReferenceFrameMatch(this.bodyFrame); dynamicWrenchToPack.getExpressedInFrame().checkReferenceFrameMatch(this.expressedInframe); massMomentOfInertiaPart.transform(acceleration.getAngularPart(), tempVector); // J * omegad dynamicWrenchToPack.setAngularPart(tempVector); // [J * omegad; 0] massMomentOfInertiaPart.transform(twist.getAngularPart(), tempVector); // J * omega tempVector.cross(twist.getAngularPart(), tempVector); // omega x J * omega dynamicWrenchToPack.addAngularPart(tempVector); // [J * omegad + omega x J * omega; 0] tempVector.set(acceleration.getLinearPart()); // vd tempVector.scale(mass); // m * vd dynamicWrenchToPack.setLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd] tempVector.set(twist.getLinearPart()); // v tempVector.scale(mass); // m * v tempVector.cross(twist.getAngularPart(), tempVector); // omega x m * v dynamicWrenchToPack.addLinearPart(tempVector); // [J * omegad + omega x J * omega; m * vd + omega x m * v] }