Biped dynamics lab

by tpeyruse

HTML

<html>

  <head>
    <title>e-nugget dynamics</title>
  </head>

  <body>
    <svg id="playground" height="500" width="500" xmlns="http://www.w3.org/2000/svg">
      <line x1="0" y1="0" x2="200" y2="200" style="stroke:rgb(255,0,0);stroke-width:2" id="p1" />
      <line x1="0" y1="0" x2="200" y2="200" style="stroke:rgb(0,0,255);stroke-width:2" id="p2" />
      <line x1="-1000" y1="250" x2="1000" y2="250" style="stroke:rgb(0,0,0);stroke-width:1" id="p3" stroke-dasharray="4" />
      <circle cx="0" cy="0" r="10" id="cB" />
      <circle cx="0" cy="0" r="5" id="phase" />
      <line x1="250" y1="-1000" x2="250" y2="1000" style="stroke:rgb(0,0,0);stroke-width:1" id="p4" stroke-dasharray="4" />
      <line x1="0" y1="0" x2="500" y2="500" style="stroke:rgb(0,0,0);stroke-width:1" id="b1" stroke-dasharray="4" />
      <line x1="0" y1="500" x2="500" y2="0" style="stroke:rgb(0,0,0);stroke-width:1" id="b2" stroke-dasharray="4" />
      <line x1="250" y1="250" x2="250" y2="250" style="stroke:rgb(0,255,0);stroke-width:5" id="nrj" />
    </svg>
    <script type='text/javascript'>

    </script>
  </body>

</html>

JavaScript

var svgns = "http://www.w3.org/2000/svg";
// parametres :
// coordonnées de l'origine (x vers la droite, y vers le bas)
var step = 0.40; // angle entre les jambes (rad)
var L1 = 1; // longueur de la jambe 1 (m)
var L2 = 1; // longueur de la jambe 1 (m)
var xA1 = 0.0; // position initiale du pied (m)
var yA1 = 0.0; // position initiale du pied (m)
var state = 1; // état initial physique
var stateCtrl = 0; // état initial controleur
var grav = 9.81; // constant g (en m/s²)
var dt = 0.010; // temps de sampling (s)
var speed = 0
var alpha = Math.acos((L1 ** 2 + L2 ** 2 - step ** 2) / (2 * L1 * L2));
// variables d'etat
var theta = Math.PI + alpha/2; //rad
//var theta = Math.PI/10
var thetap = 0; //rad/s
var nrj = -10;
var w0 = Math.sqrt(grav / L1);
var nrj0 = 10;
var t0 = Date.now();

window.setInterval(function() {
  // algo de controle ici
  if (stateCtrl == 0) {
    L1c = 1.5;
    L2c = 1.0;
    speed = 0.0005;
    console.log(nrj/nrj0);
    if ((nrj/nrj0)>-0.1) {
      stateCtrl = 1;
    }
  } else if (stateCtrl == 1) {
    L2c = 1;
    L1c = 1;
    speed = 0.001;
    if (state == 1) {
      stateCtrl = 2;
    }
  } else if (stateCtrl == 2) {
    L1c = 1;
    L2c = 1;
    speed = 0.001;
    if (state == 2) {
      stateCtrl = 1;
    }
  }
  alpha = Math.acos((L1 ** 2 + L2 ** 2 - step ** 2) / (2 * L1 * L2));
  //actionneurs
  if (L1c > L1) {
    L1 += speed;
  } else {
    L1 -= speed;
  }
  if (L2c > L2) {
    L2 += speed;
  } else {
    L2 -= speed;
  }

  // physique de la marche
  if (state == 1) {
    // frequence propre du pendule
    w0 = Math.sqrt(grav / L1);
    // integration des equations de la dynamique
    var thetapp = -(w0 ** 2) * Math.sin(theta)
    thetap = thetap + thetapp * dt;
    theta = theta + thetap * dt;
    // calcul des coordonnées du pelvis
    xB = xA1 + L1 * Math.sin(theta);
    yB = yA1 + L1 * Math.cos(theta);
    // calcul des coordonnées du pied pendouillant
    xA2 = xB - L2 * Math.sin(theta - alpha);
    yA2 = yB - L2 *...