@Override public void setDesiredAccelerationToZero() { jointAngularAccelerationDesired.setToZero(jointAngularAccelerationDesired.getReferenceFrame()); }
public static void integrateAngularVelocity(FrameVector angularVelocityToIntegrate, double integrationTime, FrameOrientation orientationResultToPack) { AxisAngle4d axisAngleResult = axisAngleForIntegrator.get(); integrateAngularVelocity(angularVelocityToIntegrate.getVector(), integrationTime, axisAngleResult); orientationResultToPack.setIncludingFrame(angularVelocityToIntegrate.getReferenceFrame(), axisAngleResult); }
/** * Sets the linear velocity part of the spatial motion vector */ public void setLinearPart(FrameVector newLinearVelocity) { expressedInFrame.checkReferenceFrameMatch(newLinearVelocity.getReferenceFrame()); linearPart.set(newLinearVelocity.getVector()); }
/** * Packs an existing FrameVector with the angular part. * The FrameVector must be in the same frame as the expressedInFrame */ public void getAngularPart(FrameVector vectorToPack) { this.getExpressedInFrame().checkReferenceFrameMatch(vectorToPack.getReferenceFrame()); vectorToPack.set(this.angularPart); }
public void setAngularAcceleration(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredAngularAcceleration) { spatialAcceleration.setToZero(bodyFrame, baseFrame, desiredAngularAcceleration.getReferenceFrame()); spatialAcceleration.setAngularPart(desiredAngularAcceleration.getVector()); }
/** * Creates a new FrameVector2d based on the x and y components of this FrameVector */ public FrameVector2d toFrameVector2d() { return new FrameVector2d(this.getReferenceFrame(), this.getX(), this.getY()); }
public void getNormal(FrameVector normalToPack) { checkReferenceFrameMatch(normalToPack.getReferenceFrame()); this.plane3d.getNormal(temporaryVector); normalToPack.set(temporaryVector); }
public void setAngularVelocity(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredAngularVelocity) { spatialVelocity.setToZero(bodyFrame, baseFrame, desiredAngularVelocity.getReferenceFrame()); spatialVelocity.setAngularPart(desiredAngularVelocity.getVector()); setSelectionMatrixForAngularControl(); }
public void setIncludingFrame(FrameVector force, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); }
public FramePlane3d(FrameVector normal, FramePoint point) { normal.checkReferenceFrameMatch(point); this.referenceFrame = normal.getReferenceFrame(); this.plane3d = new Plane3d(point.getPoint(), normal.getVector()); }
public void setDesiredCenterOfMassAcceleration(FrameVector desiredCenterOfMassAcceleration) { checkReferenceFrameMatchByName(centerOfMassAccelerationFrame, desiredCenterOfMassAcceleration.getReferenceFrame()); this.desiredCenterOfMassAcceleration.setIncludingFrame(centerOfMassAccelerationFrame, desiredCenterOfMassAcceleration.getVector()); desiredCenterOfMassAccelerationOutputPort.setData(this.desiredCenterOfMassAcceleration); }
public void setDesiredAngularAcceleration(FrameVector desiredAngularAcceleration) { checkReferenceFrameMatchByName(angularAccelerationEstimationFrame, desiredAngularAcceleration.getReferenceFrame()); this.desiredAngularAcceleration.setIncludingFrame(angularAccelerationEstimationFrame, desiredAngularAcceleration.getVector()); desiredAngularAccelerationOutputPort.setData(this.desiredAngularAcceleration); }
public void setLinearAcceleration(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearAcceleration) { spatialAcceleration.setToZero(bodyFrame, baseFrame, desiredLinearAcceleration.getReferenceFrame()); spatialAcceleration.setLinearPart(desiredLinearAcceleration.getVector()); spatialAcceleration.changeFrameNoRelativeMotion(bodyFrame); }
public static ReferenceFrame constructReferenceFrameFromPointAndZAxis(String frameName, FramePoint point, FrameVector zAxis) { point.checkReferenceFrameMatch(zAxis.getReferenceFrame()); RigidBodyTransform transformToParent = createTransformFromPointAndZAxis(point, zAxis); return constructFrameWithUnchangingTransformToParent(frameName, point.getReferenceFrame(), transformToParent); }
public void setLinearVelocity(ReferenceFrame bodyFrame, ReferenceFrame baseFrame, FrameVector desiredLinearVelocity) { spatialVelocity.setToZero(bodyFrame, baseFrame, desiredLinearVelocity.getReferenceFrame()); spatialVelocity.setLinearPart(desiredLinearVelocity.getVector()); spatialVelocity.changeFrame(bodyFrame); setSelectionMatrixForLinearControl(); }
public void setIncludingFrame(FrameVector force, FrameVector moment, FramePoint pointOfApplication) { force.checkReferenceFrameMatch(pointOfApplication); expressedInFrame = force.getReferenceFrame(); force.get(linearPart); pointOfApplication.get(tempVector); angularPart.cross(tempVector, linearPart); angularPart.add(moment.getVector()); }
@Override public void setTorqueFromWrench(Wrench jointWrench) { jointWrench.getBodyFrame().checkReferenceFrameMatch(successor.getBodyFixedFrame()); jointWrench.getExpressedInFrame().checkReferenceFrameMatch(jointTorque.getReferenceFrame()); jointTorque.set(jointWrench.getAngularPart()); }
private void getCorrectionVelocityForMeasurementFrameOffset(FrameVector correctionTermToPack) { rootJoint.getJointTwist(tempRootJointTwist); tempRootJointTwist.getAngularPart(tempRootJointAngularVelocity); measurementOffset.setToZero(measurementFrame); measurementOffset.changeFrame(rootJoint.getFrameAfterJoint()); correctionTermToPack.setToZero(tempRootJointAngularVelocity.getReferenceFrame()); correctionTermToPack.cross(tempRootJointAngularVelocity, measurementOffset); } }
public void compute(FrameVector outputToPack, FrameOrientation desiredOrientation, FrameVector desiredAngularVelocity, FrameVector feedForwardAngularAcceleration, RigidBody base) { // using twists is a bit overkill; optimize if needed. twistCalculator.getRelativeTwist(endEffectorTwist, base, endEffector); currentAngularVelocity.setToZero(endEffectorTwist.getExpressedInFrame()); endEffectorTwist.getAngularPart(currentAngularVelocity); desiredAngularVelocity.changeFrame(currentAngularVelocity.getReferenceFrame()); feedForwardAngularAcceleration.changeFrame(endEffectorTwist.getExpressedInFrame()); axisAngleOrientationController.compute(outputToPack, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAcceleration); }
public void getAdjustedDesiredCapturePoint(FramePoint2d desiredCapturePoint, FramePoint2d adjustedDesiredCapturePoint) { filteredYoAngularMomentum.getFrameTuple(angularMomentum); ReferenceFrame comFrame = angularMomentum.getReferenceFrame(); localDesiredCapturePoint.setIncludingFrame(desiredCapturePoint); localDesiredCapturePoint.changeFrameAndProjectToXYPlane(comFrame); double scaleFactor = momentumGain.getDoubleValue() * omega0.getDoubleValue() / (totalMass.getDoubleValue() * gravity); adjustedDesiredCapturePoint.setIncludingFrame(comFrame, -angularMomentum.getY(), angularMomentum.getX()); adjustedDesiredCapturePoint.scale(scaleFactor); adjustedDesiredCapturePoint.add(localDesiredCapturePoint); adjustedDesiredCapturePoint.changeFrameAndProjectToXYPlane(desiredCapturePoint.getReferenceFrame()); }