Flexiv Rizon4 URDF discrepancy of 4 centimeters

test_fk_urdf_difference.py

import time

import numpy as np
import pybullet as pb

np.random.seed(42)
rest_pose = np.random.randn(7)


def run_forward_kinematics(robot_id):
    num_joints = pb.getNumJoints(robot_id)
    joint_info = [pb.getJointInfo(robot_id, i) for i in range(num_joints)]
    joint_ids = [j[0] for j in joint_info if j[2] == pb.JOINT_REVOLUTE]

    # now this is 7
    n_joints = len(joint_ids)
    ee_idx = n_joints

    # set joint angles
    # print(n_joints, "is n_joints before setting")

    for i in range(n_joints):
        pb.resetJointState(robot_id, i, rest_pose[i])

    tcp_pose = pb.getLinkState(robot_id, ee_idx, computeForwardKinematics=1)

    return tcp_pose


def draw_coordinate_triad(position, orientation, length=0.1, line_width=2.0):
    """
    Draws a coordinate triad (X, Y, Z axes) at a given pose in the world frame.

    Args:
        position: A list or tuple of (x, y, z) coordinates.
        orientation: A list or tuple of quaternion (x, y, z, w).
        length: The length of each axis line.
        line_width: The width of the axis lines.
    """
    # Define the axes in the local frame
    axes = np.array(
        [
            [length, 0, 0],  # X-axis
            [0, length, 0],  # Y-axis
            [0, 0, length],  # Z-axis
        ]
    )

    # Convert the quaternion to a rotation matrix
    rot_matrix = np.array(pb.getMatrixFromQuaternion(orientation)).reshape(3, 3)

    # Transform the axes to the world frame
    transformed_axes = np.dot(rot_matrix, axes.T).T + np.array(position)

    # Starting point is the position
    start = np.array(position)

    # Draw the X-axis in red
    pb.addUserDebugLine(
        start,
        transformed_axes[0],
        [1, 0, 0],  # Red color
        lineWidth=line_width,
    )

    # Draw the Y-axis in green
    pb.addUserDebugLine(
        start,
        transformed_axes[1],
        [0, 1, 0],  # Green color
        lineWidth=line_width,
    )

    # Draw the Z-axis in blue
    pb.addUserDebugLine(
        start,
        transformed_axes[2],
        [0, 0, 1],  # Blue color
        lineWidth=line_width,
    )


if __name__ == "__main__":
    client = pb.connect(pb.GUI)

    pb.setGravity(0, 0, -9.81)

    # OLD URDF
    urdf_path = "robots/flexiv_rizon4_kinematics.urdf"
    robot_id = pb.loadURDF(urdf_path, useFixedBase=True)
    tcp_pose_old = run_forward_kinematics(robot_id)
    draw_coordinate_triad(tcp_pose_old[0], tcp_pose_old[1])
    tcp_pose_old_translation = tcp_pose_old[0]

    # NEW URDF
    urdf_path = "robots/flexiv_v1_4_rizon4_kinematics.urdf"
    robot_id_new = pb.loadURDF(urdf_path, useFixedBase=True)
    tcp_pose_new = run_forward_kinematics(robot_id_new)
    draw_coordinate_triad(tcp_pose_new[0], tcp_pose_new[1])
    tcp_pose_new_translation = tcp_pose_new[0]

    tcp_diff_translation = np.array(tcp_pose_old_translation) - np.array(tcp_pose_new_translation)

    import pdb

    pdb.set_trace()

    while True:
        time.sleep(1)