Add URDF model for RM75-B OmniPicker with detailed link and joint specifications
This commit is contained in:
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,503 @@
|
||||
<?xml version='1.0' encoding='UTF-8'?>
|
||||
<robot name="RM75_B_OmniPicker_fixed">
|
||||
<link name="base_link">
|
||||
<inertial>
|
||||
<origin xyz="0.00049987 5.2709E-05 0.060019" rpy="0 0 0"/>
|
||||
<mass value="1.862"/>
|
||||
<inertia ixx="0.0017232" ixy="-3.1058E-06" ixz="-3.7924E-05" iyy="0.0017051" iyz="1.3691E-06" izz="0.00090158"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/base_link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<link name="link_1">
|
||||
<inertial>
|
||||
<origin xyz="0.000241 -0.013273 -0.00995" rpy="0 0 0"/>
|
||||
<mass value="1.574"/>
|
||||
<inertia ixx="0.002487573" ixy="0.000009663" ixz="-0.000007909" iyy="0.002321038" iyz="0.000179393" izz="0.001450554"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_1.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_1" type="revolute">
|
||||
<origin xyz="0 0 0.2405" rpy="0 0 0"/>
|
||||
<parent link="base_link"/>
|
||||
<child link="link_1"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.106" upper="3.106" effort="60" velocity="3.14"/>
|
||||
</joint>
|
||||
<link name="link_2">
|
||||
<inertial>
|
||||
<origin xyz="-0.000357 -0.106789 0.005329" rpy="0 0 0"/>
|
||||
<mass value="1.217"/>
|
||||
<inertia ixx="0.003494121" ixy="0.000002921" ixz="-0.000005613" iyy="0.000892721" iyz="-0.000583884" izz="0.003444080"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_2.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_2" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0"/>
|
||||
<parent link="link_1"/>
|
||||
<child link="link_2"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-2.2689" upper="2.2689" effort="60" velocity="3.14"/>
|
||||
</joint>
|
||||
<link name="link_3">
|
||||
<inertial>
|
||||
<origin xyz="0.000003 -0.01398 -0.011324" rpy="0 0 0"/>
|
||||
<mass value="1.11"/>
|
||||
<inertia ixx="0.001836663" ixy="0.000002259" ixz="-0.000004216" iyy="0.001498875" iyz="0.000037167" izz="0.001062545"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_3.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_3" type="revolute">
|
||||
<origin xyz="0 -0.256 0" rpy="1.5708 0 0"/>
|
||||
<parent link="link_2"/>
|
||||
<child link="link_3"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.106" upper="3.106" effort="30" velocity="3.14"/>
|
||||
</joint>
|
||||
<link name="link_4">
|
||||
<inertial>
|
||||
<origin xyz="-0.000005 -0.084658 0.004747" rpy="0 0 0"/>
|
||||
<mass value="0.685"/>
|
||||
<inertia ixx="0.001282444" ixy="-0.000000551" ixz="-0.000000630" iyy="0.000373013" iyz="-0.000232084" izz="0.001256177"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_4.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_4" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0"/>
|
||||
<parent link="link_3"/>
|
||||
<child link="link_4"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-2.356" upper="2.356" effort="30" velocity="3.14"/>
|
||||
</joint>
|
||||
<link name="link_5">
|
||||
<inertial>
|
||||
<origin xyz="0.000078 -0.012937 -0.008781" rpy="0 0 0"/>
|
||||
<mass value="0.619"/>
|
||||
<inertia ixx="0.000627336" ixy="0.000001636" ixz="-0.000001345" iyy="0.000542455" iyz="0.000034970" izz="0.000370291"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_5.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_5" type="revolute">
|
||||
<origin xyz="0 -0.21 0" rpy="1.5708 0 0"/>
|
||||
<parent link="link_4"/>
|
||||
<child link="link_5"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.106" upper="3.106" effort="10" velocity="3.14"/>
|
||||
</joint>
|
||||
<link name="link_6">
|
||||
<inertial>
|
||||
<origin xyz="-0.000014 -0.078524 0.002819" rpy="0 0 0"/>
|
||||
<mass value="0.602"/>
|
||||
<inertia ixx="0.000780774" ixy="-0.000000121" ixz="-0.000000469" iyy="0.000289973" iyz="-0.000120513" izz="0.000763955"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_6.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_6" type="revolute">
|
||||
<origin xyz="0 0 0" rpy="-1.5708 0 0"/>
|
||||
<parent link="link_5"/>
|
||||
<child link="link_6"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-2.234" upper="2.234" effort="10" velocity="3.14"/>
|
||||
</joint>
|
||||
<link name="link_7">
|
||||
<inertial>
|
||||
<origin xyz="0.001094 -0.000077 -0.010119" rpy="0 0 0"/>
|
||||
<mass value="0.107"/>
|
||||
<inertia ixx="0.000044123" ixy="-0.000000064" ixz="0.0000003" iyy="0.000035078" iyz="-0.000000029" izz="0.000065445"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="1 1 1 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/rm75/link_7.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="joint_7" type="revolute">
|
||||
<origin xyz="0 -0.144 0" rpy="1.5708 0 0"/>
|
||||
<parent link="link_6"/>
|
||||
<child link="link_7"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-6.28" upper="6.28" effort="10" velocity="3.14"/>
|
||||
</joint>
|
||||
<!-- RM75 end-flange alias. link_7 is treated as the tool mounting frame. -->
|
||||
<link name="rm75_flange"/>
|
||||
<joint name="rm75_link7_to_flange" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="link_7"/>
|
||||
<child link="rm75_flange"/>
|
||||
</joint>
|
||||
<!-- OmniPicker mounting transform. Adjust xyz/rpy here if an adapter plate or different clocking is used. -->
|
||||
<joint name="rm75_flange_to_omnipicker" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="rm75_flange"/>
|
||||
<child link="omnipicker_base_link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_base_link">
|
||||
<inertial>
|
||||
<origin xyz="-0.00005520 1.2341E-05 0.03296193" rpy="0 0 0"/>
|
||||
<mass value="0.25641368"/>
|
||||
<inertia ixx="4.6351E-04" ixy="-1.0E-08" ixz="-1.04E-06" iyy="4.4525E-04" iyz="-5.00E-08" izz="1.0438E-04"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin rpy="0 0 0" xyz="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/base_link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.89804 0.91765 0.92941 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
</link>
|
||||
<link name="omnipicker_hand_narrow1_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.0094685 0.0068806 6.5437E-05" rpy="0 0 0"/>
|
||||
<mass value="0.025428"/>
|
||||
<inertia ixx="1.9574E-06" ixy="-4.2911E-07" ixz="-3.7111E-11" iyy="2.3919E-06" iyz="-3.008E-10" izz="1.5501E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow1_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_narrow1_joint" type="fixed">
|
||||
<origin xyz="0 -0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_hand_narrow1_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_narrow2_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.0088027 -0.007035 1.6424E-05" rpy="0 0 0"/>
|
||||
<mass value="0.0040132"/>
|
||||
<inertia ixx="1.0307E-07" ixy="5.7851E-08" ixz="9.5801E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow2_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_narrow2_joint" type="fixed">
|
||||
<origin xyz="0.030852 0.018551 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_hand_narrow1_Link"/>
|
||||
<child link="omnipicker_hand_narrow2_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_narrow3_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.012508 -0.0079729 9.4339E-05" rpy="0 0 0"/>
|
||||
<mass value="0.018029"/>
|
||||
<inertia ixx="1.1403E-06" ixy="5.5159E-07" ixz="-4.0096E-13" iyy="2.5704E-06" iyz="1.0951E-12" izz="2.3252E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow3_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_narrow3_joint" type="fixed">
|
||||
<origin xyz="0.018118 -0.01574 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_hand_narrow2_Link"/>
|
||||
<child link="omnipicker_hand_narrow3_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_narrow4_Link">
|
||||
</link>
|
||||
<joint name="omnipicker_hand_narrow4_joint" type="fixed">
|
||||
<origin xyz="0 -0.0104 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_hand_narrow3_Link"/>
|
||||
<child link="omnipicker_hand_narrow4_Link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_narrow_loop_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.014869 -0.0036066 0.00029307" rpy="0 0 0"/>
|
||||
<mass value="0.022591"/>
|
||||
<inertia ixx="4.3916E-06" ixy="1.114E-07" ixz="-4.9655E-12" iyy="4.737E-06" iyz="1.7121E-11" izz="6.0445E-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/narrow_loop_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_narrow_loop_joint" type="fixed">
|
||||
<origin xyz="0 -0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_hand_narrow_loop_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_wide1_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.0095051 -0.0068479 6.8268E-05" rpy="0 0 0"/>
|
||||
<mass value="0.025428"/>
|
||||
<inertia ixx="1.9565E-06" ixy="4.2798E-07" ixz="2.3844E-10" iyy="2.3928E-06" iyz="-1.4454E-10" izz="1.5501E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide1_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_wide1_joint" type="fixed">
|
||||
<origin xyz="0 0.0195 0.0565" rpy="-2.9951 -1.5708 -0.15964"/>
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_hand_wide1_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_wide2_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.0088027 0.007035 -1.6424E-05" rpy="0 0 0"/>
|
||||
<mass value="0.0040132"/>
|
||||
<inertia ixx="1.0307E-07" ixy="-5.7851E-08" ixz="-9.58E-11" iyy="1.1385E-07" iyz="-2.81E-11" izz="1.7009E-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide2_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_wide2_joint" type="fixed">
|
||||
<origin xyz="0.030852 -0.018551 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_hand_wide1_Link"/>
|
||||
<child link="omnipicker_hand_wide2_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_wide3_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.016206 0.0094593 4.7668E-05" rpy="0 0 0"/>
|
||||
<mass value="0.035835"/>
|
||||
<inertia ixx="8.5056E-06" ixy="-1.1363E-06" ixz="-4.2908E-11" iyy="1.1235E-05" iyz="-2.9251E-11" izz="4.6309E-06"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide3_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_wide3_joint" type="fixed">
|
||||
<origin xyz="0.018118 0.01574 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_hand_wide2_Link"/>
|
||||
<child link="omnipicker_hand_wide3_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_wide4_Link">
|
||||
</link>
|
||||
<joint name="omnipicker_hand_wide4_joint" type="fixed">
|
||||
<origin xyz="0 0.0104 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_hand_wide3_Link"/>
|
||||
<child link="omnipicker_hand_wide4_Link"/>
|
||||
<axis xyz="0 0 1"/>
|
||||
<limit lower="-3.14" upper="3.14" effort="0" velocity="0"/>
|
||||
</joint>
|
||||
<link name="omnipicker_hand_wide_loop_Link">
|
||||
<inertial>
|
||||
<origin xyz="0.016268 0.0040555 0.00030323" rpy="0 0 0"/>
|
||||
<mass value="0.025142"/>
|
||||
<inertia ixx="5.887E-06" ixy="-1.1234E-07" ixz="2.1954E-11" iyy="6.236E-06" iyz="-9.473E-12" izz="6.2389E-07"/>
|
||||
</inertial>
|
||||
<visual>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
|
||||
</geometry>
|
||||
<material name="">
|
||||
<color rgba="0.75294 0.75294 0.75294 1"/>
|
||||
</material>
|
||||
</visual>
|
||||
<collision>
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<geometry>
|
||||
<mesh filename="package://xr_rm_teleop/models/rm75_omnipicker/meshes/omnipicker/wide_loop_Link.STL"/>
|
||||
</geometry>
|
||||
</collision>
|
||||
</link>
|
||||
<joint name="omnipicker_hand_wide_loop_joint" type="fixed">
|
||||
<origin xyz="0 0.021633 0.07387" rpy="-2.9951 -1.5708 -0.15964"/>
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_hand_wide_loop_Link"/>
|
||||
</joint>
|
||||
<link name="omnipicker_mount_frame"/>
|
||||
<joint name="omnipicker_base_to_mount_frame" type="fixed">
|
||||
<origin xyz="0 0 0" rpy="0 0 0"/>
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_mount_frame"/>
|
||||
</joint>
|
||||
<link name="omnipicker_tcp"/>
|
||||
<joint name="omnipicker_tcp_joint" type="fixed">
|
||||
<parent link="omnipicker_base_link"/>
|
||||
<child link="omnipicker_tcp"/>
|
||||
<origin xyz="0 0 0.16" rpy="0 0 0"/>
|
||||
</joint>
|
||||
</robot>
|
||||
@@ -24,6 +24,18 @@ setup(
|
||||
f"share/{package_name}/models/rm75/meshes",
|
||||
glob("models/rm75/meshes/*.STL"),
|
||||
),
|
||||
(
|
||||
f"share/{package_name}/models/rm75_omnipicker/urdf",
|
||||
glob("models/rm75_omnipicker/urdf/*.urdf"),
|
||||
),
|
||||
(
|
||||
f"share/{package_name}/models/rm75_omnipicker/meshes/rm75",
|
||||
glob("models/rm75_omnipicker/meshes/rm75/*.STL"),
|
||||
),
|
||||
(
|
||||
f"share/{package_name}/models/rm75_omnipicker/meshes/omnipicker",
|
||||
glob("models/rm75_omnipicker/meshes/omnipicker/*.STL"),
|
||||
),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
|
||||
@@ -8,56 +8,67 @@ from pathlib import Path
|
||||
import numpy as np
|
||||
|
||||
from xr_rm_teleop.placo_ik_solver import PlacoIkSolver
|
||||
from xr_rm_teleop.realman_adapter import ArmPose
|
||||
|
||||
|
||||
CASES = {
|
||||
"left": (
|
||||
[-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
|
||||
[0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0],
|
||||
),
|
||||
"right": (
|
||||
[-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
|
||||
[0.0, 0.0, 0.16, 0.0, 0.0, 0.0, 1.0],
|
||||
),
|
||||
"left": [-79.55, -9.99, 71.01, 101.45, 95.07, -84.47, -74.52],
|
||||
"right": [-90.14, 3.76, -86.89, 87.89, -96.53, -79.62, -90.04],
|
||||
}
|
||||
|
||||
|
||||
def angle_error(actual: list[float], target: list[float]) -> float:
|
||||
deltas = [
|
||||
math.atan2(math.sin(a - b), math.cos(a - b))
|
||||
for a, b in zip(actual, target)
|
||||
]
|
||||
return math.sqrt(sum(value * value for value in deltas))
|
||||
def rotation_z(angle: float) -> np.ndarray:
|
||||
cosine = math.cos(angle)
|
||||
sine = math.sin(angle)
|
||||
return np.asarray(
|
||||
[
|
||||
[cosine, -sine, 0.0],
|
||||
[sine, cosine, 0.0],
|
||||
[0.0, 0.0, 1.0],
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def angle_error(actual: np.ndarray, target: np.ndarray) -> float:
|
||||
cosine = np.clip((np.trace(target @ actual.T) - 1.0) * 0.5, -1.0, 1.0)
|
||||
return float(math.acos(cosine))
|
||||
|
||||
|
||||
def main() -> None:
|
||||
urdf_path = Path(sys.argv[1]).resolve()
|
||||
for arm, (joint_degrees, tool_pose) in CASES.items():
|
||||
solver = PlacoIkSolver(str(urdf_path), tool_pose, 1.0 / 90.0)
|
||||
joints = np.deg2rad(joint_degrees).tolist()
|
||||
current = solver.update_joint_state(joints)
|
||||
target = ArmPose(
|
||||
current.x + 0.01,
|
||||
current.y,
|
||||
current.z,
|
||||
current.rx,
|
||||
current.ry,
|
||||
current.rz + 0.05,
|
||||
for arm, joint_degrees in CASES.items():
|
||||
initial_joints = np.deg2rad(joint_degrees)
|
||||
drift_solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
|
||||
joints = initial_joints.tolist()
|
||||
stationary_target = drift_solver.update_joint_state(joints)
|
||||
flange = drift_solver._robot.get_T_world_frame("link_7")
|
||||
flange_to_tcp = np.linalg.inv(flange) @ stationary_target
|
||||
assert np.allclose(flange_to_tcp[:3, 3], [0.0, 0.0, 0.16])
|
||||
assert np.allclose(flange_to_tcp[:3, :3], np.eye(3))
|
||||
for _ in range(250):
|
||||
drift_solver.update_joint_state(joints)
|
||||
joints = drift_solver.solve(stationary_target)
|
||||
drift_degrees = float(
|
||||
np.max(np.abs(np.rad2deg(np.asarray(joints) - initial_joints)))
|
||||
)
|
||||
|
||||
solver = PlacoIkSolver(str(urdf_path), 1.0 / 125.0)
|
||||
joints = initial_joints.tolist()
|
||||
current = solver.update_joint_state(joints)
|
||||
assert current.shape == (4, 4)
|
||||
target = current.copy()
|
||||
target[0, 3] += 0.01
|
||||
target[:3, :3] = rotation_z(0.05) @ target[:3, :3]
|
||||
|
||||
solve_durations = []
|
||||
for _ in range(45):
|
||||
for _ in range(250):
|
||||
solver.update_joint_state(joints)
|
||||
started_at = time.perf_counter()
|
||||
joints = solver.solve(target)
|
||||
solve_durations.append(time.perf_counter() - started_at)
|
||||
|
||||
actual = solver.update_joint_state(joints)
|
||||
position_error = np.linalg.norm(
|
||||
np.asarray(actual.xyz()) - np.asarray(target.xyz())
|
||||
)
|
||||
orientation_error = angle_error(actual.rpy(), target.rpy())
|
||||
position_error = np.linalg.norm(actual[:3, 3] - target[:3, 3])
|
||||
orientation_error = angle_error(actual[:3, :3], target[:3, :3])
|
||||
assert len(joints) == 7
|
||||
assert np.isfinite(joints).all()
|
||||
assert np.allclose(
|
||||
@@ -69,9 +80,10 @@ def main() -> None:
|
||||
print(
|
||||
f"{arm}: position_error={position_error:.6f}m, "
|
||||
f"orientation_error={math.degrees(orientation_error):.3f}deg, "
|
||||
f"stationary_drift={drift_degrees:.3f}deg, "
|
||||
f"solve_avg={1000.0 * np.mean(solve_durations):.3f}ms, "
|
||||
f"solve_max={1000.0 * max(solve_durations):.3f}ms, "
|
||||
f"solve_overruns={sum(value > 1.0 / 90.0 for value in solve_durations)}"
|
||||
f"solve_overruns={sum(value > 1.0 / 125.0 for value in solve_durations)}"
|
||||
)
|
||||
|
||||
|
||||
|
||||
@@ -1,13 +1,21 @@
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import SingleArmVelocityTeleop
|
||||
from xr_rm_teleop.realman_adapter import JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||
SingleArmVelocityTeleop,
|
||||
_make_transform,
|
||||
_so3_exp,
|
||||
)
|
||||
|
||||
|
||||
class FakeLogger:
|
||||
def info(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
def warn(self, *args, **kwargs):
|
||||
del args, kwargs
|
||||
|
||||
@@ -78,7 +86,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
||||
|
||||
def update_joint_state(self, joints):
|
||||
assert joints == [0.1] * 7
|
||||
return ArmPose(0.3, 0.0, 0.2)
|
||||
transform = np.eye(4)
|
||||
transform[:3, 3] = [0.3, 0.0, 0.2]
|
||||
return transform
|
||||
|
||||
def solve(self, target):
|
||||
del target
|
||||
@@ -95,7 +105,9 @@ def test_first_feedback_initializes_last_valid_target_without_solving() -> None:
|
||||
JointStateSnapshot([0.1] * 7, time.monotonic())
|
||||
)
|
||||
|
||||
assert pose == ArmPose(0.3, 0.0, 0.2)
|
||||
assert pose == pytest.approx(
|
||||
_make_transform([0.3, 0.0, 0.2], np.eye(3))
|
||||
)
|
||||
assert teleop._last_valid_joint_target == [0.1] * 7
|
||||
assert teleop._ik_solver.solve_calls == 0
|
||||
|
||||
@@ -112,7 +124,7 @@ def test_qp_failure_returns_last_known_good_target() -> None:
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||||
target = teleop._solve_joint_target(np.eye(4))
|
||||
|
||||
assert target == pytest.approx([0.1] * 7)
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.1] * 7)
|
||||
@@ -130,12 +142,88 @@ def test_qp_success_updates_last_known_good_target() -> None:
|
||||
teleop._arm_name = "left_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
|
||||
target = teleop._solve_joint_target(ArmPose(0.3, 0.0, 0.2))
|
||||
target = teleop._solve_joint_target(np.eye(4))
|
||||
|
||||
assert target == pytest.approx([0.2] * 7)
|
||||
assert teleop._last_valid_joint_target == pytest.approx([0.2] * 7)
|
||||
|
||||
|
||||
def test_enter_active_control_initializes_se3_orientation_state() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
transform = _make_transform(
|
||||
[0.3, -0.1, 0.2],
|
||||
_so3_exp(np.asarray([0.1, -0.2, 0.3])),
|
||||
)
|
||||
published = []
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop.get_logger = lambda: FakeLogger()
|
||||
teleop._publish_debug = lambda *args: published.append(args)
|
||||
|
||||
teleop._enter_active_control(
|
||||
[0.0, 0.0, 0.0],
|
||||
(0.0, 0.0, 0.0, 1.0),
|
||||
transform,
|
||||
FakeTime(),
|
||||
)
|
||||
|
||||
assert teleop._robot_start_transform == pytest.approx(transform)
|
||||
assert teleop._filtered_target == pytest.approx(transform[:3, 3])
|
||||
assert teleop._filtered_orientation_target == pytest.approx(transform[:3, :3])
|
||||
assert teleop._last_sent_orientation == pytest.approx(transform[:3, :3])
|
||||
assert len(published) == 1
|
||||
|
||||
|
||||
def test_command_angular_velocity_uses_so3_rotation_vector() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._dt = 0.1
|
||||
teleop._last_sent_target = [0.0, 0.0, 0.0]
|
||||
teleop._last_sent_orientation = np.eye(3)
|
||||
teleop._last_command_time = None
|
||||
|
||||
velocity = teleop._estimate_command_velocity(
|
||||
[0.0, 0.0, 0.0],
|
||||
_so3_exp(np.asarray([0.0, 0.0, 0.1])),
|
||||
FakeTime(),
|
||||
)
|
||||
|
||||
assert velocity == pytest.approx([0.0, 0.0, 0.0, 0.0, 0.0, 1.0])
|
||||
|
||||
|
||||
def test_timing_stats_logs_summary_and_clears_window() -> None:
|
||||
messages = []
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._arm_name = "right_rm75"
|
||||
teleop._dt = 0.008
|
||||
teleop._timing_stats_window = 2
|
||||
teleop._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
teleop.get_logger = lambda: SimpleNamespace(
|
||||
info=lambda message: messages.append(message)
|
||||
)
|
||||
|
||||
teleop._record_timing_sample(7.0, 6.0, 1.0, 0.5, 3.0)
|
||||
assert messages == []
|
||||
|
||||
teleop._record_timing_sample(9.0, 10.0, 2.0, 0.7, 4.0)
|
||||
|
||||
assert len(messages) == 1
|
||||
assert "right_rm75 timing n=2 deadline=8.000 ms" in messages[0]
|
||||
assert (
|
||||
"period[n=2 mean=8.000 p95=8.900 p99=8.980 "
|
||||
"max=9.000 ms overruns=1]"
|
||||
) in messages[0]
|
||||
assert (
|
||||
"total[n=2 mean=8.000 p95=9.800 p99=9.960 "
|
||||
"max=10.000 ms overruns=1]"
|
||||
) in messages[0]
|
||||
assert "qp[n=2" in messages[0]
|
||||
assert "send[n=2" in messages[0]
|
||||
assert "feedback_age[n=2" in messages[0]
|
||||
assert all(not samples for samples in teleop._timing_samples.values())
|
||||
|
||||
|
||||
def test_joint_send_failure_requests_slow_stop_and_resets_control() -> None:
|
||||
class FailingAdapter:
|
||||
def __init__(self) -> None:
|
||||
|
||||
@@ -2,14 +2,19 @@ import math
|
||||
import time
|
||||
from types import SimpleNamespace
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.realman_adapter import ArmPose, JointStateSnapshot
|
||||
from xr_rm_teleop.realman_adapter import JointStateSnapshot
|
||||
from xr_rm_teleop.single_arm_velocity_teleop import (
|
||||
SingleArmVelocityTeleop,
|
||||
_euler_to_quaternion,
|
||||
_make_transform,
|
||||
_matrix_to_quaternion,
|
||||
_normalize_quaternion,
|
||||
_quaternion_to_euler,
|
||||
_project_rotation,
|
||||
_quaternion_to_matrix,
|
||||
_so3_exp,
|
||||
_so3_log,
|
||||
)
|
||||
|
||||
|
||||
@@ -18,7 +23,10 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
|
||||
teleop._enable_orientation_control = True
|
||||
teleop._enable_orientation_axes = [True, True, True]
|
||||
teleop._controller_orientation_start = (0.0, 0.0, 0.0, 1.0)
|
||||
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.1, -0.2, 0.3)
|
||||
teleop._robot_start_transform = _make_transform(
|
||||
[0.3, 0.0, 0.2],
|
||||
_so3_exp(np.asarray([0.1, -0.2, 0.3])),
|
||||
)
|
||||
teleop._xr_to_robot_matrix = [
|
||||
0.0, 1.0, 0.0,
|
||||
0.0, 0.0, 1.0,
|
||||
@@ -27,47 +35,111 @@ def _make_teleop_for_orientation() -> SingleArmVelocityTeleop:
|
||||
return teleop
|
||||
|
||||
|
||||
def assert_angles_close(actual: list[float] | tuple[float, ...], expected: list[float]) -> None:
|
||||
assert len(actual) == len(expected)
|
||||
for actual_value, expected_value in zip(actual, expected):
|
||||
assert math.atan2(math.sin(actual_value - expected_value), math.cos(actual_value - expected_value)) == pytest.approx(0.0)
|
||||
|
||||
|
||||
def test_identity_controller_orientation_keeps_tcp_orientation() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
|
||||
target = teleop._raw_orientation_from_controller((0.0, 0.0, 0.0, 1.0))
|
||||
|
||||
assert_angles_close(target, teleop._robot_start_pose.rpy())
|
||||
assert target == pytest.approx(teleop._robot_start_transform[:3, :3])
|
||||
|
||||
|
||||
def test_xr_relative_rotation_maps_through_xr_to_robot_matrix() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
teleop._robot_start_pose = ArmPose(0.3, 0.0, 0.2, 0.0, 0.0, 0.0)
|
||||
xr_roll = _euler_to_quaternion(0.2, 0.0, 0.0)
|
||||
teleop._robot_start_transform = np.eye(4)
|
||||
xr_roll = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, 0.0, 0.0])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(xr_roll)
|
||||
|
||||
assert_angles_close(target, [0.0, 0.0, 0.2])
|
||||
assert _so3_log(target) == pytest.approx([0.0, 0.0, 0.2])
|
||||
|
||||
|
||||
def test_orientation_deadband_filter_and_speed_limit() -> None:
|
||||
def test_quaternion_sign_does_not_change_rotation() -> None:
|
||||
quaternion = _normalize_quaternion((0.2, -0.3, 0.1, 0.9))
|
||||
|
||||
assert _quaternion_to_matrix(quaternion) == pytest.approx(
|
||||
_quaternion_to_matrix(tuple(-value for value in quaternion))
|
||||
)
|
||||
|
||||
|
||||
@pytest.mark.parametrize("pitch", [math.pi / 2.0 - 1e-5, -math.pi / 2.0 + 1e-5])
|
||||
def test_small_rotation_near_gimbal_lock_stays_small(pitch: float) -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
start_rotation = _so3_exp(np.asarray([0.0, pitch, 0.0]))
|
||||
teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
|
||||
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
|
||||
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.01, 0.0, 0.0])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(controller)
|
||||
|
||||
error = _so3_log(target @ start_rotation.T)
|
||||
assert np.linalg.norm(error) == pytest.approx(0.01)
|
||||
|
||||
|
||||
def test_crossing_old_rpy_branch_uses_shortest_rotation() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
start_rotation = _so3_exp(np.asarray([0.0, math.pi / 2.0 - 0.001, 0.0]))
|
||||
teleop._robot_start_transform = _make_transform([0.3, 0.0, 0.2], start_rotation)
|
||||
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
|
||||
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.0, 0.002, 0.0])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(controller)
|
||||
|
||||
assert _so3_log(target @ start_rotation.T) == pytest.approx(
|
||||
[0.0, 0.002, 0.0],
|
||||
abs=1e-9,
|
||||
)
|
||||
|
||||
|
||||
def test_disabled_orientation_axis_zeros_robot_rotation_vector_component() -> None:
|
||||
teleop = _make_teleop_for_orientation()
|
||||
teleop._robot_start_transform = np.eye(4)
|
||||
teleop._xr_to_robot_matrix = np.eye(3).reshape(-1).tolist()
|
||||
teleop._enable_orientation_axes = [True, False, True]
|
||||
controller = _matrix_to_quaternion(_so3_exp(np.asarray([0.1, 0.2, 0.3])))
|
||||
|
||||
target = teleop._raw_orientation_from_controller(controller)
|
||||
|
||||
assert _so3_log(target) == pytest.approx([0.1, 0.0, 0.3])
|
||||
|
||||
|
||||
def test_orientation_deadband_filter_and_speed_limit_use_so3_angle() -> None:
|
||||
teleop = object.__new__(SingleArmVelocityTeleop)
|
||||
teleop._orientation_deadband_rad = 0.01
|
||||
teleop._orientation_filter_alpha = 0.5
|
||||
teleop._max_orientation_speed = 0.5
|
||||
teleop._dt = 0.1
|
||||
teleop._last_sent_orientation = [0.0, 0.0, 0.0]
|
||||
teleop._filtered_orientation_target = [0.0, 0.0, 0.0]
|
||||
teleop._dt = 1.0 / 125.0
|
||||
teleop._last_sent_orientation = np.eye(3)
|
||||
teleop._filtered_orientation_target = np.eye(3)
|
||||
|
||||
assert teleop._apply_orientation_deadband([0.001, 0.0, 0.0]) == [0.0, 0.0, 0.0]
|
||||
inside_deadband = _so3_exp(np.asarray([0.006, 0.006, 0.0]))
|
||||
assert teleop._apply_orientation_deadband(inside_deadband) == pytest.approx(np.eye(3))
|
||||
|
||||
filtered = teleop._filter_orientation_target([0.2, 0.0, 0.0])
|
||||
assert_angles_close(filtered, [0.1, 0.0, 0.0])
|
||||
target = _so3_exp(np.asarray([0.2, 0.0, 0.0]))
|
||||
filtered = teleop._filter_orientation_target(target)
|
||||
assert _so3_log(filtered) == pytest.approx([0.1, 0.0, 0.0])
|
||||
|
||||
limited, was_limited = teleop._limit_orientation_step([0.2, 0.0, 0.0])
|
||||
limited, was_limited = teleop._limit_orientation_step(target)
|
||||
assert was_limited
|
||||
assert_angles_close(limited, [0.05, 0.0, 0.0])
|
||||
assert np.linalg.norm(_so3_log(limited)) == pytest.approx(0.5 / 125.0)
|
||||
|
||||
|
||||
def test_rotation_matrix_to_debug_quaternion_is_normalized() -> None:
|
||||
quaternion = _matrix_to_quaternion(_so3_exp(np.asarray([0.2, -0.1, 0.3])))
|
||||
|
||||
assert np.isfinite(quaternion).all()
|
||||
assert np.linalg.norm(quaternion) == pytest.approx(1.0)
|
||||
|
||||
|
||||
def test_rotation_projection_accepts_small_error_and_rejects_invalid_matrix() -> None:
|
||||
near_rotation = np.eye(3)
|
||||
near_rotation[0, 1] = 1e-5
|
||||
|
||||
projected = _project_rotation(near_rotation)
|
||||
|
||||
assert projected.T @ projected == pytest.approx(np.eye(3))
|
||||
assert np.linalg.det(projected) == pytest.approx(1.0)
|
||||
with pytest.raises(ValueError):
|
||||
_project_rotation(np.diag([2.0, 1.0, 1.0]))
|
||||
|
||||
|
||||
def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
@@ -102,9 +174,7 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
time.monotonic(),
|
||||
)
|
||||
)
|
||||
teleop._ik_solver = SimpleNamespace(
|
||||
update_joint_state=lambda joints: ArmPose(0.3, 0.0, 0.2)
|
||||
)
|
||||
teleop._ik_solver = SimpleNamespace(update_joint_state=lambda joints: np.eye(4))
|
||||
teleop._active = False
|
||||
teleop._last_valid_joint_target = None
|
||||
teleop._last_current_pose = None
|
||||
@@ -119,11 +189,6 @@ def test_invalid_controller_quaternion_stops_current_tick() -> None:
|
||||
assert stopped == [True]
|
||||
|
||||
|
||||
def test_quaternion_roundtrip_for_small_rpy() -> None:
|
||||
quat = _normalize_quaternion(_euler_to_quaternion(0.2, -0.1, 0.3))
|
||||
assert_angles_close(_quaternion_to_euler(quat), [0.2, -0.1, 0.3])
|
||||
|
||||
|
||||
def test_zero_quaternion_is_invalid() -> None:
|
||||
with pytest.raises(ValueError):
|
||||
_normalize_quaternion([0.0, 0.0, 0.0, 0.0])
|
||||
|
||||
@@ -1,37 +1,72 @@
|
||||
import math
|
||||
from pathlib import Path
|
||||
from xml.etree import ElementTree
|
||||
|
||||
import numpy as np
|
||||
import pytest
|
||||
|
||||
from xr_rm_teleop.placo_ik_solver import (
|
||||
PlacoIkSolver,
|
||||
_arm_pose_to_transform,
|
||||
_tool_pose_to_transform,
|
||||
_transform_to_arm_pose,
|
||||
_validated_transform,
|
||||
)
|
||||
from xr_rm_teleop.realman_adapter import ArmPose
|
||||
|
||||
|
||||
def test_tool_offset_rotates_with_flange_and_roundtrips() -> None:
|
||||
flange_pose = ArmPose(0.30, -0.10, 0.20, 0.0, math.pi / 2.0, 0.0)
|
||||
tool_pose = [0.0, 0.0, 0.19, 0.0, 0.0, 0.0, 1.0]
|
||||
def test_fixed_urdf_has_seven_moving_joints_and_omnipicker_tcp() -> None:
|
||||
urdf_path = (
|
||||
Path(__file__).resolve().parents[1]
|
||||
/ "models"
|
||||
/ "rm75_omnipicker"
|
||||
/ "urdf"
|
||||
/ "RM75-B_OmniPicker_fixed.urdf"
|
||||
)
|
||||
root = ElementTree.parse(urdf_path).getroot()
|
||||
moving_joint_names = [
|
||||
joint.attrib["name"]
|
||||
for joint in root.findall("joint")
|
||||
if joint.attrib["type"] != "fixed"
|
||||
]
|
||||
tcp_joint = root.find("joint[@name='omnipicker_tcp_joint']")
|
||||
mesh_filenames = [
|
||||
mesh.attrib["filename"]
|
||||
for mesh in root.findall(".//mesh")
|
||||
]
|
||||
|
||||
base_to_flange = _arm_pose_to_transform(flange_pose)
|
||||
flange_to_tool = _tool_pose_to_transform(tool_pose)
|
||||
base_to_tool = base_to_flange @ flange_to_tool
|
||||
recovered_flange = base_to_tool @ np.linalg.inv(flange_to_tool)
|
||||
|
||||
assert base_to_tool[:3, 3] == pytest.approx([0.49, -0.10, 0.20])
|
||||
assert recovered_flange == pytest.approx(base_to_flange)
|
||||
assert moving_joint_names == [f"joint_{index}" for index in range(1, 8)]
|
||||
assert all(
|
||||
filename.startswith(
|
||||
"package://xr_rm_teleop/models/rm75_omnipicker/meshes/"
|
||||
)
|
||||
for filename in mesh_filenames
|
||||
)
|
||||
assert tcp_joint is not None
|
||||
assert tcp_joint.attrib["type"] == "fixed"
|
||||
assert tcp_joint.find("parent").attrib["link"] == "omnipicker_base_link"
|
||||
assert tcp_joint.find("child").attrib["link"] == "omnipicker_tcp"
|
||||
assert tcp_joint.find("origin").attrib["xyz"] == "0 0 0.16"
|
||||
assert tcp_joint.find("origin").attrib["rpy"] == "0 0 0"
|
||||
|
||||
|
||||
def test_transform_to_arm_pose_roundtrip() -> None:
|
||||
expected = ArmPose(0.25, -0.30, 0.40, 0.20, -0.30, 0.40)
|
||||
def test_validated_transform_accepts_finite_se3_and_returns_a_copy() -> None:
|
||||
transform = np.eye(4)
|
||||
transform[:3, 3] = [0.3, -0.1, 0.2]
|
||||
|
||||
actual = _transform_to_arm_pose(_arm_pose_to_transform(expected))
|
||||
actual = _validated_transform(transform)
|
||||
|
||||
assert actual.xyz() == pytest.approx(expected.xyz())
|
||||
assert actual.rpy() == pytest.approx(expected.rpy())
|
||||
assert actual == pytest.approx(transform)
|
||||
assert actual is not transform
|
||||
|
||||
|
||||
@pytest.mark.parametrize(
|
||||
"transform",
|
||||
[
|
||||
np.eye(3),
|
||||
np.full((4, 4), np.nan),
|
||||
np.vstack([np.eye(3, 4), [0.0, 0.0, 0.0, 2.0]]),
|
||||
np.diag([2.0, 1.0, 1.0, 1.0]),
|
||||
],
|
||||
)
|
||||
def test_validated_transform_rejects_invalid_se3(transform: np.ndarray) -> None:
|
||||
with pytest.raises(ValueError):
|
||||
_validated_transform(transform)
|
||||
|
||||
|
||||
def test_qp_result_rejects_nan_position_and_velocity_violations() -> None:
|
||||
|
||||
@@ -2,102 +2,43 @@
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import math
|
||||
from importlib.metadata import PackageNotFoundError, version
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .realman_adapter import ArmPose
|
||||
|
||||
|
||||
EXPECTED_PLACO_VERSION = "0.9.4"
|
||||
RM75_JOINT_NAMES = [f"joint_{index}" for index in range(1, 8)]
|
||||
RM75_Q_SLICE = slice(7, 14)
|
||||
|
||||
|
||||
def _rpy_to_rotation(roll: float, pitch: float, yaw: float) -> np.ndarray:
|
||||
cr, sr = math.cos(roll), math.sin(roll)
|
||||
cp, sp = math.cos(pitch), math.sin(pitch)
|
||||
cy, sy = math.cos(yaw), math.sin(yaw)
|
||||
return np.array(
|
||||
[
|
||||
[cy * cp, cy * sp * sr - sy * cr, cy * sp * cr + sy * sr],
|
||||
[sy * cp, sy * sp * sr + cy * cr, sy * sp * cr - cy * sr],
|
||||
[-sp, cp * sr, cp * cr],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
def _validated_transform(transform: np.ndarray) -> np.ndarray:
|
||||
values = np.asarray(transform, dtype=float)
|
||||
if values.shape != (4, 4) or not np.isfinite(values).all():
|
||||
raise ValueError("target transform must be a finite 4x4 matrix")
|
||||
if not np.allclose(values[3], [0.0, 0.0, 0.0, 1.0], atol=1e-9):
|
||||
raise ValueError("target transform must have a valid homogeneous row")
|
||||
|
||||
rotation = values[:3, :3]
|
||||
if (
|
||||
np.linalg.norm(rotation.T @ rotation - np.eye(3)) > 1e-3
|
||||
or np.linalg.det(rotation) <= 0.0
|
||||
):
|
||||
raise ValueError("target transform must contain a valid rotation")
|
||||
u, _, vt = np.linalg.svd(rotation)
|
||||
projected = u @ vt
|
||||
if np.linalg.det(projected) <= 0.0:
|
||||
raise ValueError("target transform must contain a proper rotation")
|
||||
|
||||
def _rotation_to_rpy(rotation: np.ndarray) -> tuple[float, float, float]:
|
||||
pitch = math.asin(-float(np.clip(rotation[2, 0], -1.0, 1.0)))
|
||||
if abs(math.cos(pitch)) > 1e-9:
|
||||
roll = math.atan2(float(rotation[2, 1]), float(rotation[2, 2]))
|
||||
yaw = math.atan2(float(rotation[1, 0]), float(rotation[0, 0]))
|
||||
else:
|
||||
roll = math.atan2(-float(rotation[1, 2]), float(rotation[1, 1]))
|
||||
yaw = 0.0
|
||||
return roll, pitch, yaw
|
||||
|
||||
|
||||
def _arm_pose_to_transform(pose: ArmPose) -> np.ndarray:
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = _rpy_to_rotation(pose.rx, pose.ry, pose.rz)
|
||||
transform[:3, 3] = pose.xyz()
|
||||
return transform
|
||||
|
||||
|
||||
def _tool_pose_to_transform(tool_pose: list[float]) -> np.ndarray:
|
||||
values = np.asarray(tool_pose, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
raise ValueError("tool pose must contain 7 finite values")
|
||||
x, y, z, qx, qy, qz, qw = values
|
||||
norm = math.sqrt(qx * qx + qy * qy + qz * qz + qw * qw)
|
||||
if norm <= 1e-9:
|
||||
raise ValueError("tool quaternion norm must be positive")
|
||||
qx, qy, qz, qw = qx / norm, qy / norm, qz / norm, qw / norm
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = np.array(
|
||||
[
|
||||
[
|
||||
1 - 2 * (qy * qy + qz * qz),
|
||||
2 * (qx * qy - qz * qw),
|
||||
2 * (qx * qz + qy * qw),
|
||||
],
|
||||
[
|
||||
2 * (qx * qy + qz * qw),
|
||||
1 - 2 * (qx * qx + qz * qz),
|
||||
2 * (qy * qz - qx * qw),
|
||||
],
|
||||
[
|
||||
2 * (qx * qz - qy * qw),
|
||||
2 * (qy * qz + qx * qw),
|
||||
1 - 2 * (qx * qx + qy * qy),
|
||||
],
|
||||
]
|
||||
)
|
||||
transform[:3, 3] = [x, y, z]
|
||||
return transform
|
||||
|
||||
|
||||
def _transform_to_arm_pose(transform: np.ndarray) -> ArmPose:
|
||||
roll, pitch, yaw = _rotation_to_rpy(transform[:3, :3])
|
||||
return ArmPose(
|
||||
float(transform[0, 3]),
|
||||
float(transform[1, 3]),
|
||||
float(transform[2, 3]),
|
||||
roll,
|
||||
pitch,
|
||||
yaw,
|
||||
)
|
||||
result = values.copy()
|
||||
result[:3, :3] = projected
|
||||
return result
|
||||
|
||||
|
||||
class PlacoIkSolver:
|
||||
def __init__(
|
||||
self,
|
||||
urdf_path: str,
|
||||
tool_pose: list[float],
|
||||
dt: float,
|
||||
) -> None:
|
||||
if dt <= 0.0:
|
||||
@@ -147,15 +88,16 @@ class PlacoIkSolver:
|
||||
for index in velocity_offsets
|
||||
]
|
||||
)
|
||||
self._tool_transform = _tool_pose_to_transform(tool_pose)
|
||||
self._tool_inverse = np.linalg.inv(self._tool_transform)
|
||||
self._actual_joints: np.ndarray | None = None
|
||||
|
||||
self._solver = placo.KinematicsSolver(self._robot)
|
||||
self._solver.dt = dt
|
||||
self._solver.mask_fbase(True)
|
||||
self._solver.enable_velocity_limits(True)
|
||||
self._frame_task = self._solver.add_frame_task("link_7", np.eye(4))
|
||||
self._frame_task = self._solver.add_frame_task(
|
||||
"omnipicker_tcp",
|
||||
np.eye(4),
|
||||
)
|
||||
self._frame_task.configure("rm75_frame", "soft", 1.0)
|
||||
manipulability = self._solver.add_manipulability_task(
|
||||
"link_7",
|
||||
@@ -169,7 +111,7 @@ class PlacoIkSolver:
|
||||
def base_configuration(self) -> list[float]:
|
||||
return self._robot.state.q[:7].tolist()
|
||||
|
||||
def update_joint_state(self, joints: list[float]) -> ArmPose:
|
||||
def update_joint_state(self, joints: list[float]) -> np.ndarray:
|
||||
values = np.asarray(joints, dtype=float)
|
||||
if values.shape != (7,) or not np.isfinite(values).all():
|
||||
raise ValueError("joint state must contain 7 finite values")
|
||||
@@ -177,18 +119,15 @@ class PlacoIkSolver:
|
||||
self._actual_joints = values.copy()
|
||||
self._robot.state.q[RM75_Q_SLICE] = values
|
||||
self._robot.update_kinematics()
|
||||
base_to_flange = self._robot.get_T_world_frame("link_7")
|
||||
base_to_tool = self._robot.get_T_world_frame("omnipicker_tcp")
|
||||
if is_first_feedback:
|
||||
self._frame_task.T_world_frame = base_to_flange.copy()
|
||||
base_to_tool = base_to_flange @ self._tool_transform
|
||||
return _transform_to_arm_pose(base_to_tool)
|
||||
self._frame_task.T_world_frame = base_to_tool.copy()
|
||||
return base_to_tool.copy()
|
||||
|
||||
def solve(self, target_tool_pose: ArmPose) -> list[float]:
|
||||
def solve(self, target_tool_pose: np.ndarray) -> list[float]:
|
||||
if self._actual_joints is None:
|
||||
raise RuntimeError("joint state must be initialized before QP solve")
|
||||
self._frame_task.T_world_frame = (
|
||||
_arm_pose_to_transform(target_tool_pose) @ self._tool_inverse
|
||||
)
|
||||
self._frame_task.T_world_frame = _validated_transform(target_tool_pose)
|
||||
self._solver.solve(True)
|
||||
result = np.asarray(
|
||||
self._robot.state.q[RM75_Q_SLICE],
|
||||
|
||||
@@ -12,6 +12,7 @@ import threading
|
||||
import time
|
||||
from typing import Iterable
|
||||
|
||||
import numpy as np
|
||||
import rclpy
|
||||
from geometry_msgs.msg import PoseStamped, TwistStamped
|
||||
from rclpy.node import Node
|
||||
@@ -23,7 +24,6 @@ from xr_rm_interfaces.msg import XrController
|
||||
from .fun_peripheral import load_peripheral_config
|
||||
from .placo_ik_solver import PlacoIkSolver
|
||||
from .realman_adapter import (
|
||||
ArmPose,
|
||||
JointStateSnapshot,
|
||||
MockRealManAdapter,
|
||||
RealManAdapter,
|
||||
@@ -38,14 +38,6 @@ def _clamp(value: float, low: float, high: float) -> float:
|
||||
return min(max(value, low), high)
|
||||
|
||||
|
||||
def _wrap_angle(angle: float) -> float:
|
||||
return math.atan2(math.sin(angle), math.cos(angle))
|
||||
|
||||
|
||||
def _angle_delta(target: float, current: float) -> float:
|
||||
return _wrap_angle(target - current)
|
||||
|
||||
|
||||
def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float, float]:
|
||||
x, y, z, w = [float(value) for value in values]
|
||||
norm = math.sqrt(x * x + y * y + z * z + w * w)
|
||||
@@ -54,31 +46,26 @@ def _normalize_quaternion(values: Iterable[float]) -> tuple[float, float, float,
|
||||
return x / norm, y / norm, z / norm, w / norm
|
||||
|
||||
|
||||
def _quaternion_conjugate(
|
||||
def _project_rotation(rotation: np.ndarray) -> np.ndarray:
|
||||
values = np.asarray(rotation, dtype=float)
|
||||
if values.shape != (3, 3) or not np.isfinite(values).all():
|
||||
raise ValueError("rotation must be a finite 3x3 matrix")
|
||||
if (
|
||||
np.linalg.norm(values.T @ values - np.eye(3)) > 1e-3
|
||||
or np.linalg.det(values) <= 0.0
|
||||
):
|
||||
raise ValueError("rotation matrix is invalid")
|
||||
u, _, vt = np.linalg.svd(values)
|
||||
projected = u @ vt
|
||||
if np.linalg.det(projected) <= 0.0:
|
||||
raise ValueError("rotation matrix must be proper")
|
||||
return projected
|
||||
|
||||
|
||||
def _quaternion_to_matrix(
|
||||
quat: tuple[float, float, float, float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
x, y, z, w = quat
|
||||
return -x, -y, -z, w
|
||||
|
||||
|
||||
def _quaternion_multiply(
|
||||
left: tuple[float, float, float, float],
|
||||
right: tuple[float, float, float, float],
|
||||
) -> tuple[float, float, float, float]:
|
||||
lx, ly, lz, lw = left
|
||||
rx, ry, rz, rw = right
|
||||
return _normalize_quaternion(
|
||||
(
|
||||
lw * rx + lx * rw + ly * rz - lz * ry,
|
||||
lw * ry - lx * rz + ly * rw + lz * rx,
|
||||
lw * rz + lx * ry - ly * rx + lz * rw,
|
||||
lw * rw - lx * rx - ly * ry - lz * rz,
|
||||
)
|
||||
)
|
||||
|
||||
|
||||
def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float]:
|
||||
x, y, z, w = quat
|
||||
) -> np.ndarray:
|
||||
x, y, z, w = _normalize_quaternion(quat)
|
||||
xx = x * x
|
||||
yy = y * y
|
||||
zz = z * z
|
||||
@@ -88,69 +75,95 @@ def _quaternion_to_matrix(quat: tuple[float, float, float, float]) -> list[float
|
||||
wx = w * x
|
||||
wy = w * y
|
||||
wz = w * z
|
||||
return [
|
||||
1.0 - 2.0 * (yy + zz),
|
||||
2.0 * (xy - wz),
|
||||
2.0 * (xz + wy),
|
||||
2.0 * (xy + wz),
|
||||
1.0 - 2.0 * (xx + zz),
|
||||
2.0 * (yz - wx),
|
||||
2.0 * (xz - wy),
|
||||
2.0 * (yz + wx),
|
||||
1.0 - 2.0 * (xx + yy),
|
||||
]
|
||||
return np.asarray(
|
||||
[
|
||||
[1.0 - 2.0 * (yy + zz), 2.0 * (xy - wz), 2.0 * (xz + wy)],
|
||||
[2.0 * (xy + wz), 1.0 - 2.0 * (xx + zz), 2.0 * (yz - wx)],
|
||||
[2.0 * (xz - wy), 2.0 * (yz + wx), 1.0 - 2.0 * (xx + yy)],
|
||||
]
|
||||
)
|
||||
|
||||
|
||||
def _matrix_multiply(left: list[float], right: list[float]) -> list[float]:
|
||||
return [
|
||||
sum(left[row * 3 + k] * right[k * 3 + col] for k in range(3))
|
||||
for row in range(3)
|
||||
for col in range(3)
|
||||
]
|
||||
|
||||
|
||||
def _matrix_transpose(matrix: list[float]) -> list[float]:
|
||||
return [
|
||||
matrix[0], matrix[3], matrix[6],
|
||||
matrix[1], matrix[4], matrix[7],
|
||||
matrix[2], matrix[5], matrix[8],
|
||||
]
|
||||
|
||||
|
||||
def _euler_to_quaternion(roll: float, pitch: float, yaw: float) -> tuple[float, float, float, float]:
|
||||
cy = math.cos(yaw * 0.5)
|
||||
sy = math.sin(yaw * 0.5)
|
||||
cp = math.cos(pitch * 0.5)
|
||||
sp = math.sin(pitch * 0.5)
|
||||
cr = math.cos(roll * 0.5)
|
||||
sr = math.sin(roll * 0.5)
|
||||
|
||||
qw = cr * cp * cy + sr * sp * sy
|
||||
qx = sr * cp * cy - cr * sp * sy
|
||||
qy = cr * sp * cy + sr * cp * sy
|
||||
qz = cr * cp * sy - sr * sp * cy
|
||||
return _normalize_quaternion((qx, qy, qz, qw))
|
||||
|
||||
|
||||
def _euler_to_matrix(roll: float, pitch: float, yaw: float) -> list[float]:
|
||||
return _quaternion_to_matrix(_euler_to_quaternion(roll, pitch, yaw))
|
||||
|
||||
|
||||
def _matrix_to_euler(matrix: list[float]) -> tuple[float, float, float]:
|
||||
sy = -_clamp(matrix[6], -1.0, 1.0)
|
||||
pitch = math.asin(sy)
|
||||
cp = math.cos(pitch)
|
||||
if abs(cp) > 1e-9:
|
||||
roll = math.atan2(matrix[7], matrix[8])
|
||||
yaw = math.atan2(matrix[3], matrix[0])
|
||||
def _matrix_to_quaternion(
|
||||
rotation: np.ndarray,
|
||||
) -> tuple[float, float, float, float]:
|
||||
matrix = _project_rotation(rotation)
|
||||
trace = float(np.trace(matrix))
|
||||
if trace > 0.0:
|
||||
scale = math.sqrt(trace + 1.0) * 2.0
|
||||
quaternion = (
|
||||
(matrix[2, 1] - matrix[1, 2]) / scale,
|
||||
(matrix[0, 2] - matrix[2, 0]) / scale,
|
||||
(matrix[1, 0] - matrix[0, 1]) / scale,
|
||||
0.25 * scale,
|
||||
)
|
||||
elif matrix[0, 0] > matrix[1, 1] and matrix[0, 0] > matrix[2, 2]:
|
||||
scale = math.sqrt(1.0 + matrix[0, 0] - matrix[1, 1] - matrix[2, 2]) * 2.0
|
||||
quaternion = (
|
||||
0.25 * scale,
|
||||
(matrix[0, 1] + matrix[1, 0]) / scale,
|
||||
(matrix[0, 2] + matrix[2, 0]) / scale,
|
||||
(matrix[2, 1] - matrix[1, 2]) / scale,
|
||||
)
|
||||
elif matrix[1, 1] > matrix[2, 2]:
|
||||
scale = math.sqrt(1.0 + matrix[1, 1] - matrix[0, 0] - matrix[2, 2]) * 2.0
|
||||
quaternion = (
|
||||
(matrix[0, 1] + matrix[1, 0]) / scale,
|
||||
0.25 * scale,
|
||||
(matrix[1, 2] + matrix[2, 1]) / scale,
|
||||
(matrix[0, 2] - matrix[2, 0]) / scale,
|
||||
)
|
||||
else:
|
||||
roll = math.atan2(-matrix[5], matrix[4])
|
||||
yaw = 0.0
|
||||
return _wrap_angle(roll), _wrap_angle(pitch), _wrap_angle(yaw)
|
||||
scale = math.sqrt(1.0 + matrix[2, 2] - matrix[0, 0] - matrix[1, 1]) * 2.0
|
||||
quaternion = (
|
||||
(matrix[0, 2] + matrix[2, 0]) / scale,
|
||||
(matrix[1, 2] + matrix[2, 1]) / scale,
|
||||
0.25 * scale,
|
||||
(matrix[1, 0] - matrix[0, 1]) / scale,
|
||||
)
|
||||
result = _normalize_quaternion(quaternion)
|
||||
return tuple(-value for value in result) if result[3] < 0.0 else result
|
||||
|
||||
|
||||
def _quaternion_to_euler(quat: tuple[float, float, float, float]) -> tuple[float, float, float]:
|
||||
return _matrix_to_euler(_quaternion_to_matrix(quat))
|
||||
def _so3_log(rotation: np.ndarray) -> np.ndarray:
|
||||
quaternion = _matrix_to_quaternion(rotation)
|
||||
vector = np.asarray(quaternion[:3])
|
||||
sine_half = float(np.linalg.norm(vector))
|
||||
if sine_half <= 1e-12:
|
||||
return 2.0 * vector
|
||||
angle = 2.0 * math.atan2(sine_half, quaternion[3])
|
||||
return vector * (angle / sine_half)
|
||||
|
||||
|
||||
def _so3_exp(rotation_vector: np.ndarray) -> np.ndarray:
|
||||
vector = np.asarray(rotation_vector, dtype=float)
|
||||
if vector.shape != (3,) or not np.isfinite(vector).all():
|
||||
raise ValueError("rotation vector must contain 3 finite values")
|
||||
angle = float(np.linalg.norm(vector))
|
||||
skew = np.asarray(
|
||||
[
|
||||
[0.0, -vector[2], vector[1]],
|
||||
[vector[2], 0.0, -vector[0]],
|
||||
[-vector[1], vector[0], 0.0],
|
||||
]
|
||||
)
|
||||
if angle <= 1e-9:
|
||||
return _project_rotation(np.eye(3) + skew + 0.5 * skew @ skew)
|
||||
return _project_rotation(
|
||||
np.eye(3)
|
||||
+ math.sin(angle) / angle * skew
|
||||
+ (1.0 - math.cos(angle)) / (angle * angle) * skew @ skew
|
||||
)
|
||||
|
||||
|
||||
def _make_transform(position: Iterable[float], rotation: np.ndarray) -> np.ndarray:
|
||||
xyz = np.asarray(list(position), dtype=float)
|
||||
if xyz.shape != (3,) or not np.isfinite(xyz).all():
|
||||
raise ValueError("position must contain 3 finite values")
|
||||
transform = np.eye(4)
|
||||
transform[:3, :3] = _project_rotation(rotation)
|
||||
transform[:3, 3] = xyz
|
||||
return transform
|
||||
|
||||
|
||||
class SingleArmVelocityTeleop(Node):
|
||||
@@ -164,7 +177,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
self.declare_parameter("arm_name", "rm75")
|
||||
self.declare_parameter("controller_topic", "/xr/right_controller")
|
||||
self.declare_parameter("control_rate_hz", 90.0)
|
||||
self.declare_parameter("control_rate_hz", 125.0)
|
||||
self.declare_parameter("command_timeout_sec", 0.12)
|
||||
self.declare_parameter("scale", 1.0)
|
||||
self.declare_parameter("deadband_m", 0.001)
|
||||
@@ -177,7 +190,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self.declare_parameter("enable_orientation_axes", [True, True, True])
|
||||
self.declare_parameter("orientation_deadband_rad", 0.005)
|
||||
self.declare_parameter("orientation_filter_alpha", 0.65)
|
||||
self.declare_parameter("max_orientation_speed", 0.6)
|
||||
self.declare_parameter("max_orientation_speed", 0.5)
|
||||
self.declare_parameter("workspace_min", [0.20, -0.35, 0.10])
|
||||
self.declare_parameter("workspace_max", [0.65, 0.35, 0.60])
|
||||
self.declare_parameter("cyl_radius_limit", [0.20, 0.60])
|
||||
@@ -252,13 +265,13 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._active = False
|
||||
self._controller_start: list[float] | None = None
|
||||
self._controller_orientation_start: tuple[float, float, float, float] | None = None
|
||||
self._robot_start_pose: ArmPose | None = None
|
||||
self._robot_start_transform: np.ndarray | None = None
|
||||
self._filtered_target: list[float] | None = None
|
||||
self._filtered_orientation_target: list[float] | None = None
|
||||
self._filtered_orientation_target: np.ndarray | None = None
|
||||
self._last_sent_target: list[float] | None = None
|
||||
self._last_sent_orientation: list[float] | None = None
|
||||
self._last_sent_orientation: np.ndarray | None = None
|
||||
self._last_command_time: Time | None = None
|
||||
self._last_current_pose: ArmPose | None = None
|
||||
self._last_current_pose: np.ndarray | None = None
|
||||
self._last_valid_joint_target: list[float] | None = None
|
||||
self._joint_feedback_ready = False
|
||||
self._stop_sent = True
|
||||
@@ -267,6 +280,12 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._tool_command_queue: queue.Queue[tuple[bool, str] | None] | None = None
|
||||
self._tool_worker_stop = threading.Event()
|
||||
self._tool_worker_thread: threading.Thread | None = None
|
||||
self._timing_stats_window = max(1, int(round(5.0 / self._dt)))
|
||||
self._timing_samples = {
|
||||
name: []
|
||||
for name in ("period", "total", "qp", "send", "feedback_age")
|
||||
}
|
||||
self._last_control_tick_started_ns: int | None = None
|
||||
|
||||
peripheral_arm = self._peripheral_arm_name()
|
||||
config_file = str(self.get_parameter("peripheral_config_file").value)
|
||||
@@ -276,7 +295,6 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
self._ik_solver = PlacoIkSolver(
|
||||
str(self.get_parameter("robot_urdf_path").value),
|
||||
self._peripheral_config.tool_pose,
|
||||
self._dt,
|
||||
)
|
||||
self._adapter = self._make_adapter()
|
||||
@@ -454,6 +472,18 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._enqueue_tool_command(self._trigger_tool_open, "trigger")
|
||||
|
||||
def _control_tick(self) -> None:
|
||||
tick_started_ns = time.perf_counter_ns()
|
||||
last_tick_started_ns = getattr(
|
||||
self,
|
||||
"_last_control_tick_started_ns",
|
||||
None,
|
||||
)
|
||||
self._last_control_tick_started_ns = tick_started_ns
|
||||
tick_period_ms = (
|
||||
None
|
||||
if last_tick_started_ns is None
|
||||
else (tick_started_ns - last_tick_started_ns) * 1e-6
|
||||
)
|
||||
now = self.get_clock().now()
|
||||
snapshot = self._fresh_joint_state()
|
||||
if snapshot is None:
|
||||
@@ -522,67 +552,93 @@ class SingleArmVelocityTeleop(Node):
|
||||
)
|
||||
return
|
||||
|
||||
feedback_age_ms = (time.monotonic() - snapshot.received_at) * 1000.0
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start_pose is not None
|
||||
assert self._robot_start_transform is not None
|
||||
|
||||
raw_target_xyz = self._raw_target_from_controller(controller_now)
|
||||
raw_target_rpy = self._raw_orientation_from_controller(controller_quat)
|
||||
raw_target_orientation = self._raw_orientation_from_controller(
|
||||
controller_quat
|
||||
)
|
||||
workspace_target, workspace_clamped = self._clamp_workspace_with_flag(raw_target_xyz)
|
||||
desired_target = self._apply_deadband(workspace_target)
|
||||
filtered_target = self._filter_target(desired_target)
|
||||
sent_target, step_limited = self._limit_target_step(filtered_target)
|
||||
sent_target, final_clamped = self._clamp_workspace_with_flag(sent_target)
|
||||
desired_orientation = self._apply_orientation_deadband(raw_target_rpy)
|
||||
desired_orientation = self._apply_orientation_deadband(
|
||||
raw_target_orientation
|
||||
)
|
||||
filtered_orientation = self._filter_orientation_target(desired_orientation)
|
||||
sent_orientation, orientation_step_limited = self._limit_orientation_step(filtered_orientation)
|
||||
|
||||
velocity = self._estimate_command_velocity(sent_target, sent_orientation, now)
|
||||
target_clamped = workspace_clamped or step_limited or final_clamped or orientation_step_limited
|
||||
raw_target_pose = ArmPose(
|
||||
x=raw_target_xyz[0],
|
||||
y=raw_target_xyz[1],
|
||||
z=raw_target_xyz[2],
|
||||
rx=raw_target_rpy[0],
|
||||
ry=raw_target_rpy[1],
|
||||
rz=raw_target_rpy[2],
|
||||
raw_target_pose = _make_transform(
|
||||
raw_target_xyz,
|
||||
raw_target_orientation,
|
||||
)
|
||||
target_pose = ArmPose(
|
||||
x=sent_target[0],
|
||||
y=sent_target[1],
|
||||
z=sent_target[2],
|
||||
rx=sent_orientation[0],
|
||||
ry=sent_orientation[1],
|
||||
rz=sent_orientation[2],
|
||||
target_pose = _make_transform(
|
||||
sent_target,
|
||||
sent_orientation,
|
||||
)
|
||||
|
||||
self._publish_debug(raw_target_pose, target_pose, velocity, target_clamped)
|
||||
qp_started_ns = time.perf_counter_ns()
|
||||
joint_target = self._solve_joint_target(target_pose)
|
||||
if self._send_joint_target(joint_target):
|
||||
qp_ms = (time.perf_counter_ns() - qp_started_ns) * 1e-6
|
||||
send_started_ns = time.perf_counter_ns()
|
||||
sent = self._send_joint_target(joint_target)
|
||||
send_ms = (time.perf_counter_ns() - send_started_ns) * 1e-6
|
||||
if sent:
|
||||
self._last_sent_target = sent_target
|
||||
self._last_sent_orientation = sent_orientation
|
||||
self._last_sent_orientation = sent_orientation.copy()
|
||||
self._last_command_time = now
|
||||
self._stop_sent = False
|
||||
total_ms = (time.perf_counter_ns() - tick_started_ns) * 1e-6
|
||||
try:
|
||||
self._record_timing_sample(
|
||||
tick_period_ms,
|
||||
total_ms,
|
||||
qp_ms,
|
||||
send_ms,
|
||||
feedback_age_ms,
|
||||
)
|
||||
except Exception as exc:
|
||||
self.get_logger().warn(
|
||||
f"{self._arm_name} 控制周期统计失败：{exc}",
|
||||
throttle_duration_sec=5.0,
|
||||
)
|
||||
|
||||
def _enter_active_control(
|
||||
self,
|
||||
controller_now: list[float],
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
robot_pose: ArmPose,
|
||||
robot_pose: np.ndarray,
|
||||
now: Time,
|
||||
) -> None:
|
||||
robot_xyz = robot_pose.xyz()
|
||||
robot_transform = _make_transform(
|
||||
robot_pose[:3, 3],
|
||||
robot_pose[:3, :3],
|
||||
)
|
||||
robot_xyz = robot_transform[:3, 3].tolist()
|
||||
robot_orientation = robot_transform[:3, :3]
|
||||
self._active = True
|
||||
self._controller_start = controller_now
|
||||
self._controller_orientation_start = controller_quat
|
||||
self._robot_start_pose = robot_pose
|
||||
self._robot_start_transform = robot_transform
|
||||
self._filtered_target = robot_xyz
|
||||
self._filtered_orientation_target = robot_pose.rpy()
|
||||
self._filtered_orientation_target = robot_orientation.copy()
|
||||
self._last_sent_target = robot_xyz
|
||||
self._last_sent_orientation = robot_pose.rpy()
|
||||
self._last_sent_orientation = robot_orientation.copy()
|
||||
self._last_command_time = now
|
||||
self._stop_sent = True
|
||||
self.get_logger().info(f"{self._arm_name} Grip 按下，已锁定手柄和机械臂初始位姿。")
|
||||
self._publish_debug(robot_pose, robot_pose, [0.0] * 6, False)
|
||||
self._publish_debug(
|
||||
robot_transform,
|
||||
robot_transform,
|
||||
[0.0] * 6,
|
||||
False,
|
||||
)
|
||||
|
||||
@staticmethod
|
||||
def _controller_xyz(msg: XrController) -> list[float]:
|
||||
@@ -602,15 +658,21 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _raw_target_from_controller(self, controller_now: list[float]) -> list[float]:
|
||||
assert self._controller_start is not None
|
||||
assert self._robot_start_pose is not None
|
||||
assert self._robot_start_transform is not None
|
||||
robot_start_xyz = self._robot_start_transform[:3, 3]
|
||||
controller_delta = [controller_now[i] - self._controller_start[i] for i in range(3)]
|
||||
robot_delta = self._map_xr_delta_to_robot(controller_delta)
|
||||
target = [
|
||||
self._robot_start_pose.x + self._scale * robot_delta[0],
|
||||
self._robot_start_pose.y + self._scale * robot_delta[1],
|
||||
self._robot_start_pose.z + self._scale * robot_delta[2],
|
||||
robot_start_xyz[0] + self._scale * robot_delta[0],
|
||||
robot_start_xyz[1] + self._scale * robot_delta[1],
|
||||
robot_start_xyz[2] + self._scale * robot_delta[2],
|
||||
]
|
||||
return [
|
||||
float(target[index])
|
||||
if self._enable_position_axes[index]
|
||||
else float(robot_start_xyz[index])
|
||||
for index in range(3)
|
||||
]
|
||||
return [target[i] if self._enable_position_axes[i] else self._robot_start_pose.xyz()[i] for i in range(3)]
|
||||
|
||||
def _map_xr_delta_to_robot(self, delta: list[float]) -> list[float]:
|
||||
matrix = self._xr_to_robot_matrix
|
||||
@@ -623,34 +685,33 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _raw_orientation_from_controller(
|
||||
self,
|
||||
controller_quat: tuple[float, float, float, float],
|
||||
) -> list[float]:
|
||||
assert self._robot_start_pose is not None
|
||||
) -> np.ndarray:
|
||||
assert self._robot_start_transform is not None
|
||||
robot_start_rotation = self._robot_start_transform[:3, :3]
|
||||
if not self._enable_orientation_control:
|
||||
return self._robot_start_pose.rpy()
|
||||
return robot_start_rotation.copy()
|
||||
|
||||
assert self._controller_orientation_start is not None
|
||||
xr_delta_quat = _quaternion_multiply(
|
||||
controller_quat,
|
||||
_quaternion_conjugate(self._controller_orientation_start),
|
||||
xr_delta_matrix = (
|
||||
_quaternion_to_matrix(controller_quat)
|
||||
@ _quaternion_to_matrix(self._controller_orientation_start).T
|
||||
)
|
||||
xr_delta_matrix = _quaternion_to_matrix(xr_delta_quat)
|
||||
matrix = self._xr_to_robot_matrix
|
||||
robot_delta_matrix = _matrix_multiply(
|
||||
_matrix_multiply(matrix, xr_delta_matrix),
|
||||
_matrix_transpose(matrix),
|
||||
mapping = np.asarray(self._xr_to_robot_matrix, dtype=float).reshape(3, 3)
|
||||
robot_delta = _project_rotation(
|
||||
mapping @ xr_delta_matrix @ mapping.T
|
||||
)
|
||||
robot_start_matrix = _euler_to_matrix(
|
||||
self._robot_start_pose.rx,
|
||||
self._robot_start_pose.ry,
|
||||
self._robot_start_pose.rz,
|
||||
rotation_vector = _so3_log(robot_delta)
|
||||
rotation_vector = np.asarray(
|
||||
[
|
||||
rotation_vector[index]
|
||||
if self._enable_orientation_axes[index]
|
||||
else 0.0
|
||||
for index in range(3)
|
||||
]
|
||||
)
|
||||
return _project_rotation(
|
||||
_so3_exp(rotation_vector) @ robot_start_rotation
|
||||
)
|
||||
target_matrix = _matrix_multiply(robot_delta_matrix, robot_start_matrix)
|
||||
target_rpy = list(_matrix_to_euler(target_matrix))
|
||||
start_rpy = self._robot_start_pose.rpy()
|
||||
return [
|
||||
target_rpy[i] if self._enable_orientation_axes[i] else start_rpy[i]
|
||||
for i in range(3)
|
||||
]
|
||||
|
||||
def _apply_deadband(self, target: list[float]) -> list[float]:
|
||||
if self._deadband_m <= 0.0 or self._last_sent_target is None:
|
||||
@@ -699,50 +760,49 @@ class SingleArmVelocityTeleop(Node):
|
||||
for i in range(3)
|
||||
], True
|
||||
|
||||
def _apply_orientation_deadband(self, target_rpy: list[float]) -> list[float]:
|
||||
def _apply_orientation_deadband(self, target_rotation: np.ndarray) -> np.ndarray:
|
||||
if self._orientation_deadband_rad <= 0.0 or self._last_sent_orientation is None:
|
||||
return [_wrap_angle(value) for value in target_rpy]
|
||||
delta = [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i])
|
||||
for i in range(3)
|
||||
]
|
||||
if _norm(delta) < self._orientation_deadband_rad:
|
||||
return list(self._last_sent_orientation)
|
||||
return [_wrap_angle(value) for value in target_rpy]
|
||||
return _project_rotation(target_rotation)
|
||||
error = _so3_log(
|
||||
target_rotation @ self._last_sent_orientation.T
|
||||
)
|
||||
if np.linalg.norm(error) < self._orientation_deadband_rad:
|
||||
return self._last_sent_orientation.copy()
|
||||
return _project_rotation(target_rotation)
|
||||
|
||||
def _filter_orientation_target(self, target_rpy: list[float]) -> list[float]:
|
||||
def _filter_orientation_target(self, target_rotation: np.ndarray) -> np.ndarray:
|
||||
if self._filtered_orientation_target is None:
|
||||
self._filtered_orientation_target = [_wrap_angle(value) for value in target_rpy]
|
||||
return list(self._filtered_orientation_target)
|
||||
self._filtered_orientation_target = _project_rotation(target_rotation)
|
||||
return self._filtered_orientation_target.copy()
|
||||
|
||||
self._filtered_orientation_target = [
|
||||
_wrap_angle(
|
||||
self._filtered_orientation_target[i]
|
||||
+ self._orientation_filter_alpha
|
||||
* _angle_delta(target_rpy[i], self._filtered_orientation_target[i])
|
||||
)
|
||||
for i in range(3)
|
||||
]
|
||||
return list(self._filtered_orientation_target)
|
||||
error = _so3_log(
|
||||
target_rotation @ self._filtered_orientation_target.T
|
||||
)
|
||||
self._filtered_orientation_target = _project_rotation(
|
||||
_so3_exp(self._orientation_filter_alpha * error)
|
||||
@ self._filtered_orientation_target
|
||||
)
|
||||
return self._filtered_orientation_target.copy()
|
||||
|
||||
def _limit_orientation_step(self, target_rpy: list[float]) -> tuple[list[float], bool]:
|
||||
def _limit_orientation_step(
|
||||
self,
|
||||
target_rotation: np.ndarray,
|
||||
) -> tuple[np.ndarray, bool]:
|
||||
if self._last_sent_orientation is None:
|
||||
return [_wrap_angle(value) for value in target_rpy], False
|
||||
return _project_rotation(target_rotation), False
|
||||
|
||||
delta = [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i])
|
||||
for i in range(3)
|
||||
]
|
||||
distance = _norm(delta)
|
||||
error = _so3_log(
|
||||
target_rotation @ self._last_sent_orientation.T
|
||||
)
|
||||
distance = float(np.linalg.norm(error))
|
||||
max_step = self._max_orientation_speed * self._dt
|
||||
if distance <= max_step or distance <= 1e-9:
|
||||
return [_wrap_angle(value) for value in target_rpy], False
|
||||
return _project_rotation(target_rotation), False
|
||||
|
||||
scale = max_step / distance
|
||||
return [
|
||||
_wrap_angle(self._last_sent_orientation[i] + delta[i] * scale)
|
||||
for i in range(3)
|
||||
], True
|
||||
return _project_rotation(
|
||||
_so3_exp(scale * error) @ self._last_sent_orientation
|
||||
), True
|
||||
|
||||
def _clamp_workspace_with_flag(self, target: list[float]) -> tuple[list[float], bool]:
|
||||
clamped = [
|
||||
@@ -777,7 +837,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _estimate_command_velocity(
|
||||
self,
|
||||
target_xyz: list[float],
|
||||
target_rpy: list[float],
|
||||
target_rotation: np.ndarray,
|
||||
now: Time,
|
||||
) -> list[float]:
|
||||
if self._last_sent_target is None or self._last_sent_orientation is None:
|
||||
@@ -788,13 +848,78 @@ class SingleArmVelocityTeleop(Node):
|
||||
measured_dt = (now - self._last_command_time).nanoseconds * 1e-9
|
||||
if measured_dt > 1e-6:
|
||||
dt = measured_dt
|
||||
angular_velocity = (
|
||||
_so3_log(target_rotation @ self._last_sent_orientation.T) / dt
|
||||
)
|
||||
return [
|
||||
(target_xyz[i] - self._last_sent_target[i]) / dt
|
||||
for i in range(3)
|
||||
] + [
|
||||
_angle_delta(target_rpy[i], self._last_sent_orientation[i]) / dt
|
||||
for i in range(3)
|
||||
] + angular_velocity.tolist()
|
||||
|
||||
@staticmethod
|
||||
def _timing_summary(
|
||||
name: str,
|
||||
samples: list[float],
|
||||
deadline_ms: float | None = None,
|
||||
) -> str:
|
||||
values = np.asarray(samples)
|
||||
result = (
|
||||
f"{name}[n={len(samples)} mean={np.mean(values):.3f} "
|
||||
f"p95={np.percentile(values, 95):.3f} "
|
||||
f"p99={np.percentile(values, 99):.3f} "
|
||||
f"max={np.max(values):.3f} ms"
|
||||
)
|
||||
if deadline_ms is not None:
|
||||
result += (
|
||||
f" overruns={int(np.count_nonzero(values > deadline_ms))}"
|
||||
)
|
||||
return result + "]"
|
||||
|
||||
def _record_timing_sample(
|
||||
self,
|
||||
period_ms: float | None,
|
||||
total_ms: float,
|
||||
qp_ms: float,
|
||||
send_ms: float,
|
||||
feedback_age_ms: float,
|
||||
) -> None:
|
||||
if period_ms is not None:
|
||||
self._timing_samples["period"].append(period_ms)
|
||||
self._timing_samples["total"].append(total_ms)
|
||||
self._timing_samples["qp"].append(qp_ms)
|
||||
self._timing_samples["send"].append(send_ms)
|
||||
self._timing_samples["feedback_age"].append(feedback_age_ms)
|
||||
if len(self._timing_samples["total"]) < self._timing_stats_window:
|
||||
return
|
||||
|
||||
sample_count = len(self._timing_samples["total"])
|
||||
deadline_ms = self._dt * 1000.0
|
||||
summaries = [
|
||||
self._timing_summary(
|
||||
"period",
|
||||
self._timing_samples["period"],
|
||||
deadline_ms,
|
||||
),
|
||||
self._timing_summary(
|
||||
"total",
|
||||
self._timing_samples["total"],
|
||||
deadline_ms,
|
||||
),
|
||||
self._timing_summary("qp", self._timing_samples["qp"]),
|
||||
self._timing_summary("send", self._timing_samples["send"]),
|
||||
self._timing_summary(
|
||||
"feedback_age",
|
||||
self._timing_samples["feedback_age"],
|
||||
),
|
||||
]
|
||||
message = (
|
||||
f"{self._arm_name} timing n={sample_count} "
|
||||
f"deadline={deadline_ms:.3f} ms | "
|
||||
+ " | ".join(summaries)
|
||||
)
|
||||
for samples in self._timing_samples.values():
|
||||
samples.clear()
|
||||
self.get_logger().info(message)
|
||||
|
||||
def _fresh_joint_state(self) -> JointStateSnapshot | None:
|
||||
snapshot = self._adapter.get_latest_joint_state()
|
||||
@@ -813,7 +938,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
def _sync_joint_feedback(
|
||||
self,
|
||||
snapshot: JointStateSnapshot,
|
||||
) -> ArmPose:
|
||||
) -> np.ndarray:
|
||||
current_pose = self._ik_solver.update_joint_state(
|
||||
snapshot.positions
|
||||
)
|
||||
@@ -822,7 +947,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._last_valid_joint_target = list(snapshot.positions)
|
||||
return current_pose
|
||||
|
||||
def _solve_joint_target(self, target_pose: ArmPose) -> list[float]:
|
||||
def _solve_joint_target(self, target_pose: np.ndarray) -> list[float]:
|
||||
if self._last_valid_joint_target is None:
|
||||
raise RuntimeError("valid joint feedback has not been initialized")
|
||||
try:
|
||||
@@ -843,7 +968,7 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._active = False
|
||||
self._controller_start = None
|
||||
self._controller_orientation_start = None
|
||||
self._robot_start_pose = None
|
||||
self._robot_start_transform = None
|
||||
self._filtered_target = None
|
||||
self._filtered_orientation_target = None
|
||||
self._last_sent_target = None
|
||||
@@ -867,14 +992,19 @@ class SingleArmVelocityTeleop(Node):
|
||||
return
|
||||
self._publish_debug(pose, pose, [0.0] * 6, False)
|
||||
|
||||
def _debug_pose_fallback(self) -> ArmPose | None:
|
||||
def _debug_pose_fallback(self) -> np.ndarray | None:
|
||||
if self._last_current_pose is not None:
|
||||
return self._last_current_pose
|
||||
if self._robot_start_pose is not None:
|
||||
return self._robot_start_pose
|
||||
if self._last_sent_target is not None:
|
||||
rpy = self._last_sent_orientation or [0.0, 0.0, 0.0]
|
||||
return ArmPose(*self._last_sent_target, *rpy)
|
||||
if self._robot_start_transform is not None:
|
||||
return self._robot_start_transform
|
||||
if (
|
||||
self._last_sent_target is not None
|
||||
and self._last_sent_orientation is not None
|
||||
):
|
||||
return _make_transform(
|
||||
self._last_sent_target,
|
||||
self._last_sent_orientation,
|
||||
)
|
||||
return None
|
||||
|
||||
def _send_joint_target(self, joints: list[float]) -> bool:
|
||||
@@ -892,8 +1022,8 @@ class SingleArmVelocityTeleop(Node):
|
||||
|
||||
def _publish_debug(
|
||||
self,
|
||||
raw_target_pose: ArmPose,
|
||||
target_pose: ArmPose,
|
||||
raw_target_pose: np.ndarray,
|
||||
target_pose: np.ndarray,
|
||||
command_velocity: list[float],
|
||||
target_clamped: bool,
|
||||
) -> None:
|
||||
@@ -924,14 +1054,15 @@ class SingleArmVelocityTeleop(Node):
|
||||
self._target_clamped_pub.publish(clamped_msg)
|
||||
|
||||
@staticmethod
|
||||
def _pose_msg(stamp, pose: ArmPose) -> PoseStamped:
|
||||
qx, qy, qz, qw = _euler_to_quaternion(pose.rx, pose.ry, pose.rz)
|
||||
def _pose_msg(stamp, pose: np.ndarray) -> PoseStamped:
|
||||
transform = _make_transform(pose[:3, 3], pose[:3, :3])
|
||||
qx, qy, qz, qw = _matrix_to_quaternion(transform[:3, :3])
|
||||
msg = PoseStamped()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = "rm_base"
|
||||
msg.pose.position.x = float(pose.x)
|
||||
msg.pose.position.y = float(pose.y)
|
||||
msg.pose.position.z = float(pose.z)
|
||||
msg.pose.position.x = float(transform[0, 3])
|
||||
msg.pose.position.y = float(transform[1, 3])
|
||||
msg.pose.position.z = float(transform[2, 3])
|
||||
msg.pose.orientation.x = qx
|
||||
msg.pose.orientation.y = qy
|
||||
msg.pose.orientation.z = qz
|
||||
|
||||
Reference in New Issue
Block a user