Skip to content

Commit

Permalink
🎮 Add new CURI model
Browse files Browse the repository at this point in the history 
curi_isaacgym_dual_arm_w_softhand.urdf
  • Loading branch information
@Skylark0924
Skylark0924 committed Jan 14, 2024
1 parent 0c2c560 commit 068ff07
Show file tree
Hide file tree
Showing 4 changed files with 2,741 additions and 21 deletions.
2 changes: 1 addition & 1 deletion examples/learning_rl/IsaacGym_RofuncRL/example_DexterousHands_RofuncRL.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -101,7 +101,7 @@ def inference(custom_args):
# ShadowHandGrasp, CURIQbSoftHandSynergyGrasp
parser.add_argument("--task", type=str, default="CURIQbSoftHandSynergyGrasp")
parser.add_argument("--agent", type=str, default="ppo") # Available agents: ppo, sac, td3, a2c
parser.add_argument("--num_envs", type=int, default=4096)
parser.add_argument("--num_envs", type=int, default=256)
parser.add_argument("--sim_device", type=int, default=0)
parser.add_argument("--rl_device", type=int, default=gpu_id)
parser.add_argument("--headless", type=str, default="False")
Expand Down
3 changes: 2 additions & 1 deletion rofunc/config/simulator/CURI.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,8 @@ env:
robot_name: "CURI"
assetRoot:
# assetFile: "urdf/curi/urdf/curi_isaacgym_dual_arm.urdf"
assetFile: "urdf/curi/urdf/curi_isaacgym_fixed_torso.urdf"
assetFile: "urdf/curi/urdf/curi_isaacgym_dual_arm_w_softhand.urdf"
# assetFile: "urdf/curi/urdf/curi_w_softhand_isaacgym.urdf"
init_pose: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]
fix_base_link: False
disable_gravity: False
Expand Down
39 changes: 20 additions & 19 deletions rofunc/learning/RofuncRL/tasks/isaacgymenv/hands/curi_qbhand_synergy_grasp.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ def __init__(self, cfg, rl_device, sim_device, graphics_device_id, headless, vir
# <editor-fold desc="obs space">
self.num_point_cloud_feature_dim = 768

self.num_hand_obs = 91*3 + 6 + 8
self.num_hand_obs = 95 * 3 + 6 + 8
num = 13 + self.num_hand_obs
self.num_obs_dict = {
"point_cloud": num + self.num_point_cloud_feature_dim * 3,
Expand All @@ -115,6 +115,9 @@ def __init__(self, cfg, rl_device, sim_device, graphics_device_id, headless, vir
self.fingertips = ["left_qbhand_thumb_distal_link", "left_qbhand_index_distal_link",
"left_qbhand_middle_distal_link", "left_qbhand_ring_distal_link",
"left_qbhand_little_distal_link"]
# self.fingertips = ["qbhand_thumb_distal_link", "qbhand_index_distal_link",
# "qbhand_middle_distal_link", "qbhand_ring_distal_link",
# "qbhand_little_distal_link"]

self.num_fingertips = len(self.fingertips)

Expand Down Expand Up @@ -164,8 +167,7 @@ def __init__(self, cfg, rl_device, sim_device, graphics_device_id, headless, vir
# get gym GPU state tensors
actor_root_state_tensor = self.gym.acquire_actor_root_state_tensor(self.sim)
dof_state_tensor = self.gym.acquire_dof_state_tensor(self.sim)
rigid_body_tensor = self.gym.acquire_rigid_body_state_tensor(
self.sim) # 38 = self.num_hand_bodies * 2 + 2 + 1 + 2 + 1
rigid_body_tensor = self.gym.acquire_rigid_body_state_tensor(self.sim) # 93 = 91 + 1 + 1

dof_force_tensor = self.gym.acquire_dof_force_tensor(self.sim)
self.dof_force_tensor = gymtorch.wrap_tensor(dof_force_tensor).view(self.num_envs,
Expand Down Expand Up @@ -256,7 +258,7 @@ def _create_envs(self, num_envs, spacing, num_per_row):
# get rofunc path from rofunc package metadata
rofunc_path = get_rofunc_path()
asset_root = os.path.join(rofunc_path, "simulator/assets")
hand_asset_file = "urdf/curi/urdf/curi_w_softhand_isaacgym.urdf"
hand_asset_file = "urdf/curi/urdf/curi_isaacgym_dual_arm_w_softhand.urdf"
table_texture_files = os.path.join(asset_root, "textures/dark-wood.png")
table_texture_handle = self.gym.create_texture_from_file(self.sim, table_texture_files)

Expand All @@ -267,7 +269,7 @@ def _create_envs(self, num_envs, spacing, num_per_row):

# <editor-fold desc="load hand asset and set hand dof properties">
asset_options = gymapi.AssetOptions()
asset_options.flip_visual_attachments = False
asset_options.flip_visual_attachments = True
asset_options.fix_base_link = False
asset_options.collapse_fixed_joints = True
asset_options.disable_gravity = True
Expand Down Expand Up @@ -381,7 +383,7 @@ def _create_envs(self, num_envs, spacing, num_per_row):
# <editor-fold desc="set initial poses">
hand_start_pose = gymapi.Transform()
hand_start_pose.p = gymapi.Vec3(-1.5, 0.0, 0.0)
hand_start_pose.r = gymapi.Quat().from_euler_zyx(3.14, 0, 0)
hand_start_pose.r = gymapi.Quat().from_euler_zyx(0, 0, 3.14)

object_start_pose = gymapi.Transform()
object_start_pose.p = gymapi.Vec3(0.0, 0, 0.7)
Expand Down Expand Up @@ -462,21 +464,20 @@ def _create_envs(self, num_envs, spacing, num_per_row):
self.hand_indices.append(hand_idx)
self.gym.set_actor_scale(env_ptr, hand_actor, 1)


# <editor-fold desc="randomize colors and textures for rigid body">
num_bodies = self.gym.get_actor_rigid_body_count(env_ptr, hand_actor)
hand_rigid_body_index = [[0], [i for i in range(1, 6)], [i for i in range(6, 13)],
[i for i in range(13, 20)],
[i for i in range(20, 27)], [i for i in range(27, 34)]]

for n in self.agent_index[0]:
colorx = random.uniform(0, 1)
colory = random.uniform(0, 1)
colorz = random.uniform(0, 1)
for m in n:
for o in hand_rigid_body_index[m]:
self.gym.set_rigid_body_color(env_ptr, hand_actor, o, gymapi.MESH_VISUAL,
gymapi.Vec3(colorx, colory, colorz))
# for n in self.agent_index[0]:
# colorx = random.uniform(0, 1)
# colory = random.uniform(0, 1)
# colorz = random.uniform(0, 1)
# for m in n:
# for o in hand_rigid_body_index[m]:
# self.gym.set_rigid_body_color(env_ptr, hand_actor, o, gymapi.MESH_VISUAL,
# gymapi.Vec3(colorx, colory, colorz))
# </editor-fold>

# add object
Expand Down Expand Up @@ -601,8 +602,8 @@ def compute_observations(self):
self.object_angvel = self.root_state_tensor[self.object_indices, 10:13]

# <editor-fold desc="hand poses">
self.hand_pos = self.rigid_body_states[:, 0, 0:3]
self.hand_rot = self.rigid_body_states[:, 0, 3:7]
self.hand_pos = self.rigid_body_states[:, 15, 0:3]
self.hand_rot = self.rigid_body_states[:, 15, 3:7]
self.hand_pos = self.hand_pos + quat_apply(self.hand_rot,
to_torch([0, 0, 1], device=self.device).repeat(
self.num_envs, 1) * 0.08)
Expand Down Expand Up @@ -1318,9 +1319,9 @@ def compute_hand_reward(
# # reward = torch.exp(-0.1*(right_hand_dist_rew * dist_reward_scale)) + torch.exp(-0.1*(left_hand_dist_rew * dist_reward_scale))

up_rew = object_pos[:, 2] - 0.7
up_rew = torch.where(up_rew > 0, up_rew, torch.zeros_like(up_rew))* 100
up_rew = torch.where(up_rew > 0, up_rew, torch.zeros_like(up_rew)) * 100
# print("Max height: {}, Avg height: {}".format(torch.max(object_pos[:, 2]), torch.mean(object_pos[:, 2])))
reward = up_rew - synergy_dist - hand_dist
reward = up_rew - synergy_dist - hand_dist
# print("up_rew: {}, synergy_rew: {}, hand_dist: {}".format(float(torch.mean(up_rew).cpu()),
# float(torch.mean(synergy_dist).cpu()),
# float(torch.mean(hand_dist).cpu())))
Expand Down
Loading

0 comments on commit 068ff07

Please sign in to comment.