Skip to content

Commit

Permalink
scale fsr sensors
Browse files Browse the repository at this point in the history
  • Loading branch information
@schmidma
schmidma committed Jan 29, 2025
1 parent 85eecd1 commit d4b765f
Show file tree
Hide file tree
Showing 5 changed files with 35 additions and 34 deletions.
15 changes: 10 additions & 5 deletions tools/machine-learning/mujoco/packages/nao_env/src/nao_env/nao_base_env.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,7 +78,13 @@ class NaoBaseEnv(MujocoEnv):
"render_fps": 83,
}

def __init__(self, *, throw_tomatoes: bool = False, **kwargs: Any) -> None:
def __init__(
self,
*,
throw_tomatoes: bool = False,
fsr_scale: float = 0.019,
**kwargs: Any,
) -> None:
observation_space = Box(
low=-np.inf,
high=np.inf,
Expand All @@ -103,6 +109,7 @@ def __init__(self, *, throw_tomatoes: bool = False, **kwargs: Any) -> None:
)
self._actuation_mask = self._get_actuation_mask()
self.action_space_size = len(ACTUATOR_NAMES)
self.nao = Nao(self.model, self.data, fsr_scale=fsr_scale)

def _get_actuation_mask(self) -> NDArray[np.bool_]:
actuation_mask = np.zeros(self.model.nu, dtype=np.bool_)
Expand All @@ -124,10 +131,8 @@ def _set_action_space(self) -> Box:
return self.action_space

def _get_obs(self) -> NDArray[np.floating]:
nao = Nao(self.model, self.data)

force_sensing_resistors_right = nao.right_fsr_values().sum()
force_sensing_resistors_left = nao.left_fsr_values().sum()
force_sensing_resistors_right = self.nao.right_fsr_values().sum()
force_sensing_resistors_left = self.nao.left_fsr_values().sum()

sensors = np.concatenate(
[
Expand Down
7 changes: 2 additions & 5 deletions tools/machine-learning/mujoco/packages/nao_env/src/nao_env/nao_standing.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import numpy as np
from gymnasium import utils
from nao_interface.nao_interface import Nao
from nao_interface.poses import PENALIZED_POSE
from numpy.typing import NDArray
from rewards import (
Expand Down Expand Up @@ -70,7 +69,6 @@ def __init__(
@override
def step(self, action: NDArray[np.floating]) -> tuple:
self.current_step += 1
nao = Nao(self.model, self.data)

if self.throw_tomatoes and self.projectile.has_ground_contact():
target = self.data.site("Robot").xpos
Expand All @@ -90,7 +88,7 @@ def step(self, action: NDArray[np.floating]) -> tuple:

terminated = head_center_z < 0.3

distinct_rewards = self.reward.rewards(RewardContext(nao, action))
distinct_rewards = self.reward.rewards(RewardContext(self.nao, action))
reward = sum(distinct_rewards.values())

if terminated:
Expand All @@ -112,6 +110,5 @@ def reset_model(self) -> NDArray[np.floating]:
self.init_qpos,
self.init_qvel,
)
nao = Nao(self.model, self.data)
nao.reset(PENALIZED_POSE)
self.nao.reset(PENALIZED_POSE)
return self._get_obs()
9 changes: 3 additions & 6 deletions tools/machine-learning/mujoco/packages/nao_env/src/nao_env/nao_standup.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,6 @@

import numpy as np
from gymnasium import utils
from nao_interface import Nao
from nao_interface.poses import PENALIZED_POSE
from numpy.typing import NDArray
from rewards import (
Expand Down Expand Up @@ -32,9 +31,8 @@ def __init__(self, *, throw_tomatoes: bool = False, **kwargs: Any) -> None:
@override
def step(self, action: NDArray[np.floating]) -> tuple:
self.do_simulation(action, self.frame_skip)
nao = Nao(self.model, self.data)

distinct_rewards = self.reward.rewards(RewardContext(nao, action))
distinct_rewards = self.reward.rewards(RewardContext(self.nao, action))
reward = sum(distinct_rewards.values())

if self.render_mode == "human":
Expand All @@ -48,9 +46,8 @@ def reset_model(self) -> NDArray[np.floating]:
self.init_qpos,
self.init_qvel,
)
nao = Nao(self.model, self.data)
nao.reset(PENALIZED_POSE)
nao.set_transform(
self.nao.reset(PENALIZED_POSE)
self.nao.set_transform(
np.array([-0.13252355, -0.0909888, 0.05897925]),
np.array([0.69360432, 0.13973604, -0.692682, 0.13992331]),
)
Expand Down
26 changes: 11 additions & 15 deletions tools/machine-learning/mujoco/packages/nao_env/src/nao_env/nao_walking.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,6 @@ def __init__(self, *, throw_tomatoes: bool, **kwargs: Any) -> None:

@override
def step(self, action: NDArray[np.floating]) -> tuple:
nao = Nao(self.model, self.data)
robot_position = self.data.site("Robot").xpos

if self.projectile.has_ground_contact() and self.throw_tomatoes:
Expand All @@ -137,7 +136,7 @@ def step(self, action: NDArray[np.floating]) -> tuple:
self.render()

distinct_rewards = self.reward.rewards(
RewardContext(nao, action, self.state)
RewardContext(self.nao, action, self.state)
)
reward = sum(distinct_rewards.values())

Expand All @@ -156,13 +155,11 @@ def do_simulation(
ctrl: NDArray[np.floating],
n_frames: int,
) -> None:
nao = Nao(self.model, self.data)

right_pressure = nao.right_fsr_values().sum()
left_pressure = nao.left_fsr_values().sum()
right_pressure = self.nao.right_fsr_values().sum()
left_pressure = self.nao.left_fsr_values().sum()

measurements = Measurements(left_pressure, right_pressure)
nao.data.ctrl[:] = OFFSET_QPOS
self.nao.data.ctrl[:] = OFFSET_QPOS

if self.enable_walking and (
measurements.pressure_left > 0.0
Expand All @@ -175,15 +172,15 @@ def do_simulation(
left=0.0,
)
apply_walking(
nao,
self.nao,
parameters=self.parameters,
state=self.state,
measurements=measurements,
control=control,
dt=dt,
)
nao.data.ctrl[self._actuation_mask] += ctrl
self._step_mujoco_simulation(nao.data.ctrl, n_frames)
self.nao.data.ctrl[self._actuation_mask] += ctrl
self._step_mujoco_simulation(self.nao.data.ctrl, n_frames)

@override
def reset_model(self) -> NDArray[np.floating]:
Expand All @@ -194,16 +191,15 @@ def reset_model(self) -> NDArray[np.floating]:
self.init_qpos,
self.init_qvel,
)
nao = Nao(self.model, self.data)
nao.reset(READY_POSE)
self.nao.reset(READY_POSE)

measurements = Measurements(
nao.left_fsr_values().sum(),
nao.right_fsr_values().sum(),
self.nao.left_fsr_values().sum(),
self.nao.right_fsr_values().sum(),
)

apply_walking(
nao,
self.nao,
self.parameters,
self.state,
measurements,
Expand Down
12 changes: 9 additions & 3 deletions tools/machine-learning/mujoco/packages/nao_interface/src/nao_interface/nao_interface.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -198,7 +198,12 @@ def from_dict(self, values: dict) -> None:


class Nao:
def __init__(self, model: mujoco.MjModel, data: mujoco.MjData) -> None:
def __init__(
self,
model: mujoco.MjModel,
data: mujoco.MjData,
fsr_scale: float = 1.0,
) -> None:
self.model = model
self.data = data
self.actuators = NaoJoints(
Expand All @@ -214,6 +219,7 @@ def __init__(self, model: mujoco.MjModel, data: mujoco.MjData) -> None:
value,
),
)
self.fsr_scale = fsr_scale

def set_transform(
self, position: NDArray[np.floating], quaternion: NDArray[np.floating]
Expand All @@ -235,7 +241,7 @@ def reset(self, positions: dict[str, dict[str, float]]) -> None:
mujoco.mj_forward(self.model, self.data)

def left_fsr_values(self) -> NDArray[np.floating]:
return np.array(
return self.fsr_scale * np.array(
[
self.data.sensor(
"force_sensitive_resistors.left.front_left",
Expand All @@ -253,7 +259,7 @@ def left_fsr_values(self) -> NDArray[np.floating]:
)

def right_fsr_values(self) -> NDArray[np.floating]:
return np.array(
return self.fsr_scale * np.array(
[
self.data.sensor(
"force_sensitive_resistors.right.front_left",
Expand Down

0 comments on commit d4b765f

Please sign in to comment.