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
Unified 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: left:
type: vector
type: DexPilot # or vector
urdf_path: unitree_hand/unitree_dex3_left.urdf urdf_path: unitree_hand/unitree_dex3_left.urdf
# Target refers to the retargeting target, which is the robot hand # Target refers to the retargeting target, which is the robot hand
# target_joint_names:
target_joint_names: target_joint_names:
[ [
"left_hand_thumb_0_joint", "left_hand_thumb_0_joint",
@ -14,27 +13,26 @@ left:
"left_hand_index_0_joint", "left_hand_index_0_joint",
"left_hand_index_1_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_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"]
target_task_link_names: ["thumb_tip","index_tip","middle_tip"] 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 scaling_factor: 1.0
# A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
low_pass_alpha: 0.2 low_pass_alpha: 0.2
right: right:
type: vector
type: DexPilot # or vector
urdf_path: unitree_hand/unitree_dex3_right.urdf urdf_path: unitree_hand/unitree_dex3_right.urdf
# Target refers to the retargeting target, which is the robot hand # Target refers to the retargeting target, which is the robot hand
@ -48,12 +46,18 @@ right:
"right_hand_middle_0_joint", "right_hand_middle_0_joint",
"right_hand_middle_1_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_origin_link_names: ["base_link_thumb","base_link_index","base_link_middle"]
target_task_link_names: ["thumb_tip", "index_tip", "middle_tip"] 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 scaling_factor: 1.0
# A smaller alpha means stronger filtering, i.e. more smooth but also larger latency # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
low_pass_alpha: 0.2 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) origin_link_index, task_link_index = self.generate_link_indices(self.num_fingers)
if target_link_human_indices is None: 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 link_names = [wrist_link_name] + finger_tip_link_names
target_origin_link_names = [link_names[index] for index in origin_link_index] 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] target_task_link_names = [link_names[index] for index in task_link_index]
@ -370,13 +375,13 @@ class DexPilotOptimizer(Optimizer):
origin_link_index = [] origin_link_index = []
task_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 i in range(1, num_fingers):
for j in range(i + 1, num_fingers + 1): for j in range(i + 1, num_fingers + 1):
origin_link_index.append(j) origin_link_index.append(j)
task_link_index.append(i) 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): for i in range(1, num_fingers + 1):
origin_link_index.append(0) origin_link_index.append(0)
task_link_index.append(i) 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: class RetargetingConfig:
type: str type: str
urdf_path: 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 # 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 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 # The link on the robot hand which corresponding to the wrist of human hand
wrist_link_name: Optional[str] = None 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 # Position retargeting link names
target_link_names: Optional[List[str]] = None target_link_names: Optional[List[str]] = None
target_link_human_indices_position: Optional[np.ndarray] = None
# Vector retargeting link names # Vector retargeting link names
target_joint_names: Optional[List[str]] = None
target_origin_link_names: Optional[List[str]] = None target_origin_link_names: Optional[List[str]] = None
target_task_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 # 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 # 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 scaling_factor: float = 1.0
# Low pass filter
low_pass_alpha: float = 0.1
# Optimization parameters # Optimization parameters
normal_delta: float = 4e-3 normal_delta: float = 4e-3
huber_delta: float = 2e-2 huber_delta: float = 2e-2
@ -59,9 +61,6 @@ class RetargetingConfig:
# Mimic joint tag # Mimic joint tag
ignore_mimic_joint: bool = False ignore_mimic_joint: bool = False
# Low pass filter
low_pass_alpha: float = 0.1
_TYPE = ["vector", "position", "dexpilot"] _TYPE = ["vector", "position", "dexpilot"]
_DEFAULT_URDF_DIR = "./" _DEFAULT_URDF_DIR = "./"
@ -78,24 +77,24 @@ class RetargetingConfig:
raise ValueError(f"Vector retargeting requires: target_origin_link_names + target_task_link_names") 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): if len(self.target_task_link_names) != len(self.target_origin_link_names):
raise ValueError(f"Vector retargeting origin and task links dim mismatch") 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") 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": elif self.type == "position":
if self.target_link_names is None: if self.target_link_names is None:
raise ValueError(f"Position retargeting requires: target_link_names") 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") 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": elif self.type == "dexpilot":
if self.finger_tip_link_names is None or self.wrist_link_name is None: 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") 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( print(
"\033[33m", "\033[33m",
"Target link human indices is provided in the DexPilot retargeting config, which is uncommon.\n" "Target link human indices is provided in the DexPilot retargeting config, which is uncommon.\n"
@ -132,8 +131,13 @@ class RetargetingConfig:
@classmethod @classmethod
def from_dict(cls, cfg: Dict[str, Any], override: Optional[Dict] = None): 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: if override is not None:
for key, value in override.items(): for key, value in override.items():
cfg[key] = value cfg[key] = value
@ -170,7 +174,7 @@ class RetargetingConfig:
robot, robot,
joint_names, joint_names,
target_link_names=self.target_link_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, norm_delta=self.normal_delta,
huber_delta=self.huber_delta, huber_delta=self.huber_delta,
) )
@ -180,7 +184,7 @@ class RetargetingConfig:
joint_names, joint_names,
target_origin_link_names=self.target_origin_link_names, target_origin_link_names=self.target_origin_link_names,
target_task_link_names=self.target_task_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, scaling=self.scaling_factor,
norm_delta=self.normal_delta, norm_delta=self.normal_delta,
huber_delta=self.huber_delta, huber_delta=self.huber_delta,
@ -191,7 +195,7 @@ class RetargetingConfig:
joint_names, joint_names,
finger_tip_link_names=self.finger_tip_link_names, finger_tip_link_names=self.finger_tip_link_names,
wrist_link_name=self.wrist_link_name, 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, scaling=self.scaling_factor,
project_dist=self.project_dist, project_dist=self.project_dist,
escape_dist=self.escape_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.left_retargeting_joint_names = self.left_retargeting.joint_names
self.right_retargeting_joint_names = self.right_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: 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 # 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__) logger_mp = logging_mp.get_logger(__name__)
unitree_tip_indices = [4, 9, 14] # [thumb, index, middle] in OpenXR
Dex3_Num_Motors = 7 Dex3_Num_Motors = 7
kTopicDex3LeftCommand = "rt/dex3/left/cmd" kTopicDex3LeftCommand = "rt/dex3/left/cmd"
kTopicDex3RightCommand = "rt/dex3/right/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[:]))) 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. 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] 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] 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