ika working

examples/main.js

@@ -76,6 +76,9 @@ export class MuJoCoDemo {
     this.ikJoints = ['shoulder1_left', 'shoulder2_left', 'elbow_left'];
     this.ikEndEffector = 'hand_left';
 
+    this.estimateControlForPositionDiff = this.estimateControlForPositionDiff.bind(this);
+    this.ikControlRecords = [];
+
     this.bodyNameToId = this.createBodyNameToIdMap();
     this.actuatorNameToId = this.createActuatorNameToIdMap();
 
@@ -224,8 +227,12 @@ export class MuJoCoDemo {
     }
   }
 
+  /* Inverse Kinematics */
   toggleIK() {
     this.ikEnabled = !this.ikEnabled;
+    if (this.ikEnabled) {
+      this.originalQpos = new Float64Array(this.simulation.qpos);
+    }
     console.log(`IK ${this.ikEnabled ? 'enabled' : 'disabled'}`);
   }
 
@@ -247,6 +254,12 @@ export class MuJoCoDemo {
     const targetPosition = this.ikTarget;
     const maxIterations = 10;
     const epsilon = 0.01;
+    const timeStep = this.model.getOptions().timestep;
+
+    let iterationRecord = {
+      targetPosition: targetPosition.toArray(),
+      joints: {}
+    };
 
     console.log(`IK Target: ${targetPosition.toArray()}`);
 
@@ -255,10 +268,6 @@ export class MuJoCoDemo {
       getPosition(this.simulation.xpos, this.getBodyIdByName(this.ikEndEffector), currentPosition);
       const error = targetPosition.clone().sub(currentPosition);
 
-      console.log(`Iteration ${iteration}:`);
-      console.log(`  Current Position: ${currentPosition.toArray()}`);
-      console.log(`  Error: ${error.toArray()} (magnitude: ${error.length()})`);
-
       if (error.length() < epsilon) {
         console.log('Target reached, stopping IK');
         break;
@@ -279,24 +288,39 @@ export class MuJoCoDemo {
         }
 
         const delta = error.dot(jacobian) / jacobian.lengthSq();
-        console.log(`  Joint ${jointName}:`);
-        console.log(`    Jacobian: ${jacobian.toArray()}`);
-        console.log(`    Delta: ${delta}`);
 
         // Limit the maximum change in joint position
-        const maxDelta = 0.1;
+        const maxDelta = 0.01;
         const clampedDelta = Math.max(-maxDelta, Math.min(maxDelta, delta));
 
         const jointQposAdr = this.model.jnt_qposadr[jointId];
         this.simulation.qpos[jointQposAdr] += clampedDelta;
 
+
+        // Estimate control for this joint
+        const estimatedControl = this.estimateControlForPositionDiff(jointId, clampedDelta, timeStep);
+
+        // Record the joint information
+        if (!iterationRecord.joints[jointName]) {
+          iterationRecord.joints[jointName] = [];
+        }
+        iterationRecord.joints[jointName].push({
+          positionChange: clampedDelta,
+          estimatedControl: estimatedControl
+        });
+
         console.log(`    New joint position: ${this.simulation.qpos[jointQposAdr]}`);
       }
 
       this.simulation.forward();
     }
+
+    // Add the iteration record to the overall IK control records
+    this.ikControlRecords.push(iterationRecord);
+
+    // Optionally, you can save the records to a file here or provide a method to do so
+    // this.saveIKControlRecords();
   }
-
 
   calculateJacobian(jointId) {
     const epsilon = 0.001;
@@ -336,15 +360,38 @@ export class MuJoCoDemo {
     this.simulation.qpos[jointQposAdr] = originalValue;
     this.simulation.forward();
 
-    console.log(`Joint ${this.actuatorNames[jointId]}:`);
-    console.log(`  Original position: ${originalPos.toArray()}`);
-    console.log(`  Positive perturbation: ${posPos.toArray()}`);
-    console.log(`  Negative perturbation: ${posNeg.toArray()}`);
-    console.log(`  Jacobian: ${jacobian.toArray()}`);
-
     return jacobian;
   }
 
+
+  estimateControlForPositionDiff(jointIndex, positionDiff, timeStep) {
+    const jointAddress = this.model.jnt_qposadr[jointIndex];
+
+    // Get inverse mass and damping for the joint
+    let invMass = this.model.dof_invweight0 ? this.model.dof_invweight0[jointAddress] : 1 / timeStep;
+    let damping = this.model.dof_damping ? this.model.dof_damping[jointAddress] : 0.1;
+
+    // Estimate velocity required to achieve position difference
+    const estimatedVelocity = positionDiff / timeStep;
+
+    // Calculate force required to overcome damping and achieve velocity
+    const dampingForce = damping * estimatedVelocity;
+    const accelerationForce = invMass * estimatedVelocity;
+
+    console.log("velocity", estimatedVelocity);
+    console.log("dampingForce", dampingForce);
+    console.log("accelerationForce", accelerationForce);
+
+    // Total force is the sum of damping and acceleration forces
+    const totalForce = dampingForce + accelerationForce;
+
+    // Convert force to control input (assuming control input is proportional to force)
+    const controlScale = 1.0; // Adjust this based on your model's characteristics
+    const estimatedControl = totalForce * controlScale;
+
+    return estimatedControl;
+  }
+
   /* MAPPINGs */
 
   getJointIdByName(name) {
@@ -411,6 +458,18 @@ export class MuJoCoDemo {
     return this.actuatorNameToId[name] !== undefined ? this.actuatorNameToId[name] : -1;
   }
 
+  getActuatorNameForJoint(jointName) {
+    // This is a simplification. You might need to adjust this based on your naming conventions
+    for (const actuatorName of this.actuatorNames) {
+      if (actuatorName.includes(jointName)) {
+        return actuatorName;
+      }
+    }
+    return null;
+  }
+
+
+  /* Actuators/Model control*/
 
   moveActuator(prefix, amount) {
     for (let i = 0; i < this.actuatorNames.length; i++) {

examples/mujocoUtils.js

@@ -120,6 +120,37 @@ export function setupGUI(parentContext) {
 
   parentContext.gui.add(uploadButton, 'upload').name('Upload Scene');
 
+  // Add a button to download IK control records
+  let downloadButton = {
+    download: function() {
+      // Convert the records to a JSON string
+      const recordsJSON = JSON.stringify(parentContext.ikControlRecords, null, 2);
+
+      // Create a Blob with the JSON data
+      const blob = new Blob([recordsJSON], { type: 'application/json' });
+
+      // Create a temporary URL for the Blob
+      const url = URL.createObjectURL(blob);
+
+      // Create a temporary anchor element and trigger the download
+      const a = document.createElement('a');
+      a.href = url;
+      a.download = 'ik_control_records.json';
+      a.click();
+
+      // Clean up by revoking the URL
+      URL.revokeObjectURL(url);
+
+      // Clear the records after saving
+      parentContext.ikControlRecords = [];
+
+      console.log('IK control records downloaded and cleared');
+    }
+  };
+
+  parentContext.gui.add(downloadButton, 'download').name('Download IK Records');
+
+
 
   function updateSceneDropdown(dropdown, scenes) {
     // Store the current onChange function

examples/scenes/dora/dora2.xml

@@ -70,7 +70,7 @@
     <light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
     <geom name="ground" type="plane" size="0 0 1" pos="0.001 0 0" quat="1 0 0 0" material="matplane" condim="3" conaffinity='15'/>
 
-    <body name="base_link" pos="0 0 1">
+    <body name="base_link" pos="0 0 0.7">
       <inertial pos="-0.026899 -1.898e-06 0.035918" quat="0.99999 0.00379473 0.00224104 -9.36135e-05" mass="5.7651" diaginertia="0.0200531 0.0145821 0.0131238"/>
       <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.89804 0.91765 0.92941 1" mesh="base_link"/>
       <geom type="mesh" rgba="0.89804 0.91765 0.92941 1" mesh="base_link" class="visualgeom"/>

examples/scenes/dora/my_dora2.xml

@@ -38,7 +38,8 @@
     <geom name="floor" type="plane" pos="0.01 0 0" size="10 10 0.1" material="matplane"/>
     <light directional="true" diffuse=".4 .4 .4" specular="0.1 0.1 0.1" pos="0 0 5.0" dir="0 0 -1" castshadow="false"/>
     <light directional="true" diffuse=".6 .6 .6" specular="0.2 0.2 0.2" pos="0 0 4" dir="0 0 -1"/>
-    <body name="base_link" pos="0 0 0.7">
+    <!-- .73 -> close to ground -->
+    <body name="base_link" pos="0 0 0.73">
       <site name="imu_site" pos="0 0 0" size="0.01"/>
       <freejoint name="root"/>
       <inertial mass="5.7651" diaginertia="0.020053 0.014582 0.013124" pos="-0.026899 -1.898e-06 0.035918" quat="0 0 0 1"/>