Skip to content

Commit

Permalink
current version
Browse files Browse the repository at this point in the history
  • Loading branch information
@is2ac2
is2ac2 committed Jul 18, 2024
1 parent a8d2481 commit fa8be99
Show file tree
Hide file tree
Showing 5 changed files with 84 additions and 81 deletions.
2 changes: 1 addition & 1 deletion sim/deploy/policy.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ def pd_control(
# breakpoint()
res = ( (target_dof_pos - dof_pos) * kp + (target_dof_vel - dof_vel) * kd )
# breakpoint()
return res * 0.75
return res * 0.0025

def parse_action(
self, dof_pos: np.ndarray, dof_vel: np.ndarray, eu_ang: np.ndarray, ang_vel: np.ndarray, count_lowlevel: int
Expand Down
16 changes: 9 additions & 7 deletions sim/deploy/run.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -88,7 +88,7 @@ def step(
dq: The current joint velocities.
target_dq: The target joint velocities (optional).
"""
# tau = np.clip(tau, -self.cfg.tau_limit, self.cfg.tau_limit)
tau = np.clip(tau, -self.cfg.tau_limit, self.cfg.tau_limit)
self.data.ctrl = tau
mujoco.mj_step(self.model, self.data)

Expand Down Expand Up @@ -136,6 +136,8 @@ def simulate(self, policy: SimPolicy) -> None:
# target_dof_pos = np.zeros_like(target_dof_pos)
tau = policy.pd_control(target_dof_pos, dof_pos, cfg.kps, dof_vel, cfg.kds)
# breakpoint()
if step % 1000 == 0:
breakpoint()
# set tau to zero for now
# tau = np.zeros_like(tau)
self.step(tau=tau)
Expand Down Expand Up @@ -273,12 +275,12 @@ def main(args: argparse.Namespace, cfg: RobotConfig) -> None:
num_single_obs = dof * 3 + 11

# is2ac, lets scale kps and kds to be the same as our stiffness and dmaping
kps = np.ones((dof), dtype=np.double) * 0.00001 * 25
kds = np.ones((dof), dtype=np.double) * 1
tau_limit = np.ones((dof), dtype=np.double) * 0.00001 * 25
# kps = np.ones((dof), dtype=np.double) * 200
# kds = np.ones((dof), dtype=np.double) * 10
# tau_limit = np.ones((dof), dtype=np.double) * 200
# kps = np.ones((dof), dtype=np.double)
# kds = np.ones((dof), dtype=np.double) * 1
# tau_limit = np.ones((dof), dtype=np.double)
kps = np.ones((dof), dtype=np.double) * 200
kds = np.ones((dof), dtype=np.double) * 10
tau_limit = np.ones((dof), dtype=np.double) * 200

cfg = RobotConfig(
robot_model_path=robot_path, dof=dof, kps=kps, kds=kds, tau_limit=tau_limit, num_single_obs=num_single_obs
Expand Down
43 changes: 21 additions & 22 deletions sim/humanoid_gym/envs/only_legs_config.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,10 +117,10 @@ class control(LeggedRobotCfg.control):
"ankle": 10,
"knee": 10,
}
for k in stiffness:
stiffness[k] *= 0.00001
damping[k] *= 0.1
action_scale = 2500
# for k in stiffness:
# stiffness[k] *= 0.00001
# damping[k] *= 0.1
action_scale = 0.25
# decimation: Number of control action updates @ sim DT per policy DT
decimation = 10 # 100hz

Expand All @@ -135,7 +135,7 @@ class physx(LeggedRobotCfg.sim.physx):
num_position_iterations = 4
num_velocity_iterations = 1
contact_offset = 0.01 # [m]
rest_offset = 0.0 # -0.02 # [m]
rest_offset = 0 # -0.02 # [m]
bounce_threshold_velocity = 0.1 # [m/s]
max_depenetration_velocity = 1.0
max_gpu_contact_pairs = 2**23 # 2**24 -> needed for 8000 envs and more
Expand Down Expand Up @@ -190,22 +190,22 @@ class rewards:

class scales:
# reference motion tracking
# joint_pos = 1.6
# feet_clearance = 1.0
# feet_contact_number = 1.2
# # gait
# feet_air_time = 1.0
# foot_slip = -0.05
# feet_distance = 0.2
# knee_distance = 0.2
# # contact
# feet_contact_forces = -0.01
# # vel tracking
# tracking_lin_vel = 1.2
# tracking_ang_vel = 1.1
# vel_mismatch_exp = 0.5 # lin_z; ang x,y
# low_speed = 0.2
# track_vel_hard = 0.5
joint_pos = 1.6
feet_clearance = 1.0
feet_contact_number = 1.2
# gait
feet_air_time = 1.0
foot_slip = -0.05
feet_distance = 0.2
knee_distance = 0.2
# contact
feet_contact_forces = -0.01
# vel tracking
tracking_lin_vel = 1.2
tracking_ang_vel = 1.1
vel_mismatch_exp = 0.5 # lin_z; ang x,y
low_speed = 0.2
track_vel_hard = 0.5

# above this was removed for standing policy
# base pos
Expand All @@ -223,7 +223,6 @@ class scales:

class normalization:
class obs_scales:
# is2ac
lin_vel = 2.0
ang_vel = 1.0
dof_pos = 1.0
Expand Down
46 changes: 23 additions & 23 deletions sim/stompy_legs/joints.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,31 +86,31 @@ def default_positions(cls) -> Dict[str, float]:
def default_standing(cls) -> Dict[str, float]:
return {
# legs
Stompy.legs.right.hip_pitch: 0,
Stompy.legs.right.hip_yaw: -2.15,
Stompy.legs.right.hip_roll: -1.6,
Stompy.legs.right.knee_pitch: 2.16,
Stompy.legs.right.ankle_pitch: 0.5,
Stompy.legs.right.ankle_roll: 1.72,
Stompy.legs.left.hip_pitch: 1.61,
Stompy.legs.left.hip_yaw: 1,
Stompy.legs.left.hip_roll: 0,
Stompy.legs.left.knee_pitch: 2.05,
Stompy.legs.left.ankle_pitch: 0.33,
Stompy.legs.left.ankle_roll: 1.73,
# legs in squat
# Stompy.legs.left.hip_pitch: 1.17,
# Stompy.legs.left.hip_roll: 0.1,
# Stompy.legs.left.hip_yaw: 1.03,
# Stompy.legs.left.knee_pitch: 1.13,
# Stompy.legs.left.ankle_pitch: -0.27,
# Stompy.legs.left.ankle_roll: 1.73,
# Stompy.legs.right.hip_pitch: 0.38,
# Stompy.legs.right.hip_roll: -1.6,
# Stompy.legs.right.hip_pitch: 0,
# Stompy.legs.right.hip_yaw: -2.15,
# Stompy.legs.right.knee_pitch: 2.94,
# Stompy.legs.right.ankle_pitch: 0.8,
# Stompy.legs.right.hip_roll: -1.6,
# Stompy.legs.right.knee_pitch: 2.16,
# Stompy.legs.right.ankle_pitch: 0.5,
# Stompy.legs.right.ankle_roll: 1.72,
# Stompy.legs.left.hip_pitch: 1.61,
# Stompy.legs.left.hip_yaw: 1,
# Stompy.legs.left.hip_roll: 0,
# Stompy.legs.left.knee_pitch: 2.05,
# Stompy.legs.left.ankle_pitch: 0.33,
# Stompy.legs.left.ankle_roll: 1.73,
# legs in squat
Stompy.legs.left.hip_pitch: 1.17,
Stompy.legs.left.hip_roll: 0.1,
Stompy.legs.left.hip_yaw: 1.03,
Stompy.legs.left.knee_pitch: 1.13,
Stompy.legs.left.ankle_pitch: -0.27,
Stompy.legs.left.ankle_roll: 1.73,
Stompy.legs.right.hip_pitch: 0.38,
Stompy.legs.right.hip_roll: -1.6,
Stompy.legs.right.hip_yaw: -2.15,
Stompy.legs.right.knee_pitch: 2.94,
Stompy.legs.right.ankle_pitch: 0.8,
Stompy.legs.right.ankle_roll: 1.72,
}

@classmethod
Expand Down
Loading

0 comments on commit fa8be99

Please sign in to comment.