public void reset() { se3pdController.reset(); }
public void setGains(SE3PIDGainsInterface gains) { se3pdController.setGains(gains); }
public void setOrientationDerivativeGains(double kdx, double kdy, double kdz) { se3pdController.setOrientationDerivativeGains(kdx, kdy, kdz); }
/** * 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()); }
private void checkBodyFrames(TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkBodyFrames(desiredTwist, currentTwist); feedForwardAcceleration.getBodyFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkBaseFrames(TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkBaseFrames(desiredTwist, currentTwist); desiredTwist.getBaseFrame().checkReferenceFrameMatch(feedForwardAcceleration.getBaseFrame()); }
private void checkExpressedInFrames(Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkExpressedInFrames(desiredTwist, currentTwist); feedForwardAcceleration.getExpressedInFrame().checkReferenceFrameMatch(bodyFrame); }
public void setOrientationIntegralGains(double kix, double kiy, double kiz, double maxIntegralError) { se3pdController.setOrientationIntegralGains(kix, kiy, kiz, maxIntegralError); }
public void setOrientationGains(OrientationPIDGainsInterface gains) { se3pdController.setOrientationGains(gains); }
public RigidBodySpatialAccelerationControlModule(String namePrefix, TwistCalculator twistCalculator, RigidBody endEffector, ReferenceFrame endEffectorFrame, double dt, YoSE3PIDGainsInterface taskspaceGains, YoVariableRegistry parentRegistry) { this.registry = new YoVariableRegistry(namePrefix + getClass().getSimpleName()); this.twistCalculator = twistCalculator; this.endEffector = endEffector; this.endEffectorFrame = endEffectorFrame; this.se3pdController = new SE3PIDController(namePrefix, endEffectorFrame, dt, taskspaceGains, registry); this.acceleration = new SpatialAccelerationVector(); parentRegistry.addChild(registry); }
public void doPositionControl(FramePose desiredEndEffectorPose, Twist desiredEndEffectorTwist, SpatialAccelerationVector feedForwardEndEffectorSpatialAcceleration, RigidBody base) { twistCalculator.getRelativeTwist(currentTwist, base, endEffector); currentTwist.changeBodyFrameNoRelativeTwist(endEffectorFrame); currentTwist.changeFrame(endEffectorFrame); se3pdController.compute(acceleration, desiredEndEffectorPose, desiredEndEffectorTwist, feedForwardEndEffectorSpatialAcceleration, currentTwist); }
/** * 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()); }
private void checkBodyFrames(Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkBodyFrames(desiredTwist, currentTwist); feedForwardAcceleration.getBodyFrame().checkReferenceFrameMatch(bodyFrame); }
private void checkBaseFrames(Twist desiredTwist, SpatialAccelerationVector feedForwardAcceleration, Twist currentTwist) { checkBaseFrames(desiredTwist, currentTwist); desiredTwist.getBaseFrame().checkReferenceFrameMatch(feedForwardAcceleration.getBaseFrame()); }
private void checkExpressedInFrames(TwistReadOnly desiredTwist, SpatialAccelerationReadOnly feedForwardAcceleration, TwistReadOnly currentTwist) { checkExpressedInFrames(desiredTwist, currentTwist); feedForwardAcceleration.getReferenceFrame().checkReferenceFrameMatch(bodyFrame); }
/** * 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); }
/** * 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); }