Revert "Add implementation of LevenbegMarquardt IK"

This reverts commit d37e586.

examples/mujoco_simple_invk.py

@@ -5,7 +5,7 @@
 import mujoco.viewer
 import numpy as np
 
-from gym_lowcostrobot.simulated_robot import LevenbegMarquardtIK
+from gym_lowcostrobot.simulated_robot import SimulatedRobot
 
 
 def displace_object(data, m, object_id, viewer, square_size=0.2, invert_y=False, origin_pos=[0, 0.1]):
@@ -20,16 +20,24 @@ def displace_object(data, m, object_id, viewer, square_size=0.2, invert_y=False,
     # data.joint(object_id).qpos[:3] = [np.random.rand() * coef_sample + min_dist_obj, np.random.rand() * coef_sample + min_dist_obj, 0.01]
     data.joint(object_id).qpos[:3] = [x, y, 0.01]
 
-    mujoco.mj_forward(m, data)
+    mujoco.mj_step(m, data)
     viewer.sync()
 
 
+def do_simple_trajectory_end_effector(current_pos, target_pos):
+    # Define the trajectory
+    nb_points = 10
+    traj = np.linspace(current_pos, target_pos, nb_points)
+    return traj
+
+
 def do_simple_invk(robot_id="6dof", do_reset=False):
     if robot_id == "6dof":
         path_scene = "gym_lowcostrobot/assets/low_cost_robot_6dof/reach_cube.xml"
         joint_name = "moving_side"
         object_id = "cube"
-        min_dist = 0.065
+        nb_dof = 6
+        min_dist = 0.02
         max_dist = 0.35
         invert_y = False
         square_size = 0.2
@@ -39,6 +47,7 @@ def do_simple_invk(robot_id="6dof", do_reset=False):
 
     m = mujoco.MjModel.from_xml_path(path_scene)
     data = mujoco.MjData(m)
+    robot = SimulatedRobot(m, data)
 
     m.opt.timestep = 1 / 10000
 
@@ -51,15 +60,27 @@ def do_simple_invk(robot_id="6dof", do_reset=False):
         # Run the simulation
         while viewer.is_running():
             step_start = time.time()
+            q_target_pos = robot.inverse_kinematics_reg(ee_target_pos=cube_pos, joint_name=joint_name, nb_dof=nb_dof, step=0.4)
+            q_target_pos[-1] = 0.0
+            robot.set_target_qpos(q_target_pos)
+
+            # Step the simulation forward
+            mujoco.mj_step(m, data)
+            viewer.sync()
 
-            g_ik = LevenbegMarquardtIK(m, data, tol=min_dist)
-            g_ik.calculate(body_id=m.body(joint_name).id, goal=cube_pos, viewer=viewer)
+            # Rudimentary time keeping, will drift relative to wall clock.
+            time_until_next_step = m.opt.timestep - (time.time() - step_start)
+            if time_until_next_step > 0:
+                time.sleep(time_until_next_step)
 
             # Get the final position of the cube
-            error = np.linalg.norm(np.subtract(cube_pos, data.body(m.body(joint_name).id).xpos))
+            cube_pos = data.joint(object_id).qpos[:3]
+            ee_id = m.body(joint_name).id
+            ee_pos = data.geom_xpos[ee_id]
+            print("Cube dist:", np.linalg.norm(cube_pos - ee_pos))
 
             if do_reset:
-                if error < min_dist or error > max_dist:
+                if np.linalg.norm(cube_pos - ee_pos) < min_dist or np.linalg.norm(cube_pos - ee_pos) > max_dist:
                     print("Cube reached the target position")
 
                     # displace_object(data, m, object_id, coef_sample, min_dist_obj, viewer)

gym_lowcostrobot/simulated_robot.py

@@ -2,65 +2,6 @@
 import numpy as np
 
 
-## https://alefram.github.io/posts/Basic-inverse-kinematics-in-Mujoco
-#Levenberg-Marquardt method
-class LevenbegMarquardtIK:
-
-    def __init__(self, model, data, tol=0.04, step_size=0.5):
-        self.model = model
-        self.data = data
-        self.jacp = np.zeros((3, model.nv)) #translation jacobian
-        self.jacr = np.zeros((3, model.nv)) #rotational jacobian
-        self.step_size = step_size
-        self.tol = tol
-        self.alpha = 0.5
-        self.damping =  0.15
-
-    def check_joint_limits(self, q):
-        """Check if the joints is under or above its limits"""
-        for i in range(len(q)):
-            q[i] = max(self.model.jnt_range[i][0], 
-                       min(q[i], self.model.jnt_range[i][1]))
-
-    #Levenberg-Marquardt pseudocode implementation
-    def calculate(self, goal, body_id, viewer):
-
-        """Calculate the desire joints angles for goal"""
-        current_pose = self.data.body(body_id).xpos
-        error = np.subtract(goal, current_pose)
-
-        while (np.linalg.norm(error) >= self.tol):
-
-            #calculate jacobian
-            mujoco.mj_jac(self.model, self.data, self.jacp, self.jacr, goal, body_id)
-
-            #calculate delta of joint q
-            n = self.jacp.shape[1]
-            I = np.identity(n)
-            product = self.jacp.T @ self.jacp + self.damping * I
-
-            if np.isclose(np.linalg.det(product), 0):
-                j_inv = np.linalg.pinv(product) @ self.jacp.T
-            else:
-                j_inv = np.linalg.inv(product) @ self.jacp.T
-
-            delta_q = j_inv @ error
-
-            #compute next step
-            self.data.qpos[:1] += self.step_size * delta_q
-
-            #check limits
-            #self.check_joint_limits(self.data.qpos)
-
-            #compute forward kinematics
-            mujoco.mj_forward(self.model, self.data)
-            viewer.sync()
-
-            #calculate new error
-            error = np.subtract(goal, self.data.body(body_id).xpos)
-            print("Error: ", np.linalg.norm(error))
-
-
 class SimulatedRobot:
     def __init__(self, m, d) -> None:
         """
@@ -190,61 +131,5 @@ def inverse_kinematics_reg(self, ee_target_pos, step=0.2, joint_name="end_effect
 
         return q_target_pos
 
-    def inverse_kinematics_null_reg(self, ee_target_pos, step=0.2, joint_name="moving_side", nb_dof=6, regularization=1e-6, home_position=None, nullspace_weight=0.1):
-        """
-        Computes the inverse kinematics for a robotic arm to reach the target end effector position.
-
-        :param ee_target_pos: numpy array of target end effector position [x, y, z]
-        :param step: float, step size for the iteration
-        :param joint_name: str, name of the end effector joint
-        :param nb_dof: int, number of degrees of freedom
-        :param regularization: float, regularization factor for the pseudoinverse computation
-        :param home_position: numpy array of home joint positions to regularize towards
-        :param nullspace_weight: float, weight for the nullspace regularization
-        :return: numpy array of target joint positions
-        """
-        if home_position is None:
-            home_position = np.zeros(nb_dof)  # Default to zero if no home position is provided
-
-        try:
-            # Get the joint ID from the name
-            joint_id = self.m.body(joint_name).id
-        except KeyError:
-            raise ValueError(f"Body name '{joint_name}' not found in the model.")
-
-        # Get the current end effector position
-        ee_id = self.m.body(joint_name).id
-        ee_pos = self.d.geom_xpos[ee_id]
-
-        # Compute the Jacobian
-        jac = np.zeros((3, self.m.nv))
-        mujoco.mj_jacBodyCom(self.m, self.d, jac, None, joint_id)
-        print(jac)
-
-        # Compute the difference between target and current end effector positions
-        delta_pos = ee_target_pos - ee_pos
-
-        # Compute the pseudoinverse of the Jacobian with regularization
-        jac_reg = jac[:, :nb_dof].T @ jac[:, :nb_dof] + regularization * np.eye(nb_dof)
-        jac_pinv = np.linalg.inv(jac_reg) @ jac[:, :nb_dof].T
-
-        # Compute target joint velocities
-        qdot = jac_pinv @ delta_pos
-
-        # Add nullspace regularization to keep joint positions close to the home position
-        qpos = self.d.qpos[:nb_dof]
-        nullspace_reg = nullspace_weight * (home_position - qpos)
-        qdot += nullspace_reg
-
-        # Normalize joint velocities to avoid excessive movements
-        qdot_norm = np.linalg.norm(qdot)
-        if qdot_norm > 1.0:
-            qdot /= qdot_norm
-
-        # Compute the new joint positions
-        q_target_pos = qpos + qdot * step
-
-        return q_target_pos
-
     def set_target_qpos(self, target_pos):
         self.d.ctrl = target_pos