silencht
8 months ago
45 changed files with 1657 additions and 44 deletions
-
63README.md
-
55README_zh-CN.md
-
61assets/brainco_hand/brainco.yml
-
618assets/brainco_hand/brainco_left.urdf
-
618assets/brainco_hand/brainco_right.urdf
-
BINassets/brainco_hand/meshes/left_base_link.STL
-
BINassets/brainco_hand/meshes/left_index_distal_Link.STL
-
BINassets/brainco_hand/meshes/left_index_proximal_Link.STL
-
BINassets/brainco_hand/meshes/left_index_tip_Link.STL
-
BINassets/brainco_hand/meshes/left_middle_distal_Link.STL
-
BINassets/brainco_hand/meshes/left_middle_proximal_Link.STL
-
BINassets/brainco_hand/meshes/left_middle_tip_Link.STL
-
BINassets/brainco_hand/meshes/left_pinky_distal_Link.STL
-
BINassets/brainco_hand/meshes/left_pinky_proximal_Link.STL
-
BINassets/brainco_hand/meshes/left_pinky_tip_Link.STL
-
BINassets/brainco_hand/meshes/left_ring_distal_Link.STL
-
BINassets/brainco_hand/meshes/left_ring_proximal_Link.STL
-
BINassets/brainco_hand/meshes/left_ring_tip_Link.STL
-
BINassets/brainco_hand/meshes/left_thumb_distal_Link.STL
-
BINassets/brainco_hand/meshes/left_thumb_metacarpal_Link.STL
-
BINassets/brainco_hand/meshes/left_thumb_proximal_Link.STL
-
BINassets/brainco_hand/meshes/left_thumb_tip_Link.STL
-
BINassets/brainco_hand/meshes/right_base_link.STL
-
BINassets/brainco_hand/meshes/right_index_distal_link.STL
-
BINassets/brainco_hand/meshes/right_index_proximal_link.STL
-
BINassets/brainco_hand/meshes/right_index_tip.STL
-
BINassets/brainco_hand/meshes/right_middle_distal_link.STL
-
BINassets/brainco_hand/meshes/right_middle_proximal_link.STL
-
BINassets/brainco_hand/meshes/right_middle_tip.STL
-
BINassets/brainco_hand/meshes/right_pinky_distal_link.STL
-
BINassets/brainco_hand/meshes/right_pinky_proximal_link.STL
-
BINassets/brainco_hand/meshes/right_pinky_tip.STL
-
BINassets/brainco_hand/meshes/right_ring_distal_link.STL
-
BINassets/brainco_hand/meshes/right_ring_proximal_link.STL
-
BINassets/brainco_hand/meshes/right_ring_tip.STL
-
BINassets/brainco_hand/meshes/right_thumb_distal_link.STL
-
BINassets/brainco_hand/meshes/right_thumb_metacarpal_link.STL
-
BINassets/brainco_hand/meshes/right_thumb_proximal_link.STL
-
BINassets/brainco_hand/meshes/right_thumb_tip.STL
-
25teleop/robot_control/hand_retargeting.py
-
29teleop/robot_control/robot_arm.py
-
194teleop/robot_control/robot_hand_brainco.py
-
8teleop/robot_control/robot_hand_inspire.py
-
11teleop/robot_control/robot_hand_unitree.py
-
19teleop/teleop_hand_and_arm.py
@ -0,0 +1,61 @@ |
|||
left: |
|||
type: DexPilot # or vector |
|||
urdf_path: brainco_hand/brainco_left.urdf |
|||
|
|||
# Target refers to the retargeting target, which is the robot hand |
|||
target_joint_names: |
|||
[ |
|||
"left_thumb_metacarpal_joint", |
|||
"left_thumb_proximal_joint", |
|||
"left_index_proximal_joint", |
|||
"left_middle_proximal_joint", |
|||
"left_ring_proximal_joint", |
|||
"left_pinky_proximal_joint", |
|||
] |
|||
|
|||
# for DexPilot type |
|||
wrist_link_name: "base_link" |
|||
finger_tip_link_names: [ "left_thumb_tip", "left_index_tip", "left_middle_tip", "left_ring_tip", "left_pinky_tip" ] |
|||
# If you do not know exactly how it is used, please leave it to None for default. |
|||
target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] |
|||
|
|||
# for vector type |
|||
target_origin_link_names: ["base_link", "base_link", "base_link", "base_link", "base_link"] |
|||
target_task_link_names: [ "left_thumb_tip", "left_index_tip", "left_middle_tip", "left_ring_tip", "left_pinky_tip" ] |
|||
target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] |
|||
|
|||
# Scaling factor for vector retargeting only |
|||
# For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 |
|||
scaling_factor: 1.00 |
|||
# A smaller alpha means stronger filtering, i.e. more smooth but also larger latency |
|||
low_pass_alpha: 0.5 |
|||
|
|||
right: |
|||
type: DexPilot # or vector |
|||
urdf_path: brainco_hand/brainco_right.urdf |
|||
|
|||
# Target refers to the retargeting target, which is the robot hand |
|||
target_joint_names: |
|||
[ |
|||
"right_thumb_metacarpal_joint", |
|||
"right_thumb_proximal_joint", |
|||
"right_index_proximal_joint", |
|||
"right_middle_proximal_joint", |
|||
"right_ring_proximal_joint", |
|||
"right_pinky_proximal_joint", |
|||
] |
|||
# for DexPilot type |
|||
wrist_link_name: "base_link" |
|||
finger_tip_link_names: [ "right_thumb_tip", "right_index_tip", "right_middle_tip", "right_ring_tip", "right_pinky_tip" ] |
|||
target_link_human_indices_dexpilot: [[ 9, 14, 19, 24, 14, 19, 24, 19, 24, 24, 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 9, 9, 9, 14, 14, 19, 4, 9, 14, 19, 24]] |
|||
|
|||
# for vector type |
|||
target_origin_link_names: ["base_link", "base_link", "base_link", "base_link", "base_link"] |
|||
target_task_link_names: [ "right_thumb_tip", "right_index_tip", "right_middle_tip", "right_ring_tip", "right_pinky_tip" ] |
|||
target_link_human_indices_vector: [ [ 0, 0, 0, 0, 0 ], [ 4, 9, 14, 19, 24 ] ] |
|||
|
|||
# Scaling factor for vector retargeting only |
|||
# For example, Allegro is 1.6 times larger than normal human hand, then this scaling factor should be 1.6 |
|||
scaling_factor: 1.00 |
|||
# A smaller alpha means stronger filtering, i.e. more smooth but also larger latency |
|||
low_pass_alpha: 0.5 |
|||
@ -0,0 +1,618 @@ |
|||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) |
|||
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578 |
|||
For more information, please see http://wiki.ros.org/sw_urdf_exporter --> |
|||
<!-- This URDF is modified and download at https://github.com/BrainCoTech/stark-serialport-example/tree/revo2?tab=readme-ov-file --> |
|||
<robot name="brainco-lefthand-V2"> |
|||
<mujoco> |
|||
<compiler meshdir="meshes" discardvisual="false" /> |
|||
</mujoco> |
|||
|
|||
<link name="base_link"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<mass value="1e-6" /> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" /> |
|||
</inertial> |
|||
|
|||
<!-- Visualize coordinate axes --> |
|||
<visual> |
|||
<origin xyz="0.075 0 0" rpy="0 -1.5708 0" /> |
|||
<geometry> |
|||
<cylinder length="0.15" radius="0.004" /> |
|||
</geometry> |
|||
<material name="red"> |
|||
<color rgba="1 0 0 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0.15 0 0" rpy="0 -1.5708 0" /> |
|||
<geometry> |
|||
<sphere radius="0.015" /> |
|||
</geometry> |
|||
<material name="red"> |
|||
<color rgba="1 0 0 1" /> |
|||
</material> |
|||
</visual> |
|||
|
|||
<visual> |
|||
<origin xyz="0 0.075 0" rpy="1.5708 0 0" /> |
|||
<geometry> |
|||
<cylinder length="0.15" radius="0.004" /> |
|||
</geometry> |
|||
<material name="green"> |
|||
<color rgba="0 1 0 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0.15 0" rpy="1.5708 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.015" /> |
|||
</geometry> |
|||
<material name="green"> |
|||
<color rgba="0 1 0 1" /> |
|||
</material> |
|||
</visual> |
|||
|
|||
<visual> |
|||
<origin xyz="0 0 0.075" rpy="0 0 0" /> |
|||
<geometry> |
|||
<cylinder length="0.15" radius="0.004" /> |
|||
</geometry> |
|||
<material name="blue"> |
|||
<color rgba="0 0 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0.15" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.015" /> |
|||
</geometry> |
|||
<material name="blue"> |
|||
<color rgba="0 0 1 1" /> |
|||
</material> |
|||
</visual> |
|||
</link> |
|||
|
|||
<joint name="base" type="fixed"> |
|||
<parent link="base_link" /> |
|||
<child link="left_base_link" /> |
|||
<origin rpy="1.57 3.14 0" xyz="0 0 0" /> |
|||
</joint> |
|||
|
|||
<link name="left_base_link"> |
|||
<inertial> |
|||
<origin xyz="-0.00478670623240344 0.00141981128680179 0.0505239077176535" rpy="0 0 0" /> |
|||
<mass value="0.238" /> |
|||
<inertia ixx="2.2266510056133E-05" ixy="-2.6318583761649E-07" ixz="1.83638592892823E-07" iyy="1.55367220548952E-05" iyz="1.26586485241028E-06" izz="8.58669326263691E-06" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_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="meshes/left_base_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<link name="left_thumb_metacarpal_Link"> |
|||
<inertial> |
|||
<origin xyz="1.52303696287007E-05 -0.00707240283482268 -0.00420839832903629" rpy="0 0 0" /> |
|||
<mass value="0.01" /> |
|||
<inertia ixx="1.59238866130872E-07" ixy="4.69114607598504E-09" ixz="4.93233725002146E-10" iyy="1.60911967461755E-07" iyz="1.48144948560702E-08" izz="5.13544258620214E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_thumb_metacarpal_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="meshes/left_thumb_metacarpal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_thumb_metacarpal_joint" type="revolute"> |
|||
<origin xyz="0.0045 -0.019 0.027825" rpy="0 0 0.16787" /> |
|||
<parent link="left_base_link" /> |
|||
<child link="left_thumb_metacarpal_Link" /> |
|||
<axis xyz="0 0 1" /> |
|||
<limit lower="0" upper="1.5184" effort="0.5" velocity="2.6175" /> |
|||
</joint> |
|||
<link name="left_thumb_proximal_Link"> |
|||
<inertial> |
|||
<origin xyz="3.41001513141231E-07 0.0250090688174992 -0.0017505675193664" rpy="0 0 0" /> |
|||
<mass value="0.05" /> |
|||
<inertia ixx="3.23318665954068E-06" ixy="1.54514148596387E-10" ixz="-1.24375216968143E-11" iyy="5.64488567432875E-07" iyz="5.04309083812214E-08" izz="3.39928573090826E-06" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_thumb_proximal_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="meshes/left_thumb_proximal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_thumb_proximal_joint" type="revolute"> |
|||
<origin xyz="0 -0.014227 0" rpy="0.17488 -0.23736 2.9737" /> |
|||
<parent link="left_thumb_metacarpal_Link" /> |
|||
<child link="left_thumb_proximal_Link" /> |
|||
<axis xyz="1 0 0" /> |
|||
<limit lower="0" upper="1.0472" effort="1.1" velocity="2.5303" /> |
|||
</joint> |
|||
<link name="left_thumb_distal_Link"> |
|||
<inertial> |
|||
<origin xyz="-1.65618684180163E-07 0.0117550891981017 1.1366609064185E-05" rpy="0 0 0" /> |
|||
<mass value="0.01" /> |
|||
<inertia ixx="1.6780928553031E-07" ixy="1.91856562375255E-11" ixz="-2.694752026736E-12" iyy="1.15357239984326E-07" iyz="1.25182560702848E-08" izz="2.1956615213811E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_thumb_distal_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="meshes/left_thumb_distal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_thumb_distal_joint" type="revolute"> |
|||
<origin xyz="0 0.052 0" rpy="0 0 0" /> |
|||
<parent link="left_thumb_proximal_Link" /> |
|||
<child link="left_thumb_distal_Link" /> |
|||
<axis xyz="1 0 0" /> |
|||
<limit lower="0" upper="1.0472" effort="1.1" velocity="2.5303" /> |
|||
<mimic joint="left_thumb_proximal_joint" multiplier="1" offset="0" /> |
|||
</joint> |
|||
<link name="left_thumb_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<mass value="0.001" /> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_thumb_tip_Link.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="red"> |
|||
<color rgba="1 0 0 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_thumb_tip_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_thumb_tip_joint" type="fixed"> |
|||
<origin xyz="0 0.0265 0" rpy="0.0667504507189052 0 3.14159265358979" /> |
|||
<parent link="left_thumb_distal_Link" /> |
|||
<child link="left_thumb_tip" /> |
|||
<axis xyz="-1 0 0" /> |
|||
<limit lower="0" upper="0" effort="0" velocity="0" /> |
|||
</joint> |
|||
<link name="left_index_proximal_Link"> |
|||
<inertial> |
|||
<origin xyz="-0.002235387426012 -3.80544615179342E-06 0.0176439423080373" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="2.51863526940901E-07" ixy="-3.10934654014177E-11" ixz="-6.36901300651624E-09" iyy="2.36343210839019E-07" iyz="-1.71663811116817E-10" izz="9.30397543724724E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_index_proximal_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="meshes/left_index_proximal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_index_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0021349 -0.029578 0.080876" rpy="0.11819 0.14743 0.17422" /> |
|||
<parent link="left_base_link" /> |
|||
<child link="left_index_proximal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="left_index_distal_Link"> |
|||
<inertial> |
|||
<origin xyz="0.00418325886379583 -8.45607972810303E-08 0.0153641608762859" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="3.65266271559531E-07" ixy="5.6892108332606E-13" ixz="-1.02335061442931E-07" iyy="3.96961530855654E-07" iyz="7.73961385851005E-13" izz="1.04991723103838E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_index_distal_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="meshes/left_index_distal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_index_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.032" rpy="0 0 0" /> |
|||
<parent link="left_index_proximal_Link" /> |
|||
<child link="left_index_distal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="left_index_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="left_index_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<mass value="0.001" /> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_index_tip_Link.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="green"> |
|||
<color rgba="0 1 0 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_index_tip_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_index_tip_joint" type="fixed"> |
|||
<origin xyz="0.0125 0 0.031" rpy="0 0.48723 0" /> |
|||
<parent link="left_index_distal_Link" /> |
|||
<child link="left_index_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="1" upper="1" effort="1" velocity="1" /> |
|||
</joint> |
|||
<link name="left_middle_proximal_Link"> |
|||
<inertial> |
|||
<origin xyz="-0.00222419445309931 -3.61990820813615E-06 0.0203049526045434" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="3.66885320718611E-07" ixy="-3.64303343703497E-11" ixz="-9.77995332041374E-09" iyy="3.5252138716754E-07" iyz="-2.5809821190624E-10" izz="1.12242284171831E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_middle_proximal_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="meshes/left_middle_proximal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_middle_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0045935 -0.010061 0.084993" rpy="0.039468 0.1483 0.057975" /> |
|||
<parent link="left_base_link" /> |
|||
<child link="left_middle_proximal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="left_middle_distal_Link"> |
|||
<inertial> |
|||
<origin xyz="0.00269750139301865 3.27210103736979E-07 0.0183076461156786" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="5.88341898887764E-07" ixy="-1.34073376318216E-12" ixz="-1.04743131832206E-07" iyy="6.07118927271625E-07" iyz="-1.31817421243194E-11" izz="1.11265454837883E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_middle_distal_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="meshes/left_middle_distal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_middle_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.037" rpy="0 0.12654 0" /> |
|||
<parent link="left_middle_proximal_Link" /> |
|||
<child link="left_middle_distal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="left_middle_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="left_middle_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<mass value="0.001" /> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_middle_tip_Link.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="blue"> |
|||
<color rgba="0 0 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_middle_tip_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_middle_tip_joint" type="fixed"> |
|||
<origin xyz="0.01 0 0.038" rpy="0 0.34907 0" /> |
|||
<parent link="left_middle_distal_Link" /> |
|||
<child link="left_middle_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="1" upper="1" effort="1" velocity="1" /> |
|||
</joint> |
|||
<link name="left_ring_proximal_Link"> |
|||
<inertial> |
|||
<origin xyz="-0.00234461360118865 -5.32058096490429E-06 0.0191785931520923" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="3.1418221085363E-07" ixy="-3.44825864493742E-11" ixz="-7.78011903077359E-09" iyy="2.97623388067888E-07" iyz="-2.45023710126388E-10" izz="1.02120946203075E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_ring_proximal_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="meshes/left_ring_proximal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_ring_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.00468767815858587 0.0100262714211602 0.0839819548795939" rpy="-0.0394618478609065 0.147209177132223 -0.0579317909005277" /> |
|||
<parent link="left_base_link" /> |
|||
<child link="left_ring_proximal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="left_ring_distal_Link"> |
|||
<inertial> |
|||
<origin xyz="0.00249839540877306 -2.78996376279483E-08 0.0177055538603162" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="5.40369389752843E-07" ixy="-3.16120850919061E-12" ixz="-9.71738751449028E-08" iyy="5.56818359755755E-07" iyz="-5.08675656941986E-12" izz="1.03096676549004E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_ring_distal_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="meshes/left_ring_distal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_ring_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.035" rpy="0 0.12654 0" /> |
|||
<parent link="left_ring_proximal_Link" /> |
|||
<child link="left_ring_distal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="left_ring_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="left_ring_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<mass value="0.001" /> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_ring_tip_Link.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="cyan"> |
|||
<color rgba="0 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_ring_tip_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_ring_tip_joint" type="fixed"> |
|||
<origin xyz="0.01 0 0.037" rpy="0 0.34907 0" /> |
|||
<parent link="left_ring_distal_Link" /> |
|||
<child link="left_ring_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="1" upper="1" effort="1" velocity="1" /> |
|||
</joint> |
|||
<link name="left_pinky_proximal_Link"> |
|||
<inertial> |
|||
<origin xyz="-0.00213266608574047 -7.02260316783054E-06 0.0160680350593264" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="1.97554078210582E-07" ixy="-1.34133676966608E-11" ixz="-4.58601823842206E-09" iyy="1.82022483454667E-07" iyz="-1.01838144940489E-10" izz="8.21758160050774E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_pinky_proximal_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="meshes/left_pinky_proximal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_pinky_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0023733 0.029356 0.078694" rpy="-0.11815 0.14491 -0.17392" /> |
|||
<parent link="left_base_link" /> |
|||
<child link="left_pinky_proximal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="left_pinky_distal_Link"> |
|||
<inertial> |
|||
<origin xyz="0.0039856213046876 -8.08551930131518E-08 0.014106664402408" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="2.88250623683654E-07" ixy="-1.42865648587319E-12" ixz="-7.78112273929214E-08" iyy="3.10833114148178E-07" iyz="1.51641220465817E-13" izz="9.12872269672051E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_pinky_distal_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="meshes/left_pinky_distal_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_pinky_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.029" rpy="0 0 0" /> |
|||
<parent link="left_pinky_proximal_Link" /> |
|||
<child link="left_pinky_distal_Link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="left_pinky_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="left_pinky_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<mass value="0.001" /> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_pinky_tip_Link.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="white"> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/left_pinky_tip_Link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="left_pinky_tip_joint" type="fixed"> |
|||
<origin xyz="0.0125 0 0.03" rpy="0 0.4756 0" /> |
|||
<parent link="left_pinky_distal_Link" /> |
|||
<child link="left_pinky_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="1" upper="1" effort="1" velocity="1" /> |
|||
</joint> |
|||
</robot> |
|||
@ -0,0 +1,618 @@ |
|||
<!-- This URDF was automatically created by SolidWorks to URDF Exporter! Originally created by Stephen Brawner (brawner@gmail.com) |
|||
Commit Version: 1.6.0-4-g7f85cfe Build Version: 1.6.7995.38578 |
|||
For more information, please see http://wiki.ros.org/sw_urdf_exporter --> |
|||
<!-- This URDF is modified and download at https://github.com/BrainCoTech/stark-serialport-example/tree/revo2?tab=readme-ov-file --> |
|||
<robot name="brainco-righthand-V2"> |
|||
<mujoco> |
|||
<compiler meshdir="meshes" discardvisual="false" /> |
|||
</mujoco> |
|||
|
|||
<link name="base_link"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0"/> |
|||
<mass value="1e-6"/> |
|||
<inertia ixx="1e-9" ixy="0" ixz="0" iyy="1e-9" iyz="0" izz="1e-9"/> |
|||
</inertial> |
|||
|
|||
<!-- Visualize coordinate axes --> |
|||
<visual> |
|||
<origin xyz="0.075 0 0" rpy="0 -1.5708 0"/> |
|||
<geometry> |
|||
<cylinder length="0.15" radius="0.004"/> |
|||
</geometry> |
|||
<material name="red"> |
|||
<color rgba="1 0 0 1"/> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0.15 0 0" rpy="0 -1.5708 0"/> |
|||
<geometry> |
|||
<sphere radius="0.015"/> |
|||
</geometry> |
|||
<material name="red"> |
|||
<color rgba="1 0 0 1"/> |
|||
</material> |
|||
</visual> |
|||
|
|||
<visual> |
|||
<origin xyz="0 0.075 0" rpy="1.5708 0 0"/> |
|||
<geometry> |
|||
<cylinder length="0.15" radius="0.004"/> |
|||
</geometry> |
|||
<material name="green"> |
|||
<color rgba="0 1 0 1"/> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0.15 0" rpy="1.5708 0 0"/> |
|||
<geometry> |
|||
<sphere radius="0.015"/> |
|||
</geometry> |
|||
<material name="green"> |
|||
<color rgba="0 1 0 1"/> |
|||
</material> |
|||
</visual> |
|||
|
|||
<visual> |
|||
<origin xyz="0 0 0.075" rpy="0 0 0"/> |
|||
<geometry> |
|||
<cylinder length="0.15" radius="0.004"/> |
|||
</geometry> |
|||
<material name="blue"> |
|||
<color rgba="0 0 1 1"/> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0.15" rpy="0 0 0"/> |
|||
<geometry> |
|||
<sphere radius="0.015"/> |
|||
</geometry> |
|||
<material name="blue"> |
|||
<color rgba="0 0 1 1"/> |
|||
</material> |
|||
</visual> |
|||
</link> |
|||
|
|||
<joint name="base" type="fixed"> |
|||
<parent link="base_link" /> |
|||
<child link="right_base_link" /> |
|||
<origin rpy="1.57 3.14 0" xyz="0 0 0" /> |
|||
</joint> |
|||
|
|||
<link name="right_base_link"> |
|||
<inertial> |
|||
<origin xyz="-0.00477210157415217 -0.0014368255627066 0.0505186815078315" rpy="0 0 0" /> |
|||
<mass value="0.238" /> |
|||
<inertia ixx="2.22653147661907E-05" ixy="2.61914657870284E-07" ixz="1.85322175528149E-07" iyy="1.55386682543955E-05" iyz="-1.26178670079141E-06" izz="8.58493952480684E-06" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_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="meshes/right_base_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<link name="right_thumb_metacarpal_link"> |
|||
<inertial> |
|||
<origin xyz="-1.51105893291953E-05 0.00707242532016975 0.00420847293427094" rpy="0 0 0" /> |
|||
<mass value="0.01" /> |
|||
<inertia ixx="1.59239837111329E-07" ixy="4.69100836631313E-09" ixz="4.92933775261589E-10" iyy="1.60913456568321E-07" iyz="1.48146725871811E-08" izz="5.13550255314669E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_thumb_metacarpal_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="meshes/right_thumb_metacarpal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_thumb_metacarpal_joint" type="revolute"> |
|||
<origin xyz="0.005619 0.019867 0.027825" rpy="3.1416 0 2.9637" /> |
|||
<parent link="right_base_link" /> |
|||
<child link="right_thumb_metacarpal_link" /> |
|||
<axis xyz="0 0 1" /> |
|||
<limit lower="0" upper="1.5184" effort="0.5" velocity="2.6175" /> |
|||
</joint> |
|||
<link name="right_thumb_proximal_link"> |
|||
<inertial> |
|||
<origin xyz="-4.57221370792071E-07 0.0250090111157988 -0.0017504940716868" rpy="0 0 0" /> |
|||
<mass value="0.05" /> |
|||
<inertia ixx="3.23319868561771E-06" ixy="-1.19078810940838E-10" ixz="1.24464917587322E-11" iyy="5.64505997265396E-07" iyz="5.04129402047053E-08" izz="3.39928433889051E-06" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_thumb_proximal_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="meshes/right_thumb_proximal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_thumb_proximal_joint" type="revolute"> |
|||
<origin xyz="0 0.014227 0" rpy="-2.9667 -0.23736 2.9737" /> |
|||
<parent link="right_thumb_metacarpal_link" /> |
|||
<child link="right_thumb_proximal_link" /> |
|||
<axis xyz="1 0 0" /> |
|||
<limit lower="0" upper="1.0472" effort="1.1" velocity="2.5303" /> |
|||
</joint> |
|||
<link name="right_thumb_distal_link"> |
|||
<inertial> |
|||
<origin xyz="-5.88258313588552E-09 0.0117544716247142 1.13324397438969E-05" rpy="0 0 0" /> |
|||
<mass value="0.01" /> |
|||
<inertia ixx="1.67793545309794E-07" ixy="7.15927847824524E-12" ixz="-6.80095370887594E-12" iyy="1.15362128819332E-07" iyz="1.2517499089377E-08" izz="2.19553178338424E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_thumb_distal_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="meshes/right_thumb_distal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_thumb_distal_joint" type="revolute"> |
|||
<origin xyz="0 0.052 0" rpy="0 0 0" /> |
|||
<parent link="right_thumb_proximal_link" /> |
|||
<child link="right_thumb_distal_link" /> |
|||
<axis xyz="1 0 0" /> |
|||
<limit lower="0" upper="1.0472" effort="1.1" velocity="2.5303" /> |
|||
<mimic joint="right_thumb_proximal_joint" multiplier="1" offset="0" /> |
|||
</joint> |
|||
<link name="right_thumb_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0"/> |
|||
<mass value="0.001"/> |
|||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_thumb_tip.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="red"> |
|||
<color rgba="1 0 0 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_thumb_tip.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_thumb_tip" type="fixed"> |
|||
<origin xyz="0 0.0265 0" rpy="-0.06675 0 0" /> |
|||
<parent link="right_thumb_distal_link" /> |
|||
<child link="right_thumb_tip" /> |
|||
<axis xyz="1 0 0" /> |
|||
<limit lower="0" upper="0" effort="0" velocity="0" /> |
|||
</joint> |
|||
<link name="right_index_proximal_link"> |
|||
<inertial> |
|||
<origin xyz="-0.00223495193199565 3.15874577787312E-06 0.0176402020341366" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="2.51945688372283E-07" ixy="1.19772964297344E-11" ixz="-6.37275644668915E-09" iyy="2.36435336139899E-07" iyz="1.65019165740605E-10" izz="9.30351046594029E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_index_proximal_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="meshes/right_index_proximal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_index_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0021181 0.029568 0.080876" rpy="-0.11819 0.14743 -0.17422" /> |
|||
<parent link="right_base_link" /> |
|||
<child link="right_index_proximal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="right_index_distal_link"> |
|||
<inertial> |
|||
<origin xyz="0.0041834128909843 1.16430016613989E-07 0.0153643358620559" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="3.65279317721019E-07" ixy="-2.87605986323987E-12" ixz="-1.02338200214364E-07" iyy="3.96973285716405E-07" iyz="-3.64509010623639E-12" izz="1.04994687350786E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_index_distal_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="meshes/right_index_distal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_index_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.032" rpy="0 0 0" /> |
|||
<parent link="right_index_proximal_link" /> |
|||
<child link="right_index_distal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="right_index_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="right_index_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0"/> |
|||
<mass value="0.001"/> |
|||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_index_tip.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="green"> |
|||
<color rgba="0 1 0 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_index_tip.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_index_tip_joint" type="fixed"> |
|||
<origin xyz="0.0125 0 0.031" rpy="0 0.4756 0" /> |
|||
<parent link="right_index_distal_link" /> |
|||
<child link="right_index_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="0" effort="0" velocity="0" /> |
|||
</joint> |
|||
<link name="right_middle_proximal_link"> |
|||
<inertial> |
|||
<origin xyz="-0.00222511101223747 5.25632286623237E-06 0.0203007776295123" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="3.6699623719538E-07" ixy="2.68340899395606E-11" ixz="-9.79836587937101E-09" iyy="3.52645347928613E-07" iyz="2.57865582739813E-10" izz="1.12233932527467E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_middle_proximal_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="meshes/right_middle_proximal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_middle_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0045767 0.010051 0.084993" rpy="-0.039468 0.1483 -0.057975" /> |
|||
<parent link="right_base_link" /> |
|||
<child link="right_middle_proximal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="right_middle_distal_link"> |
|||
<inertial> |
|||
<origin xyz="0.00498634699274855 3.20799606209204E-07 0.0178210324631583" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="5.54531287704842E-07" ixy="-3.8261699427749E-12" ixz="-1.61139304348347E-07" iyy="6.07133284933246E-07" iyz="-1.20510743801577E-11" izz="1.45091120995859E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_middle_distal_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="meshes/right_middle_distal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_middle_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.037" rpy="0 0 0" /> |
|||
<parent link="right_middle_proximal_link" /> |
|||
<child link="right_middle_distal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="right_middle_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="right_middle_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0"/> |
|||
<mass value="0.001"/> |
|||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_middle_tip.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="blue"> |
|||
<color rgba="0 0 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_middle_tip.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_middle_tip_joint" type="fixed"> |
|||
<origin xyz="0.0145 0 0.0365" rpy="0 0.4756 0" /> |
|||
<parent link="right_middle_distal_link" /> |
|||
<child link="right_middle_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="0" effort="0" velocity="0" /> |
|||
</joint> |
|||
<link name="right_ring_proximal_link"> |
|||
<inertial> |
|||
<origin xyz="-0.00234494526421452 5.87954755615957E-06 0.019177402549597" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="3.14177684848335E-07" ixy="3.34382331089794E-11" ixz="-7.78981780892178E-09" iyy="2.97613170150896E-07" iyz="2.41251871803812E-10" izz="1.0211543233675E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_ring_proximal_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="meshes/right_ring_proximal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_ring_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0046709 -0.010037 0.083982" rpy="0.039462 0.14721 0.057932" /> |
|||
<parent link="right_base_link" /> |
|||
<child link="right_ring_proximal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="right_ring_distal_link"> |
|||
<inertial> |
|||
<origin xyz="0.0047128205390484 -2.41851534580478E-08 0.0172486576240172" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="5.09075595345636E-07" ixy="-3.1183534714212E-12" ixz="-1.48822343330519E-07" iyy="5.56818020505642E-07" iyz="-3.81405399596009E-12" izz="1.34389180077007E-07" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_ring_distal_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="meshes/right_ring_distal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_ring_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.035" rpy="0 0 0" /> |
|||
<parent link="right_ring_proximal_link" /> |
|||
<child link="right_ring_distal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="right_ring_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="right_ring_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0"/> |
|||
<mass value="0.001"/> |
|||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_ring_tip.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="cyan"> |
|||
<color rgba="0 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_ring_tip.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_ring_tip_joint" type="fixed"> |
|||
<origin xyz="0.0145 0 0.036" rpy="0 0.4756 0" /> |
|||
<parent link="right_ring_distal_link" /> |
|||
<child link="right_ring_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="0" effort="0" velocity="0" /> |
|||
</joint> |
|||
<link name="right_pinky_proximal_link"> |
|||
<inertial> |
|||
<origin xyz="-0.00213212021184178 7.75039446098032E-06 0.0160671583560383" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="1.97563213550085E-07" ixy="2.23402433057897E-11" ixz="-4.5842566369208E-09" iyy="1.82023043083307E-07" iyz="1.08028441152503E-10" izz="8.21624712033168E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_pinky_proximal_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="meshes/right_pinky_proximal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_pinky_proximal_joint" type="revolute"> |
|||
<origin xyz="-0.0023566 -0.029366 0.078694" rpy="0.11815 0.14491 0.17392" /> |
|||
<parent link="right_base_link" /> |
|||
<child link="right_pinky_proximal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.4661" effort="2" velocity="2.2685" /> |
|||
</joint> |
|||
<link name="right_pinky_distal_link"> |
|||
<inertial> |
|||
<origin xyz="0.00398575709812823 -1.18733454507539E-07 0.0141073520225541" rpy="0 0 0" /> |
|||
<mass value="0.009" /> |
|||
<inertia ixx="2.88287304139793E-07" ixy="8.06230341569425E-13" ixz="-7.78224822397468E-08" iyy="3.10873492837471E-07" iyz="3.24621869724066E-12" izz="9.12935606299289E-08" /> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_pinky_distal_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="meshes/right_pinky_distal_link.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_pinky_distal_joint" type="revolute"> |
|||
<origin xyz="0 0 0.029" rpy="0 0 0" /> |
|||
<parent link="right_pinky_proximal_link" /> |
|||
<child link="right_pinky_distal_link" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="1.693" effort="2" velocity="2.2685" /> |
|||
<mimic joint="right_pinky_proximal_joint" multiplier="1.155" offset="0" /> |
|||
</joint> |
|||
<link name="right_pinky_tip"> |
|||
<inertial> |
|||
<origin xyz="0 0 0" rpy="0 0 0"/> |
|||
<mass value="0.001"/> |
|||
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/> |
|||
</inertial> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_pinky_tip.STL" /> |
|||
</geometry> |
|||
<material name=""> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<visual> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<sphere radius="0.005" /> |
|||
</geometry> |
|||
<material name="white"> |
|||
<color rgba="1 1 1 1" /> |
|||
</material> |
|||
</visual> |
|||
<collision> |
|||
<origin xyz="0 0 0" rpy="0 0 0" /> |
|||
<geometry> |
|||
<mesh filename="meshes/right_pinky_tip.STL" /> |
|||
</geometry> |
|||
</collision> |
|||
</link> |
|||
<joint name="right_pinky_tip_joint" type="fixed"> |
|||
<origin xyz="0.0125 0 0.03" rpy="0 0.4756 0" /> |
|||
<parent link="right_pinky_distal_link" /> |
|||
<child link="right_pinky_tip" /> |
|||
<axis xyz="0 1 0" /> |
|||
<limit lower="0" upper="0" effort="0" velocity="0" /> |
|||
</joint> |
|||
</robot> |
|||
@ -0,0 +1,194 @@ |
|||
from unitree_sdk2py.core.channel import ChannelPublisher, ChannelSubscriber, ChannelFactoryInitialize # dds |
|||
from unitree_sdk2py.idl.unitree_go.msg.dds_ import MotorCmds_, MotorStates_ # idl |
|||
from unitree_sdk2py.idl.default import unitree_go_msg_dds__MotorCmd_ |
|||
|
|||
from teleop.robot_control.hand_retargeting import HandRetargeting, HandType |
|||
import numpy as np |
|||
from enum import IntEnum |
|||
import threading |
|||
import time |
|||
from multiprocessing import Process, Array |
|||
|
|||
import logging_mp |
|||
logger_mp = logging_mp.get_logger(__name__) |
|||
|
|||
brainco_Num_Motors = 6 |
|||
kTopicbraincoLeftCommand = "rt/brainco/left/cmd" |
|||
kTopicbraincoLeftState = "rt/brainco/left/state" |
|||
kTopicbraincoRightCommand = "rt/brainco/right/cmd" |
|||
kTopicbraincoRightState = "rt/brainco/right/state" |
|||
|
|||
class Brainco_Controller: |
|||
def __init__(self, left_hand_array, right_hand_array, dual_hand_data_lock = None, dual_hand_state_array = None, |
|||
dual_hand_action_array = None, fps = 100.0, Unit_Test = False, simulation_mode = False): |
|||
logger_mp.info("Initialize Brainco_Controller...") |
|||
self.fps = fps |
|||
self.hand_sub_ready = False |
|||
self.Unit_Test = Unit_Test |
|||
self.simulation_mode = simulation_mode |
|||
|
|||
if not self.Unit_Test: |
|||
self.hand_retargeting = HandRetargeting(HandType.BRAINCO_HAND) |
|||
else: |
|||
self.hand_retargeting = HandRetargeting(HandType.BRAINCO_HAND_Unit_Test) |
|||
|
|||
if self.simulation_mode: |
|||
ChannelFactoryInitialize(1) |
|||
else: |
|||
ChannelFactoryInitialize(0) |
|||
|
|||
# initialize handcmd publisher and handstate subscriber |
|||
self.LeftHandCmb_publisher = ChannelPublisher(kTopicbraincoLeftCommand, MotorCmds_) |
|||
self.LeftHandCmb_publisher.Init() |
|||
self.RightHandCmb_publisher = ChannelPublisher(kTopicbraincoRightCommand, MotorCmds_) |
|||
self.RightHandCmb_publisher.Init() |
|||
|
|||
self.LeftHandState_subscriber = ChannelSubscriber(kTopicbraincoLeftState, MotorStates_) |
|||
self.LeftHandState_subscriber.Init() |
|||
self.RightHandState_subscriber = ChannelSubscriber(kTopicbraincoRightState, MotorStates_) |
|||
self.RightHandState_subscriber.Init() |
|||
|
|||
# Shared Arrays for hand states |
|||
self.left_hand_state_array = Array('d', brainco_Num_Motors, lock=True) |
|||
self.right_hand_state_array = Array('d', brainco_Num_Motors, lock=True) |
|||
|
|||
# initialize subscribe thread |
|||
self.subscribe_state_thread = threading.Thread(target=self._subscribe_hand_state) |
|||
self.subscribe_state_thread.daemon = True |
|||
self.subscribe_state_thread.start() |
|||
|
|||
while not self.hand_sub_ready: |
|||
time.sleep(0.1) |
|||
logger_mp.warning("[brainco_Controller] Waiting to subscribe dds...") |
|||
logger_mp.info("[brainco_Controller] Subscribe dds ok.") |
|||
|
|||
hand_control_process = Process(target=self.control_process, args=(left_hand_array, right_hand_array, self.left_hand_state_array, self.right_hand_state_array, |
|||
dual_hand_data_lock, dual_hand_state_array, dual_hand_action_array)) |
|||
hand_control_process.daemon = True |
|||
hand_control_process.start() |
|||
|
|||
logger_mp.info("Initialize brainco_Controller OK!\n") |
|||
|
|||
def _subscribe_hand_state(self): |
|||
while True: |
|||
left_hand_msg = self.LeftHandState_subscriber.Read() |
|||
right_hand_msg = self.RightHandState_subscriber.Read() |
|||
self.hand_sub_ready = True |
|||
if left_hand_msg is not None and right_hand_msg is not None: |
|||
# Update left hand state |
|||
for idx, id in enumerate(Brainco_Left_Hand_JointIndex): |
|||
self.left_hand_state_array[idx] = left_hand_msg.states[id].q |
|||
# Update right hand state |
|||
for idx, id in enumerate(Brainco_Right_Hand_JointIndex): |
|||
self.right_hand_state_array[idx] = right_hand_msg.states[id].q |
|||
time.sleep(0.002) |
|||
|
|||
def ctrl_dual_hand(self, left_q_target, right_q_target): |
|||
""" |
|||
Set current left, right hand motor state target q |
|||
""" |
|||
for idx, id in enumerate(Brainco_Left_Hand_JointIndex): |
|||
self.left_hand_msg.cmds[id].q = left_q_target[idx] |
|||
for idx, id in enumerate(Brainco_Right_Hand_JointIndex): |
|||
self.right_hand_msg.cmds[id].q = right_q_target[idx] |
|||
|
|||
self.LeftHandCmb_publisher.Write(self.left_hand_msg) |
|||
self.RightHandCmb_publisher.Write(self.right_hand_msg) |
|||
# logger_mp.debug("hand ctrl publish ok.") |
|||
|
|||
def control_process(self, left_hand_array, right_hand_array, left_hand_state_array, right_hand_state_array, |
|||
dual_hand_data_lock = None, dual_hand_state_array = None, dual_hand_action_array = None): |
|||
self.running = True |
|||
|
|||
left_q_target = np.full(brainco_Num_Motors, 0) |
|||
right_q_target = np.full(brainco_Num_Motors, 0) |
|||
|
|||
# initialize brainco hand's cmd msg |
|||
self.left_hand_msg = MotorCmds_() |
|||
self.left_hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(len(Brainco_Left_Hand_JointIndex))] |
|||
self.right_hand_msg = MotorCmds_() |
|||
self.right_hand_msg.cmds = [unitree_go_msg_dds__MotorCmd_() for _ in range(len(Brainco_Right_Hand_JointIndex))] |
|||
|
|||
for idx, id in enumerate(Brainco_Left_Hand_JointIndex): |
|||
self.left_hand_msg.cmds[id].q = 0.0 |
|||
self.left_hand_msg.cmds[id].dq = 1.0 |
|||
for idx, id in enumerate(Brainco_Right_Hand_JointIndex): |
|||
self.right_hand_msg.cmds[id].q = 0.0 |
|||
self.right_hand_msg.cmds[id].dq = 1.0 |
|||
|
|||
try: |
|||
while self.running: |
|||
start_time = time.time() |
|||
# get dual hand state |
|||
with left_hand_array.get_lock(): |
|||
left_hand_data = np.array(left_hand_array[:]).reshape(25, 3).copy() |
|||
with right_hand_array.get_lock(): |
|||
right_hand_data = np.array(right_hand_array[:]).reshape(25, 3).copy() |
|||
|
|||
# Read left and right q_state from shared arrays |
|||
state_data = np.concatenate((np.array(left_hand_state_array[:]), np.array(right_hand_state_array[:]))) |
|||
|
|||
if not np.all(right_hand_data == 0.0) and not np.all(left_hand_data[4] == np.array([-1.13, 0.3, 0.15])): # if hand data has been initialized. |
|||
ref_left_value = left_hand_data[self.hand_retargeting.left_indices[1,:]] - left_hand_data[self.hand_retargeting.left_indices[0,:]] |
|||
ref_right_value = right_hand_data[self.hand_retargeting.right_indices[1,:]] - right_hand_data[self.hand_retargeting.right_indices[0,:]] |
|||
|
|||
left_q_target = self.hand_retargeting.left_retargeting.retarget(ref_left_value)[self.hand_retargeting.left_dex_retargeting_to_hardware] |
|||
right_q_target = self.hand_retargeting.right_retargeting.retarget(ref_right_value)[self.hand_retargeting.right_dex_retargeting_to_hardware] |
|||
|
|||
# In the official document, the angles are in the range [0, 1] ==> 0.0: fully open 1.0: fully closed |
|||
# The q_target now is in radians, ranges: |
|||
# - idx 0: 0~1.52 |
|||
# - idx 1: 0~1.05 |
|||
# - idx 2~5: 0~1.47 |
|||
# We normalize them using (max - value) / range |
|||
def normalize(val, min_val, max_val): |
|||
return 1.0 - np.clip((max_val - val) / (max_val - min_val), 0.0, 1.0) |
|||
|
|||
for idx in range(brainco_Num_Motors): |
|||
if idx == 0: |
|||
left_q_target[idx] = normalize(left_q_target[idx], 0.0, 1.52) |
|||
right_q_target[idx] = normalize(right_q_target[idx], 0.0, 1.52) |
|||
elif idx == 1: |
|||
left_q_target[idx] = normalize(left_q_target[idx], 0.0, 1.05) |
|||
right_q_target[idx] = normalize(right_q_target[idx], 0.0, 1.05) |
|||
elif idx >= 2: |
|||
left_q_target[idx] = normalize(left_q_target[idx], 0.0, 1.47) |
|||
right_q_target[idx] = normalize(right_q_target[idx], 0.0, 1.47) |
|||
|
|||
# get dual hand action |
|||
action_data = np.concatenate((left_q_target, right_q_target)) |
|||
if dual_hand_state_array and dual_hand_action_array: |
|||
with dual_hand_data_lock: |
|||
dual_hand_state_array[:] = state_data |
|||
dual_hand_action_array[:] = action_data |
|||
# logger_mp.info(f"left_q_target:{left_q_target}") |
|||
self.ctrl_dual_hand(left_q_target, right_q_target) |
|||
current_time = time.time() |
|||
time_elapsed = current_time - start_time |
|||
sleep_time = max(0, (1 / self.fps) - time_elapsed) |
|||
time.sleep(sleep_time) |
|||
finally: |
|||
logger_mp.info("brainco_Controller has been closed.") |
|||
|
|||
# according to the official documentation, https://www.brainco-hz.com/docs/revolimb-hand/product/parameters.html |
|||
# the motor sequence is as shown in the table below |
|||
# ┌──────┬───────┬────────────┬────────┬────────┬────────┬────────┐ |
|||
# │ Id │ 0 │ 1 │ 2 │ 3 │ 4 │ 5 │ |
|||
# ├──────┼───────┼────────────┼────────┼────────┼────────┼────────┤ |
|||
# │Joint │ thumb │ thumb-aux | index │ middle │ ring │ pinky │ |
|||
# └──────┴───────┴────────────┴────────┴────────┴────────┴────────┘ |
|||
class Brainco_Right_Hand_JointIndex(IntEnum): |
|||
kRightHandThumb = 0 |
|||
kRightHandThumbAux = 1 |
|||
kRightHandIndex = 2 |
|||
kRightHandMiddle = 3 |
|||
kRightHandRing = 4 |
|||
kRightHandPinky = 5 |
|||
|
|||
class Brainco_Left_Hand_JointIndex(IntEnum): |
|||
kLeftHandThumb = 0 |
|||
kLeftHandThumbAux = 1 |
|||
kLeftHandIndex = 2 |
|||
kLeftHandMiddle = 3 |
|||
kLeftHandRing = 4 |
|||
kLeftHandPinky = 5 |
|||
Write
Preview
Loading…
Cancel
Save
Reference in new issue