Very Small Discrepancies between the forward kinematics and the ground truth value

[kczttm]

Bug description

Hello, I'm trying to implement a Kinova Gen3 7dof robot into the Pinocchio framework on python. My goal is to utilize the fast calculation in real time. I understand that I need to convert the standard joint angles of size 7x1 to the augmented one 11x1 (we have 4 unbounded joint angles). Below is my conversion code as well as a forward kinematics calculation of the end-effector. As I compared the calculated value with the robot motor reading, there was about 6-7 mm difference. I checked to see if I used a wrong URDF or if I used the wrong frame, both seemed to be correct.

kinova_urdf = os.path.join(ropo_root, "gen3_7dof", "config", "GEN3_URDF_V12.urdf")

class SpringMassOscillationModel:
    def __init__(self):
        ## Mimic a spring-mass system with no damping
        # Robot model
        self.model = pin.buildModelFromUrdf(kinova_urdf)
        self.data = self.model.createData()
        self.EE_frame_id = self.model.getFrameId("EndEffector")

        # Oscillation parameters
        self.omega = 2 * np.pi  # Natural frequency
        self.k = 500.0  # Spring stiffness
        self.m = 1.0    # Apparent mass

        # Gravity constant
        self.g = 9.81  # m/s^2

        # Initial conditions
        self.z_0 = 0.5  # Initial z-position
        self.amplitude = 0.2  # Oscillation amplitude

    def get_end_effector_pose(self, q: np.ndarray) -> np.ndarray:
        """Get current end-effector pose"""
        pin.forwardKinematics(self.model, self.data, q)
        pin.updateFramePlacement(self.model, self.data, self.EE_frame_id)
        T = self.data.oMf[self.EE_frame_id]
        position = T.translation
        rotation = np.degrees(pin.rpy.matrixToRpy(T.rotation))
        return np.concatenate([position, rotation])

    def standard_to_pinocchio(self, q: np.ndarray) -> np.ndarray:
        """Convert standard joint angles (rad) to Pinocchio joint angles"""
        q_pin = np.zeros(self.model.nq)
        for i, j in enumerate(self.model.joints[1:]):
            if j.nq == 1:
                q_pin[j.idx_q] = q[j.idx_v]
            else:
                # cos(theta), sin(theta)
                q_pin[j.idx_q:j.idx_q+2] = np.array([np.cos(q[j.idx_v]), np.sin(q[j.idx_v])])
        return q_pin

    def pinocchio_to_standard(self, q_pin: np.ndarray) -> np.ndarray:
        """Convert Pinocchio joint angles to standard joint angles (rad)"""
        q = np.zeros(self.model.nv)
        for i, j in enumerate(self.model.joints[1:]):
            if j.nq == 1:
                q[j.idx_v] = q_pin[j.idx_q]
            else:
                q_back = np.arctan2(q_pin[j.idx_q+1], q_pin[j.idx_q])
                q[j.idx_v] = q_back + 2*np.pi if q_back < 0 else q_back
        return q

Below is the main function to obtain the comparison

with utilities.DeviceConnection.createTcpConnection(args) as router, utilities.DeviceConnection.createUdpConnection(args) as router_real_time:
    torque_example = TorqueExample(router, router_real_time, 3.0)
    base_feedback = torque_example.SendCallWithRetry(torque_example.base_cyclic.RefreshFeedback, 3)
    q, qdot = get_realtime_q_qdot(base_feedback)
    print(f"q: {np.degrees(q)}")
    # get end effector pose
    x_current = base_feedback.base.tool_pose_x
    y_current = base_feedback.base.tool_pose_y
    z_current = base_feedback.base.tool_pose_z
    theta_x = base_feedback.base.tool_pose_theta_x
    theta_y = base_feedback.base.tool_pose_theta_y
    theta_z = base_feedback.base.tool_pose_theta_z
    p_ee = np.array([x_current, y_current, z_current, theta_x, theta_y, theta_z])
    print(f"p_ee: {p_ee}")

    q_pin = p_model.standard_to_pinocchio(q)
    # q_back = p_model.pinocchio_to_standard(q_pin)
    # print(f"q_back: {q_back.T}")
    # forward kinematics
    p_cal = p_model.get_end_effector_pose(q_pin)
    print(f"p_cal: {p_cal.T}")

and the result was

q: [359.9981   7.6853  97.9248 290.6336 359.999   28.0874  46.0158]
p_ee: [   0.1361    0.4065    0.9202 -150.9147 -144.3868   29.7897]
p_cal: [   0.1303    0.3909    0.9306   27.9311  -33.9848 -149.7489]

Now I can only suspect my standard_to_pinocchio() conversion since I couldn't find how other people hand this.

[jcarpent]

It is hard to dive into your comments as it is a very long explanation. Why do you want to convert between the two conventions? The idea, is that the configuration vector of an unbounded joint is [cos(theta),sin(theta)] while theta is the configuration of classic convention. Hope it will be help you.

[jcarpent]

In q: [359.9981 7.6853 97.9248 290.6336 359.999 28.0874 46.0158], it seems these values are in def, no?