Collada Kinematics Tweening to desired position with three.js and tween.js

Question

I want to replicate the existent example of loading Collada kinematics and using them with my own model, to do so I have created a class as follows:

import { Object3D, MathUtils } from "three";
import { ColladaLoader } from "three/addons/loaders/ColladaLoader.js";
import yumi_path from "../assets/dae/yumi.dae";
import { Tween, Easing, update } from "@tweenjs/tween.js";

export class YuMiMotion {
  constructor(scene, joint_vlaues) {
    this.scene = scene;
    this.tgt_jnt_vals = joint_vlaues;

    this.loader = new ColladaLoader();
    this.yumi_model = new Object3D();
    this.kinematics;
    this.kinematicsTween;
    this.tweenParameters = {};
    this.loadYuMi();
    this.startMotion();
  }

  startMotion = () => {
    if (this.kinematics == undefined) {
      console.log("Kinematics Still not loaded!");
      this.anim_frame_id1 = requestAnimationFrame(this.startMotion);
    }
    else {
      console.log(this.kinematics);
      this.setupTween();
      cancelAnimationFrame(this.anim_frame_id1);
      this.animate();
     }    
  }

  animate = () => { 
    update();
    this.anim_frame_id2 = requestAnimationFrame(this.animate);
  }
  
  loadYuMi = async() => {
    const onLoad = (result, yumi) => {
      const model = result.scene.children[0];
      yumi.add(model.clone(true));
      yumi.traverse(function (child) {
        if (child.isMesh) {
          child.material.flatShading = true;
        }
      });
      this.kinematics = result.kinematics;
    };

    await this.loader.load(yumi_path, (collada) =>
      onLoad(collada, this.yumi_model, this.kinematics)
    ),
      undefined,
      function (error) {
        console.log("An error happened", error);
      };

    this.yumi_model.position.set(0.0, 0.0, -6.0);
    this.yumi_model.rotation.set(-Math.PI / 2, 0.0, -Math.PI / 2);
    this.yumi_model.scale.set(20, 20, 20);

    this.scene.add(this.yumi_model);
  }

  setupTween =() =>{

    const duration = MathUtils.randInt( 1000, 5000 );
    const target = {};

    for (const prop in this.tgt_jnt_vals) {
          const joint = this.kinematics.joints[ prop ];
          const old = this.tweenParameters[ prop ];
          const position = old ? old : joint.zeroPosition;
          this.tweenParameters[ prop ] = position;
      target[prop] = this.tgt_jnt_vals[prop]; //MathUtils.randInt( joint.limits.min, joint.limits.max );
      // console.log('target:', prop, this.tgt_jnt_vals[prop]);
    }

    this.kinematicsTween = new Tween( this.tweenParameters ).to( target, duration ).easing( Easing.Quadratic.Out );
    this.kinematicsTween.onUpdate( ( object )=> {
      for ( const prop in this.kinematics.joints ) {
        if ( prop in this.kinematics.joints) {
          if ( ! this.kinematics.joints[ prop ].static ) {
            this.kinematics.setJointValue( prop, object[ prop ] );
          }
        }
      }
    });
    // console.log("Tween Parameters", this.tweenParameters);
    // console.log("kinematics tween", this.kinematicsTween);
    this.kinematicsTween.start();
    setTimeout( this.setupTween, duration );
  }
}

And I'm calling this class as follows:

let target_position = {
  gripper_l_joint: 0.01,
  gripper_l_joint_m: 0.01,
  gripper_r_joint: 0.01,
  gripper_r_joint_m: 0.01,
  yumi_joint_1_l: 10.0,
  yumi_joint_1_r:  10.0,
  yumi_joint_2_l:  20.0,
  yumi_joint_2_r:  20.0,
  yumi_joint_3_l:  30.0,
  yumi_joint_3_r:  30.0,
  yumi_joint_4_l:  40.0,
  yumi_joint_4_r:  40.0,
  yumi_joint_5_l:  50.0,
  yumi_joint_5_r:  50.0,
  yumi_joint_6_l:  60.0,
  yumi_joint_6_r:  60.0,
  yumi_joint_7_l:  70.0,
  yumi_joint_7_r:  70.0,

};
new YuMiMotion(this.scene, target_position);

I have created a repo to reproduce my example here.

Can you please tell me how can I Move my model properly to the desired joints position using Three.js and Tween.js? thanks in advance.

Answer 1

The problem was animating the motion, this problem was solved by calling the animation loop outside the motion class:

Motion Class

import { Tween, Easing } from "@tweenjs/tween.js";
import { ColladaLoader } from "three/examples/jsm/loaders/ColladaLoader.js";
import yumi_path from "../assets/dae/yumi.dae";


export class LoadMoveRobot {
  constructor(scene) {
    this.scene = scene;
    this.yumi;
    this.kinematics;
    this.duration = 1000;
    this.target = null;
    this.tweenParameters = {};
    this.joints = {
      left: [
        "yumi_joint_1_l",
        "yumi_joint_2_l",
        "yumi_joint_3_l",
        "yumi_joint_4_l",
        "yumi_joint_5_l",
        "yumi_joint_6_l",
        "yumi_joint_7_l",
      ],
      right: [
        "yumi_joint_1_r",
        "yumi_joint_2_r",
        "yumi_joint_3_r",
        "yumi_joint_4_r",
        "yumi_joint_5_r",
        "yumi_joint_6_r",
        "yumi_joint_7_r",
      ],
      both: [
        "yumi_joint_1_l",
        "yumi_joint_2_l",
        "yumi_joint_3_l",
        "yumi_joint_4_l",
        "yumi_joint_5_l",
        "yumi_joint_6_l",
        "yumi_joint_7_l",
        "yumi_joint_1_r",
        "yumi_joint_2_r",
        "yumi_joint_3_r",
        "yumi_joint_4_r",
        "yumi_joint_5_r",
        "yumi_joint_6_r",
        "yumi_joint_7_r",
      ],
    };
    this.end_effector = {
      left:  ["gripper_l_joint", "gripper_l_joint_m"],
      right: ["gripper_r_joint", "gripper_r_joint_m"],
      both: [
        "gripper_l_joint",
        "gripper_l_joint_m",
        "gripper_r_joint",
        "gripper_r_joint_m",
      ],
    };
    this.loadRobot();
  }

  loadRobot = () => {
    const loader = new ColladaLoader();
    loader.load(yumi_path, (collada) => {
      this.yumi = collada.scene;
      this.yumi.traverse((child) => {
        if (child.isMesh) {
          // model does not have normals
          child.material.flatShading = true;
        }
      });

      this.yumi.position.set(0.0, 0.0, -6.0);
      this.yumi.rotation.set(-Math.PI / 2, 0.0, -Math.PI / 2);
      this.yumi.scale.set(20, 20, 20);

      this.yumi.updateMatrix();

      this.kinematics = collada.kinematics;

      // Add the COLLADA
      this.scene.add(this.yumi);

      this.moveJointValues("left");
      // this.moveXYZValues();
      this.controlGripper("both", 2);
    });
  };

  moveJointValues = (arm, target) => {
    if (target !== undefined) {
      this.target = target;
    }

    if (this.kinematics === undefined) {
      /**
       * TODO: target object is undefined in the recursive calls!
       * It will be nice to replace setTimeout by a Promise.
       */
      setTimeout(this.moveJointValues.bind(null, arm, target), 3000);
    } else {
      this.joints[arm].forEach((joint) => {
        if (!this.kinematics.joints[joint].static) {
          const old = this.tweenParameters[joint];
          const position = old ? old : this.kinematics.getJointValue(joint);
          this.tweenParameters[joint] = position;
        }
      });
      this.kinematicsTween = new Tween(this.tweenParameters)
        .to(this.target, this.duration)
        .easing(Easing.Quadratic.Out)
        .onUpdate((object) => {
          this.joints[arm].forEach((joint) => {
            if (!this.kinematics.joints[joint].static) {
              this.kinematics.setJointValue(joint, object[joint]);
            }
          });
        })
        .start();
    }
  };

  controlGripper = (arm, act_idx, val = null) => {
    if (act_idx === 1) {
      this.end_effector[arm].forEach((finger) =>
        this.kinematics.setJointValue(finger, 0.0)
      );
    } else if (act_idx === 2) {
      this.end_effector[arm].forEach((finger) =>
        this.kinematics.setJointValue(finger, 0.025)
      );
    } else if (act_idx === 3) {
      if (val !== null) {
        if (val >= 0.0 && val <= 0.25) {
          this.end_effector[arm].forEach((finger) =>
            this.kinematics.setJointValue(finger, val)
          );
        } else {
          console.warn(
            "Gripper finger values has to be between 0.0 and 0.025!"
          );
        }
      } else {
        console.warn("Wrong Gripper Move_To value!");
      }
    } else {
      console.warn("Gripper Parameters has to be 1, 2, or 3!");
    }
  };
}

Animating the motion

import { update } from "@tweenjs/tween.js";

animate(){
      requestAnimationFrame( this.animate );
      this.renderer.render(this.scene, this.camera);
      update();
}

let target_position = {
  gripper_l_joint: 0.0125,      // 0.0 --> 0.025    , Prismatic
  gripper_l_joint_m: 0.0125,    // 0.0 --> 0.025    , Prismatic
  gripper_r_joint: 0.0125,      // 0.0 --> 0.025    , Prismatic
  gripper_r_joint_m: 0.0125,    // 0.0 --> 0.025    , Prismatic
  yumi_joint_1_l: 0.0,          // -168.5 --> 168.5 , Revolute
  yumi_joint_1_r: 0.0,          // -168.5 --> 168.5 , Revolute
  yumi_joint_2_l: -130.0,        // -143.5 --> 43.5  , Revolute
  yumi_joint_2_r: -130.0,        // -143.5 --> 43.5  , Revolute
  yumi_joint_3_l: 30,       // -123.5 --> 80.0  , Revolute
  yumi_joint_3_r: 30,       // -123.5 --> 80.0  , Revolute
  yumi_joint_4_l: 0.0,          // -290.0 --> 290.0 , Revolute
  yumi_joint_4_r: 0.0,          // -290.0 --> 290.0 , Revolute
  yumi_joint_5_l: 40.0,         // -88.0 --> 130.0  , Revolute
  yumi_joint_5_r: 40.0,         // -88.0 --> 130.0  , Revolute
  yumi_joint_6_l: 0.0,          // -229.0 --> 229.0 , Revolute
  yumi_joint_6_r: 0.0,          // -229.0 --> 229.0 , Revolute
  yumi_joint_7_l: 135.0,          // -168.0 --> 168.0 , Revolute
  yumi_joint_7_r: -135.0           // -168.0 --> 168.0 , Revolute
};

const LMR_ = new LoadMoveRobot(this.scene);
LMR_.moveJointValues('left', target_position);

this.animate();