UAI

by eulomelo

HTML

<!DOCTYPE html>
<html lang="en">
  <head>
    <meta charset="UTF-8" />
    <meta name="viewport" content="width=device-width, initial-scale=1.0" />
    <title>UAIbot</title>
    <style>
      body {
        padding: 0;
        margin: 0;
      }
    </style>
  </head>
  <body>
    <canvas id="scene" width="800" height="600"></canvas>
    <script type="module" src="../js/dp_inv.js"></script>
  </body>
</html>

JavaScript

import * as UAIbot from 'https://cdn.jsdelivr.net/gh/UAIbot/[email protected]/UAIbotJS/UAIbot.js';
import * as Utils from 'https://cdn.jsdelivr.net/gh/UAIbot/UAIbotJS@main/UAIbotJS/Utils.js';
import * as math from 'https://cdn.jsdelivr.net/npm/[email protected]/+esm'

let links= [[ 1.570, -1.570,  0.000,  0.000,  0.000,  0.000], // "theta" rotation in z
            [ 0.335,  0.000,  0.000, -0.405,  0.000, -0.080], // "d" translation in z
            [-1.570,  0.000,  1.570, -1.570,  1.570,  3.140], // "alfa" rotation in x
            [ 0.075,  0.365,  0.090,  0.000,  0.000,  0.000], // "a" translation in x
            [ 0.000,  0.000,  0.000,  0.000,  0.000,  0.000,  2.000]];// kind of link

let sim = new UAIbot.Simulation();
//let bot = new UAIbot.Robot(links);
let bot = new UAIbot.Robot().create_kuka_kr5();
let frames = new UAIbot.Frame(0.1);
sim.add(bot);
sim.add(frames);

let q0 = math.matrix([[0],
                      [0],
                      [-math.pi/4],
                      [0],
                      [0],
                      [0]]);

bot.config(q0);

//Cria Tdes
 
let translation = math.matrix([[Math.random()*0.4 + 0.1],
                               [Math.random()*0.4 + 0.1],
                               [Math.random()*0.4 + 0.1]]);

let Tdes = math.multiply(Utils.trn(translation),Utils.rotx(-math.pi/(Math.random()*6)));

frames.setHTM(Tdes);

//Constantes

let k = 7;
let dt = 0.01;

let histu = [];
let histq = [];
let histr = [];

//loop principal
let i = 0;
sim.setAnimationLoop(() => {
sim.fitWindow();
    if(i > 1){


      


        let p_de = math.matrix([[Tdes._data[0][3]],
                                [Tdes._data[1][3]],
                                [Tdes._data[2][3]]]);

        let x_de = math.matrix([[Tdes._data[0][0]],
                                [Tdes._data[1][0]],
                                [Tdes._data[2][0]]]);

        let y_de = math.matrix([[Tdes._data[0][1]],
                                [Tdes._data[1][1]],
        ...