plunkerで倒立振子

# 概要

plunkerで倒立振子やってみた。
box2dで、剛体シミュレーションやってみた。
倒立振子をPID制御した。
カルマンフィルター使ってみた。

# 写真

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/18104/3d251352-79d8-d336-03be-aec787bf5bd1.png)

# サンプルコード

```
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;
}
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);
});


```

# 成果物

https://embed.plnkr.co/JTq3vqyhx2WtFLhT1L0Z/

以上。