myCobot 320 m5 2022 kinematics

[Green1sGood]

Hello! I have 2 questions about myCobot 320 m5 2022 robot:

1. Where can I find the most accurate urdf file and corresponding DH parameters for this robot?
2. Currently I have the latest versions: 2.4 for minirobot, 1.5 for picoMain and 5.2 for AtomMain (also got these values with 'get_basic_version', 'get_system_version' and 'get_atom_version' methods). 'angles_to_coords' method for forward kinematics works as expected, but I got strange [8241, 8241, 8241, 8241, 8241, 8241] result list from 'solve_inv_kinematics' every time regardless of input data

[anla-xu]

Please check here to see if you have the information you need: https://github.com/elephantrobotics/mycobot_ros/tree/noetic/mycobot_description/urdf/mycobot_320_m5_2022

[anla-xu]

Regarding the second question, please provide some abnormal test data so that we can check if there is any problem.

[Green1sGood]

Please check here to see if you have the information you need: https://github.com/elephantrobotics/mycobot_ros/tree/noetic/mycobot_description/urdf/mycobot_320_m5_2022

Thank you. And what about the DH parameter matrix corresponding to, for example, new_mycobot_pro_320_m5_2022.urdf file? If I take it from here, the kinematics obtained using the parameters and using the file do not match: I get different results when calculating coordinates from angles (forward kinematics) and vice versa (inverse kinematics).

[Green1sGood]

Regarding the second question, please provide some abnormal test data so that we can check if there is any problem.

As is: With robot connected via usb port

from pymycobot import MyCobot320


robot = MyCobot320("/dev/ttyACM0")
robot.set_fresh_mode(1)
robot.set_gripper_mode(0)

target_coords = [0., -154.2,  523.9, -90., 0., 180.]
current_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

angles = robot.solve_inv_kinematics(target_coords, current_angles)
print(angles)

prints [8241, 8241, 8241, 8241, 8241, 8241].

Expected: List of floats [0.0, 0.0, 0.0, 0.0, 0.0, 0.0].

[Green1sGood]

Regarding the second question, please provide some abnormal test data so that we can check if there is any problem.

As is: With robot connected via usb port

from pymycobot import MyCobot320


robot = MyCobot320("/dev/ttyACM0")
robot.set_fresh_mode(1)
robot.set_gripper_mode(0)

target_coords = [0., -154.2,  523.9, -90., 0., 180.]
current_angles = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

angles = robot.solve_inv_kinematics(target_coords, current_angles)
print(angles)

prints [8241, 8241, 8241, 8241, 8241, 8241].

Expected: List of floats [0.0, 0.0, 0.0, 0.0, 0.0, 0.0].

Regarding this question. I found that if I divide the 'solve_inv_kinematics' method's result by 100, it will give me something close to the truth for a few of my other test inputs. However, for this particular example, it still gives me an incorrect result: [82.41, 82.41, 82.41, 82.41, 82.41, 82.41].

[MagiaYan]

Please check here to see if you have the information you need: https://github.com/elephantrobotics/mycobot_ros/tree/noetic/mycobot_description/urdf/mycobot_320_m5_2022

Thank you. And what about the DH parameter matrix corresponding to, for example, new_mycobot_pro_320_m5_2022.urdf file? If I take it from here, the kinematics obtained using the parameters and using the file do not match: I get different results when calculating coordinates from angles (forward kinematics) and vice versa (inverse kinematics).

[anla-xu]

8241 is indeed wrong data, it should be 82.41. This data indicates that there is no solution. It has been fixed. Please reinstall pymycobot through the source code

[Green1sGood]

Please check here to see if you have the information you need: https://github.com/elephantrobotics/mycobot_ros/tree/noetic/mycobot_description/urdf/mycobot_320_m5_2022

Thank you. And what about the DH parameter matrix corresponding to, for example, new_mycobot_pro_320_m5_2022.urdf file? If I take it from here, the kinematics obtained using the parameters and using the file do not match: I get different results when calculating coordinates from angles (forward kinematics) and vice versa (inverse kinematics).

Thank you. As I said, I can obtain exactly the same matrix from the parameters from here. But this matrix doesn't correspond to any of the urdf files in this directory!

[Green1sGood]

Please check here to see if you have the information you need: https://github.com/elephantrobotics/mycobot_ros/tree/noetic/mycobot_description/urdf/mycobot_320_m5_2022

Thank you. And what about the DH parameter matrix corresponding to, for example, new_mycobot_pro_320_m5_2022.urdf file? If I take it from here, the kinematics obtained using the parameters and using the file do not match: I get different results when calculating coordinates from angles (forward kinematics) and vice versa (inverse kinematics).

Thank you. As I said, I can obtain exactly the same matrix from the parameters from here. But this matrix doesn't correspond to any of the urdf files in this directory!

Here is some minimal code for reproducing (the library used for kinematics - ikpy):

# Install dependencies:
# pip install ikpy==3.4.2
# pip install numpy==2.2.6

from ikpy.chain import Chain
from ikpy.link import DHLink

import numpy as np


# Assume the urdf file is in the 'data' folder
URDF_FILE_PATH = "data/new_mycobot_pro_320_m5_2022.urdf"

BASE_ELEMENTS = ["base"]

ACTIVE_LINKS_MASK = [False, True, True, True, True, True, True]

# Matrix you provided
#    d [m],   a [m],  alpha [rad], theta offset [rad]
DH_PARAMETER_MATRIX = np.array([
    [0.1739,  0.0,    np.pi / 2,   0.0       ],
    [0.0,     -0.135, 0.0,         -np.pi / 2],
    [0.0,     -0.12,  0.0,         0.0       ],
    [0.08878, 0.0,    np.pi / 2,   -np.pi / 2],
    [0.095,   0.0,    -np.pi / 2,  0.0       ],
    [0.0655,  0.0,    0.0,         0.0       ],
])

# [deg]
ANGLE_LIMITS_DEG = np.array([
    [-168.0, 168.0],
    [-135.0, 135.0],
    [-145.0, 145.0],
    [-148.0, 148.0],
    [-168.0, 168.0],
    [-180.0, 180.0],
])

ANGLE_LIMITS_RAD = np.deg2rad(ANGLE_LIMITS_DEG)


def init_ikpy(use_urdf=False):
    if use_urdf:
        return Chain.from_urdf_file(
            URDF_FILE_PATH,
            base_elements=BASE_ELEMENTS,
            active_links_mask=ACTIVE_LINKS_MASK,
        )

    links = []
    for dh, angle_limits_rad in zip(DH_PARAMETER_MATRIX, ANGLE_LIMITS_RAD):
        link = DHLink(d=dh[0], a=dh[1], alpha=dh[2], theta=dh[3])
        # link.bounds = angle_limits_rad

        links.append(link)

    return Chain(links)


def forward_kinematics_test():
    # [deg]
    joint_pos = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

    joint_pos = np.deg2rad(joint_pos)

    ikpy_kinematics_dh = init_ikpy(use_urdf=False)

    ee_pose_dh = ikpy_kinematics_dh.forward_kinematics(joint_pos)

    print("[DH] Rotation matrix:")
    print(ee_pose_dh[:3, :3].round(decimals=6))
    # Prints:
    # [[-1.  0. -0.]
    # [-0. -0. -1.]
    # [-0. -1.  0.]]
    print("[DH] Translation vector:")
    print(ee_pose_dh[:3, 3].round(decimals=6))
    # Prints:
    # [-0.      -0.15428  0.5239 ]
    # I got very close result with pymycobot

    print('-' * 35)

    ikpy_kinematics_urdf = init_ikpy(use_urdf=True)

    joint_pos_ = np.zeros(len(ACTIVE_LINKS_MASK))
    joint_pos_[ACTIVE_LINKS_MASK] = joint_pos
    joint_pos = joint_pos_

    ee_pose_urdf = ikpy_kinematics_urdf.forward_kinematics(joint_pos)

    print("[URDF] Rotation matrix:")
    print(ee_pose_urdf[:3, :3].round(decimals=6))
    # Prints:
    # [[-1.e+00  4.e-06  4.e-06]
    # [-4.e-06  4.e-06 -1.e+00]
    # [-4.e-06 -1.e+00 -4.e-06]]
    print("[URDF] Translation vector:")
    print(ee_pose_urdf[:3, 3].round(decimals=6))
    # Prints:
    # [-0.       -0.137001  0.523   ]

    # Rotation matrices are close enough, translation vectors - not


forward_kinematics_test()