Kinovarobotics / kinova-ros

ROS packages for Jaco2 and Mico robotic arms
BSD 3-Clause "New" or "Revised" License
363 stars 318 forks source link

URDF mass does not match spec sheet #442

Open tmckenna-1 opened 1 month ago

tmckenna-1 commented 1 month ago

I am trying to do some center of mass and inertial calculations with the kinova arm. The spec sheet for the Jaco Gen2 7DoF spherical arm lists the mass (with 3 finger gripper) as 6.18 kg.

However, adding up the link masses from the urdf (j2s7s300.xacro) gets you a mass of 5.23 kg.

<xacro:property name="link_base_mesh" value="base" />
<xacro:property name="link_1_mesh" value="shoulder" />
<xacro:property name="link_2_mesh" value="arm_half_1" />
<xacro:property name="link_3_mesh" value="arm_half_2" />
<xacro:property name="link_4_mesh" value="forearm" />
<xacro:property name="link_5_mesh" value="wrist_spherical_1" />
<xacro:property name="link_6_mesh" value="wrist_spherical_2" />
<xacro:property name="link_7_mesh" value="hand_3finger" />

Is there something I am missing here? Is the urdf incorrect? Are the joint motor masses included in the urdf link masses?

Update: I have checked the spec sheet mass vs. urdf mass for a few other arm models and the urdf estimate seems to be consistently below the listed spec sheet mass

martinleroux commented 1 month ago

Hi @tmckenna-1 ,

You are correct, there seem to be an error in the .xacro file - the correct values are the ones from the user guide. Just looking at the numbers quickly, I can't quite figure out what went wrong, but I was able to confirm that our models in Matlab do sum up to the correct masses.

I'll add to my TODO list to correct the values for all of the models when I have time, but in the meantime, here is the URDF for j2s7s300.

<?xml version="1.0" encoding="UTF-8"?>
<robot name="Robot">
    <link name="j2s7s300_end_effector">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <mass value="0.00000000"/>
            <inertia ixx="0.00000000" ixy="0.00000000" ixz="0.00000000" iyy="0.00000000" iyz="0.00000000" izz="0.00000000"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <box size="0.00000000 0.00000000 0.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <box size="0.00000000 0.00000000 0.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_1">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 -0.00200000 -0.06050000"/>
            <mass value="0.74770000"/>
            <inertia ixx="0.00152032" ixy="0.00000000" ixz="0.00000000" iyy="0.00152032" iyz="0.00000000" izz="0.00059816"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\shoulder.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_big.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\shoulder.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_2">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 -0.10356321  0.00000000"/>
            <mass value="0.84470000"/>
            <inertia ixx="0.00247074" ixy="0.00000000" ixz="0.00000000" iyy="0.00038011" iyz="0.00000000" izz="0.00247074"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\arm_half_1.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_big.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\arm_half_1.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_3">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000  0.00000000 -0.10224474"/>
            <mass value="0.84470000"/>
            <inertia ixx="0.00247074" ixy="0.00000000" ixz="0.00000000" iyy="0.00247074" iyz="0.00000000" izz="0.00038011"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\arm_half_2.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_big.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\arm_half_2.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_4">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000  0.08100000 -0.00860000"/>
            <mass value="0.67630000"/>
            <inertia ixx="0.00142022" ixy="0.00000000" ixz="0.00000000" iyy="0.00030433" iyz="0.00000000" izz="0.00142022"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\forearm.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_small.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\forearm.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_5">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000  0.00288489 -0.05419326"/>
            <mass value="0.46300000"/>
            <inertia ixx="0.00043213" ixy="0.00000000" ixz="0.00000000" iyy="0.00043213" iyz="0.00000000" izz="0.00009260"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\wrist_spherical_1.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_small.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\wrist_spherical_1.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_6">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000  0.04972089 -0.00285628"/>
            <mass value="0.46300000"/>
            <inertia ixx="0.00043213" ixy="0.00000000" ixz="0.00000000" iyy="0.00009260" iyz="0.00000000" izz="0.00043213"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\wrist_spherical_2.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_small.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\wrist_spherical_2.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_7">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000  0.00000000 -0.06000000"/>
            <mass value="0.99000000"/>
            <inertia ixx="0.00034532" ixy="0.00000000" ixz="0.00000000" iyy="0.00034532" iyz="0.00000000" izz="0.00058160"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\hand_3finger.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\ring_small.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\hand_3finger.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_base">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.12550000"/>
            <mass value="0.46784000"/>
            <inertia ixx="0.00095127" ixy="0.00000000" ixz="0.00000000" iyy="0.00095127" iyz="0.00000000" izz="0.00037427"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\base.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\base.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_finger_1">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.02200000 0.00000000 0.00000000"/>
            <mass value="0.01000000"/>
            <inertia ixx="0.00000079" ixy="0.00000000" ixz="0.00000000" iyy="0.00000079" iyz="0.00000000" izz="0.00000008"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\finger_proximal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\finger_proximal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_finger_2">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.02200000 0.00000000 0.00000000"/>
            <mass value="0.01000000"/>
            <inertia ixx="0.00000079" ixy="0.00000000" ixz="0.00000000" iyy="0.00000079" iyz="0.00000000" izz="0.00000008"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\finger_proximal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\finger_proximal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_finger_3">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.02200000 0.00000000 0.00000000"/>
            <mass value="0.01000000"/>
            <inertia ixx="0.00000079" ixy="0.00000000" ixz="0.00000000" iyy="0.00000079" iyz="0.00000000" izz="0.00000008"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\finger_proximal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\finger_proximal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_finger_tip_1">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.02200000 0.00000000 0.00000000"/>
            <mass value="0.01000000"/>
            <inertia ixx="0.00000079" ixy="0.00000000" ixz="0.00000000" iyy="0.00000079" iyz="0.00000000" izz="0.00000008"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\finger_distal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\finger_distal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_finger_tip_2">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.02200000 0.00000000 0.00000000"/>
            <mass value="0.01000000"/>
            <inertia ixx="0.00000079" ixy="0.00000000" ixz="0.00000000" iyy="0.00000079" iyz="0.00000000" izz="0.00000008"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\finger_distal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\finger_distal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="j2s7s300_link_finger_tip_3">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.02200000 0.00000000 0.00000000"/>
            <mass value="0.01000000"/>
            <inertia ixx="0.00000079" ixy="0.00000000" ixz="0.00000000" iyy="0.00000079" iyz="0.00000000" izz="0.00000008"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\visual\finger_distal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.79215686 0.81960784 0.93333333 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <mesh filename="meshes\collision\finger_distal.STL" scale="1.00000000 1.00000000 1.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="root">
        <inertial>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <mass value="0.00000000"/>
            <inertia ixx="0.00000000" ixy="0.00000000" ixz="0.00000000" iyy="0.00000000" iyz="0.00000000" izz="0.00000000"/>
        </inertial>
        <visual>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <box size="0.00000000 0.00000000 0.00000000"/>
            </geometry>
            <material name="">
                <color rgba="0.50000000 0.50000000 0.50000000 1.00000000"/>
            </material>
        </visual>
        <collision>
            <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
            <geometry>
                <box size="0.00000000 0.00000000 0.00000000"/>
            </geometry>
        </collision>
    </link>
    <link name="world">
        <inertial>
            <mass value="0"/>
            <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
        </inertial>
    </link>
    <joint name="connect_root_and_world" type="fixed">
        <parent link="world"/>
        <child link="root"/>
        <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
        <axis xyz="100"/>
    </joint>
    <joint name="j2s7s300_joint_1" type="revolute">
        <parent link="j2s7s300_link_base"/>
        <child link="j2s7s300_link_1"/>
        <origin rpy="-3.14159265 -0.00000000 -3.14159265" xyz="0.00000000 0.00000000 0.15675000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="-Inf" upper="Inf" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_2" type="revolute">
        <parent link="j2s7s300_link_1"/>
        <child link="j2s7s300_link_2"/>
        <origin rpy="-1.57079633  0.00000000 -3.14159265" xyz="0.00000000  0.00160000 -0.11875000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="0.82030475" upper="5.46288056" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_3" type="revolute">
        <parent link="j2s7s300_link_2"/>
        <child link="j2s7s300_link_3"/>
        <origin rpy="-1.57079633  0.00000000  0.00000000" xyz="0.00000000 -0.20500000  0.00000000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="-Inf" upper="Inf" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_4" type="revolute">
        <parent link="j2s7s300_link_3"/>
        <child link="j2s7s300_link_4"/>
        <origin rpy="1.57079633  0.00000000 -3.14159265" xyz="0.00000000  0.00000000 -0.20500000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="0.52359878" upper="5.75958653" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_5" type="revolute">
        <parent link="j2s7s300_link_4"/>
        <child link="j2s7s300_link_5"/>
        <origin rpy="-1.57079633  0.00000000 -3.14159265" xyz="0.00000000  0.20730000 -0.01140000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="-Inf" upper="Inf" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_6" type="revolute">
        <parent link="j2s7s300_link_5"/>
        <child link="j2s7s300_link_6"/>
        <origin rpy="1.57079633  0.00000000 -3.14159265" xyz="0.00000000  0.00000000 -0.10375000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="1.13446401" upper="5.14872129" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_7" type="revolute">
        <parent link="j2s7s300_link_6"/>
        <child link="j2s7s300_link_7"/>
        <origin rpy="-1.57079633  0.00000000 -3.14159265" xyz="0.00000000 0.10375000 0.00000000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="-Inf" upper="Inf" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_base" type="fixed">
        <parent link="root"/>
        <child link="j2s7s300_link_base"/>
        <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.00000000 0.00000000 0.00000000"/>
        <axis xyz="100"/>
    </joint>
    <joint name="j2s7s300_joint_end_effector" type="fixed">
        <parent link="j2s7s300_link_7"/>
        <child link="j2s7s300_end_effector"/>
        <origin rpy="-3.14159265  0.00000000  1.57079633" xyz="0.00000000  0.00000000 -0.16000000"/>
        <axis xyz="100"/>
    </joint>
    <joint name="j2s7s300_joint_finger_1" type="revolute">
        <parent link="j2s7s300_link_7"/>
        <child link="j2s7s300_link_finger_1"/>
        <origin rpy="-1.57079633  0.64926248  1.35961149" xyz="0.00279000  0.03126000 -0.11467000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="0.00000000" upper="2.00000000" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_finger_2" type="revolute">
        <parent link="j2s7s300_link_7"/>
        <child link="j2s7s300_link_finger_2"/>
        <origin rpy="-1.57079633  0.64926248 -1.38614049" xyz="0.02226000 -0.02707000 -0.11482000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="0.00000000" upper="2.00000000" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_finger_3" type="revolute">
        <parent link="j2s7s300_link_7"/>
        <child link="j2s7s300_link_finger_3"/>
        <origin rpy="-1.57079633  0.64926248 -1.75545216" xyz="-0.02226000 -0.02707000 -0.11482000"/>
        <axis xyz="0.00000000 0.00000000 1.00000000"/>
        <limit lower="0.00000000" upper="2.00000000" effort="0" velocity="0"/>
    </joint>
    <joint name="j2s7s300_joint_finger_tip_1" type="fixed">
        <parent link="j2s7s300_link_finger_1"/>
        <child link="j2s7s300_link_finger_tip_1"/>
        <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.04400000 -0.00300000  0.00000000"/>
        <axis xyz="100"/>
    </joint>
    <joint name="j2s7s300_joint_finger_tip_2" type="fixed">
        <parent link="j2s7s300_link_finger_2"/>
        <child link="j2s7s300_link_finger_tip_2"/>
        <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.04400000 -0.00300000  0.00000000"/>
        <axis xyz="100"/>
    </joint>
    <joint name="j2s7s300_joint_finger_tip_3" type="fixed">
        <parent link="j2s7s300_link_finger_3"/>
        <child link="j2s7s300_link_finger_tip_3"/>
        <origin rpy="0.00000000 0.00000000 0.00000000" xyz="0.04400000 -0.00300000  0.00000000"/>
        <axis xyz="100"/>
    </joint>
</robot>

kinovaJacoJ2S7S300.txt

tmckenna-1 commented 1 month ago

Hi @martinleroux, thanks for the info. Just to clarify, is the urdf you supplied a corrected version, because adding up those masses I am only getting about 5.55 kg. It seems the difference between the inertial.xacro file and the one that you provided is the mass of link 7 (0.727 kg vs. 0.99 kg)

martinleroux commented 1 month ago

5.55 kg matches the weight indicated in our User Guide (see page 12) and on our Website. I had not realized you were referring to the specsheet (I read your post too fast). At this time I don't know which value is the correct one - I will see if I can find a scale to put a robot on early next week and confirm whether the specsheet or the URDF is wrong.

Thank you for notifying us.

tmckenna-1 commented 1 month ago

@martinleroux, based on the spec sheet, 5.5 kg should be the weight of the arm without the 3 finger gripper, but your urdf includes the 3 finger gripper. The User Guide value on page 12 does not specify whether the listed weight includes the gripper or not (the picture associated does have the arm + gripper). I weighed our arm without the gripper and got 5.5 kg. But let me know your findings once you have time to investigate further.

martinleroux commented 1 month ago

I confirm, I just weighed an arm and got 5.5kg without the gripper. It's surprising that the data is wrong since we generate the URDF from CAD, but I'm clearly not going to argue with reality.

I'll escalate the issue with our engineering team, but unless they can find the issue in a matter of minutes, the investigation will likely only begin next sprint. If it is determined that we need to make actual measurements instead of relying on the cad, it may be a couple of months before we have public data.

In the meantime, the only thing I can offer is this snippet of code directly from the robot's firmware, but that is a piece of code nobody looked at for around 10 years so admittedly I don't know if the data is accurate (it does not sum to the right mass, but it does not include the base link).

// Links mass
            m_Gravity_Param_Mass[0] = 0.77;
            m_Gravity_Param_Mass[1] = 0.803;
            m_Gravity_Param_Mass[2] = 0.825;
            m_Gravity_Param_Mass[3] = 0.790;
            m_Gravity_Param_Mass[4] = 0.468;
            m_Gravity_Param_Mass[5] = 0.468;

            if (lowLevelManagement->deviceInspector.finger.GetCount() == 3)// If 3 fingers
            {
                m_Gravity_Param_Mass[6] = 1.025f;
            }
            else if (lowLevelManagement->deviceInspector.finger.GetCount() == 2)//If 2 fingers
            {
                m_Gravity_Param_Mass[6] = 0.910f;
            }
            else//If no hand
            {
                m_Gravity_Param_Mass[6] = 0.355f;
            }
tmckenna-1 commented 1 month ago

@martinleroux thanks for looking into this. Let me know what you can discover.