box2djsの作法 その9

概要

box2djsの作法、調べてみた。
倒立振子、やってみた。

写真

サンプルコード

function KalmanFilter(rate, pnoise, mnoise) {
  this.F = math.matrix([[1, rate], [0, 1]]);
  this.G = math.matrix([[math.square(rate) / 2], [rate]]);
  this.H = math.matrix([1, 0]);
  this.Q = math.multiply(math.multiply(this.G, math.transpose(this.G)), pnoise);
  this.R = mnoise;
  this.P = this.Q;
}
KalmanFilter.prototype.update = function(m) {
	if (!this.X)
  {
		this.X = math.matrix([[m], [0]]);
	}
	this.X = math.multiply(this.F, this.X);
	this.P = math.add(math.multiply(math.multiply(this.F, this.P), math.transpose(this.F)), this.Q); 
	var K = math.multiply(math.multiply(this.P, math.transpose(this.H)), math.inv(math.add(math.multiply(math.multiply(this.H, this.P), math.transpose(this.H)), this.R)));
	this.X = math.add(this.X, math.multiply(K, math.subtract(m, math.multiply(this.H, this.X))));
	this.P = math.multiply(math.subtract(math.eye(2), math.multiply(K, this.H)), this.P);
	return this.X._data[0];
}

var Agent = function() {
	this.err2 = 0;
  this.err = 0;
  var rate = 0.5;
  var pnoise = 0.1;
  var mnoise = 0.11;
  this.kf = new KalmanFilter(rate, pnoise, mnoise);
};
Agent.prototype.get_action = function(input, reward, done) {
  var setPoint = 0;
  var dt = 10;
  var Kp = 0.03;
  var Kd = 0.2;
  var input2;
	input2 = this.kf.update(input);
	var error = setPoint - input2;
	var u = Kp * error - (Kd * (input2 - this.err2)) / dt;
	this.err2 = this.err;
	this.err = input;
	return u;
}

var Agent3 = function() {
	this.err2 = 0;
  this.err = 0;
};
Agent3.prototype.get_action = function(input, reward, done) {
  var setPoint = 0;
  var dt = 10;
  var Kp = 0.03;
  var Kd = 0.2;
  var input2;
  //input2 = (this.err + input) / 2;
	input2 = this.err * 0.2 + input * 0.8;
	//input2 = input;
	var error = setPoint - input2;
	var u = Kp * error - (Kd * (input2 - this.err2)) / dt;
	this.err2 = this.err;
	this.err = input;
	return u;
}

var Agent2 = function() {
	this.lastErr = 0;
  this.errSum = 0;
};
Agent2.prototype.get_action = function(observation, reward, done) {
  var kp = 0.03;
  var ki = 0.0001;
  var kd = 0.00001;
  var timeChange = 10;
  var error = - observation;
  this.errSum += error / timeChange;　
  var dErr = (error - this.lastErr) / timeChange;　
  var u = kp * error + ki * this.errSum + kd * dErr;
  this.lastErr = error;
  return u;
}

var Agent1 = function() {
	this.Setpoint = 0;
  this.lastInput = 0;
  this.outputSum = 0;
  this.kp = 0.02;
  this.ki = 0.001;
  this.kd = 0.00005;
};
Agent1.prototype.get_action = function(observation, reward, done) {
  var output;
  var error = this.Setpoint - observation;
  var dInput = (observation - this.lastInput);
  //this.outputSum += (this.ki * error);
  this.outputSum -= this.kp * dInput;	
  output = this.kp * error;
  output += this.outputSum - this.kd * dInput;
  this.lastInput = observation;
	return output;
}

var Agent0 = function() {
	this.errd2 = 0;
  this.errd = 0;
  this.setd = 0;
  this.Kp = 0.03;
  this.Kd = 0.2;
	this.error;
	this.u;
  this.dt = 10;
  this.setPoint = 0;
};
Agent0.prototype.get_action = function(observation, reward, done) {
  this.error = this.setPoint - observation;
	this.u = this.Kp * this.error + (this.Kd * (this.setPoint - this.setd) - this.Kd * (observation - this.errd2)) / this.dt;
	this.errd2 = this.errd;
	this.errd = observation;
	this.setd = this.setPoint;
	return this.u;
}


function drawWorld(world, context) {
	for (var j = world.m_jointList; j; j = j.m_next) 
  {
		drawJoint(j, context);
	}
	for (var b = world.m_bodyList; b; b = b.m_next)
  {
		for (var s = b.GetShapeList(); s != null; s = s.GetNext()) 
    {
			drawShape(s, context);
		}
	}
}
function drawJoint(joint, context) {
	var b1 = joint.m_body1;
	var b2 = joint.m_body2;
	var x1 = b1.m_position;
	var x2 = b2.m_position;
	var p1 = joint.GetAnchor1();
	var p2 = joint.GetAnchor2();
	context.strokeStyle = '#00eeee';
	context.beginPath();
	switch (joint.m_type)
  {
	case b2Joint.e_distanceJoint:
		context.moveTo(p1.x, p1.y);
		context.lineTo(p2.x, p2.y);
	break;
	case b2Joint.e_pulleyJoint:
	break;
	default:
		if (b1 == world.m_groundBody)
    {
			context.moveTo(p1.x, p1.y);
			context.lineTo(x2.x, x2.y);
		}
		else if (b2 == world.m_groundBody)
    {
			context.moveTo(p1.x, p1.y);
			context.lineTo(x1.x, x1.y);
		}
		else
    {
			context.moveTo(x1.x, x1.y);
			context.lineTo(p1.x, p1.y);
			context.lineTo(x2.x, x2.y);
			context.lineTo(p2.x, p2.y);
		}
	break;
	}
	context.stroke();
}
function drawShape(shape, context) {
	context.strokeStyle = '#ff0000';
	context.beginPath();
	switch (shape.m_type) 
  {
	case b2Shape.e_circleShape:
	{
			var circle = shape;
			var pos = circle.m_position;
			var r = circle.m_radius;
			var segments = 16.0;
			var theta = 0.0;
			var dtheta = 2.0 * Math.PI / segments;
			context.moveTo(pos.x + r, pos.y);
			for (var i = 0; i < segments; i++)
      {
				var d = new b2Vec2(r * Math.cos(theta), r * Math.sin(theta));
				var v = b2Math.AddVV(pos, d);
				context.lineTo(v.x, v.y);
				theta += dtheta;
			}
			context.lineTo(pos.x + r, pos.y);
			context.moveTo(pos.x, pos.y);
			var ax = circle.m_R.col1;
			var pos2 = new b2Vec2(pos.x + r * ax.x, pos.y + r * ax.y);
			context.lineTo(pos2.x, pos2.y);
	}
	break;
	case b2Shape.e_polyShape:
	{
			var poly = shape;
			var tV = b2Math.AddVV(poly.m_position, b2Math.b2MulMV(poly.m_R, poly.m_vertices[0]));
			context.moveTo(tV.x, tV.y);
			for (var i = 0; i < poly.m_vertexCount; i++)
      {
				var v = b2Math.AddVV(poly.m_position, b2Math.b2MulMV(poly.m_R, poly.m_vertices[i]));
				context.lineTo(v.x, v.y);
			}
			context.lineTo(tV.x, tV.y);
	}
	break;
	}
	context.stroke();
}
var worldAABB = new b2AABB();	
worldAABB.minVertex.Set(-1000, -1000);
worldAABB.maxVertex.Set(1000, 1000);
var gravity = new b2Vec2(0, 300);
var doSleep = false;		
var world = new b2World(worldAABB, gravity, doSleep);
var wheelSd = new b2CircleDef();
wheelSd.density = 1.0;
wheelSd.radius = 40;
var wheelBd = new b2BodyDef();
wheelBd.AddShape(wheelSd);
wheelBd.position.Set(200, 290);
var wheel = world.CreateBody(wheelBd);
var carSd = new b2BoxDef();
carSd.density = 1.0;
carSd.extents.Set(20, 60);
var carBd = new b2BodyDef();
carBd.AddShape(carSd);
carBd.position.Set(200, 250);
var car = world.CreateBody(carBd);
var pinJd = new b2RevoluteJointDef();
pinJd.body1 = wheel;
pinJd.body2 = car;
pinJd.anchorPoint = wheel.GetCenterPosition();
pinJd.enableMotor = true;
pinJd.motorTorque = 100000000;
pinJd.motorSpeed = -0.9;
var frontPin = world.CreateJoint(pinJd);
var groundSd = new b2BoxDef();
groundSd.extents.Set(2000, 50);
groundSd.restitution = 0.2;
var groundBd = new b2BodyDef();
groundBd.AddShape(groundSd);
groundBd.position.Set(-500, 400);
var ground = world.CreateBody(groundBd);
Event.observe(window, 'load', function(e) {
  var context = document.querySelector('#c').getContext('2d');
	var timeStep = 1.0 / 30;
	var iteration = 1;
  var agent = new Agent();
	setInterval(function() {
		context.clearRect(0, 0, 400, 400);
		world.Step(timeStep, iteration);
		drawWorld(world, context);
    var observation = car.m_linearVelocity.x;
    var reward = 1.0;
    var done = false;
    var u = agent.get_action(observation, reward, done);
    frontPin.SetMotorSpeed(u);
    var str = "observation: " + observation + "<br> action: " + u;
    document.getElementById('helloWorld').innerHTML = str;
	}, timeStep);
});

成果物

以上。