public void reset() { axisAngleOrientationController.reset(); }
public void setDerivativeGains(double derivativeGainX, double derivativeGainY, double derivativeGainZ) { axisAngleOrientationController.setDerivativeGains(derivativeGainX, derivativeGainY, derivativeGainZ); }
public void setOrientationGains(PID3DGains gains) { orientationController.setGains(gains); } }
/** * Computes using Twists, ignores linear part */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); }
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 setOrientationMaxFeedbackAndFeedbackRate(double maxAcceleration, double maxFeedbackRate) { orientationController.setMaxFeedbackAndFeedbackRate(maxAcceleration, maxFeedbackRate); }
public RigidBodyOrientationControlModule(String namePrefix, RigidBody endEffector, TwistCalculator twistCalculator, double dt, YoOrientationPIDGainsInterface gains, YoVariableRegistry parentRegistry) { this.endEffector = endEffector; this.axisAngleOrientationController = new AxisAngleOrientationController(namePrefix, endEffector.getBodyFixedFrame(), dt, gains, parentRegistry); this.twistCalculator = twistCalculator; }
public void setOrientationMaxDerivativeError(double maxDerivativeError) { orientationController.setMaxDerivativeError(maxDerivativeError); }
public void setOrientationProportionalGains(double kpx, double kpy, double kpz) { orientationController.setProportionalGains(kpx, kpy, kpz); }
public void setOrientationMaxProportionalError(double maxProportionalError) { orientationController.setMaxProportionalError(maxProportionalError); }
/** * Computes using Twists, ignores linear part */ public void compute(Twist twistToPack, FramePose3D desiredPose, TwistReadOnly desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredOrientation.setIncludingFrame(desiredPose.getOrientation()); desiredAngularVelocity.setIncludingFrame(desiredTwist.getAngularPart()); feedForwardAngularAction.setIncludingFrame(desiredTwist.getAngularPart()); compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.getAngularPart().set(angularActionFromOrientationController); }
/** * This method provides a twist feedback controller, intended for spatial velocity control. * @param twistToPack twist to return * @param desiredPose desired pose that we want to achieve. * @param desiredTwist feed forward twist from a reference trajectory */ public void compute(Twist twistToPack, FramePose desiredPose, Twist desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); desiredTwist.getAngularPart(feedForwardAngularAction); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
public void setOrientationMaxFeedbackAndFeedbackRate(double maxAcceleration, double maxFeedbackRate) { orientationController.setMaxFeedbackAndFeedbackRate(maxAcceleration, maxFeedbackRate); }
public SE3PIDController(String namePrefix, ReferenceFrame bodyFrame, double dt, YoSE3PIDGainsInterface gains, YoVariableRegistry registry) { this.bodyFrame = bodyFrame; if (gains != null) { orientationController = new AxisAngleOrientationController(namePrefix, bodyFrame, dt, gains.getOrientationGains(), registry); positionController = new EuclideanPositionController(namePrefix, bodyFrame, dt, gains.getPositionGains(), registry); } else { orientationController = new AxisAngleOrientationController(namePrefix, bodyFrame, dt, registry); positionController = new EuclideanPositionController(namePrefix, bodyFrame, dt, registry); } }
public void setMaxDerivativeError(double maxDerivativeError) { axisAngleOrientationController.setMaxDerivativeError(maxDerivativeError); }
public void setProportionalGains(double proportionalGainX, double proportionalGainY, double proportionalGainZ) { axisAngleOrientationController.setProportionalGains(proportionalGainX, proportionalGainY, proportionalGainZ); }
public void setMaxProportionalError(double maxProportionalError) { axisAngleOrientationController.setMaxProportionalError(maxProportionalError); }
public void setOrientationGains(OrientationPIDGainsInterface gains) { orientationController.setGains(gains); } }
/** * This method provides a spatial acceleration controller. * @param spatialAccelerationToPack spatial acceleration to return. * @param desiredPose desired pose that we want to achieve. * @param desiredTwist desired twist to damp around. * @param feedForwardAcceleration feed forward acceleration from a reference trajectory. * @param currentTwist current twist of the rigid body. */ public void compute(SpatialAccelerationVector spatialAccelerationToPack, FramePose desiredPose, Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkBodyFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkBaseFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkExpressedInFrames(desiredTwist, feedForwardAcceleration, currentTwist); spatialAccelerationToPack.setToZero(bodyFrame, feedForwardAcceleration.getBaseFrame(), bodyFrame); desiredPose.getOrientationIncludingFrame(desiredOrientation); desiredTwist.getAngularPart(desiredAngularVelocity); feedForwardAcceleration.getAngularPart(feedForwardAngularAction); currentTwist.getAngularPart(currentAngularVelocity); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAction); spatialAccelerationToPack.setAngularPart(angularActionFromOrientationController.getVector()); desiredPose.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); feedForwardAcceleration.getLinearPart(feedForwardLinearAction); currentTwist.getLinearPart(currentVelocity); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, currentVelocity, feedForwardLinearAction); spatialAccelerationToPack.setLinearPart(actionFromPositionController.getVector()); }
public void setMaxFeedbackAndFeedbackRate(double maxFeedback, double maxFeedbackRate) { axisAngleOrientationController.setMaxFeedbackAndFeedbackRate(maxFeedback, maxFeedbackRate); }