diff --git a/examples/repl.js b/examples/repl.js new file mode 100644 index 0000000..4c8e81e --- /dev/null +++ b/examples/repl.js @@ -0,0 +1,9 @@ +var arDrone = require('ar-drone'); +var autonomy = require('..'); + +var client = arDrone.createClient(); +var ctrl = new autonomy.Controller(client); +var repl = client.createRepl(); + +repl._repl.context['ctrl'] = ctrl; + diff --git a/index.js b/index.js index d05c21c..dbf812a 100644 --- a/index.js +++ b/index.js @@ -3,6 +3,7 @@ var autonomy = exports; exports.StateEstimator = require('./lib/StateEstimator'); exports.EKF = require('./lib/EKF'); exports.Camera = require('./lib/Camera'); +exports.Controller = require('./lib/Controller'); exports.estimateState = function(client, options) { var estimator = new autonomy.StateEstimator(client, options); diff --git a/lib/Controller.js b/lib/Controller.js new file mode 100644 index 0000000..0fcaa69 --- /dev/null +++ b/lib/Controller.js @@ -0,0 +1,236 @@ +var Timers = require('timers'); +var PID = require('./PID'); +var EKF = require('./EKF'); +var Camera = require('./Camera'); + +CONTROL_FREQ = 100; // Interval in ms. + +// Settings for controling translation motion +EPS_LIN = 0.1; // We are ok with 10 cm precision +KP_LIN = 0.15; +KI_LIN = 0.05; +KD_LIN = 0.05; + +// Settings for controlling angular motion +EPS_ANG = 0.05; // We are ok with 0.05 rad precision (2.5 deg) +KP_ANG = 1; +KI_ANG = 0.1; +KD_ANG = 0.1; + +module.exports = Controller; +function Controller(client, options) { + + options = options || {}; + + this._state = options.state || {x: 0, y:0, z:1, yaw: 0}; + this._tag = options.tag || {x: 0, y: 0, yaw: 0}; + this._freq = options.control_freq || CONTROL_FREQ; + this._debug = options.debug || false; + this._pid_x = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN}); + this._pid_y = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN}); + this._pid_z = new PID({kp: KP_LIN, ki: KI_LIN, kd: KD_LIN}); + this._pid_yaw = new PID({kp: KP_ANG, ki: KI_ANG, kd: KD_ANG}); + this._ekf = new EKF(options); + this._camera = new Camera(); + this._enabled = false; + this._client = client; + this._goal = null; + + var self = this; + client.on('navdata', function(d) { + self._processNavdata(d); + }); + + Timers.setInterval(function() { + self._control(); + }, this._freq); +} + +/* + * Enable auto-pilot. The controller will attempt to bring + * the drone (and maintain it) to the goal. + */ +Controller.prototype.enable = function() { + this._enabled = true; +}; + +/* + * Disable auto-pilot. The controller will stop all actions + * and send a stop command to the drone. + */ +Controller.prototype.disable = function() { + this._enabled = false; + this._client.stop(); +} + +/* + * Return the drone state (x,y,z,yaw) as estimated + * by the Kalman Filter. + */ +Controller.prototype.state = function() { + return this._state; +} + +/* + * Sets a new goal and enable the controller. When the goal + * is reached, the callback is called with the current state. + */ +Controller.prototype.go = function(goal, callback) { + // Since we are going to modify goal settings, we + // disable the controller, just in case. + this.disable(); + + // Goals can be incomplete. In this case, we attempt to maintain + // the current state. + var state = this._state; + if (goal.x == undefined) {goal.x = state.x}; + if (goal.y == undefined) {goal.y = state.y}; + if (goal.z == undefined) {goal.z = state.z}; + + // Normalize the yaw, to make sure we don't spin 360deg for + // nothing :-) + if (goal.yaw != undefined) { + var yaw = goal.yaw.toRad(); + goal.yaw = Math.atan2(Math.sin(yaw),Math.cos(yaw)); + } else { + goal.yaw = state.yaw; + } + + // Update our goal + this._goal = goal; + + // Keep track of the callback to trigger when we reach the goal + this._callback = callback; + + // (Re)-Enable the controller + this.enable(); +} + +Controller.prototype._processNavdata = function(d) { + + // EKF prediction step + this._ekf.predict(d); + + // If a tag is detected by the bottom camera, we attempt a correction step + // This require prior configuration of the client to detect the oriented + // roundel and to enable the vision detect in navdata. + // TODO: Add documentation about this + if (d.visionDetect && d.visionDetect.nbDetected > 0) { + // Fetch detected tag position, size and orientation + var xc = d.visionDetect.xc[0] + , yc = d.visionDetect.yc[0] + , wc = d.visionDetect.width[0] + , hc = d.visionDetect.height[0] + , yaw = d.visionDetect.orientationAngle[0] + , dist = d.visionDetect.dist[0] / 100 // Need meters + ; + + // Compute measure tag position (relative to drone) by + // back-projecting the pixel position p(x,y) to the drone + // coordinate system P(X,Y). + // TODO: Should we use dist or the measure altitude ? + var measured = camera.p2m(xc + wc/2, yc + hc/2, dist); + + // Rotation is provided by the drone, we convert to radians + measured.yaw = yaw.toRad(); + + // Execute the EKS correction step + this._ekf.correct(measured, this._tag); + } + + // Keep a local copy of the state + this._state = this._ekf.state(); + this._state.z = d.demo.altitude; +} + +Controller.prototype._control = function() { + // Do not control if not enabled + if (!this._enabled) return; + + // Do not control if no known state or no goal defines + if (this._goal == null || this._state == null) return; + + // Initialize control commands to zero + var ux = uy = uz = uyaw = 0; + + // Compute error between current state and goal + var ex = this._goal.x - this._state.x + , ey = this._goal.y - this._state.y + , ez = this._goal.z - this._state.z + , eyaw = this._goal.yaw - this._state.yaw + ; + + // Compute control commands + if (Math.abs(ex) > EPS_LIN) { + ux = this._pid_x.getCommand(ex); + } + if (Math.abs(ey) > EPS_LIN) { + uy = this._pid_y.getCommand(ey); + } + if (Math.abs(ez) > EPS_LIN) { + uz = this._pid_z.getCommand(ez); + } + if (Math.abs(eyaw) > EPS_ANG) { + uyaw = this._pid_yaw.getCommand(eyaw); + } + + // If all commands are zero, we have reached our destination; + // we go into hover mode and trigger the callback if defined. + if (ux == 0 && uy == 0 && uz == 0 && uyaw == 0) { + this._client.stop(); + + if (this._callback != null) { + var cb = this._callback; + this._callback = null; + this.disable(); + cb(this.state()); + } + } + // Else map controller commands to drone commands + else { + var yaw = this._state.yaw; + var cx = Math.cos(yaw) * ux - Math.sin(yaw) * uy; + var cy = -Math.sin(yaw) * ux + Math.cos(yaw) * uy; + var cz = uz; + var cyaw = uyaw; + + + // Send commands to drone + if (Math.abs(cx) > EPS_LIN) { + if (cx >= 0) { + this._client.front(Math.min(cx, 1)); + } else { + this._client.back(Math.min(Math.abs(cx), 1)); + } + } + + if (Math.abs(cy) > EPS_LIN) { + if (cy >= 0) { + this._client.right(Math.min(cy, 1)); + } else { + this._client.left(Math.min(Math.abs(cy), 1)); + } + } + + if (cz >= 0) { + this._client.up(Math.min(cz, 1)); + } else { + this._client.down(Math.min(Math.abs(cz), 1)); + } + + if (cyaw >= 0) { + this._client.clockwise(Math.min(cyaw, 1)); + } else { + this._client.counterClockwise(Math.min(Math.abs(cyaw), 1)); + } + } + + if (this._debug) { + console.log("--------------------- Control step ----------------------------------------------"); + console.log("Goal: \t %d,%d,%d,%d", this._goal.x, this._goal.y, this._goal.z, this._goal.yaw); + console.log("State:\t %d,%d,%d,%d", this._state.x, this._state.y, this._state.z, this._state.yaw); + console.log("Error:\t %d,%d,%d,%d", ex, ey, ez, eyaw); + console.log("Control: \t%d,%d,%d,%d", ux, uy, uz, uyaw); + console.log("Commands:\t %d,%d,%d,%d", cx, cy, cz, cyaw); + } +} diff --git a/lib/EKF.js b/lib/EKF.js index 83bffed..1b1dbdd 100644 --- a/lib/EKF.js +++ b/lib/EKF.js @@ -7,21 +7,19 @@ var Vector = sylvester.Vector; EKF.DELTA_T = 1 / 15; // In demo mode, 15 navdata per second module.exports = EKF; -function EKF(client, options) { +function EKF(options) { options = options || {}; this._options = options; this._delta_t = options.delta_t || EKF.DELTA_T; - this._state = options.state || {x: 0, y: 0, yaw: 0}; + this._state = options.state || {x: 0, y: 0, z: 1, yaw: 0}; this._tag = options.tag || {x: 0, y: 0, yaw: 0}; this._last_yaw = null; - + this._sigma = Matrix.I(3); this._q = Matrix.Diagonal([0.0003, 0.0003, 0.0001]); this._r = Matrix.Diagonal([0.3, 0.3, 0.1]); - - console.log('Initialize Extended Kalman Filter.'); } EKF.prototype.state = function() { @@ -54,14 +52,14 @@ EKF.prototype.predict = function(data) { var o = {dx: vx * dt, dy: vy * dt, dphi: yaw - this._last_yaw}; this._last_yaw = yaw; - // Update the state estimate + // Update the state estimate var state = this._state; state.x = state.x + o.dx * Math.cos(state.yaw) - o.dy * Math.sin(state.yaw); state.y = state.y + o.dx * Math.sin(state.yaw) + o.dy * Math.cos(state.yaw); state.yaw = state.yaw + o.dphi; // Normalize the yaw value - state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw)); + state.yaw = Math.atan2(Math.sin(state.yaw),Math.cos(state.yaw)); // Compute the G term (due to the Taylor approximation to linearize the function). var G = $M( @@ -89,7 +87,7 @@ EKF.prototype.correct = function(measure, pose) { // Normalized the measure yaw measure.yaw = normAngle(measure.yaw); - + var z1 = Math.cos(psi) * (pose.x - state.x) + Math.sin(psi) * (pose.y - state.y); var z2 = -1 * Math.sin(psi) * (pose.x - state.x) + Math.cos(psi) * (pose.y - state.y); var z3 = pose.yaw - psi; @@ -107,7 +105,7 @@ EKF.prototype.correct = function(measure, pose) { // Compute the Kalman Gain var Ht = H.transpose(); var K = this._sigma.multiply(Ht).multiply(H.multiply(this._sigma).multiply(Ht).add(this._r).inverse()) - + // Correct the pose estimate var err = $V([e1, e2, e3]); var c = K . multiply(err); diff --git a/lib/PID.js b/lib/PID.js new file mode 100644 index 0000000..2bee1b1 --- /dev/null +++ b/lib/PID.js @@ -0,0 +1,47 @@ +DEFAULT_KP = 0.15; +DEFAULT_KI = 0.1; +DEFAULT_KD = 0.1; + +module.exports = PID; +function PID(options) { + this.configure(options || {}); + this.reset(); +} + +PID.prototype.configure = function(options) { + this._kp = options.kp || DEFAULT_KP; + this._ki = options.ki || DEFAULT_KI; + this._kd = options.kd || DEFAULT_KD; +} + +PID.prototype.reset = function() { + this._last_time = Date.now(); + this._last_error = Infinity; + this._error_sum = 0; +} + +PID.prototype.getCommand = function(e) { + // Compute dt in seconds + var time = Date.now(); + var dt = (time - this._last_time) /1000 + + // Compute de (error derivation) + var de = 0; + if (this._last_error < Infinity) { + de = (e - this._last_error) / dt; + } + + // Integrate error + this._error_sum += e * dt; + + // Update our trackers + this._last_time = time; + this._last_error = e; + + // Compute commands + var command = this._kp * e + + this._ki * this._error_sum + + this._kd * de; + + return command; +} diff --git a/tests/mock/client.js b/tests/mock/client.js new file mode 100644 index 0000000..3872f5a --- /dev/null +++ b/tests/mock/client.js @@ -0,0 +1,84 @@ +var EventEmitter = require('events').EventEmitter; +var Timers = require('timers'); +var util = require('util'); + +DT = 30; + +module.exports = Client; +util.inherits(Client, EventEmitter); +function Client(options) { + EventEmitter.call(this); + + options = options || {}; + + this._state = options.state || {x: 0, y: 0, z: 1, yaw: 0}; + this._speed = {vx: 0, vy: 0, vz: 0, vyaw: 0}; + + var self = this; + Timers.setInterval(function() { + self._sendNavdata(); + }, DT); +} + +Client.prototype.front = function(speed) { + this._speed.vx = speed; +} + +Client.prototype.back = function(speed) { + this._speed.vx = -speed; +} + +Client.prototype.right = function(speed) { + this._speed.vy = speed; +} + +Client.prototype.left = function(speed) { + this._speed.vy = -speed; +} + +Client.prototype.up = function(speed) { + this._speed.vz = speed; +} + +Client.prototype.down = function(speed) { + this._speed.vz = -speed; +} + +Client.prototype.clockwise = function(speed) { + this._speed.vyaw = speed; +} + +Client.prototype.counterClockwise = function(speed) { + this._speed.vyaw = -speed; +} + +Client.prototype.stop = function() { + this._speed = {vx: 0, vy: 0, vz: 0, vyaw: 0}; +} + +Client.prototype._sendNavdata = function() { + // First we update the state based on speed + this._state.z = Math.max(0, this._state.z + this._speed.vz); + this._state.yaw = this._state.yaw + this._speed.vyaw; + + var navdata = { + demo: { + rotation: { + pitch: 0, + roll: 0, + yaw: this._state.yaw + }, + velocity: { + x: this._speed.vx * 1000, + y: this._speed.vy * 1000, + z: this._speed.vz * 1000 + }, + altitude: this._state.z + }, + visionDetect: { + nbDetected: 0 + } + }; + + this.emit('navdata', navdata); +} diff --git a/tests/simple.js b/tests/simple.js new file mode 100644 index 0000000..feef5b7 --- /dev/null +++ b/tests/simple.js @@ -0,0 +1,10 @@ +var autonomy = require('..'); +var client = require('./mock/client'); + +var controller = new autonomy.Controller(new client(), {state: {x: 0, y:0, z:1, yaw: Math.PI/4}, debug: true}); + +console.log("State: %j", controller.state()); + +controller.go({x: 0, y: 0, z:1, yaw: -45}, function(state) { + console.log("Reached state %j", state); +});