diff --git a/Makefile b/Makefile
index 9ded39e0..7e9c6829 100755
--- a/Makefile
+++ b/Makefile
@@ -43,13 +43,13 @@ play:
# ------------------------ #
install:
+ @pip install torch==2.3.1 torchvision==0.18.1 torchaudio==2.3.1 --index-url https://download.pytorch.org/whl/cu121
@pip install --verbose -e .
- @bash sim/scripts/download_assets.sh
.PHONY: install
install-dev:
+ @pip install torch==2.3.1 torchvision==0.18.1 torchaudio==2.3.1 --index-url https://download.pytorch.org/whl/cu121
@pip install --verbose -e '.[dev]'
- @bash sim/scripts/download_assets.sh
.PHONY: install
install-third-party:
diff --git a/examples/gpr_standing.kinfer b/examples/gpr_standing.kinfer
new file mode 100644
index 00000000..c280bff8
Binary files /dev/null and b/examples/gpr_standing.kinfer differ
diff --git a/examples/gpr_walking.kinfer b/examples/gpr_walking.kinfer
index 84fa052a..ea313640 100644
Binary files a/examples/gpr_walking.kinfer and b/examples/gpr_walking.kinfer differ
diff --git a/policy.pt b/policy.pt
deleted file mode 100644
index f483c38c..00000000
Binary files a/policy.pt and /dev/null differ
diff --git a/sim/envs/base/legged_robot.py b/sim/envs/base/legged_robot.py
index ea8d31d4..cb3bdf60 100644
--- a/sim/envs/base/legged_robot.py
+++ b/sim/envs/base/legged_robot.py
@@ -579,7 +579,7 @@ def _init_buffers(self):
if not found:
self.p_gains[:, i] = 0.0
self.d_gains[:, i] = 0.0
- print(f"PD gain of joint {name} were not defined, setting them to zero")
+ raise ValueError(f"PD gain of joint {name} were not defined, setting them to zero")
self.rand_push_force = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device)
self.rand_push_torque = torch.zeros((self.num_envs, 3), dtype=torch.float32, device=self.device)
diff --git a/sim/envs/humanoids/gpr_config.py b/sim/envs/humanoids/gpr_config.py
index 91d66d7f..c59372d3 100644
--- a/sim/envs/humanoids/gpr_config.py
+++ b/sim/envs/humanoids/gpr_config.py
@@ -51,8 +51,8 @@ class asset(LeggedRobotCfg.asset):
fix_base_link = False
class terrain(LeggedRobotCfg.terrain):
- mesh_type = "plane"
- # mesh_type = "trimesh"
+ # mesh_type = "plane"
+ mesh_type = "trimesh"
curriculum = False
# rough terrain only:
measure_heights = False
@@ -122,7 +122,7 @@ class domain_rand(LeggedRobotCfg.domain_rand):
friction_range = [0.1, 2.0]
randomize_base_mass = True
- added_mass_range = [-3.0, 3.0]
+ added_mass_range = [-2.0, 2.0]
push_robots = True
push_interval_s = 4
max_push_vel_xy = 0.2
@@ -152,7 +152,7 @@ class rewards:
target_joint_pos_scale = 0.17 # rad
target_feet_height = 0.05 # m
- cycle_time = 0.25 # sec
+ cycle_time = 0.4 # sec
# if true negative total rewards are clipped at zero (avoids early termination problems)
only_positive_rewards = True
# tracking reward = exp(error*sigma)
@@ -162,10 +162,10 @@ class rewards:
class scales:
# reference motion tracking
joint_pos = 1.6
- feet_clearance = 1.0
- feet_contact_number = 1.2
+ feet_clearance = 1.2
+ feet_contact_number = 1.4
# gait
- feet_air_time = 1.0
+ feet_air_time = 1.2
foot_slip = -0.05
feet_distance = 0.2
knee_distance = 0.2
@@ -186,8 +186,8 @@ class scales:
# energy
action_smoothness = -0.002
torques = -1e-5
- dof_vel = -5e-4
- dof_acc = -1e-7
+ dof_vel = -5e-4 # -1e-3
+ dof_acc = -1e-7 # -2.5e-7
collision = -1.0
class normalization:
@@ -211,9 +211,14 @@ class viewer:
class GprStandingCfg(GprCfg):
"""Configuration class for the GPR humanoid robot."""
+ class init_state(LeggedRobotCfg.init_state):
+ pos = [0.0, 0.0, Robot.standing_height]
+ rot = Robot.rotation
+ default_joint_angles = {k: 0.0 for k in Robot.all_joints()}
+
class rewards:
# quite important to keep it right
- base_height_target = Robot.height
+ base_height_target = Robot.standing_height
min_dist = 0.2
max_dist = 0.5
# put some settings here for LLM parameter tuning
@@ -221,10 +226,10 @@ class rewards:
target_feet_height = 0.05 # m
cycle_time = 0.5 # sec
# if true negative total rewards are clipped at zero (avoids early termination problems)
- only_positive_rewards = True
+ only_positive_rewards = False
# tracking reward = exp(error*sigma)
tracking_sigma = 5
- max_contact_force = 500 # forces above this value are penalized
+ max_contact_force = 200 # forces above this value are penalized
class scales:
# base pos
@@ -235,8 +240,8 @@ class scales:
# energy
action_smoothness = -0.002
torques = -1e-5
- dof_vel = -5e-4
- dof_acc = -1e-7
+ dof_vel = -1e-3
+ dof_acc = -2.5e-7
collision = -1.0
diff --git a/sim/requirements.txt b/sim/requirements.txt
index a4bf7761..fbbdc4e6 100755
--- a/sim/requirements.txt
+++ b/sim/requirements.txt
@@ -11,4 +11,5 @@ wandb
tensorboard==2.14.0
onnxscript
mujoco==2.3.6
-kinfer
+kinfer==0.0.5
+opencv-python
\ No newline at end of file
diff --git a/sim/resources/gpr/joints.py b/sim/resources/gpr/joints.py
index a5b6472c..3c4dd5d5 100755
--- a/sim/resources/gpr/joints.py
+++ b/sim/resources/gpr/joints.py
@@ -72,6 +72,7 @@ class Robot(Node):
legs = Legs()
height = 1.05
+ standing_height = 1.05 + 0.025
rotation = [0, 0, 0, 1]
@classmethod
@@ -106,7 +107,18 @@ def isaac_to_mujoco_signs(cls) -> Dict[str, int]:
@classmethod
def default_positions(cls) -> Dict[str, float]:
- return {}
+ return {
+ Robot.legs.left.hip_pitch: 0.0,
+ Robot.legs.left.hip_yaw: 0.0,
+ Robot.legs.left.hip_roll: 0.0,
+ Robot.legs.left.knee_pitch: 0.0,
+ Robot.legs.left.ankle_pitch: 0.0,
+ Robot.legs.right.hip_pitch: 0.0,
+ Robot.legs.right.hip_yaw: 0.0,
+ Robot.legs.right.hip_roll: 0.0,
+ Robot.legs.right.knee_pitch: 0.0,
+ Robot.legs.right.ankle_pitch: 0.0,
+ }
# CONTRACT - this should be ordered according to how the policy is trained.
# E.g. the first entry should be the angle of the first joint in the policy.
@@ -136,36 +148,36 @@ def default_limits(cls) -> Dict[str, Dict[str, float]]:
@classmethod
def stiffness(cls) -> Dict[str, float]:
return {
- "hip_y": 120,
- "hip_x": 60,
- "hip_z": 60,
- "knee": 120,
- "ankle_y": 17,
+ "hip_y": 300,
+ "hip_x": 120,
+ "hip_z": 120,
+ "knee": 300,
+ "ankle_y": 40,
}
# d_gains
@classmethod
def damping(cls) -> Dict[str, float]:
return {
- "hip_y": 10,
+ "hip_y": 5,
"hip_x": 5,
"hip_z": 5,
- "knee": 10,
+ "knee": 5,
"ankle_y": 5,
}
- # # pos_limits
+ # effort_limits
@classmethod
def effort(cls) -> Dict[str, float]:
return {
- "hip_y": 40,
- "hip_x": 20,
- "hip_z": 20,
- "knee": 40,
+ "hip_y": 60,
+ "hip_x": 40,
+ "hip_z": 40,
+ "knee": 60,
"ankle_y": 17,
}
- # # vel_limits
+ # vel_limits
@classmethod
def velocity(cls) -> Dict[str, float]:
return {"hip": 10, "knee": 10, "ankle": 10}
@@ -175,7 +187,7 @@ def friction(cls) -> Dict[str, float]:
return {
"hip": 0,
"knee": 0,
- "ankle": 0.01,
+ "ankle": 0.1,
}
diff --git a/sim/resources/gpr/robot_fixed.urdf b/sim/resources/gpr/robot_fixed.urdf
index c1e2aef7..904e5ec3 100644
--- a/sim/resources/gpr/robot_fixed.urdf
+++ b/sim/resources/gpr/robot_fixed.urdf
@@ -95,9 +95,9 @@
-
+
-
+
@@ -124,9 +124,9 @@
-
+
-
+
@@ -211,9 +211,9 @@
-
+
-
+
@@ -240,9 +240,9 @@
-
+
-
+
@@ -327,9 +327,9 @@
-
+
-
+
@@ -356,9 +356,9 @@
-
+
-
+
@@ -443,9 +443,9 @@
-
+
-
+
@@ -472,9 +472,9 @@
-
+
-
+
@@ -561,7 +561,7 @@
-
+
@@ -590,7 +590,7 @@
-
+
diff --git a/sim/resources/gpr/robot_fixed.xml b/sim/resources/gpr/robot_fixed.xml
index 9349bdd3..3bb52cbe 100644
--- a/sim/resources/gpr/robot_fixed.xml
+++ b/sim/resources/gpr/robot_fixed.xml
@@ -50,7 +50,7 @@
-
+
@@ -189,5 +189,6 @@
+
\ No newline at end of file
diff --git a/sim/scripts/create_fixed_torso.py b/sim/scripts/create_fixed_torso.py
index f93a4960..6cb9550b 100755
--- a/sim/scripts/create_fixed_torso.py
+++ b/sim/scripts/create_fixed_torso.py
@@ -54,6 +54,17 @@ def update_urdf(model_path: str, embodiment: str) -> None:
for key, value in friction.items():
if key in joint_name: # type: ignore[operator]
dynamics.set("friction", str(value))
+ else:
+ # Create and add new dynamics element
+ dynamics = ET.SubElement(joint, "dynamics")
+ dynamics.set("damping", "0.0")
+ # Set friction if exists for this joint
+ for key, value in friction.items():
+ if key in joint_name: # type: ignore[operator]
+ dynamics.set("friction", str(value))
+ break
+ else:
+ dynamics.set("friction", "0.0")
# Save the modified URDF to a new file
tree.write(Path(model_path) / "robot_fixed.urdf", xml_declaration=False)
diff --git a/sim/scripts/download_assets.sh b/sim/scripts/download_assets.sh
deleted file mode 100755
index 03ff11a4..00000000
--- a/sim/scripts/download_assets.sh
+++ /dev/null
@@ -1,32 +0,0 @@
-#!/bin/zsh
-
-echo "If any of the downloads fail, you can manually download the assets and place them in the respective directories."
-
-# Stompymicro
-echo
-echo "Downloading Stompymicro assets..."
-gdown --folder https://drive.google.com/drive/folders/1C_v0FKoc6um0tUK2f1e6cWXtfvuc-xsD -O sim/resources/stompymicro/
-
-# Xbot
-echo
-echo "Downloading Xbot assets..."
-gdown --id 1tpl95OdUhg9VL88FhKWMReY4qtLHHKoh
-tar -xzvf meshes.zip -C sim/resources/xbot/
-rm meshes.zip
-
-# Dora
-echo
-echo "Downloading Dora assets..."
-gdown --folder https://drive.google.com/drive/folders/1tQiMtOwGg3PGo9AygX3sj6X_HBpnW2S_ -O sim/resources/dora/
-
-# G1
-echo
-echo "Downloading G1 assets..."
-gdown --folder https://drive.google.com/drive/folders/1OxYcIJpeih89NY5auRayxCXpndrRnLJx -O sim/resources/g1/
-
-# H1_2
-echo
-echo "Downloading H1_2 assets..."
-gdown --id 19ih7zG6Ky8xJVJD5M1th2hmqtxaNiZyh
-tar -xzvf meshes.zip -C sim/resources/h1_2/
-rm meshes.zip