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

[update] support dex3 DexPilot algorithm.

main
silencht 9 months ago
parent
aee701fdf8
commit
7d91cfb6b8
5 changed files with 62 additions and 54 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. 40
      assets/unitree_hand/unitree_dex3.yml
  2. 11
      teleop/robot_control/dex_retargeting/optimizer.py
  3. 52
      teleop/robot_control/dex_retargeting/retargeting_config.py
  4. 2
      teleop/robot_control/hand_retargeting.py
  5. 11
      teleop/robot_control/robot_hand_unitree.py

40
assets/unitree_hand/unitree_dex3.yml
View File

@ -1,9 +1,8 @@
left:
type: vector
type: DexPilot # or vector
urdf_path: unitree_hand/unitree_dex3_left.urdf
# Target refers to the retargeting target, which is the robot hand
# target_joint_names:
target_joint_names:
[
"left_hand_thumb_0_joint",
@ -14,27 +13,26 @@ left:
"left_hand_index_0_joint",
"left_hand_index_1_joint",
]
wrist_link_name: None
# for DexPilot type
wrist_link_name: "base_link"
finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_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, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]]
# for vector type
target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"]
target_task_link_names: ["thumb_tip","index_tip","middle_tip"]
target_link_human_indices: [[0, 0, 0], [4, 9, 14]]
target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]]
# Currently, the scaling factor for each finger is individually distinguished in the robot_hand_unitree.py file.
# The Unitree Dex3 has three fingers with the same specifications, so the retarget scaling factors need to be adjusted separately.
# The relevant code is as follows:
# ref_left_value[0] = ref_left_value[0] * 1.15
# ref_left_value[1] = ref_left_value[1] * 1.05
# ref_left_value[2] = ref_left_value[2] * 0.95
# ref_right_value[0] = ref_right_value[0] * 1.15
# ref_right_value[1] = ref_right_value[1] * 1.05
# ref_right_value[2] = ref_right_value[2] * 0.95
# 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.0
# A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
low_pass_alpha: 0.2
right:
type: vector
type: DexPilot # or vector
urdf_path: unitree_hand/unitree_dex3_right.urdf
# Target refers to the retargeting target, which is the robot hand
@ -48,12 +46,18 @@ right:
"right_hand_middle_0_joint",
"right_hand_middle_1_joint",
]
wrist_link_name: None
# for DexPilot type
wrist_link_name: "base_link"
finger_tip_link_names: [ "thumb_tip", "index_tip", "middle_tip"]
target_link_human_indices_dexpilot: [[ 9, 14, 14, 0, 0, 0], [ 4, 4, 9, 4, 9, 14]]
# for vector type
target_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"]
target_task_link_names: ["thumb_tip", "index_tip", "middle_tip"]
target_link_human_indices: [[0, 0, 0], [4, 9, 14]]
target_link_human_indices_vector: [[0, 0, 0], [4, 9, 14]]
# 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.0
# A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
low_pass_alpha: 0.2

11
teleop/robot_control/dex_retargeting/optimizer.py
View File

@ -324,7 +324,12 @@ class DexPilotOptimizer(Optimizer):
origin_link_index, task_link_index = self.generate_link_indices(self.num_fingers)
if target_link_human_indices is None:
target_link_human_indices = (np.stack([origin_link_index, task_link_index], axis=0) * 4).astype(int)
logical_indices = np.stack([origin_link_index, task_link_index], axis=0)
target_link_human_indices = np.where(
logical_indices > 0,
logical_indices * 5 - 1,
0
).astype(int)
link_names = [wrist_link_name] + finger_tip_link_names
target_origin_link_names = [link_names[index] for index in origin_link_index]
target_task_link_names = [link_names[index] for index in task_link_index]
@ -370,13 +375,13 @@ class DexPilotOptimizer(Optimizer):
origin_link_index = []
task_link_index = []
# Add indices for connections between fingers
# S1：Add indices for connections between fingers
for i in range(1, num_fingers):
for j in range(i + 1, num_fingers + 1):
origin_link_index.append(j)
task_link_index.append(i)
# Add indices for connections to the base (0)
# S2：Add indices for connections to the base (0)
for i in range(1, num_fingers + 1):
origin_link_index.append(0)
task_link_index.append(i)

52
teleop/robot_control/dex_retargeting/retargeting_config.py
View File

@ -19,32 +19,34 @@ from .yourdfpy import DUMMY_JOINT_NAMES
class RetargetingConfig:
type: str
urdf_path: str
target_joint_names: Optional[List[str]] = None
# Whether to add free joint to the root of the robot. Free joint enable the robot hand move freely in the space
add_dummy_free_joint: bool = False
# Source refers to the retargeting input, which usually corresponds to the human hand
# The joint indices of human hand joint which corresponds to each link in the target_link_names
target_link_human_indices: Optional[np.ndarray] = None
# DexPilot retargeting related
# The link on the robot hand which corresponding to the wrist of human hand
wrist_link_name: Optional[str] = None
# DexPilot retargeting link names
finger_tip_link_names: Optional[List[str]] = None
target_link_human_indices_dexpilot: Optional[np.ndarray] = None
# Position retargeting link names
target_link_names: Optional[List[str]] = None
target_link_human_indices_position: Optional[np.ndarray] = None
# Vector retargeting link names
target_joint_names: Optional[List[str]] = None
target_origin_link_names: Optional[List[str]] = None
target_task_link_names: Optional[List[str]] = None
# DexPilot retargeting link names
finger_tip_link_names: Optional[List[str]] = None
target_link_human_indices_vector: Optional[np.ndarray] = None
# 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: float = 1.0
# Low pass filter
low_pass_alpha: float = 0.1
# Optimization parameters
normal_delta: float = 4e-3
huber_delta: float = 2e-2
@ -59,9 +61,6 @@ class RetargetingConfig:
# Mimic joint tag
ignore_mimic_joint: bool = False
# Low pass filter
low_pass_alpha: float = 0.1
_TYPE = ["vector", "position", "dexpilot"]
_DEFAULT_URDF_DIR = "./"
@ -78,24 +77,24 @@ class RetargetingConfig:
raise ValueError(f"Vector retargeting requires: target_origin_link_names + target_task_link_names")
if len(self.target_task_link_names) != len(self.target_origin_link_names):
raise ValueError(f"Vector retargeting origin and task links dim mismatch")
if self.target_link_human_indices.shape != (2, len(self.target_origin_link_names)):
if self.target_link_human_indices_vector.shape != (2, len(self.target_origin_link_names)):
raise ValueError(f"Vector retargeting link names and link indices dim mismatch")
if self.target_link_human_indices is None:
raise ValueError(f"Vector retargeting requires: target_link_human_indices")
if self.target_link_human_indices_vector is None:
raise ValueError(f"Vector retargeting requires: target_link_human_indices_vector")
elif self.type == "position":
if self.target_link_names is None:
raise ValueError(f"Position retargeting requires: target_link_names")
self.target_link_human_indices = self.target_link_human_indices.squeeze()
if self.target_link_human_indices.shape != (len(self.target_link_names),):
self.target_link_human_indices_position = self.target_link_human_indices_position.squeeze()
if self.target_link_human_indices_position.shape != (len(self.target_link_names),):
raise ValueError(f"Position retargeting link names and link indices dim mismatch")
if self.target_link_human_indices is None:
raise ValueError(f"Position retargeting requires: target_link_human_indices")
if self.target_link_human_indices_position is None:
raise ValueError(f"Position retargeting requires: target_link_human_indices_position")
elif self.type == "dexpilot":
if self.finger_tip_link_names is None or self.wrist_link_name is None:
raise ValueError(f"Position retargeting requires: finger_tip_link_names + wrist_link_name")
if self.target_link_human_indices is not None:
if self.target_link_human_indices_dexpilot is not None:
print(
"\033[33m",
"Target link human indices is provided in the DexPilot retargeting config, which is uncommon.\n"
@ -132,8 +131,13 @@ class RetargetingConfig:
@classmethod
def from_dict(cls, cfg: Dict[str, Any], override: Optional[Dict] = None):
if "target_link_human_indices" in cfg:
cfg["target_link_human_indices"] = np.array(cfg["target_link_human_indices"])
if "target_link_human_indices_position" in cfg:
cfg["target_link_human_indices_position"] = np.array(cfg["target_link_human_indices_position"])
if "target_link_human_indices_vector" in cfg:
cfg["target_link_human_indices_vector"] = np.array(cfg["target_link_human_indices_vector"])
if "target_link_human_indices_dexpilot" in cfg:
cfg["target_link_human_indices_dexpilot"] = np.array(cfg["target_link_human_indices_dexpilot"])
if override is not None:
for key, value in override.items():
cfg[key] = value
@ -170,7 +174,7 @@ class RetargetingConfig:
robot,
joint_names,
target_link_names=self.target_link_names,
target_link_human_indices=self.target_link_human_indices,
target_link_human_indices=self.target_link_human_indices_position,
norm_delta=self.normal_delta,
huber_delta=self.huber_delta,
)
@ -180,7 +184,7 @@ class RetargetingConfig:
joint_names,
target_origin_link_names=self.target_origin_link_names,
target_task_link_names=self.target_task_link_names,
target_link_human_indices=self.target_link_human_indices,
target_link_human_indices=self.target_link_human_indices_vector,
scaling=self.scaling_factor,
norm_delta=self.normal_delta,
huber_delta=self.huber_delta,
@ -191,7 +195,7 @@ class RetargetingConfig:
joint_names,
finger_tip_link_names=self.finger_tip_link_names,
wrist_link_name=self.wrist_link_name,
target_link_human_indices=self.target_link_human_indices,
target_link_human_indices=self.target_link_human_indices_dexpilot,
scaling=self.scaling_factor,
project_dist=self.project_dist,
escape_dist=self.escape_dist,

2
teleop/robot_control/hand_retargeting.py
View File

@ -38,6 +38,8 @@ class HandRetargeting:
self.left_retargeting_joint_names = self.left_retargeting.joint_names
self.right_retargeting_joint_names = self.right_retargeting.joint_names
self.left_indices = self.left_retargeting.optimizer.target_link_human_indices
self.right_indices = self.right_retargeting.optimizer.target_link_human_indices
if hand_type == HandType.UNITREE_DEX3 or hand_type == HandType.UNITREE_DEX3_Unit_Test:
# In section "Sort by message structure" of https://support.unitree.com/home/en/G1_developer/dexterous_hand

11
teleop/robot_control/robot_hand_unitree.py
View File

@ -24,7 +24,6 @@ import logging_mp
logger_mp = logging_mp.get_logger(__name__)
unitree_tip_indices = [4, 9, 14] # [thumb, index, middle] in OpenXR
Dex3_Num_Motors = 7
kTopicDex3LeftCommand = "rt/dex3/left/cmd"
kTopicDex3RightCommand = "rt/dex3/right/cmd"
@ -183,14 +182,8 @@ class Dex3_1_Controller:
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[unitree_tip_indices]
ref_right_value = right_hand_data[unitree_tip_indices]
ref_left_value[0] = ref_left_value[0] * 1.15
ref_left_value[1] = ref_left_value[1] * 1.05
ref_left_value[2] = ref_left_value[2] * 0.95
ref_right_value[0] = ref_right_value[0] * 1.15
ref_right_value[1] = ref_right_value[1] * 1.05
ref_right_value[2] = ref_right_value[2] * 0.95
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.right_dex_retargeting_to_hardware]
right_q_target = self.hand_retargeting.right_retargeting.retarget(ref_right_value)[self.hand_retargeting.right_dex_retargeting_to_hardware]

Write Preview
Loading…
Cancel
Save