Inverse Kinematics for custom robot. Help required to build proper reduced model to generate IK solver.

[threedee003]

Dear @jorisv,

Previously you helped me out in this discussion thread.
#2074

import pinocchio as pin
import numpy as np
import os
import math
from typing import Optional
from scipy.spatial.transform import Rotation





def quaternion_to_rotation_matrix(q):
    w, x, y, z = q
    rotation_matrix = np.array([
        [1 - 2*y**2 - 2*z**2, 2*x*y - 2*w*z, 2*x*z + 2*w*y],
        [2*x*y + 2*w*z, 1 - 2*x**2 - 2*z**2, 2*y*z - 2*w*x],
        [2*x*z - 2*w*y, 2*y*z + 2*w*x, 1 - 2*x**2 - 2*y**2]
    ])
    return rotation_matrix





class Pinocchio_Husky_UR3_IK_Solver(object):

      def __init__(self,
                   urdf_file_path: str,
                   include_wheels: Optional[bool] = False,
                  ) -> None:
            
            self.damping_coefficient = 1e-10
            self.model = pin.buildModelFromUrdf(urdf_file_path)


            
            name_end_effector = "ur_arm_flange"
            self.joints_of_interest = ["ur_arm_shoulder_pan_joint",
                                    "ur_arm_shoulder_lift_joint",
                                    "ur_arm_elbow_joint",
                                    "ur_arm_wrist_1_joint",
                                    "ur_arm_wrist_2_joint",
                                    "ur_arm_wrist_3_joint"]          
            self.wheels = ["front_left_wheel",
                              "front_right_wheel",
                              "rear_left_wheel",
                              "rear_right_wheel"]
            if include_wheels:
                  self.joints_of_interest += self.wheels

            self.remove_ids = list()
            for joint in self.joints_of_interest:
                  if self.model.existJointName(joint):
                        self.remove_ids.append(self.model.getJointId(joint))
                  else:
                        raise NameError("[IK Warning]: The joint " + str(joint) + " does not belong to the model !! Please check the URDF !!")


            self.jointIds_to_exclude = np.delete(np.arange(0,self.model.njoints), self.remove_ids)
            self.reference_config = pin.neutral(self.model)


            self.joints_of_interest_upper_limits = np.array([6.28318, 6.28318, 6.28318, 6.28318, 6.28318, 6.28318])
            self.joints_of_interest_lower_limits = -1*self.joints_of_interest_upper_limits
            self.reduced_model = pin.buildReducedModel(self.model, self.jointIds_to_exclude[1:].tolist(), reference_configuration = self.reference_config)
            assert (len(self.reduced_model.joints) == (len(self.joints_of_interest)+1)), "[IK Error]: Number of joints to move != nDOFs"
            self.model_data = self.reduced_model.createData()
            self.joint_position_mid = (self.joints_of_interest_upper_limits + self.joints_of_interest_lower_limits)/2
            self.id_end_effector = self.reduced_model.getFrameId(name_end_effector)
            self.id_EE = self.reduced_model.getFrameId(name_end_effector)
            # for jts in self.joints_of_interest:
            #       print(f"{jts} ==== {self.reduced_model.getFrameId(jts)}")
            # print(self.model)




      def solve_fwd_kinematics(self, current_joints) -> np.ndarray:
            pin.framesForwardKinematics(self.reduced_model, self.model_data, current_joints)
            #pin.updateFramePlacements(self.reduced_model, self.model_data)
            oMf = self.model_data.oMf[self.id_end_effector]
            ee_pos = oMf.translation
            ee_quat = pin.Quaternion(oMf.rotation)

            return ee_pos, np.array([ee_quat.w, ee_quat.x, ee_quat.y, ee_quat.z])


            



      def solve_ik(self, pos, orien):
            oMdes = pin.SE3(quaternion_to_rotation_matrix(orien), pos)
            epochs = 1e6
            eps = 1e-4
            dt = 0.1
            damp = 1e-12
            q = pin.neutral(self.reduced_model)
            iter = 0
            print(self.reduced_model.joints[5])
            #print(self.model_data.oMi[self.id_end_effector])
            while True:
                  pin.forwardKinematics(self.reduced_model, self.model_data, q)
                  iMd = self.model_data.oMi[6].actInv(oMdes)
                  err = pin.log(iMd).vector
                  if np.linalg.norm(err) < eps:
                        success = True
                        break
                  if iter >= epochs:
                        success = False
                        break
                  J = pin.computeJointJacobian(self.reduced_model, self.model_data, q, 6)
                  J = -np.dot(pin.Jlog6(iMd.inverse()), J)
                  v = - J.T.dot(np.linalg.solve(J.dot(J.T) + damp*np.eye(6), err))
                  q = pin.integrate(self.reduced_model, q, v*dt)
                  iter += 1

            if success:
                  print("[IK message] : Convergence achieved !")
            else:
                  print("[IK Failure] : IK could not find a solution in the {} epochs. This position cannot be reached by the EE".format(epochs))
           
            return q

   

The Forward Kinematics is working fine.
I am now trying to implement the Inverse Kinematics solver using Pinocchio but I am not getting the desired value which I am getting from the forward kinematics.

My end effector link name is ur_arm_flange
and
for a end effector pos and orientation value of
pos = np.array([0.0767773, 0.13673637, 0.70474549]), orien=np.array([-0.24786546, -0.3646479, 0.8693914, -0.22305428])

(quaternion in w,x,y,z format) I should get a joint values of

j2 = np.array([-3.42705, -1.4965, -1.35691, -2.49577, -2.31094, -2.142])

For inverse kinematics I think I am probably not passing the correct joint ID.
I checked out the joint ID of my end effector is 46 in my case. Could you please check ? What is the correct way of passing joint ID ?

[jorisv]

Hello @threedee003,

Your IK code is minimizing the error between the joint 6 (that support ur_arm_flange) and the desired configuration.
You should minimize the error between the frame ur_arm_flange and the desired configuration.

The following code will do the job (I put comment to explain the difference with your code) :

        while True:
            # framesForwardKinematics instead of forwardKinematics to compute the frame configuration
            pin.framesForwardKinematics(self.reduced_model, self.model_data, q)
            # oMf instead of oMi to take the frame (not the joint)
            iMd = self.model_data.oMf[self.id_end_effector].actInv(oMdes)
            err = pin.log(iMd).vector
            if np.linalg.norm(err) < eps:
                success = True
                break
            if iter >= epochs:
                success = False
                break
            # computeFrameJacobian instead of computeJointJacobian to compute the frame jacobian
            J = pin.computeFrameJacobian(
                self.reduced_model,
                self.model_data,
                q,
                self.id_end_effector,
            )
            J = -np.dot(pin.Jlog6(iMd.inverse()), J)
            v = -J.T.dot(np.linalg.solve(J.dot(J.T) + damp * np.eye(6), err))
            q = pin.integrate(self.reduced_model, q, v * dt)
            iter += 1

Also, instead of defining you own quaternion to matrix function quaternion_to_rotation_matrix you can package your quaternion (X, Y, Z, W) inside a pin.Quaternion.

The result configuration can still be different from the one you want. Because of singularities, the optimization process can make big step. You can play with the initial configuration, dt and damp variable to overcome this.

[threedee003]

Dear @jorisv ,

Thanks for your reply and help. The IK solver works very nicely but there is a minute error I think which can be solved by tuning the dt and damping_factor.

Thanks and regards,
Tribikram.