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); } }
/** * Computes linear portion of twist to pack */ 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.getPositionIncludingFrame(desiredPosition); desiredTwist.getLinearPart(desiredVelocity); desiredTwist.getLinearPart(feedForwardLinearAction); compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.setLinearPart(actionFromPositionController.getVector()); }
public void reset() { orientationController.reset(); positionController.reset(); }
@Override public void compute(FrameVector output, FramePoint desiredPosition, FrameVector desiredVelocity, FrameVector currentVelocity, FrameVector feedForward) { computeProportionalTerm(desiredPosition); if (currentVelocity != null) computeDerivativeTerm(desiredVelocity, currentVelocity); computeIntegralTerm(); output.setToNaN(bodyFrame); output.add(proportionalTerm, derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearActionMagnitude = output.length(); if (feedbackLinearActionMagnitude > gains.getMaximumFeedback()) { output.scale(gains.getMaximumFeedback() / feedbackLinearActionMagnitude); } feedbackLinearAction.set(output); rateLimitedFeedbackLinearAction.update(); rateLimitedFeedbackLinearAction.getFrameTuple(output); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
/** * 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 resetIntegrator() { orientationController.resetIntegrator(); positionController.resetIntegrator(); }
@Override public void compute(FrameVector3D output, FramePoint3D desiredPosition, FrameVector3D desiredVelocity, FrameVector3D currentVelocity, FrameVector3D feedForward) { computeProportionalTerm(desiredPosition); if (currentVelocity != null) computeDerivativeTerm(desiredVelocity, currentVelocity); computeIntegralTerm(); output.setToNaN(bodyFrame); output.add(proportionalTerm, derivativeTerm); output.add(integralTerm); // Limit the max acceleration of the feedback, but not of the feedforward... // JEP changed 150430 based on Atlas hitting limit stops. double feedbackLinearActionMagnitude = output.length(); if (feedbackLinearActionMagnitude > gains.getMaximumFeedback()) { output.scale(gains.getMaximumFeedback() / feedbackLinearActionMagnitude); } feedbackLinearAction.set(output); rateLimitedFeedbackLinearAction.update(); output.set(rateLimitedFeedbackLinearAction); feedForward.changeFrame(bodyFrame); output.add(feedForward); }
/** * 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 resetIntegrator() { orientationController.resetIntegrator(); positionController.resetIntegrator(); }
/** * Computes linear portion of twist to pack */ public void compute(Twist twistToPack, FramePose3D desiredPose, TwistReadOnly desiredTwist) { checkBodyFrames(desiredTwist, twistToPack); checkBaseFrames(desiredTwist, twistToPack); checkExpressedInFrames(desiredTwist, twistToPack); twistToPack.setToZero(bodyFrame, desiredTwist.getBaseFrame(), bodyFrame); desiredPosition.setIncludingFrame(desiredPose.getPosition()); desiredVelocity.setIncludingFrame(desiredTwist.getLinearPart()); feedForwardLinearAction.setIncludingFrame(desiredTwist.getLinearPart()); compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.getLinearPart().set(actionFromPositionController); }
/** * 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, 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()); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, null, feedForwardAngularAction); twistToPack.getAngularPart().set(angularActionFromOrientationController); desiredPosition.setIncludingFrame(desiredPose.getPosition()); desiredVelocity.setIncludingFrame(desiredTwist.getLinearPart()); feedForwardLinearAction.setIncludingFrame(desiredTwist.getLinearPart()); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, null, feedForwardLinearAction); twistToPack.getLinearPart().set(actionFromPositionController); }
public void reset() { orientationController.reset(); positionController.reset(); }
public SE3PIDController(String namePrefix, ReferenceFrame bodyFrame, double dt, YoPIDSE3Gains 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); } }
/** * 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(SpatialAcceleration spatialAccelerationToPack, FramePose3D desiredPose, TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkBodyFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkBaseFrames(desiredTwist, feedForwardAcceleration, currentTwist); checkExpressedInFrames(desiredTwist, feedForwardAcceleration, currentTwist); spatialAccelerationToPack.setToZero(bodyFrame, feedForwardAcceleration.getBaseFrame(), bodyFrame); desiredOrientation.setIncludingFrame(desiredPose.getOrientation()); desiredAngularVelocity.setIncludingFrame(desiredTwist.getAngularPart()); feedForwardAngularAction.setIncludingFrame(feedForwardAcceleration.getAngularPart()); currentAngularVelocity.setIncludingFrame(currentTwist.getAngularPart()); orientationController.compute(angularActionFromOrientationController, desiredOrientation, desiredAngularVelocity, currentAngularVelocity, feedForwardAngularAction); spatialAccelerationToPack.getAngularPart().set(angularActionFromOrientationController); desiredPosition.setIncludingFrame(desiredPose.getPosition()); desiredVelocity.setIncludingFrame(desiredTwist.getLinearPart()); feedForwardLinearAction.setIncludingFrame(feedForwardAcceleration.getLinearPart()); currentVelocity.setIncludingFrame(currentTwist.getLinearPart()); positionController.compute(actionFromPositionController, desiredPosition, desiredVelocity, currentVelocity, feedForwardLinearAction); spatialAccelerationToPack.getLinearPart().set(actionFromPositionController); }