How to solve forward kinematics for a robot arm whose base is not at the origin of the robot ? #2074

threedee003 · 2023-11-02T07:45:58Z

threedee003
Nov 2, 2023

I am trying to use pinocchio for solving FK and IK for a custom robot ( a UR3 arm on top of a mounted Clearpath Husky ) now when I am solving the FK the Y value of the position array is coming fine but the X and Z values are coming different and I am not getting it. Can anyone please help me out ? How does the universe work in the model generated from
pinocchio.buildModelsFromUrdf ?

The kinematic tree I am using here is

 Joint 0 universe: parent=0
 Joint 1 ur_arm_shoulder_pan_joint: parent=0
 Joint 2 ur_arm_shoulder_lift_joint: parent=1
 Joint 3 ur_arm_elbow_joint: parent=2
 Joint 4 ur_arm_wrist_1_joint: parent=3
 Joint 5 ur_arm_wrist_2_joint: parent=4
 Joint 6 ur_arm_wrist_3_joint: parent=5

Here is the link for the urdf https://github.com/threedee003/urdfs-here/blob/main/husky_ur.urdf

Answered by jorisv

Nov 2, 2023

You can add the following code at the end your file to see all the transformations between universe and the end effector :

model = ik_solver.reduced_model
data = ik_solver.model_data
end_effector_id = ik_solver.id_end_effector

id = end_effector_id
# store frame name and transformation in reverse order since the end effector
frame_transform = []
while True:
    frame = model.frames[id]
    previous_frame = frame.previousFrame
    frame_transform.append((frame.name, data.oMf[id]))
    if id == previous_frame:
        break
    id = previous_frame

# Display frame from universe to end effector
for ft in reversed(frame_transform):
    print(f"frame: {ft[0]}")
    print(f"transform: {ft[1]}")

T…

View full answer

jorisv · 2023-11-02T09:22:14Z

jorisv
Nov 2, 2023
Maintainer

Hello @threedee003,

I don't have access to the urdf (I think your repository is private).
Can you also provide me a piece of code to give me a better understanding of your issue ?

0 replies

threedee003 · 2023-11-02T10:12:38Z

threedee003
Nov 2, 2023
Author

@jorisv

https://github.com/threedee003/urdfs-here/blob/main/husky_ur.urdf

This is the urdf link. The repo is Public.
This urdf contains a custom robot with mobile Husky base which has a UR3 mounted on top of the mobile robot. ( Although the robot name is husky but it is integrated with the UR3 arm ).
Now I want to calculate fwd kinematics and inverse kinematics for the robot arm w.r.t to the world ie the universe frame. I am using Nvidia's Isaac Sim to simulate the robot. So in the sim I want to move the arm using Pinocchio's IK.

The kinematic tree that I am using in this case

 Joint 0 universe: parent=0
 Joint 1 ur_arm_shoulder_pan_joint: parent=0
 Joint 2 ur_arm_shoulder_lift_joint: parent=1
 Joint 3 ur_arm_elbow_joint: parent=2
 Joint 4 ur_arm_wrist_1_joint: parent=3
 Joint 5 ur_arm_wrist_2_joint: parent=4
 Joint 6 ur_arm_wrist_3_joint: parent=5





import pinocchio as pin
import numpy as np
import os
import math
from typing import Optional



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_wrist_3_joint"
            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)




      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])



      @property
      def show_model(self):
            return self.reduced_model

      def solve_inv_kinematics(self):
            #to be updated
            pass

This is the class I have written using Pinocchio, now I would like to enter 6 joint values in the form of numpy array and generate the position and orientation of the end effector. Now when I run this code using


import numpy as np
import pinocchio as pin
from IK_utils import Pinocchio_Husky_UR3_IK_Solver
import pinocchio as pin







ik_solver = Pinocchio_Husky_UR3_IK_Solver(urdf_file_path="./husky_ur3.urdf")



joints = np.array([1.0528518, -1.8536792, -0.02006058, -0.7816168, 1.2877516, 1.8681929])

x = ik_solver.show_model
pos, orien = ik_solver.solve_fwd_kinematics(joints)



print(pos)
print(orien)

print(x)

I am getting a value in which the Y component of the position is coming correct. But X, Z values are different and orientation is not same as in the simualtor. I am spawning the robot at pos = [0,0,0] in the simualtor with the joint angles
joints = np.array([1.0528518, -1.8536792, -0.02006058, -0.7816168, 1.2877516, 1.8681929])
How can I check and debug the transformation vectors that Pinocchio is using ? Or I would be obliged if you tell me what to do to get a good set of fwd kinematic values ?

The values that are being generated from forward kinematics those values are w.r.t which frame ?

5 replies

jorisv Nov 2, 2023
Maintainer

You can add the following code at the end your file to see all the transformations between universe and the end effector :

model = ik_solver.reduced_model
data = ik_solver.model_data
end_effector_id = ik_solver.id_end_effector

id = end_effector_id
# store frame name and transformation in reverse order since the end effector
frame_transform = []
while True:
    frame = model.frames[id]
    previous_frame = frame.previousFrame
    frame_transform.append((frame.name, data.oMf[id]))
    if id == previous_frame:
        break
    id = previous_frame

# Display frame from universe to end effector
for ft in reversed(frame_transform):
    print(f"frame: {ft[0]}")
    print(f"transform: {ft[1]}")

The result I get is the following :

frame: universe
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0 0 0

frame: root_joint
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0 0 0

frame: base_link
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0 0 0

frame: top_plate_joint
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0.0812      0  0.245

frame: top_plate_link
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0.0812      0  0.245

frame: top_plate_front_joint
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0.44487       0 0.25139

frame: top_plate_front_link
transform:   R =
1 0 0
0 1 0
0 0 1
  p = 0.44487       0 0.25139

frame: arm_mount_joint
transform:   R =
          -1  4.64102e-08            0
-4.64102e-08           -1            0
           0            0            1
  p = 0.33987       0 0.25139

frame: ur_arm_base_link
transform:   R =
          -1  4.64102e-08            0
-4.64102e-08           -1            0
           0            0            1
  p = 0.33987       0 0.25139

frame: ur_arm_base_link-base_link_inertia
transform:   R =
           1 -4.64102e-08            0
 4.64102e-08            1            0
           0            0            1
  p = 0.33987       0 0.25139

frame: ur_arm_base_link_inertia
transform:   R =
           1 -4.64102e-08            0
 4.64102e-08            1            0
           0            0            1
  p = 0.33987       0 0.25139

frame: ur_arm_shoulder_pan_joint
transform:   R =
 0.495095 -0.868839         0
 0.868839  0.495095         0
        0         0         1
  p = 0.33987       0 0.40329

frame: ur_arm_shoulder_link
transform:   R =
 0.495095 -0.868839         0
 0.868839  0.495095         0
        0         0         1
  p = 0.33987       0 0.40329

frame: ur_arm_shoulder_lift_joint
transform:   R =
   -0.138194     0.475418     0.868839
   -0.242515     0.834306    -0.495095
   -0.960255    -0.279125 -2.05103e-10
  p = 0.33987       0 0.40329

frame: ur_arm_upper_arm_link
transform:   R =
   -0.138194     0.475418     0.868839
   -0.242515     0.834306    -0.495095
   -0.960255    -0.279125 -2.05103e-10
  p = 0.33987       0 0.40329

frame: ur_arm_elbow_joint
transform:   R =
   -0.147702      0.47255     0.868839
   -0.259201     0.829274    -0.495095
   -0.954463    -0.298331 -2.05103e-10
  p =  0.373541 0.0590887  0.637256

frame: ur_arm_forearm_link
transform:   R =
   -0.147702      0.47255     0.868839
   -0.259201     0.829274    -0.495095
   -0.954463    -0.298331 -2.05103e-10
  p =  0.373541 0.0590887  0.637256

frame: ur_arm_wrist_1_joint
transform:   R =
   -0.437713     0.231359     0.868839
   -0.768139      0.40601    -0.495095
   -0.467302    -0.884098 -2.05103e-10
  p =  0.502652 0.0587395  0.840795

frame: ur_arm_wrist_1_link
transform:   R =
   -0.437713     0.231359     0.868839
   -0.768139      0.40601    -0.495095
   -0.467302    -0.884098 -2.05103e-10
  p =  0.502652 0.0587395  0.840795

frame: ur_arm_wrist_2_joint
transform:   R =
 0.712023  0.662946 -0.231359
-0.689921  0.599304  -0.40601
-0.130508  0.448707  0.884098
  p =  0.482906 0.0240865  0.916253

frame: ur_arm_wrist_2_link
transform:   R =
 0.712023  0.662946 -0.231359
-0.689921  0.599304  -0.40601
-0.130508  0.448707  0.884098
  p =  0.482906 0.0240865  0.916253

frame: ur_arm_wrist_3_joint
transform:   R =
0.0125573 -0.748562  0.662946
 0.590356  0.540662  0.599304
-0.807045  0.383848  0.448707
  p =  0.537201 0.0731695  0.953002

You can maybe compare frame by frame with the Nvidia's Isaac Sim to see where a divergence occur.

Answer selected by threedee003

threedee003 Nov 3, 2023
Author

@jorisv Thanks for your reply and support. I figured out my issue.

The Forward Kinematics value which is being generated using Pinocchio is working fine, the position value of the fwd kinematics and the value generated from Nvidia's Isaac Sim is coming same but the orientation is being different in some cases.
In certatin cases the values of orientation are perfectly fine. But for some cases of joint values orientation is coming different.

Can you guess why ?

jorisv Nov 3, 2023
Maintainer

@threedee003 How do you compare orientation ? Are you comparing quaternion ? rotation matrix ?

Can you give two joints configuration (q) : one that give same orientation between Nvidia's Isaac Sim and Pinocchio and one that give different orientation. Can you also provide me Nvidia Isaac Sim orientation for these two configuration (since I can't run it) ?

threedee003 Nov 3, 2023
Author

I am comparing the orientation in the form of quaternion and the format is numpy.array([w,x,y,z]).

But I have solved my issue. It was a mistake from my end. Sorry to bother you @jorisv. I had a different end effector id so it was giving a different quaternion result. My apologies.

Thanks for your help and support.
Regards,
Tribikram.

jorisv Nov 6, 2023
Maintainer

No problem, this kind of mistake happen ;)

I'm still available if you have others issues.
Regards,
Joris

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

How to solve forward kinematics for a robot arm whose base is not at the origin of the robot ? #2074

{{title}}

Replies: 2 comments 5 replies

{{title}}

{{title}}

{{editor}}'s edit

{{editor}}'s edit

{{title}}

{{title}}

{{title}}

{{title}}

{{title}}

Select a reply

How to solve forward kinematics for a robot arm whose base is not at the origin of the robot ? #2074

threedee003 Nov 2, 2023

Replies: 2 comments · 5 replies

jorisv Nov 2, 2023 Maintainer

threedee003 Nov 2, 2023 Author

jorisv Nov 2, 2023 Maintainer

threedee003 Nov 3, 2023 Author

jorisv Nov 3, 2023 Maintainer

threedee003 Nov 3, 2023 Author

jorisv Nov 6, 2023 Maintainer

threedee003
Nov 2, 2023

Replies: 2 comments 5 replies

jorisv
Nov 2, 2023
Maintainer

threedee003
Nov 2, 2023
Author

jorisv Nov 2, 2023
Maintainer

threedee003 Nov 3, 2023
Author

jorisv Nov 3, 2023
Maintainer

threedee003 Nov 3, 2023
Author

jorisv Nov 6, 2023
Maintainer