epilectrik
/
teleop
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

[update] support g1-23 dof

main
silencht 12 months ago
parent
20633f692d
commit
1a25ee0d49
5 changed files with 1414 additions and 8 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 5
      README.md
  2. 903
      assets/g1/g1_body23.urdf
  3. 261
      teleop/robot_control/robot_arm.py
  4. 236
      teleop/robot_control/robot_arm_ik.py
  5. 17
      teleop/teleop_hand_and_arm.py

5
README.md
View File

@ -33,6 +33,11 @@ Here are the robots that will be supported,
<td style="text-align: center;"> &#9989; Completed </td>
<td style="text-align: center;"> main branch </td>
</tr>
<tr>
<td style="text-align: center;"> G1 (23DoF) </td>
<td style="text-align: center;"> &#9989; Completed </td>
<td style="text-align: center;"> main branch </td>
</tr>
<tr>
<td style="text-align: center;"> H1 (Arm 4DoF) </td>
<td style="text-align: center;"> &#9201; In Progress </td>

903
assets/g1/g1_body23.urdf
View File

@ -0,0 +1,903 @@
<robot name="g1_23dof">
<mujoco>
<compiler meshdir="meshes" discardvisual="false"/>
</mujoco>
<!-- [CAUTION] uncomment when convert to mujoco -->
<!-- <link name="world"></link>
<joint name="floating_base_joint" type="floating">
<parent link="world"/>
<child link="pelvis"/>
</joint> -->
<link name="pelvis">
<inertial>
<origin xyz="0 0 -0.07605" rpy="0 0 0"/>
<mass value="3.813"/>
<inertia ixx="0.010549" ixy="0" ixz="2.1E-06" iyy="0.0093089" iyz="0" izz="0.0079184"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/pelvis.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
</link>
<link name="pelvis_contour_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.001"/>
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/pelvis_contour_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/pelvis_contour_link.STL"/>
</geometry>
</collision>
</link>
<joint name="pelvis_contour_joint" type="fixed">
<parent link="pelvis"/>
<child link="pelvis_contour_link"/>
</joint>
<!-- Legs -->
<link name="left_hip_pitch_link">
<inertial>
<origin xyz="0.002741 0.047791 -0.02606" rpy="0 0 0"/>
<mass value="1.35"/>
<inertia ixx="0.001811" ixy="3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="0.000171" izz="0.0012812"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_pitch_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_pitch_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_hip_pitch_joint" type="revolute">
<origin xyz="0 0.064452 -0.1027" rpy="0 0 0"/>
<parent link="pelvis"/>
<child link="left_hip_pitch_link"/>
<axis xyz="0 1 0"/>
<limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
</joint>
<link name="left_hip_roll_link">
<inertial>
<origin xyz="0.029812 -0.001045 -0.087934" rpy="0 0 0"/>
<mass value="1.52"/>
<inertia ixx="0.0023773" ixy="-3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="1.84E-05" izz="0.0016595"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_roll_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_roll_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_hip_roll_joint" type="revolute">
<origin xyz="0 0.052 -0.030465" rpy="0 -0.1749 0"/>
<parent link="left_hip_pitch_link"/>
<child link="left_hip_roll_link"/>
<axis xyz="1 0 0"/>
<limit lower="-0.5236" upper="2.9671" effort="88" velocity="32"/>
</joint>
<link name="left_hip_yaw_link">
<inertial>
<origin xyz="-0.057709 -0.010981 -0.15078" rpy="0 0 0"/>
<mass value="1.702"/>
<inertia ixx="0.0057774" ixy="-0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="-0.0007072" izz="0.003149"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_yaw_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_hip_yaw_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_hip_yaw_joint" type="revolute">
<origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
<parent link="left_hip_roll_link"/>
<child link="left_hip_yaw_link"/>
<axis xyz="0 0 1"/>
<limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
</joint>
<link name="left_knee_link">
<inertial>
<origin xyz="0.005457 0.003964 -0.12074" rpy="0 0 0"/>
<mass value="1.932"/>
<inertia ixx="0.011329" ixy="4.82E-05" ixz="-4.49E-05" iyy="0.011277" iyz="-0.0007146" izz="0.0015168"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_knee_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_knee_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_knee_joint" type="revolute">
<origin xyz="-0.078273 0.0021489 -0.17734" rpy="0 0.1749 0"/>
<parent link="left_hip_yaw_link"/>
<child link="left_knee_link"/>
<axis xyz="0 1 0"/>
<limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
</joint>
<link name="left_ankle_pitch_link">
<inertial>
<origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
<mass value="0.074"/>
<inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_ankle_pitch_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_ankle_pitch_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_ankle_pitch_joint" type="revolute">
<origin xyz="0 -9.4445E-05 -0.30001" rpy="0 0 0"/>
<parent link="left_knee_link"/>
<child link="left_ankle_pitch_link"/>
<axis xyz="0 1 0"/>
<limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
</joint>
<link name="left_ankle_roll_link">
<inertial>
<origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
<mass value="0.608"/>
<inertia ixx="0.0002231" ixy="2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="-1E-07" izz="0.0016667"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_ankle_roll_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
<collision>
<origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
<collision>
<origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
<collision>
<origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</link>
<joint name="left_ankle_roll_joint" type="revolute">
<origin xyz="0 0 -0.017558" rpy="0 0 0"/>
<parent link="left_ankle_pitch_link"/>
<child link="left_ankle_roll_link"/>
<axis xyz="1 0 0"/>
<limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
</joint>
<link name="right_hip_pitch_link">
<inertial>
<origin xyz="0.002741 -0.047791 -0.02606" rpy="0 0 0"/>
<mass value="1.35"/>
<inertia ixx="0.001811" ixy="-3.68E-05" ixz="-3.44E-05" iyy="0.0014193" iyz="-0.000171" izz="0.0012812"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_hip_pitch_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_hip_pitch_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_hip_pitch_joint" type="revolute">
<origin xyz="0 -0.064452 -0.1027" rpy="0 0 0"/>
<parent link="pelvis"/>
<child link="right_hip_pitch_link"/>
<axis xyz="0 1 0"/>
<limit lower="-2.5307" upper="2.8798" effort="88" velocity="32"/>
</joint>
<link name="right_hip_roll_link">
<inertial>
<origin xyz="0.029812 0.001045 -0.087934" rpy="0 0 0"/>
<mass value="1.52"/>
<inertia ixx="0.0023773" ixy="3.8E-06" ixz="-0.0003908" iyy="0.0024123" iyz="-1.84E-05" izz="0.0016595"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_hip_roll_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_hip_roll_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_hip_roll_joint" type="revolute">
<origin xyz="0 -0.052 -0.030465" rpy="0 -0.1749 0"/>
<parent link="right_hip_pitch_link"/>
<child link="right_hip_roll_link"/>
<axis xyz="1 0 0"/>
<limit lower="-2.9671" upper="0.5236" effort="88" velocity="32"/>
</joint>
<link name="right_hip_yaw_link">
<inertial>
<origin xyz="-0.057709 0.010981 -0.15078" rpy="0 0 0"/>
<mass value="1.702"/>
<inertia ixx="0.0057774" ixy="0.0005411" ixz="-0.0023948" iyy="0.0076124" iyz="0.0007072" izz="0.003149"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_hip_yaw_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_hip_yaw_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_hip_yaw_joint" type="revolute">
<origin xyz="0.025001 0 -0.12412" rpy="0 0 0"/>
<parent link="right_hip_roll_link"/>
<child link="right_hip_yaw_link"/>
<axis xyz="0 0 1"/>
<limit lower="-2.7576" upper="2.7576" effort="88" velocity="32"/>
</joint>
<link name="right_knee_link">
<inertial>
<origin xyz="0.005457 -0.003964 -0.12074" rpy="0 0 0"/>
<mass value="1.932"/>
<inertia ixx="0.011329" ixy="-4.82E-05" ixz="4.49E-05" iyy="0.011277" iyz="0.0007146" izz="0.0015168"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_knee_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_knee_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_knee_joint" type="revolute">
<origin xyz="-0.078273 -0.0021489 -0.17734" rpy="0 0.1749 0"/>
<parent link="right_hip_yaw_link"/>
<child link="right_knee_link"/>
<axis xyz="0 1 0"/>
<limit lower="-0.087267" upper="2.8798" effort="139" velocity="20"/>
</joint>
<link name="right_ankle_pitch_link">
<inertial>
<origin xyz="-0.007269 0 0.011137" rpy="0 0 0"/>
<mass value="0.074"/>
<inertia ixx="8.4E-06" ixy="0" ixz="-2.9E-06" iyy="1.89E-05" iyz="0" izz="1.26E-05"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_ankle_pitch_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_ankle_pitch_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_ankle_pitch_joint" type="revolute">
<origin xyz="0 9.4445E-05 -0.30001" rpy="0 0 0"/>
<parent link="right_knee_link"/>
<child link="right_ankle_pitch_link"/>
<axis xyz="0 1 0"/>
<limit lower="-0.87267" upper="0.5236" effort="50" velocity="37"/>
</joint>
<link name="right_ankle_roll_link">
<inertial>
<origin xyz="0.026505 0 -0.016425" rpy="0 0 0"/>
<mass value="0.608"/>
<inertia ixx="0.0002231" ixy="-2E-07" ixz="8.91E-05" iyy="0.0016161" iyz="1E-07" izz="0.0016667"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_ankle_roll_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="-0.05 0.025 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
<collision>
<origin xyz="-0.05 -0.025 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
<collision>
<origin xyz="0.12 0.03 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
<collision>
<origin xyz="0.12 -0.03 -0.03" rpy="0 0 0"/>
<geometry>
<sphere radius="0.005"/>
</geometry>
</collision>
</link>
<joint name="right_ankle_roll_joint" type="revolute">
<origin xyz="0 0 -0.017558" rpy="0 0 0"/>
<parent link="right_ankle_pitch_link"/>
<child link="right_ankle_roll_link"/>
<axis xyz="1 0 0"/>
<limit lower="-0.2618" upper="0.2618" effort="50" velocity="37"/>
</joint>
<!-- Torso -->
<link name="waist_yaw_fixed_link">
<inertial>
<origin xyz="0.003964 0 0.018769" rpy="0 0 0"/>
<mass value="0.244"/>
<inertia ixx="9.9587E-05" ixy="-1.833E-06" ixz="-1.2617E-05" iyy="0.00012411" iyz="-1.18E-07" izz="0.00015586"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/waist_yaw_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
</link>
<joint name="waist_yaw_fixed_joint" type="fixed">
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
<parent link="torso_link"/>
<child link="waist_yaw_fixed_link"/>
</joint>
<joint name="waist_yaw_joint" type="revolute">
<origin xyz="-0.0039635 0 0.054" rpy="0 0 0"/>
<parent link="pelvis"/>
<child link="torso_link"/>
<axis xyz="0 0 1"/>
<limit lower="-2.618" upper="2.618" effort="88" velocity="32"/>
</joint>
<link name="torso_link">
<inertial>
<origin xyz="0.002601 0.000257 0.153719" rpy="0 0 0"/>
<mass value="8.562"/>
<inertia ixx="0.065674966" ixy="-8.597E-05" ixz="-0.001737252" iyy="0.053535188" iyz="8.6899E-05" izz="0.030808125"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/torso_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/torso_link.STL"/>
</geometry>
</collision>
</link>
<!-- LOGO -->
<joint name="logo_joint" type="fixed">
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
<parent link="torso_link"/>
<child link="logo_link"/>
</joint>
<link name="logo_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.001"/>
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/logo_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/logo_link.STL"/>
</geometry>
</collision>
</link>
<!-- Head -->
<link name="head_link">
<inertial>
<origin xyz="0.005267 0.000299 0.449869" rpy="0 0 0"/>
<mass value="1.036"/>
<inertia ixx="0.004085051" ixy="-2.543E-06" ixz="-6.9455E-05" iyy="0.004185212" iyz="-3.726E-06" izz="0.001807911"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/head_link.STL"/>
</geometry>
<material name="dark">
<color rgba="0.2 0.2 0.2 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/head_link.STL"/>
</geometry>
</collision>
</link>
<joint name="head_joint" type="fixed">
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
<parent link="torso_link"/>
<child link="head_link"/>
</joint>
<!-- Waist Support -->
<link name="waist_support_link">
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="0.001"/>
<inertia ixx="1e-7" ixy="0" ixz="0" iyy="1e-7" iyz="0" izz="1e-7"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/waist_support_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/waist_support_link.STL"/>
</geometry>
</collision>
</link>
<joint name="waist_support_joint" type="fixed">
<origin xyz="0.0039635 0 -0.054" rpy="0 0 0"/>
<parent link="torso_link"/>
<child link="waist_support_link"/>
</joint>
<!-- IMU -->
<link name="imu_in_torso"></link>
<joint name="imu_in_torso_joint" type="fixed">
<origin xyz="-0.03959 -0.00224 0.13792" rpy="0 0 0"/>
<parent link="torso_link"/>
<child link="imu_in_torso"/>
</joint>
<link name="imu_in_pelvis"></link>
<joint name="imu_in_pelvis_joint" type="fixed">
<origin xyz="0.04525 0 -0.08339" rpy="0 0 0"/>
<parent link="pelvis"/>
<child link="imu_in_pelvis"/>
</joint>
<!-- d435 -->
<link name="d435_link"></link>
<joint name="d435_joint" type="fixed">
<origin xyz="0.0576235 0.01753 0.41987" rpy="0 0.8307767239493009 0"/>
<parent link="torso_link"/>
<child link="d435_link"/>
</joint>
<!-- mid360 -->
<link name="mid360_link"></link>
<joint name="mid360_joint" type="fixed">
<origin xyz="0.0002835 0.00003 0.40618" rpy="0 0.04014257279586953 0"/>
<parent link="torso_link"/>
<child link="mid360_link"/>
</joint>
<!-- Arm -->
<link name="left_shoulder_pitch_link">
<inertial>
<origin xyz="0 0.035892 -0.011628" rpy="0 0 0"/>
<mass value="0.718"/>
<inertia ixx="0.0004291" ixy="-9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="2.26E-05" izz="0.000423"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_shoulder_pitch_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0.04 -0.01" rpy="0 1.5707963267948966 0"/>
<geometry>
<cylinder radius="0.03" length="0.05"/>
</geometry>
</collision>
</link>
<joint name="left_shoulder_pitch_joint" type="revolute">
<origin xyz="0.0039563 0.10022 0.23778" rpy="0.27931 5.4949E-05 -0.00019159"/>
<parent link="torso_link"/>
<child link="left_shoulder_pitch_link"/>
<axis xyz="0 1 0"/>
<limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
</joint>
<link name="left_shoulder_roll_link">
<inertial>
<origin xyz="-0.000227 0.00727 -0.063243" rpy="0 0 0"/>
<mass value="0.643"/>
<inertia ixx="0.0006177" ixy="-1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="-5.3E-06" izz="0.0003894"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_shoulder_roll_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="-0.004 0.006 -0.053" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
</collision>
</link>
<joint name="left_shoulder_roll_joint" type="revolute">
<origin xyz="0 0.038 -0.013831" rpy="-0.27925 0 0"/>
<parent link="left_shoulder_pitch_link"/>
<child link="left_shoulder_roll_link"/>
<axis xyz="1 0 0"/>
<limit lower="-1.5882" upper="2.2515" effort="25" velocity="37"/>
</joint>
<link name="left_shoulder_yaw_link">
<inertial>
<origin xyz="0.010773 -0.002949 -0.072009" rpy="0 0 0"/>
<mass value="0.734"/>
<inertia ixx="0.0009988" ixy="7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="-2.86E-05" izz="0.0004354"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_shoulder_yaw_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_shoulder_yaw_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_shoulder_yaw_joint" type="revolute">
<origin xyz="0 0.00624 -0.1032" rpy="0 0 0"/>
<parent link="left_shoulder_roll_link"/>
<child link="left_shoulder_yaw_link"/>
<axis xyz="0 0 1"/>
<limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
</joint>
<link name="left_elbow_link">
<inertial>
<origin xyz="0.064956 0.004454 -0.010062" rpy="0 0 0"/>
<mass value="0.6"/>
<inertia ixx="0.0002891" ixy="6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="-5.6E-06" izz="0.0004197"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_elbow_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_elbow_link.STL"/>
</geometry>
</collision>
</link>
<joint name="left_elbow_joint" type="revolute">
<origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
<parent link="left_shoulder_yaw_link"/>
<child link="left_elbow_link"/>
<axis xyz="0 1 0"/>
<limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
</joint>
<joint name="left_wrist_roll_joint" type="revolute">
<origin xyz="0.100 0.00188791 -0.010" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="left_elbow_link"/>
<child link="left_wrist_roll_rubber_hand"/>
<limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
</joint>
<link name="left_wrist_roll_rubber_hand">
<inertial>
<origin xyz="0.10794656650 0.00163511945 0.00202244863" rpy="0 0 0"/>
<mass value="0.35692864"/>
<inertia ixx="0.00019613494735" ixy="-0.00000419816908" ixz="-0.00003950860580" iyy="0.00200280358206" iyz="0.00000249774203" izz="0.00194181412808"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_wrist_roll_rubber_hand.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/left_wrist_roll_rubber_hand.STL"/>
</geometry>
</collision>
</link>
<link name="right_shoulder_pitch_link">
<inertial>
<origin xyz="0 -0.035892 -0.011628" rpy="0 0 0"/>
<mass value="0.718"/>
<inertia ixx="0.0004291" ixy="9.2E-06" ixz="6.4E-06" iyy="0.000453" iyz="-2.26E-05" izz="0.000423"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_shoulder_pitch_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 -0.04 -0.01" rpy="0 1.5707963267948966 0"/>
<geometry>
<cylinder radius="0.03" length="0.05"/>
</geometry>
</collision>
</link>
<joint name="right_shoulder_pitch_joint" type="revolute">
<origin xyz="0.0039563 -0.10021 0.23778" rpy="-0.27931 5.4949E-05 0.00019159"/>
<parent link="torso_link"/>
<child link="right_shoulder_pitch_link"/>
<axis xyz="0 1 0"/>
<limit lower="-3.0892" upper="2.6704" effort="25" velocity="37"/>
</joint>
<link name="right_shoulder_roll_link">
<inertial>
<origin xyz="-0.000227 -0.00727 -0.063243" rpy="0 0 0"/>
<mass value="0.643"/>
<inertia ixx="0.0006177" ixy="1E-06" ixz="8.7E-06" iyy="0.0006912" iyz="5.3E-06" izz="0.0003894"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_shoulder_roll_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="-0.004 -0.006 -0.053" rpy="0 0 0"/>
<geometry>
<cylinder radius="0.03" length="0.03"/>
</geometry>
</collision>
</link>
<joint name="right_shoulder_roll_joint" type="revolute">
<origin xyz="0 -0.038 -0.013831" rpy="0.27925 0 0"/>
<parent link="right_shoulder_pitch_link"/>
<child link="right_shoulder_roll_link"/>
<axis xyz="1 0 0"/>
<limit lower="-2.2515" upper="1.5882" effort="25" velocity="37"/>
</joint>
<link name="right_shoulder_yaw_link">
<inertial>
<origin xyz="0.010773 0.002949 -0.072009" rpy="0 0 0"/>
<mass value="0.734"/>
<inertia ixx="0.0009988" ixy="-7.9E-06" ixz="0.0001412" iyy="0.0010605" iyz="2.86E-05" izz="0.0004354"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_shoulder_yaw_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_shoulder_yaw_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_shoulder_yaw_joint" type="revolute">
<origin xyz="0 -0.00624 -0.1032" rpy="0 0 0"/>
<parent link="right_shoulder_roll_link"/>
<child link="right_shoulder_yaw_link"/>
<axis xyz="0 0 1"/>
<limit lower="-2.618" upper="2.618" effort="25" velocity="37"/>
</joint>
<link name="right_elbow_link">
<inertial>
<origin xyz="0.064956 -0.004454 -0.010062" rpy="0 0 0"/>
<mass value="0.6"/>
<inertia ixx="0.0002891" ixy="-6.53E-05" ixz="1.72E-05" iyy="0.0004152" iyz="5.6E-06" izz="0.0004197"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_elbow_link.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_elbow_link.STL"/>
</geometry>
</collision>
</link>
<joint name="right_elbow_joint" type="revolute">
<origin xyz="0.015783 0 -0.080518" rpy="0 0 0"/>
<parent link="right_shoulder_yaw_link"/>
<child link="right_elbow_link"/>
<axis xyz="0 1 0"/>
<limit lower="-1.0472" upper="2.0944" effort="25" velocity="37"/>
</joint>
<joint name="right_wrist_roll_joint" type="revolute">
<origin xyz="0.100 -0.00188791 -0.010" rpy="0 0 0"/>
<axis xyz="1 0 0"/>
<parent link="right_elbow_link"/>
<child link="right_wrist_roll_rubber_hand"/>
<limit effort="25" velocity="37" lower="-1.972222054" upper="1.972222054"/>
</joint>
<link name="right_wrist_roll_rubber_hand">
<inertial>
<origin xyz="0.10794656650 -0.00163511945 0.00202244863" rpy="0 0 0"/>
<mass value="0.35692864"/>
<inertia ixx="0.00019613494735" ixy="0.00000419816908" ixz="-0.00003950860580" iyy="0.00200280358206" iyz="-0.00000249774203" izz="0.00194181412808"/>
</inertial>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_wrist_roll_rubber_hand.STL"/>
</geometry>
<material name="white">
<color rgba="0.7 0.7 0.7 1"/>
</material>
</visual>
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="meshes/right_wrist_roll_rubber_hand.STL"/>
</geometry>
</collision>
</link>
</robot>

261
teleop/robot_control/robot_arm.py
View File

@ -11,6 +11,7 @@ from unitree_sdk2py.utils.crc import CRC
kTopicLowCommand = "rt/lowcmd"
kTopicLowState = "rt/lowstate"
G1_29_Num_Motors = 35
G1_23_Num_Motors = 35
H1_2_Num_Motors = 35
@ -23,6 +24,10 @@ class G1_29_LowState:
def __init__(self):
self.motor_state = [MotorState() for _ in range(G1_29_Num_Motors)]
class G1_23_LowState:
def __init__(self):
self.motor_state = [MotorState() for _ in range(G1_23_Num_Motors)]
class H1_2_LowState:
def __init__(self):
self.motor_state = [MotorState() for _ in range(H1_2_Num_Motors)]
@ -303,6 +308,262 @@ class G1_29_JointIndex(IntEnum):
kNotUsedJoint4 = 33
kNotUsedJoint5 = 34
class G1_23_ArmController:
def __init__(self):
print("Initialize G1_23_ArmController...")
self.q_target = np.zeros(10)
self.tauff_target = np.zeros(10)
self.kp_high = 300.0
self.kd_high = 3.0
self.kp_low = 80.0
self.kd_low = 3.0
self.kp_wrist = 40.0
self.kd_wrist = 1.5
self.all_motor_q = None
self.arm_velocity_limit = 20.0
self.control_dt = 1.0 / 250.0
self._speed_gradual_max = False
self._gradual_start_time = None
self._gradual_time = None
# initialize lowcmd publisher and lowstate subscriber
ChannelFactoryInitialize(0)
self.lowcmd_publisher = ChannelPublisher(kTopicLowCommand, LowCmd_)
self.lowcmd_publisher.Init()
self.lowstate_subscriber = ChannelSubscriber(kTopicLowState, LowState_)
self.lowstate_subscriber.Init()
self.lowstate_buffer = DataBuffer()
# initialize subscribe thread
self.subscribe_thread = threading.Thread(target=self._subscribe_motor_state)
self.subscribe_thread.daemon = True
self.subscribe_thread.start()
while not self.lowstate_buffer.GetData():
time.sleep(0.01)
print("[G1_23_ArmController] Waiting to subscribe dds...")
# initialize hg's lowcmd msg
self.crc = CRC()
self.msg = unitree_hg_msg_dds__LowCmd_()
self.msg.mode_pr = 0
self.msg.mode_machine = self.get_mode_machine()
self.all_motor_q = self.get_current_motor_q()
print(f"Current all body motor state q:\n{self.all_motor_q} \n")
print(f"Current two arms motor state q:\n{self.get_current_dual_arm_q()}\n")
print("Lock all joints except two arms...\n")
arm_indices = set(member.value for member in G1_23_JointArmIndex)
for id in G1_23_JointIndex:
self.msg.motor_cmd[id].mode = 1
if id.value in arm_indices:
if self._Is_wrist_motor(id):
self.msg.motor_cmd[id].kp = self.kp_wrist
self.msg.motor_cmd[id].kd = self.kd_wrist
else:
self.msg.motor_cmd[id].kp = self.kp_low
self.msg.motor_cmd[id].kd = self.kd_low
else:
if self._Is_weak_motor(id):
self.msg.motor_cmd[id].kp = self.kp_low
self.msg.motor_cmd[id].kd = self.kd_low
else:
self.msg.motor_cmd[id].kp = self.kp_high
self.msg.motor_cmd[id].kd = self.kd_high
self.msg.motor_cmd[id].q = self.all_motor_q[id]
print("Lock OK!\n")
# initialize publish thread
self.publish_thread = threading.Thread(target=self._ctrl_motor_state)
self.ctrl_lock = threading.Lock()
self.publish_thread.daemon = True
self.publish_thread.start()
print("Initialize G1_23_ArmController OK!\n")
def _subscribe_motor_state(self):
while True:
msg = self.lowstate_subscriber.Read()
if msg is not None:
lowstate = G1_23_LowState()
for id in range(G1_23_Num_Motors):
lowstate.motor_state[id].q = msg.motor_state[id].q
lowstate.motor_state[id].dq = msg.motor_state[id].dq
self.lowstate_buffer.SetData(lowstate)
time.sleep(0.002)
def clip_arm_q_target(self, target_q, velocity_limit):
current_q = self.get_current_dual_arm_q()
delta = target_q - current_q
motion_scale = np.max(np.abs(delta)) / (velocity_limit * self.control_dt)
cliped_arm_q_target = current_q + delta / max(motion_scale, 1.0)
return cliped_arm_q_target
def _ctrl_motor_state(self):
while True:
start_time = time.time()
with self.ctrl_lock:
arm_q_target = self.q_target
arm_tauff_target = self.tauff_target
cliped_arm_q_target = self.clip_arm_q_target(arm_q_target, velocity_limit = self.arm_velocity_limit)
for idx, id in enumerate(G1_23_JointArmIndex):
self.msg.motor_cmd[id].q = cliped_arm_q_target[idx]
self.msg.motor_cmd[id].dq = 0
self.msg.motor_cmd[id].tau = arm_tauff_target[idx]
self.msg.crc = self.crc.Crc(self.msg)
self.lowcmd_publisher.Write(self.msg)
if self._speed_gradual_max is True:
t_elapsed = start_time - self._gradual_start_time
self.arm_velocity_limit = 20.0 + (10.0 * min(1.0, t_elapsed / 5.0))
current_time = time.time()
all_t_elapsed = current_time - start_time
sleep_time = max(0, (self.control_dt - all_t_elapsed))
time.sleep(sleep_time)
# print(f"arm_velocity_limit:{self.arm_velocity_limit}")
# print(f"sleep_time:{sleep_time}")
def ctrl_dual_arm(self, q_target, tauff_target):
'''Set control target values q & tau of the left and right arm motors.'''
with self.ctrl_lock:
self.q_target = q_target
self.tauff_target = tauff_target
def get_mode_machine(self):
'''Return current dds mode machine.'''
return self.lowstate_subscriber.Read().mode_machine
def get_current_motor_q(self):
'''Return current state q of all body motors.'''
return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_23_JointIndex])
def get_current_dual_arm_q(self):
'''Return current state q of the left and right arm motors.'''
return np.array([self.lowstate_buffer.GetData().motor_state[id].q for id in G1_23_JointArmIndex])
def get_current_dual_arm_dq(self):
'''Return current state dq of the left and right arm motors.'''
return np.array([self.lowstate_buffer.GetData().motor_state[id].dq for id in G1_23_JointArmIndex])
def ctrl_dual_arm_go_home(self):
'''Move both the left and right arms of the robot to their home position by setting the target joint angles (q) and torques (tau) to zero.'''
print("[G1_23_ArmController] ctrl_dual_arm_go_home start...")
with self.ctrl_lock:
self.q_target = np.zeros(10)
# self.tauff_target = np.zeros(10)
tolerance = 0.05 # Tolerance threshold for joint angles to determine "close to zero", can be adjusted based on your motor's precision requirements
while True:
current_q = self.get_current_dual_arm_q()
if np.all(np.abs(current_q) < tolerance):
print("[G1_23_ArmController] both arms have reached the home position.")
break
time.sleep(0.05)
def speed_gradual_max(self, t = 5.0):
'''Parameter t is the total time required for arms velocity to gradually increase to its maximum value, in seconds. The default is 5.0.'''
self._gradual_start_time = time.time()
self._gradual_time = t
self._speed_gradual_max = True
def speed_instant_max(self):
'''set arms velocity to the maximum value immediately, instead of gradually increasing.'''
self.arm_velocity_limit = 30.0
def _Is_weak_motor(self, motor_index):
weak_motors = [
G1_23_JointIndex.kLeftAnklePitch.value,
G1_23_JointIndex.kRightAnklePitch.value,
# Left arm
G1_23_JointIndex.kLeftShoulderPitch.value,
G1_23_JointIndex.kLeftShoulderRoll.value,
G1_23_JointIndex.kLeftShoulderYaw.value,
G1_23_JointIndex.kLeftElbow.value,
# Right arm
G1_23_JointIndex.kRightShoulderPitch.value,
G1_23_JointIndex.kRightShoulderRoll.value,
G1_23_JointIndex.kRightShoulderYaw.value,
G1_23_JointIndex.kRightElbow.value,
]
return motor_index.value in weak_motors
def _Is_wrist_motor(self, motor_index):
wrist_motors = [
G1_23_JointIndex.kLeftWristRoll.value,
G1_23_JointIndex.kRightWristRoll.value,
]
return motor_index.value in wrist_motors
class G1_23_JointArmIndex(IntEnum):
# Left arm
kLeftShoulderPitch = 15
kLeftShoulderRoll = 16
kLeftShoulderYaw = 17
kLeftElbow = 18
kLeftWristRoll = 19
# Right arm
kRightShoulderPitch = 22
kRightShoulderRoll = 23
kRightShoulderYaw = 24
kRightElbow = 25
kRightWristRoll = 26
class G1_23_JointIndex(IntEnum):
# Left leg
kLeftHipPitch = 0
kLeftHipRoll = 1
kLeftHipYaw = 2
kLeftKnee = 3
kLeftAnklePitch = 4
kLeftAnkleRoll = 5
# Right leg
kRightHipPitch = 6
kRightHipRoll = 7
kRightHipYaw = 8
kRightKnee = 9
kRightAnklePitch = 10
kRightAnkleRoll = 11
kWaistYaw = 12
kWaistRollNotUsed = 13
kWaistPitchNotUsed = 14
# Left arm
kLeftShoulderPitch = 15
kLeftShoulderRoll = 16
kLeftShoulderYaw = 17
kLeftElbow = 18
kLeftWristRoll = 19
kLeftWristPitchNotUsed = 20
kLeftWristyawNotUsed = 21
# Right arm
kRightShoulderPitch = 22
kRightShoulderRoll = 23
kRightShoulderYaw = 24
kRightElbow = 25
kRightWristRoll = 26
kRightWristPitchNotUsed = 27
kRightWristYawNotUsed = 28
# not used
kNotUsedJoint0 = 29
kNotUsedJoint1 = 30
kNotUsedJoint2 = 31
kNotUsedJoint3 = 32
kNotUsedJoint4 = 33
kNotUsedJoint5 = 34
class H1_2_ArmController:
def __init__(self):
print("Initialize H1_2_ArmController...")

236
teleop/robot_control/robot_arm_ik.py
View File

@ -160,7 +160,7 @@ class G1_29_ArmIK:
self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model)
self.vis.initViewer(open=True)
self.vis.loadViewerModel("pinocchio")
self.vis.displayFrames(True, frame_ids=[101, 102], axis_length = 0.15, axis_width = 5)
self.vis.displayFrames(True, frame_ids=[107, 108], axis_length = 0.15, axis_width = 5)
self.vis.display(pin.neutral(self.reduced_robot.model))
# Enable the display of end effector target frames with short axis lengths and greater width.
@ -176,7 +176,233 @@ class G1_29_ArmIK:
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
)
axis_length = 0.1
axis_width = 10
axis_width = 20
for frame_viz_name in frame_viz_names:
self.vis.viewer[frame_viz_name].set_object(
mg.LineSegments(
mg.PointsGeometry(
position=axis_length * FRAME_AXIS_POSITIONS,
color=FRAME_AXIS_COLORS,
),
mg.LineBasicMaterial(
linewidth=axis_width,
vertexColors=True,
),
)
)
# If the robot arm is not the same size as your arm :)
def scale_arms(self, human_left_pose, human_right_pose, human_arm_length=0.60, robot_arm_length=0.75):
scale_factor = robot_arm_length / human_arm_length
robot_left_pose = human_left_pose.copy()
robot_right_pose = human_right_pose.copy()
robot_left_pose[:3, 3] *= scale_factor
robot_right_pose[:3, 3] *= scale_factor
return robot_left_pose, robot_right_pose
def solve_ik(self, left_wrist, right_wrist, current_lr_arm_motor_q = None, current_lr_arm_motor_dq = None):
if current_lr_arm_motor_q is not None:
self.init_data = current_lr_arm_motor_q
self.opti.set_initial(self.var_q, self.init_data)
# left_wrist, right_wrist = self.scale_arms(left_wrist, right_wrist)
if self.Visualization:
self.vis.viewer['L_ee_target'].set_transform(left_wrist) # for visualization
self.vis.viewer['R_ee_target'].set_transform(right_wrist) # for visualization
self.opti.set_value(self.param_tf_l, left_wrist)
self.opti.set_value(self.param_tf_r, right_wrist)
self.opti.set_value(self.var_q_last, self.init_data) # for smooth
try:
sol = self.opti.solve()
# sol = self.opti.solve_limited()
sol_q = self.opti.value(self.var_q)
self.smooth_filter.add_data(sol_q)
sol_q = self.smooth_filter.filtered_data
if current_lr_arm_motor_dq is not None:
v = current_lr_arm_motor_dq * 0.0
else:
v = (sol_q - self.init_data) * 0.0
self.init_data = sol_q
sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv))
if self.Visualization:
self.vis.display(sol_q) # for visualization
return sol_q, sol_tauff
except Exception as e:
print(f"ERROR in convergence, plotting debug info.{e}")
sol_q = self.opti.debug.value(self.var_q)
self.smooth_filter.add_data(sol_q)
sol_q = self.smooth_filter.filtered_data
if current_lr_arm_motor_dq is not None:
v = current_lr_arm_motor_dq * 0.0
else:
v = (sol_q - self.init_data) * 0.0
self.init_data = sol_q
sol_tauff = pin.rnea(self.reduced_robot.model, self.reduced_robot.data, sol_q, v, np.zeros(self.reduced_robot.model.nv))
print(f"sol_q:{sol_q} \nmotorstate: \n{current_lr_arm_motor_q} \nleft_pose: \n{left_wrist} \nright_pose: \n{right_wrist}")
if self.Visualization:
self.vis.display(sol_q) # for visualization
# return sol_q, sol_tauff
return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv)
class G1_23_ArmIK:
def __init__(self, Unit_Test = False, Visualization = False):
np.set_printoptions(precision=5, suppress=True, linewidth=200)
self.Unit_Test = Unit_Test
self.Visualization = Visualization
if not self.Unit_Test:
self.robot = pin.RobotWrapper.BuildFromURDF('../assets/g1/g1_body23.urdf', '../assets/g1/')
else:
self.robot = pin.RobotWrapper.BuildFromURDF('../../assets/g1/g1_body23.urdf', '../../assets/g1/') # for test
self.mixed_jointsToLockIDs = [
"left_hip_pitch_joint" ,
"left_hip_roll_joint" ,
"left_hip_yaw_joint" ,
"left_knee_joint" ,
"left_ankle_pitch_joint" ,
"left_ankle_roll_joint" ,
"right_hip_pitch_joint" ,
"right_hip_roll_joint" ,
"right_hip_yaw_joint" ,
"right_knee_joint" ,
"right_ankle_pitch_joint" ,
"right_ankle_roll_joint" ,
"waist_yaw_joint" ,
]
self.reduced_robot = self.robot.buildReducedRobot(
list_of_joints_to_lock=self.mixed_jointsToLockIDs,
reference_configuration=np.array([0.0] * self.robot.model.nq),
)
self.reduced_robot.model.addFrame(
pin.Frame('L_ee',
self.reduced_robot.model.getJointId('left_wrist_roll_joint'),
pin.SE3(np.eye(3),
np.array([0.20,0,0]).T),
pin.FrameType.OP_FRAME)
)
self.reduced_robot.model.addFrame(
pin.Frame('R_ee',
self.reduced_robot.model.getJointId('right_wrist_roll_joint'),
pin.SE3(np.eye(3),
np.array([0.20,0,0]).T),
pin.FrameType.OP_FRAME)
)
# for i in range(self.reduced_robot.model.nframes):
# frame = self.reduced_robot.model.frames[i]
# frame_id = self.reduced_robot.model.getFrameId(frame.name)
# print(f"Frame ID: {frame_id}, Name: {frame.name}")
# Creating Casadi models and data for symbolic computing
self.cmodel = cpin.Model(self.reduced_robot.model)
self.cdata = self.cmodel.createData()
# Creating symbolic variables
self.cq = casadi.SX.sym("q", self.reduced_robot.model.nq, 1)
self.cTf_l = casadi.SX.sym("tf_l", 4, 4)
self.cTf_r = casadi.SX.sym("tf_r", 4, 4)
cpin.framesForwardKinematics(self.cmodel, self.cdata, self.cq)
# Get the hand joint ID and define the error function
self.L_hand_id = self.reduced_robot.model.getFrameId("L_ee")
self.R_hand_id = self.reduced_robot.model.getFrameId("R_ee")
self.translational_error = casadi.Function(
"translational_error",
[self.cq, self.cTf_l, self.cTf_r],
[
casadi.vertcat(
self.cdata.oMf[self.L_hand_id].translation - self.cTf_l[:3,3],
self.cdata.oMf[self.R_hand_id].translation - self.cTf_r[:3,3]
)
],
)
self.rotational_error = casadi.Function(
"rotational_error",
[self.cq, self.cTf_l, self.cTf_r],
[
casadi.vertcat(
cpin.log3(self.cdata.oMf[self.L_hand_id].rotation @ self.cTf_l[:3,:3].T),
cpin.log3(self.cdata.oMf[self.R_hand_id].rotation @ self.cTf_r[:3,:3].T)
)
],
)
# Defining the optimization problem
self.opti = casadi.Opti()
self.var_q = self.opti.variable(self.reduced_robot.model.nq)
self.var_q_last = self.opti.parameter(self.reduced_robot.model.nq) # for smooth
self.param_tf_l = self.opti.parameter(4, 4)
self.param_tf_r = self.opti.parameter(4, 4)
self.translational_cost = casadi.sumsqr(self.translational_error(self.var_q, self.param_tf_l, self.param_tf_r))
self.rotation_cost = casadi.sumsqr(self.rotational_error(self.var_q, self.param_tf_l, self.param_tf_r))
self.regularization_cost = casadi.sumsqr(self.var_q)
self.smooth_cost = casadi.sumsqr(self.var_q - self.var_q_last)
# Setting optimization constraints and goals
self.opti.subject_to(self.opti.bounded(
self.reduced_robot.model.lowerPositionLimit,
self.var_q,
self.reduced_robot.model.upperPositionLimit)
)
self.opti.minimize(50 * self.translational_cost + 0.5 * self.rotation_cost + 0.02 * self.regularization_cost + 0.1 * self.smooth_cost)
opts = {
'ipopt':{
'print_level':0,
'max_iter':50,
'tol':1e-6
},
'print_time':False,# print or not
'calc_lam_p':False # https://github.com/casadi/casadi/wiki/FAQ:-Why-am-I-getting-%22NaN-detected%22in-my-optimization%3F
}
self.opti.solver("ipopt", opts)
self.init_data = np.zeros(self.reduced_robot.model.nq)
self.smooth_filter = WeightedMovingFilter(np.array([0.4, 0.3, 0.2, 0.1]), 10)
self.vis = None
if self.Visualization:
# Initialize the Meshcat visualizer for visualization
self.vis = MeshcatVisualizer(self.reduced_robot.model, self.reduced_robot.collision_model, self.reduced_robot.visual_model)
self.vis.initViewer(open=True)
self.vis.loadViewerModel("pinocchio")
self.vis.displayFrames(True, frame_ids=[67, 68], axis_length = 0.15, axis_width = 5)
self.vis.display(pin.neutral(self.reduced_robot.model))
# Enable the display of end effector target frames with short axis lengths and greater width.
frame_viz_names = ['L_ee_target', 'R_ee_target']
FRAME_AXIS_POSITIONS = (
np.array([[0, 0, 0], [1, 0, 0],
[0, 0, 0], [0, 1, 0],
[0, 0, 0], [0, 0, 1]]).astype(np.float32).T
)
FRAME_AXIS_COLORS = (
np.array([[1, 0, 0], [1, 0.6, 0],
[0, 1, 0], [0.6, 1, 0],
[0, 0, 1], [0, 0.6, 1]]).astype(np.float32).T
)
axis_length = 0.1
axis_width = 20
for frame_viz_name in frame_viz_names:
self.vis.viewer[frame_viz_name].set_object(
mg.LineSegments(
@ -258,6 +484,7 @@ class G1_29_ArmIK:
# return sol_q, sol_tauff
return current_lr_arm_motor_q, np.zeros(self.reduced_robot.model.nv)
class H1_2_ArmIK:
def __init__(self, Unit_Test = False, Visualization = False):
np.set_printoptions(precision=5, suppress=True, linewidth=200)
@ -511,7 +738,8 @@ class H1_2_ArmIK:
if __name__ == "__main__":
# arm_ik = G1_29_ArmIK(Unit_Test = True, Visualization = True)
arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = True)
# arm_ik = H1_2_ArmIK(Unit_Test = True, Visualization = True)
arm_ik = G1_23_ArmIK(Unit_Test = True, Visualization = True)
# initial positon
L_tf_target = pin.SE3(
@ -526,7 +754,7 @@ if __name__ == "__main__":
rotation_speed = 0.005
noise_amplitude_translation = 0.001
noise_amplitude_rotation = 0.1
noise_amplitude_rotation = 0.05
user_input = input("Please enter the start signal (enter 's' to start the subsequent program):\n")
if user_input.lower() == 's':

17
teleop/teleop_hand_and_arm.py
View File

@ -12,8 +12,8 @@ parent_dir = os.path.dirname(current_dir)
sys.path.append(parent_dir)
from teleop.open_television.tv_wrapper import TeleVisionWrapper
from teleop.robot_control.robot_arm import G1_29_ArmController, H1_2_ArmController
from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, H1_2_ArmIK
from teleop.robot_control.robot_arm import G1_29_ArmController, G1_23_ArmController, H1_2_ArmController
from teleop.robot_control.robot_arm_ik import G1_29_ArmIK, G1_23_ArmIK, H1_2_ArmIK
from teleop.robot_control.robot_hand_unitree import Dex3_1_Controller, Gripper_Controller
from teleop.image_server.image_client import ImageClient
from teleop.utils.episode_writer import EpisodeWriter
@ -28,7 +28,7 @@ if __name__ == '__main__':
parser.add_argument('--no-record', dest = 'record', action = 'store_false', help = 'Do not save data')
parser.set_defaults(record = False)
parser.add_argument('--arm', type=str, choices=['G1_29', 'H1_2'], default='G1_29', help='Select arm controller')
parser.add_argument('--arm', type=str, choices=['G1_29', 'G1_23', 'H1_2'], default='G1_29', help='Select arm controller')
parser.add_argument('--hand', type=str, choices=['dex3', 'gripper', 'inspire1'], help='Select hand controller')
@ -83,6 +83,9 @@ if __name__ == '__main__':
if args.arm == 'G1_29':
arm_ctrl = G1_29_ArmController()
arm_ik = G1_29_ArmIK()
elif args.arm == 'G1_23':
arm_ctrl = G1_23_ArmController()
arm_ik = G1_23_ArmIK()
elif args.arm == 'H1_2':
arm_ctrl = H1_2_ArmController()
arm_ik = H1_2_ArmIK()
@ -160,12 +163,18 @@ if __name__ == '__main__':
right_hand_state = dual_hand_state_array[-7:]
left_hand_action = dual_hand_action_array[:7]
right_hand_action = dual_hand_action_array[-7:]
else:
elif args.hand == "gripper":
with dual_gripper_data_lock:
left_hand_state = [dual_gripper_state_array[1]]
right_hand_state = [dual_gripper_state_array[0]]
left_hand_action = [dual_gripper_action_array[1]]
right_hand_action = [dual_gripper_action_array[0]]
elif args.hand == "inspire1":
print("Inspire1_Controller comming soon.")
pass
else:
print("No dexterous hand set.")
pass
# head image
current_tv_image = tv_img_array.copy()
# wrist image

Write Preview
Loading…
Cancel
Save