/** * Packs an existing FrameVector with the linear velocity part */ public void getLinearPart(FrameVector vectorToPack) { vectorToPack.setIncludingFrame(expressedInFrame, this.linearPart); }
@Override public void getContactNormalFrameVector(FrameVector frameVectorToPack) { frameVectorToPack.setIncludingFrame(contactNormalFrameVector); }
@Override public void getJointAxis(FrameVector axisToPack) { axisToPack.setIncludingFrame(jointAxis); }
public void setContactNormalVector(FrameVector normalContactVector) { if (normalContactVector == null) { this.contactNormalFrameVector.setIncludingFrame(planeFrame, 0.0, 0.0, 1.0); } else { this.contactNormalFrameVector.setIncludingFrame(normalContactVector); } }
public void get(FrameVector frameVectorToPack) { frameVectorToPack.setIncludingFrame(frameVector); } }
public void getIncludingFrame(FrameOrientation desiredOrientationToPack, FrameVector desiredAngularVelocityToPack, FrameVector feedForwardAngularAccelerationToPack) { desiredOrientationToPack.setIncludingFrame(worldFrame, desiredOrientationInWorld); desiredAngularVelocityToPack.setIncludingFrame(worldFrame, desiredAngularVelocityInWorld); feedForwardAngularAccelerationToPack.setIncludingFrame(worldFrame, feedForwardAngularAccelerationInWorld); }
public void getIncludingFrame(FrameOrientation desiredOrientationToPack, FrameVector desiredAngularVelocityToPack, FrameVector feedForwardAngularAccelerationToPack) { desiredOrientationToPack.setIncludingFrame(worldFrame, desiredOrientationInWorld); desiredAngularVelocityToPack.setIncludingFrame(worldFrame, desiredAngularVelocityInWorld); feedForwardAngularAccelerationToPack.setIncludingFrame(worldFrame, feedForwardAngularAccelerationInWorld); }
public void getIncludingFrame(FramePoint desiredPositionToPack, FrameVector desiredLinearVelocityToPack, FrameVector feedForwardLinearAccelerationToPack) { desiredPositionToPack.setIncludingFrame(worldFrame, desiredPositionInWorld); desiredLinearVelocityToPack.setIncludingFrame(worldFrame, desiredLinearVelocityInWorld); feedForwardLinearAccelerationToPack.setIncludingFrame(worldFrame, feedForwardLinearAccelerationInWorld); }
public void setInitialConditions(FramePoint initialPosition, FrameVector initialVelocity) { this.initialPosition.setIncludingFrame(initialPosition); this.initialVelocity.setIncludingFrame(initialVelocity); }
public static void getSupportVector(FrameVector normalizedSupportVectorToPack, double angle, double mu, ReferenceFrame contactPlaneFrame) { double x = mu * Math.cos(angle); double y = mu * Math.sin(angle); double z = 1.0; normalizedSupportVectorToPack.setIncludingFrame(contactPlaneFrame, x, y, z); normalizedSupportVectorToPack.normalize(); }
/** * Packs an existing FrameVector with the angular part. */ public void getAngularPartIncludingFrame(FrameVector vectorToPack) { vectorToPack.setIncludingFrame(getExpressedInFrame(), this.angularPart); }
/** * Packs an existing FrameVector with the linear part. */ public void getLinearPartIncludingFrame(FrameVector vectorToPack) { vectorToPack.setIncludingFrame(getExpressedInFrame(), linearPart); }
public void setAndMatchFrameLinearMomentumRate(FrameVector linearMomentumRate) { this.linearMomentumRate.setIncludingFrame(linearMomentumRate); this.linearMomentumRate.changeFrame(ReferenceFrame.getWorldFrame()); }
public void run() { estimationJoint.physics.getAngularAccelerationsInBodyFrame(desiredAngularAcceleration); if (CORRUPT_DESIRED_ACCELERATIONS) signalCorruptor.corrupt(desiredAngularAcceleration); desiredAngularAccelerationFrameVector.setIncludingFrame(estimationFrame, desiredAngularAcceleration); desiredCoMAndAngularAccelerationDataSource.setDesiredAngularAcceleration(desiredAngularAccelerationFrameVector); }
private void updateLinearAcceleration() { firstIMU.getLinearAccelerationMeasurement(firstVector); secondIMU.getLinearAccelerationMeasurement(secondVector); firstFrameVector.setIncludingFrame(firstIMU.getMeasurementFrame(), firstVector); secondFrameVector.setIncludingFrame(secondIMU.getMeasurementFrame(), secondVector); firstFrameVector.changeFrame(fusedMeasurementFrame); secondFrameVector.changeFrame(fusedMeasurementFrame); linearAcceleration.add(firstFrameVector, secondFrameVector); linearAcceleration.scale(0.5); }
public void getVelocity(FrameVector velocityToPack) { double parameter = minimumJerkTrajectory.getPosition(); parameter = MathTools.clipToMinMax(parameter, 0.0, 1.0); parabolicTrajectoryGenerator.getVelocity(tempVector, parameter); velocityToPack.setIncludingFrame(tempVector); velocityToPack.scale(minimumJerkTrajectory.getVelocity()); }
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 startComputation() { kinematicPoint.getVelocity(pointVelocity); yoFrameVectorPerfect.set(pointVelocity); corrupt(pointVelocity); yoFrameVectorNoisy.set(pointVelocity); measurementPointInBodyFrame.setIncludingFrame(bodyFrame, kinematicPoint.getOffsetCopy()); velocityOfMeasurementPointInWorldFrame.setIncludingFrame(ReferenceFrame.getWorldFrame(), pointVelocity); boolean isPointVelocityValid = true; pointVelocityDataObject.set(rigidBody, measurementPointInBodyFrame, velocityOfMeasurementPointInWorldFrame, isPointVelocityValid); pointVelocityOutputPort.setData(pointVelocityDataObject); }
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 computeOrientationStateOutputBlock(Matrix3d rotationFromPelvisToWorld) { computeVelocityOfStationaryPoint(tempFrameVector); tempFrameVector.changeFrame(estimationFrame); tempCenterOfMassVelocity.setIncludingFrame(centerOfMassVelocityPort.getData()); tempCenterOfMassVelocity.changeFrame(estimationFrame); tempFrameVector.sub(tempCenterOfMassVelocity); tempFrameVector.scale(-1.0); MatrixTools.toTildeForm(tempMatrix, tempFrameVector.getVector()); tempMatrix.mul(rotationFromPelvisToWorld, tempMatrix); MatrixTools.setDenseMatrixFromMatrix3d(0, 0, tempMatrix, getOutputMatrixBlock(orientationPort)); }