mirror of
https://github.com/wassname/rl_2d_walker.js.git
synced 2026-08-24 12:22:21 +08:00
684 lines
24 KiB
JavaScript
684 lines
24 KiB
JavaScript
// walker has fixed shapes and structures
|
|
// shape definitions are in the constructor
|
|
var randf = (low, high) => Math.random() * (high - low) + low
|
|
|
|
function deg2rad(deg) {
|
|
return deg / 180 * Math.PI
|
|
}
|
|
|
|
const STRENGTH = 3
|
|
|
|
var Walker = function () {
|
|
this.__constructor.apply(this, arguments);
|
|
}
|
|
|
|
|
|
Walker.prototype.__constructor = function (world, floor, config) {
|
|
this.world = world;
|
|
this.floor = floor
|
|
this.config = config
|
|
|
|
this.density = 106.2; // common for all fixtures, no reason to be too specific
|
|
|
|
this.max_distance = -5;
|
|
this.health = 10 //config.walker_health;
|
|
this.score = 0;
|
|
this.low_foot_height = 0;
|
|
this.head_height = 0;
|
|
this.steps = 0;
|
|
this.distance = 0
|
|
this.last_left_left_forward = true
|
|
|
|
this.hue = randf(200, 360)
|
|
|
|
this.bd = new b2.BodyDef({
|
|
positions: {
|
|
x: 10,
|
|
y: -10
|
|
}
|
|
});
|
|
this.bd.position.x += randf(-10, 10)
|
|
this.bd.type = b2.Body.b2_dynamicBody;
|
|
this.bd.linearDamping = 0;
|
|
this.bd.angularDamping = 20; // decay in force/ air friction
|
|
this.bd.allowSleep = true;
|
|
this.bd.awake = true;
|
|
|
|
this.fd = new b2.FixtureDef();
|
|
this.fd.density = this.density;
|
|
this.fd.restitution = 0.1; // bounciness
|
|
this.fd.friction = 100; // only on contact (grip)
|
|
this.fd.shape = new b2.PolygonShape();
|
|
this.fd.filter.groupIndex = -1;
|
|
|
|
this.torso_def = {
|
|
upper_width: 0.25,
|
|
upper_height: 0.45,
|
|
lower_width: 0.25,
|
|
lower_height: 0.2
|
|
};
|
|
|
|
this.leg_def = {
|
|
femur_width: 0.18,
|
|
femur_length: 0.45,
|
|
tibia_width: 0.13,
|
|
tibia_length: 0.38,
|
|
foot_height: 0.08,
|
|
foot_length: 0.28
|
|
};
|
|
|
|
this.arm_def = {
|
|
arm_width: 0.12,
|
|
arm_length: 0.37,
|
|
forearm_width: 0.1,
|
|
forearm_length: 0.30,
|
|
hand_height: 0.08,
|
|
hand_length: 0.18,
|
|
}
|
|
|
|
this.head_def = {
|
|
head_width: 0.22,
|
|
head_height: 0.22,
|
|
neck_width: 0.1,
|
|
neck_height: 0.08
|
|
}
|
|
|
|
this.joints = [];
|
|
this.otherJoints = []
|
|
this.otherBodies = []
|
|
this.grips = [false, false, false, false]
|
|
// this.frictionJoints = []
|
|
|
|
this.torso = this.createTorso();
|
|
this.left_leg = this.createLeg('left_');
|
|
this.right_leg = this.createLeg('right_');
|
|
this.left_arm = this.createArm('left_');
|
|
this.right_arm = this.createArm('right_');
|
|
this.head = this.createHead();
|
|
this.connectParts();
|
|
|
|
this.bodies = this.getBodies();
|
|
|
|
// now apply a random starting positions and orientation
|
|
this.randomise(0.5);
|
|
|
|
|
|
// add grip
|
|
// we don't have data on external forces, so I will just punish for contact with the ground
|
|
http: //blog.sethladd.com/2011/09/box2d-collision-damage-for-javascript.html
|
|
// However we could use a listener or calc force
|
|
var self = this
|
|
this.contactListener = new b2.ContactListener()
|
|
this.contactListener.BeginContact = function (contact, impulse) {
|
|
if (contact.m_fixtureA.m_body.m_userData == "floor" | contact.m_fixtureB.m_body.m_userData) {
|
|
var otherFixture = contact.m_fixtureA.m_body.m_userData == "floor" ? contact.m_fixtureB : contact.m_fixtureA
|
|
if (otherFixture.m_body === self.right_leg.foot) {
|
|
// TODO let the agent act to grip or not. Only if palm or foot down?
|
|
self.right_leg.frictionJoint.maxForce = 1000 * self.grips[0]
|
|
self.right_leg.frictionJoint.maxTorque = 1000 * self.grips[0]
|
|
} else if (otherFixture.m_body === self.left_leg.foot) {
|
|
self.left_leg.frictionJoint.maxForce = 1000 * self.grips[1]
|
|
self.left_leg.frictionJoint.maxTorque = 1000 * self.grips[1]
|
|
} else if (otherFixture.m_body == self.right_arm.hand) {
|
|
self.right_arm.frictionJoint.maxForce = 1000 * self.grips[2]
|
|
self.right_arm.frictionJoint.maxTorque = 1000 * self.grips[2]
|
|
} else if (otherFixture.m_body === self.left_arm.hand) {
|
|
self.left_arm.frictionJoint.maxForce = 1000 * self.grips[3]
|
|
self.left_arm.frictionJoint.maxTorque = 1000 * self.grips[3]
|
|
}
|
|
}
|
|
}
|
|
this.contactListener.EndContact = function (contact, impulse) {
|
|
if (contact.m_fixtureA.m_body.m_userData == "floor" | contact.m_fixtureB.m_body.m_userData) {
|
|
var otherFixture = contact.m_fixtureA.m_body.m_userData == "floor" ? contact.m_fixtureB : contact.m_fixtureA
|
|
if (otherFixture.m_body === self.right_leg.foot) {
|
|
setTimeout(() => {
|
|
self.right_leg.frictionJoint.maxForce = 0
|
|
}, 100)
|
|
setTimeout(() => {
|
|
self.right_leg.frictionJoint.maxTorque = 0
|
|
}, 100)
|
|
} else if (otherFixture.m_body === self.left_leg.foot) {
|
|
self.left_leg.frictionJoint.maxForce = 0
|
|
self.left_leg.frictionJoint.maxTorque = 0
|
|
} else if (otherFixture.m_body == self.right_arm.hand) {
|
|
setTimeout(() => {
|
|
self.right_arm.frictionJoint.maxForce = 0
|
|
}, 100)
|
|
setTimeout(() => {
|
|
self.right_arm.frictionJoint.maxTorque = 0
|
|
}, 100)
|
|
} else if (otherFixture.m_body === self.left_arm.hand) {
|
|
self.left_arm.frictionJoint.maxForce = 0
|
|
self.left_arm.frictionJoint.maxTorque = 0
|
|
}
|
|
}
|
|
}
|
|
this.world.SetContactListener(this.contactListener)
|
|
|
|
// if (typeof document!==undefined)
|
|
// this.renderer = new Renderer()
|
|
|
|
}
|
|
|
|
// Walker.prototype.destroy = function () {
|
|
// this.bodies.map(body=>body.DestroyFixture(body.GetFixtureList()))
|
|
// this.bodies.map(body=>this.world.DestroyBody(body))
|
|
// this.joints.map(joint => this.world.DestroyJoint(joint))
|
|
// this.otherJoints.map(joint => this.world.DestroyJoint(joint))
|
|
// }
|
|
|
|
Walker.prototype.createTorso = function () {
|
|
// upper torso
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height + this.torso_def.upper_height / 2);
|
|
var upper_torso = this.world.CreateBody(this.bd);
|
|
upper_torso.SetUserData('upper_torso')
|
|
|
|
this.fd.shape.SetAsBox(this.torso_def.upper_width / 2, this.torso_def.upper_height / 2);
|
|
upper_torso.CreateFixture(this.fd);
|
|
|
|
// lower torso
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height / 2);
|
|
var lower_torso = this.world.CreateBody(this.bd);
|
|
lower_torso.SetUserData('lower_torso')
|
|
|
|
this.fd.shape.SetAsBox(this.torso_def.lower_width / 2, this.torso_def.lower_height / 2);
|
|
lower_torso.CreateFixture(this.fd);
|
|
|
|
// torso joint
|
|
// Note for more info see : https://github.com/openai/gym/wiki/Humanoid-V1
|
|
// For 3d definition https://github.com/openai/gym/blob/master/gym/envs/mujoco/assets/humanoid.xml
|
|
var jd = new b2.RevoluteJointDef();
|
|
var position = upper_torso.GetPosition().Clone();
|
|
position.y -= this.torso_def.upper_height / 2;
|
|
position.x -= this.torso_def.lower_width / 3;
|
|
jd.Initialize(upper_torso, lower_torso, position);
|
|
jd.lowerAngle = deg2rad(-75 / 2);
|
|
jd.upperAngle = deg2rad(30 / 2);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 150 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData('torso_joint')
|
|
this.joints.push(j);
|
|
|
|
return {
|
|
upper_torso: upper_torso,
|
|
lower_torso: lower_torso
|
|
};
|
|
}
|
|
|
|
Walker.prototype.createLeg = function (label) {
|
|
// upper leg
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length / 2);
|
|
var upper_leg = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.leg_def.femur_width / 2, this.leg_def.femur_length / 2);
|
|
upper_leg.CreateFixture(this.fd);
|
|
upper_leg.SetUserData(label + 'upper_leg')
|
|
|
|
// lower leg
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length / 2);
|
|
var lower_leg = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.leg_def.tibia_width / 2, this.leg_def.tibia_length / 2);
|
|
lower_leg.CreateFixture(this.fd);
|
|
lower_leg.SetUserData(label + 'lower_leg')
|
|
|
|
// foot
|
|
this.bd.position.Set(0.5, this.leg_def.foot_height / 2);
|
|
var foot = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.leg_def.foot_length / 2, this.leg_def.foot_height / 2);
|
|
foot.CreateFixture(this.fd);
|
|
foot.SetUserData(label + 'foot')
|
|
|
|
var fjd = new b2.FrictionJointDef();
|
|
var position = new b2.Vec2(0, 0)
|
|
fjd.Initialize(foot, this.floor, position)
|
|
fjd.maxForce = 0; //This the most force the joint will apply to your object. The faster its moving the more force applied
|
|
fjd.maxTorque = 0; //Set to 0 to prevent rotation
|
|
fjd.userData = label + 'foot_friction_joint'
|
|
fjd.collideConnected = true
|
|
var frictionJoint = this.world.CreateJoint(fjd)
|
|
this.otherJoints.push(frictionJoint)
|
|
|
|
// leg joints
|
|
var jd = new b2.RevoluteJointDef();
|
|
var position = upper_leg.GetPosition().Clone();
|
|
position.y -= this.leg_def.femur_length / 2;
|
|
position.x += this.leg_def.femur_width / 4;
|
|
jd.Initialize(upper_leg, lower_leg, position);
|
|
jd.lowerAngle = deg2rad(-100);
|
|
jd.upperAngle = deg2rad(-2);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 260 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData(label + 'leg_joint')
|
|
this.joints.push(j);
|
|
|
|
// foot joint
|
|
var jd = new b2.RevoluteJointDef();
|
|
var position = lower_leg.GetPosition().Clone();
|
|
position.y -= this.leg_def.tibia_length / 2;
|
|
jd.Initialize(lower_leg, foot, position);
|
|
jd.lowerAngle = -deg2rad(-36);
|
|
jd.upperAngle = deg2rad(30);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 90 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData(label + 'foot_joint')
|
|
this.joints.push(j);
|
|
|
|
return {
|
|
upper_leg: upper_leg,
|
|
lower_leg: lower_leg,
|
|
foot: foot,
|
|
frictionJoint: frictionJoint
|
|
};
|
|
}
|
|
|
|
Walker.prototype.createArm = function (label) {
|
|
// upper arm
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height + this.torso_def.upper_height - this.arm_def.arm_length / 2);
|
|
var upper_arm = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.arm_def.arm_width / 2, this.arm_def.arm_length / 2);
|
|
upper_arm.CreateFixture(this.fd);
|
|
upper_arm.SetUserData(label + 'upper_arm')
|
|
|
|
// lower arm
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height + this.torso_def.upper_height - this.arm_def.arm_length - this.arm_def.forearm_length / 2);
|
|
var lower_arm = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.arm_def.forearm_width / 2, this.arm_def.forearm_length / 2);
|
|
lower_arm.CreateFixture(this.fd);
|
|
lower_arm.SetUserData(label + 'lower_arm')
|
|
|
|
// hand
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height + this.torso_def.upper_height - this.arm_def.arm_length - this.arm_def.forearm_length - this.arm_def.hand_length / 2);
|
|
var hand = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.arm_def.hand_height / 2, this.arm_def.hand_length / 2);
|
|
hand.CreateFixture(this.fd);
|
|
hand.SetUserData(label + 'hand')
|
|
|
|
|
|
var fjd = new b2.FrictionJointDef();
|
|
var position = new b2.Vec2(0, 0)
|
|
fjd.Initialize(hand, this.floor, position)
|
|
fjd.maxForce = 0; //This the most force the joint will apply to your object. The faster its moving the more force applied
|
|
fjd.maxTorque = 0; //Set to 0 to prevent rotation
|
|
fjd.userData = label + 'hand_friction_joint'
|
|
fjd.collideConnected = true
|
|
var frictionJoint = this.world.CreateJoint(fjd)
|
|
this.otherJoints.push(frictionJoint)
|
|
|
|
|
|
// arm join
|
|
var jd = new b2.RevoluteJointDef();
|
|
var position = upper_arm.GetPosition().Clone();
|
|
position.y -= this.arm_def.arm_length / 2;
|
|
jd.Initialize(upper_arm, lower_arm, position);
|
|
jd.lowerAngle = deg2rad(0);
|
|
jd.upperAngle = deg2rad(85);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 150 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData(label + 'arm_joint')
|
|
this.joints.push(j);
|
|
|
|
// hand joint
|
|
var jd = new b2.RevoluteJointDef();
|
|
var position = lower_arm.GetPosition().Clone();
|
|
position.y -= this.arm_def.forearm_length / 2;
|
|
jd.Initialize(lower_arm, hand, position);
|
|
jd.lowerAngle = deg2rad(-35);
|
|
jd.upperAngle = deg2rad(35);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 90 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData(label + 'hand_joint')
|
|
this.joints.push(j);
|
|
|
|
return {
|
|
upper_arm: upper_arm,
|
|
lower_arm: lower_arm,
|
|
hand: hand,
|
|
frictionJoint: frictionJoint
|
|
};
|
|
}
|
|
|
|
Walker.prototype.createHead = function () {
|
|
// neck
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height + this.torso_def.upper_height + this.head_def.neck_height / 2);
|
|
var neck = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.head_def.neck_width / 2, this.head_def.neck_height / 2);
|
|
neck.CreateFixture(this.fd);
|
|
neck.SetUserData('neck')
|
|
|
|
// head
|
|
this.bd.position.Set(0.5 - this.leg_def.foot_length / 2 + this.leg_def.tibia_width / 2, this.leg_def.foot_height / 2 + this.leg_def.foot_height / 2 + this.leg_def.tibia_length + this.leg_def.femur_length + this.torso_def.lower_height + this.torso_def.upper_height + this.head_def.neck_height + this.head_def.head_height / 2);
|
|
var head = this.world.CreateBody(this.bd);
|
|
|
|
this.fd.shape.SetAsBox(this.head_def.head_width / 2, this.head_def.head_height / 2);
|
|
head.CreateFixture(this.fd);
|
|
head.SetUserData('head')
|
|
|
|
// neck joint
|
|
var jd = new b2.RevoluteJointDef();
|
|
var position = neck.GetPosition().Clone();
|
|
position.y += this.head_def.neck_height / 2;
|
|
jd.Initialize(head, neck, position);
|
|
jd.lowerAngle = -0.1;
|
|
jd.upperAngle = 0.2;
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 4 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData('neck_joint')
|
|
this.joints.push(j);
|
|
|
|
return {
|
|
head: head,
|
|
neck: neck
|
|
};
|
|
}
|
|
|
|
Walker.prototype.connectParts = function () {
|
|
|
|
//neck/torso
|
|
var jd = new b2.WeldJointDef();
|
|
jd.bodyA = this.head.neck;
|
|
jd.bodyB = this.torso.upper_torso;
|
|
jd.localAnchorA = new b2.Vec2(0, -this.head_def.neck_height / 2);
|
|
jd.localAnchorB = new b2.Vec2(0, this.torso_def.upper_height / 2);
|
|
jd.referenceAngle = 0;
|
|
var j = this.world.CreateJoint(jd);
|
|
j.SetUserData('neck-joint')
|
|
|
|
|
|
// torso/arms
|
|
var jd = new b2.RevoluteJointDef();
|
|
position = this.torso.upper_torso.GetPosition().Clone();
|
|
position.y += this.torso_def.upper_height / 2;
|
|
jd.Initialize(this.torso.upper_torso, this.right_arm.upper_arm, position);
|
|
jd.lowerAngle = deg2rad(-60);
|
|
jd.upperAngle = deg2rad(125);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 250 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData('shoulder_right_joint')
|
|
this.joints.push(j);
|
|
|
|
var jd = new b2.RevoluteJointDef();
|
|
jd.Initialize(this.torso.upper_torso, this.left_arm.upper_arm, position);
|
|
jd.lowerAngle = deg2rad(-60);
|
|
jd.upperAngle = deg2rad(125);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 250 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData('shoulder_right_joint')
|
|
this.joints.push(j);
|
|
|
|
// torso/legs
|
|
var jd = new b2.RevoluteJointDef();
|
|
position = this.torso.lower_torso.GetPosition().Clone();
|
|
position.y -= this.torso_def.lower_height / 2;
|
|
jd.Initialize(this.torso.lower_torso, this.right_leg.upper_leg, position);
|
|
jd.lowerAngle = deg2rad(-10);
|
|
jd.upperAngle = deg2rad(80);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 400 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData('waist_right_joint')
|
|
this.joints.push(j);
|
|
|
|
var jd = new b2.RevoluteJointDef();
|
|
jd.Initialize(this.torso.lower_torso, this.left_leg.upper_leg, position);
|
|
jd.lowerAngle = deg2rad(-10);
|
|
jd.upperAngle = deg2rad(80);
|
|
jd.enableLimit = true;
|
|
jd.maxMotorTorque = 400 * STRENGTH;
|
|
jd.motorSpeed = 0;
|
|
jd.enableMotor = true;
|
|
var j = this.world.CreateJoint(jd)
|
|
j.SetUserData('waist_left_joint')
|
|
this.joints.push(j);
|
|
}
|
|
|
|
Walker.prototype.getBodies = function () {
|
|
|
|
return [
|
|
this.head.head,
|
|
this.head.neck,
|
|
this.torso.upper_torso,
|
|
this.torso.lower_torso,
|
|
this.left_arm.upper_arm,
|
|
this.left_arm.lower_arm,
|
|
this.left_arm.hand,
|
|
this.right_arm.upper_arm,
|
|
this.right_arm.lower_arm,
|
|
this.right_arm.hand,
|
|
this.left_leg.upper_leg,
|
|
this.left_leg.lower_leg,
|
|
this.left_leg.foot,
|
|
this.right_leg.upper_leg,
|
|
this.right_leg.lower_leg,
|
|
this.right_leg.foot
|
|
];
|
|
}
|
|
|
|
Walker.prototype.randomise = function (n) {
|
|
for (var k = 0; k < this.joints.length; k++) {
|
|
this.joints[k].SetMotorSpeed(randf(-n, n))
|
|
}
|
|
}
|
|
|
|
Walker.prototype.getState = function () {
|
|
var self = this;
|
|
var state = []
|
|
this.bodies
|
|
.forEach((body) => {
|
|
// see http://www.box2dflash.org/docs/2.0.2/reference/Box2D/Dynamics/b2Body.html#GetLocalVector()
|
|
var t = body.GetTransform() // world transform of the body's origin.
|
|
state.push(t.p.x) // world transform of the body's origin.
|
|
state.push(t.p.y) // world transform of the body's origin.
|
|
state.push(t.q.s) // world transform of the body's origin.
|
|
state.push(t.q.c) // world transform of the body's origin.
|
|
|
|
var dt = body.GetLinearVelocity() // Get the linear velocity of the center of mass (world).
|
|
state.push(dt.x)
|
|
state.push(dt.y)
|
|
|
|
var lp = self.torso.upper_torso.GetLocalPoint(body.GetWorldCenter()) // Get the bodypart position relative to the upper torso
|
|
state.push(lp.x)
|
|
state.push(lp.y)
|
|
|
|
state.push(body.GetAngularVelocity()) // the angular velocity in radians/second.
|
|
state.push(body.GetAngle()) // the current world rotation angle in radians.
|
|
}, [])
|
|
this.joints.forEach(joint => {
|
|
// http://www.box2dflash.org/docs/2.0.2/reference/Box2D/Dynamics/Joints/b2RevoluteJoint.html
|
|
state.push(joint.GetJointAngle()) // Get the current joint angle in radians.
|
|
state.push(joint.GetJointSpeed()) // Get the current joint angle speed in radians per second
|
|
state.push(joint.GetMotorSpeed())
|
|
})
|
|
return state
|
|
}
|
|
|
|
Walker.prototype.simulationPreStep = function (motorSpeeds) {
|
|
// act
|
|
for (var k = 0; k < this.joints.length; k++) {
|
|
this.joints[k].SetMotorSpeed(motorSpeeds[k] * 10); // action can range from -3 to 3, radians per second
|
|
}
|
|
for (let i = 0; i < motorSpeeds.length - this.joints.length; i++) {
|
|
this.grips[i] = motorSpeeds[i] > 0
|
|
}
|
|
if (motorSpeeds[0] <= 0) this.right_leg.frictionJoint.maxForce = this.right_leg.frictionJoint.maxTorque
|
|
if (motorSpeeds[1] <= 0) this.left_leg.frictionJoint.maxForce = this.left_leg.frictionJoint.maxTorque
|
|
if (motorSpeeds[2] <= 0) this.left_arm.frictionJoint.maxForce = this.left_arm.frictionJoint.maxTorque
|
|
if (motorSpeeds[3] <= 0) this.right_arm.frictionJoint.maxForce = this.right_arm.frictionJoint.maxTorque
|
|
}
|
|
|
|
Walker.prototype.step = function (motorSpeeds) {
|
|
/*
|
|
Take one step into the environement
|
|
@delta (Float) time since the last update
|
|
@action: (Integer) The action to take (can be null if no action)
|
|
*/
|
|
this.simulationPreStep(motorSpeeds)
|
|
this.world.Step(1 / this.config.time_step, this.config.velocity_iterations, this.config.position_iterations);
|
|
this.steps++
|
|
/* score/reward */
|
|
// reward copied from OpenAI Gym Humanoid Walker https://github.com/openai/gym/blob/master/gym/envs/mujoco/humanoid.py
|
|
// also see https://github.com/AdamStelmaszczyk/learning2run/blob/master/osim-rl/osim/env/run.py#L67
|
|
// https://github.com/openai/gym/blob/master/gym/envs/mujoco/assets/humanoidstandup.xml
|
|
|
|
// reward for keeping head up, compared to feet
|
|
var mean_foot_height = (this.left_leg.foot.GetPosition().y + this.right_leg.foot.GetPosition().y) / 2
|
|
|
|
var head_height_reward = (this.head.head.GetPosition().y - mean_foot_height) * 400; // it's head should be above it's feet 2*(-0.25-2)
|
|
|
|
// reward for moving one leg beyond the other (stepping)
|
|
var left_leg_forward = this.right_leg.foot.GetPosition().x > this.left_leg.foot.GetPosition().x;
|
|
var leg_switch_reward = (left_leg_forward != this.last_left_left_forward) ? 4 : 0
|
|
this.last_left_left_forward = left_leg_forward
|
|
|
|
// cost for moving joints to unnatural positions (fraction of movement range in the relevant direction)
|
|
var jointFractionMovement = j => j.GetJointAngle() > 0 ? j.GetJointAngle() / (j.GetUpperLimit() + 1) : j.GetJointAngle() / (j.GetLowerLimit() - 1)
|
|
var quad_joint_angle_cost = -0.10 * this.joints.map(j => jointFractionMovement(j) * 1.2)
|
|
.reduce((o, v) => o + v * v, 0)
|
|
quad_joint_angle_cost = Math.max(quad_joint_angle_cost, -10)
|
|
|
|
// reward for moving right
|
|
var position = this.torso.upper_torso.GetPosition().x
|
|
if (this.last_position === undefined) this.last_position = position
|
|
var velocity = (position - this.last_position) * 100
|
|
this.last_position = position
|
|
lin_vel_reward = 3 * velocity
|
|
|
|
// punish for using energy, squared
|
|
var quad_power_cost = -0.01 * this.joints.map(j => j.GetJointSpeed()).reduce((sum, speed) => sum + speed ** 2)
|
|
quad_power_cost = Math.max(quad_power_cost, -10)
|
|
|
|
// Lets be nice, all entities should find overall happiness in what they do
|
|
var bonus_happiness = 5
|
|
|
|
var contacts = this.bodies.map(b => b.GetContactList()).filter(b => b).length
|
|
quad_contact_cost = -Math.min(contacts - 4, 10) / 2
|
|
|
|
this.rewards = {
|
|
lin_vel_reward,
|
|
quad_power_cost,
|
|
quad_contact_cost,
|
|
quad_joint_angle_cost,
|
|
bonus_happiness,
|
|
head_height_reward,
|
|
leg_switch_reward
|
|
}
|
|
|
|
this.reward = Object.values(this.rewards).reduce((tot, v) => tot + v, 0) / 3
|
|
|
|
var info = {
|
|
episodeSteps: this.steps,
|
|
reward: this.reward,
|
|
position,
|
|
...this.rewards
|
|
}
|
|
var done = 0
|
|
this.world.ClearForces();
|
|
console.debug('reward', this.rewards)
|
|
return [this.getState(), this.reward, done, info]
|
|
}
|
|
|
|
Walker.prototype.getLastReward = function () {
|
|
return this.reward
|
|
}
|
|
|
|
Walker.prototype.render = function (val) {
|
|
if (val) {
|
|
this.renderer.setFps(this.config.draw_fps)
|
|
this.steping = true;
|
|
} else {
|
|
this.renderer.setFps(0)
|
|
this.steping = false;
|
|
}
|
|
}
|
|
Walker.prototype.reset = function () {
|
|
/** Reset position to initial or random position TODO */
|
|
console.log('reset not implemented')
|
|
}
|
|
Walker.prototype.shuffle = function () {
|
|
/** Reset position to initial or random position TODO */
|
|
console.log('shuffle not implemented')
|
|
}
|
|
|
|
|
|
config = {
|
|
time_step: 60,
|
|
simulation_fps: 60,
|
|
draw_fps: 60,
|
|
velocity_iterations: 8,
|
|
position_iterations: 3,
|
|
max_zoom_factor: 130,
|
|
min_motor_speed: -2,
|
|
max_motor_speed: 2,
|
|
population_size: 1,
|
|
walker_health: 100,
|
|
max_floor_tiles: 50,
|
|
round_length: 1000,
|
|
min_body_delta: 0,
|
|
min_leg_delta: 0.0,
|
|
};
|
|
|
|
var b2 = require('../vendor/jsbox2d')
|
|
var createFloor = require('./floor.js')
|
|
var DDPGAgent = require('./ddpg/ddpg_agent')
|
|
var DDPGAgent = require('./ddpg/ddpg_agent')
|
|
|
|
|
|
var world = new b2.World(new b2.Vec2(0, -10))
|
|
floor = createFloor(world, config.max_floor_tiles);
|
|
var env = new Walker(world, floor, config)
|
|
|
|
var nbActions = env.joints.length + 4
|
|
var stateSize = env.bodies.length * 10 + env.joints.length * 3
|
|
|
|
var agent = new DDPGAgent(env, {
|
|
stateSize,
|
|
nbActions,
|
|
resetEpisode: true,
|
|
desiredActionStddev: 0.4,
|
|
initialStddev: 0.4,
|
|
actorFirstLayerSize: 128,
|
|
actorSecondLayerSize: 64,
|
|
criticFirstLayerSSize: 128,
|
|
criticFirstLayerASize: 128,
|
|
criticSecondLayerSize: 64,
|
|
nbEpochs: 1000
|
|
});
|
|
agent.train(true);
|
|
|
|
// setInterval(() => {
|
|
// walker.step()
|
|
// console.log(walker.getState())
|
|
// }, 100)
|