|
const CHIMP_SPEED_HIP = 4; |
|
const CHIMP_SPEED_KNEE = 6; |
|
const CHIMP_SPEED_HAND = 8; |
|
|
|
|
|
|
|
|
|
class ClimbingProfileChimpanzee extends ClimberAbstractBody { |
|
|
|
|
|
|
|
|
|
|
|
|
|
constructor(scale, motors_torque=100, nb_steps_under_water=600){ |
|
super(scale, motors_torque, nb_steps_under_water); |
|
|
|
this.LEG_DOWN = 12 / this.SCALE; |
|
this.ARM_UP = 22 / this.SCALE; |
|
this.LIMB_W = 8 / this.SCALE; |
|
this.LIMB_H = 28 / this.SCALE; |
|
this.HAND_PART_W = 4 / this.SCALE; |
|
this.HAND_PART_H = 8 / this.SCALE; |
|
this.LEG_H = this.LIMB_H; |
|
this.TORQUE_PENALTY = 0.00035 / 5; |
|
this.BODY_HEIGHT = 45; |
|
this.HEAD_HEIGHT = 20; |
|
this.HEAD_UP = 2.5 / this.SCALE; |
|
|
|
this.AGENT_WIDTH = 24 / this.SCALE; |
|
this.AGENT_HEIGHT = this.BODY_HEIGHT / this.SCALE |
|
+ this.HEAD_HEIGHT / this.SCALE |
|
+ this.HEAD_UP + this.LEG_H * 2 |
|
- this.LEG_DOWN; |
|
this.AGENT_CENTER_HEIGHT = this.LEG_H * 2 + this.LEG_DOWN; |
|
|
|
this.remove_reward_on_head_angle = true; |
|
} |
|
|
|
draw(world, init_x, init_y, force_to_center){ |
|
|
|
|
|
let head_fd = new b2.FixtureDef(); |
|
head_fd.shape = new b2.PolygonShape(); |
|
head_fd.shape.SetAsBox(5 / this.SCALE, 10 / this.SCALE); |
|
|
|
|
|
|
|
|
|
|
|
|
|
head_fd.density = 5.0; |
|
head_fd.friction = 0.1; |
|
head_fd.filter.categoryBits = 0x20; |
|
head_fd.filter.maskBits = 0x1; |
|
|
|
let head_bd = new b2.BodyDef(); |
|
head_bd.type = b2.Body.b2_dynamicBody; |
|
head_bd.position.Set(init_x, |
|
init_y + this.BODY_HEIGHT / this.SCALE / 2 + this.HEAD_HEIGHT / this.SCALE / 2 + this.HEAD_UP); |
|
let head = world.CreateBody(head_bd); |
|
head.CreateFixture(head_fd); |
|
head.color1 = "#806682"; |
|
head.color2 = "#4D4D80"; |
|
head.ApplyForceToCenter(new b2.Vec2(force_to_center, 0), true); |
|
head.SetUserData(new CustomBodyUserData(true, true, "head")); |
|
this.body_parts.push(head); |
|
this.reference_head_object = head; |
|
|
|
|
|
let body_fd = new b2.FixtureDef(); |
|
body_fd.shape = new b2.PolygonShape(); |
|
body_fd.shape.Set([ |
|
new b2.Vec2(-12 / this.SCALE, 25 / this.SCALE), |
|
new b2.Vec2(12 / this.SCALE, 25 / this.SCALE), |
|
new b2.Vec2(8 / this.SCALE, -20 / this.SCALE), |
|
new b2.Vec2(-8 / this.SCALE, -20 / this.SCALE), |
|
], 4); |
|
body_fd.density = 5.0; |
|
body_fd.friction = 0.1; |
|
body_fd.filter.categoryBits = 0x20; |
|
body_fd.filter.maskBits = 0x1; |
|
|
|
let body_bd = new b2.BodyDef(); |
|
body_bd.type = b2.Body.b2_dynamicBody; |
|
body_bd.position.Set(init_x, init_y); |
|
let body = world.CreateBody(body_bd); |
|
body.CreateFixture(body_fd); |
|
body.color1 = "#806682"; |
|
body.color2 = "#4D4D80"; |
|
body.SetUserData(new CustomBodyUserData(true, true, "body")); |
|
this.body_parts.push(body); |
|
|
|
|
|
let rjd = new b2.RevoluteJointDef(); |
|
rjd.Initialize(head, body, new b2.Vec2(init_x, init_y + this.BODY_HEIGHT / this.SCALE / 2)); |
|
rjd.enableMotor = false; |
|
rjd.enableLimit = true; |
|
rjd.lowerAngle = -0.1 * Math.PI; |
|
rjd.upperAngle = 0.1 * Math.PI; |
|
let joint_motor = world.CreateJoint(rjd); |
|
joint_motor.SetUserData(new CustomMotorUserData("neck", 0, false)); |
|
this.neck_joint = joint_motor; |
|
|
|
|
|
let UPPER_LIMB_FD = new b2.FixtureDef(); |
|
UPPER_LIMB_FD.shape = new b2.PolygonShape(); |
|
UPPER_LIMB_FD.shape.SetAsBox(this.LIMB_W / 2, this.LIMB_H / 2); |
|
UPPER_LIMB_FD.density = 1.0; |
|
UPPER_LIMB_FD.restitution = 0; |
|
UPPER_LIMB_FD.filter.categoryBits = 0x20; |
|
UPPER_LIMB_FD.filter.maskBits = 0x1; |
|
|
|
let LOWER_LIMB_FD = new b2.FixtureDef(); |
|
LOWER_LIMB_FD.shape = new b2.PolygonShape(); |
|
LOWER_LIMB_FD.shape.SetAsBox(0.8 * this.LIMB_W / 2, this.LIMB_H / 2); |
|
LOWER_LIMB_FD.density = 1.0; |
|
LOWER_LIMB_FD.restitution = 0; |
|
LOWER_LIMB_FD.filter.categoryBits = 0x20; |
|
LOWER_LIMB_FD.filter.maskBits = 0x1; |
|
|
|
let HAND_PART_FD = new b2.FixtureDef(); |
|
HAND_PART_FD.shape = new b2.PolygonShape(); |
|
HAND_PART_FD.shape.SetAsBox(this.HAND_PART_W / 2, this.HAND_PART_H / 2); |
|
HAND_PART_FD.density = 1.0; |
|
HAND_PART_FD.restitution = 0; |
|
HAND_PART_FD.filter.categoryBits = 0x20; |
|
HAND_PART_FD.filter.maskBits = 0x1; |
|
|
|
|
|
for(let i = 0; i < 2; i++){ |
|
|
|
|
|
let upper_bd = new b2.BodyDef(); |
|
upper_bd.type = b2.Body.b2_dynamicBody; |
|
upper_bd.position.Set(init_x, init_y - this.LIMB_H / 2 - this.LEG_DOWN); |
|
let upper = world.CreateBody(upper_bd); |
|
upper.CreateFixture(UPPER_LIMB_FD); |
|
upper.color1 = "#964A7D"; |
|
upper.color2 = "#63304A"; |
|
upper.SetUserData(new CustomBodyUserData(false, false, "upper_leg")); |
|
this.body_parts.push(upper); |
|
|
|
|
|
rjd = new b2.RevoluteJointDef(); |
|
rjd.Initialize(body, upper, new b2.Vec2(init_x, init_y - this.LEG_DOWN)); |
|
rjd.enableMotor = true; |
|
rjd.enableLimit = true; |
|
rjd.maxMotorTorque = this.MOTORS_TORQUE; |
|
rjd.motorSpeed = 1; |
|
rjd.lowerAngle = -0.3 * Math.PI; |
|
rjd.upperAngle = 0.6 * Math.PI; |
|
joint_motor = world.CreateJoint(rjd); |
|
joint_motor.SetUserData(new CustomMotorUserData("hip", CHIMP_SPEED_HIP, false)); |
|
this.motors.push(joint_motor); |
|
|
|
|
|
let lower_bd = new b2.BodyDef(); |
|
lower_bd.type = b2.Body.b2_dynamicBody; |
|
lower_bd.position.Set(init_x, init_y - this.LIMB_H * 3/2 - this.LEG_DOWN); |
|
let lower = world.CreateBody(lower_bd); |
|
lower.CreateFixture(LOWER_LIMB_FD); |
|
lower.color1 = "#964A7D"; |
|
lower.color2 = "#63304A"; |
|
lower.SetUserData(new CustomBodyUserData(true, true, "lower_leg")); |
|
this.body_parts.push(lower); |
|
|
|
|
|
rjd = new b2.RevoluteJointDef(); |
|
rjd.Initialize(upper, lower, new b2.Vec2(init_x, init_y - this.LEG_H - this.LEG_DOWN)); |
|
rjd.enableMotor = true; |
|
rjd.enableLimit = true; |
|
rjd.maxMotorTorque = this.MOTORS_TORQUE; |
|
rjd.motorSpeed = 1; |
|
rjd.lowerAngle = -0.75 * Math.PI; |
|
rjd.upperAngle = -0.1; |
|
joint_motor = world.CreateJoint(rjd); |
|
joint_motor.SetUserData(new CustomMotorUserData("knee", |
|
CHIMP_SPEED_KNEE, |
|
true, |
|
1, |
|
lower)); |
|
this.motors.push(joint_motor); |
|
} |
|
|
|
|
|
for(let i = 0; i < 2; i++){ |
|
|
|
let upper_bd = new b2.BodyDef(); |
|
upper_bd.type = b2.Body.b2_dynamicBody; |
|
upper_bd.position.Set(init_x, init_y + this.LIMB_H / 2 + this.ARM_UP); |
|
let upper = world.CreateBody(upper_bd); |
|
upper.CreateFixture(UPPER_LIMB_FD); |
|
upper.color1 = "#9C4F82"; |
|
upper.color2 = "#69364F"; |
|
upper.SetUserData(new CustomBodyUserData(false, false, "upper_arm")); |
|
this.body_parts.push(upper); |
|
|
|
|
|
rjd = new b2.RevoluteJointDef(); |
|
rjd.Initialize(body, upper, new b2.Vec2(init_x, init_y + this.ARM_UP)); |
|
rjd.enableMotor = true; |
|
rjd.enableLimit = true; |
|
rjd.maxMotorTorque = this.MOTORS_TORQUE; |
|
rjd.motorSpeed = 1; |
|
rjd.lowerAngle = -0.75 * 2 * Math.PI; |
|
rjd.upperAngle = 0; |
|
joint_motor = world.CreateJoint(rjd); |
|
joint_motor.SetUserData(new CustomMotorUserData("shoulder", CHIMP_SPEED_HIP, false)); |
|
this.motors.push(joint_motor); |
|
|
|
|
|
let lower_bd = new b2.BodyDef(); |
|
lower_bd.type = b2.Body.b2_dynamicBody; |
|
lower_bd.position.Set(init_x, init_y + this.LIMB_H * 3/2 + this.ARM_UP); |
|
let lower = world.CreateBody(lower_bd); |
|
lower.CreateFixture(LOWER_LIMB_FD); |
|
lower.color1 = "#9C4F82"; |
|
lower.color2 = "#69364F"; |
|
lower.SetUserData(new CustomBodyUserData(false, false, "lower_arm")); |
|
this.body_parts.push(lower); |
|
|
|
|
|
rjd = new b2.RevoluteJointDef(); |
|
rjd.Initialize(upper, lower, new b2.Vec2(init_x, init_y + this.LIMB_H + this.ARM_UP)); |
|
rjd.enableMotor = true; |
|
rjd.enableLimit = true; |
|
rjd.maxMotorTorque = this.MOTORS_TORQUE; |
|
rjd.motorSpeed = 1; |
|
rjd.lowerAngle = 0; |
|
rjd.upperAngle = 0.75 * Math.PI; |
|
joint_motor = world.CreateJoint(rjd); |
|
joint_motor.SetUserData(new CustomMotorUserData("elbow", |
|
CHIMP_SPEED_HIP, |
|
false)); |
|
this.motors.push(joint_motor); |
|
|
|
|
|
let prev_part = lower; |
|
let initial_y = init_y + this.LIMB_H * 2 + this.ARM_UP; |
|
let angle_boundaries = [[-0.5, 0.5]]; |
|
let nb_hand_parts = 1; |
|
for(let u = 0; u < nb_hand_parts; u++){ |
|
let hand_part_bd = new b2.BodyDef(); |
|
hand_part_bd.type = b2.Body.b2_dynamicBody; |
|
hand_part_bd.position.Set(init_x, initial_y + this.HAND_PART_H / 2 + this.HAND_PART_H * u); |
|
let hand_part = world.CreateBody(hand_part_bd); |
|
hand_part.CreateFixture(HAND_PART_FD); |
|
hand_part.color1 = "#9C4F82"; |
|
hand_part.color2 = "#69364F"; |
|
hand_part.SetUserData(new CustomBodyUserData(true, false, "hand")); |
|
this.body_parts.push(hand_part); |
|
|
|
|
|
rjd = new b2.RevoluteJointDef(); |
|
rjd.Initialize(prev_part, hand_part, new b2.Vec2(init_x, initial_y + this.HAND_PART_H * u)); |
|
rjd.enableMotor = true; |
|
rjd.enableLimit = true; |
|
rjd.maxMotorTorque = this.MOTORS_TORQUE; |
|
rjd.motorSpeed = 1; |
|
rjd.lowerAngle = angle_boundaries[u][0] * Math.PI; |
|
rjd.upperAngle = angle_boundaries[u][1] * Math.PI; |
|
joint_motor = world.CreateJoint(rjd); |
|
joint_motor.SetUserData(new CustomMotorUserData("hand", |
|
CHIMP_SPEED_HAND, |
|
true, |
|
0, |
|
hand_part)); |
|
this.motors.push(joint_motor); |
|
|
|
prev_part = hand_part; |
|
} |
|
|
|
|
|
let hand_sensor_position = new b2.Vec2(init_x, initial_y + this.HAND_PART_H * nb_hand_parts); |
|
let hand_sensor_part_bd = new b2.BodyDef(); |
|
hand_sensor_part_bd.type = b2.Body.b2_dynamicBody; |
|
hand_sensor_part_bd.position.Assign(hand_sensor_position); |
|
let hand_sensor_part = world.CreateBody(hand_sensor_part_bd); |
|
hand_sensor_part.CreateFixture(this.SENSOR_FD); |
|
hand_sensor_part.color1 = "#FF0000"; |
|
hand_sensor_part.color2 = "#FF0000"; |
|
hand_sensor_part.SetUserData(new CustomBodySensorUserData(true, false, "hand_sensor")); |
|
this.sensors.push(hand_sensor_part); |
|
|
|
let wjd = new b2.WeldJointDef(); |
|
wjd.Initialize(prev_part, hand_sensor_part, hand_sensor_position); |
|
world.CreateJoint(wjd); |
|
} |
|
|
|
|
|
} |
|
} |