Skip to content

Commit

Permalink
[core] Replace controller telemetry prefix by 'controller'.
Browse files Browse the repository at this point in the history
  • Loading branch information
Alexis Duburcq committed Apr 12, 2024
1 parent 5cd242c commit 82c97fb
Show file tree
Hide file tree
Showing 12 changed files with 28 additions and 28 deletions.
2 changes: 1 addition & 1 deletion core/include/jiminy/core/control/abstract_controller.h
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
namespace jiminy
{
/// \brief Namespace of the telemetry object.
inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"HighLevelController"};
inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"controller"};

class TelemetrySender;
class TelemetryData;
Expand Down
4 changes: 2 additions & 2 deletions core/unit/engine_sanity_check.cc
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ TEST(EngineSanity, EnergyConservation)
std::shared_ptr<const LogData> logDataPtr = engine.getLog();
const Eigen::VectorXd timesCont = getLogVariable(*logDataPtr, "Global.Time");
ASSERT_DOUBLE_EQ(timesCont[timesCont.size() - 1], tf);
const Eigen::VectorXd energyCont = getLogVariable(*logDataPtr, "HighLevelController.energy");
const Eigen::VectorXd energyCont = getLogVariable(*logDataPtr, "controller.energy");
ASSERT_GT(energyCont.size(), 0);

// Check that energy is constant
Expand Down Expand Up @@ -140,7 +140,7 @@ TEST(EngineSanity, EnergyConservation)
logDataPtr = engine.getLog();
const Eigen::VectorXd timesDisc = getLogVariable(*logDataPtr, "Global.Time");
ASSERT_DOUBLE_EQ(timesDisc[timesDisc.size() - 1], tf);
const Eigen::VectorXd energyDisc = getLogVariable(*logDataPtr, "HighLevelController.energy");
const Eigen::VectorXd energyDisc = getLogVariable(*logDataPtr, "controller.energy");
ASSERT_GT(energyDisc.size(), 0);

// Check that energy is constant
Expand Down
4 changes: 2 additions & 2 deletions docs/spec/src/tlmc_format_specification.md
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ GROUP "/" {
(0): 1
}
}
DATASET "HighLevelController.controlOffsetTimestamp" {
DATASET "controller.controlOffsetTimestamp" {
DATATYPE H5T_STRING {
STRSIZE 8;
STRPAD H5T_STR_NULLPAD;
Expand All @@ -73,7 +73,7 @@ GROUP "/" {
...
}
GROUP "variables" {
GROUP "HighLevelController.currentPositionLeftSagittalHip" {
GROUP "controller.currentPositionLeftSagittalHip" {
DATASET "time" {
DATATYPE H5T_STD_I64LE
DATASPACE SIMPLE { ( 338623 ) / ( 338623 ) }
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -425,7 +425,7 @@ def compute_reward(self,
# perfectly symmetric wrt Y-axis over the whole trajectory.
if 'direction' in reward_mixture_keys:
frontal_displacement = abs(np.mean(self.log_data[
'HighLevelController.currentFreeflyerPositionTransY']))
'controller.currentFreeflyerPositionTransY']))
reward_dict['direction'] = - frontal_displacement

# Compute the total reward
Expand Down
8 changes: 4 additions & 4 deletions python/gym_jiminy/unit_py/test_pipeline_control.py
Original file line number Diff line number Diff line change
Expand Up @@ -93,14 +93,14 @@ def _test_pid_standing(self):
time = log_vars["Global.Time"]
velocity_target = np.stack([
log_vars['.'.join((
'HighLevelController', self.env.controller.name, name))]
'controller', self.env.controller.name, name))]
for name in self.env.controller.fieldnames], axis=-1)
self.assertTrue(np.all(
np.abs(velocity_target[time > time[-1] - 1.0]) < 1.0e-9))

# Check that the whole-body robot velocity is close to zero at the end
velocity_mes = np.stack([
log_vars['.'.join(('HighLevelController', name))]
log_vars['.'.join(('controller', name))]
for name in self.env.robot.log_velocity_fieldnames], axis=-1)
self.assertTrue(np.all(
np.abs(velocity_mes[time > time[-1] - 1.0]) < 1.0e-3))
Expand Down Expand Up @@ -202,9 +202,9 @@ def test_pid_controller(self):
ctrl_name = controller.name
n_motors = len(controller.fieldnames)
pos = env.log_data["variables"][".".join((
"HighLevelController", ctrl_name, "state", str(n_motors - 1)))]
"controller", ctrl_name, "state", str(n_motors - 1)))]
vel = env.log_data["variables"][".".join((
"HighLevelController", ctrl_name, controller.fieldnames[-1]))]
"controller", ctrl_name, controller.fieldnames[-1]))]

# Make sure that the position and velocity targets are consistent
np.testing.assert_allclose(
Expand Down
2 changes: 1 addition & 1 deletion python/gym_jiminy/unit_py/test_pipeline_design.py
Original file line number Diff line number Diff line change
Expand Up @@ -217,7 +217,7 @@ def configure_telemetry() -> InterfaceJiminyEnv:

# Check that the command is updated 1/2 low-level controller update
log_vars = env.log_data['variables']
u_log = log_vars['HighLevelController.currentCommandLF_HAA']
u_log = log_vars['controller.currentCommandLF_HAA']
self.assertEqual(env.control_dt, 2 * env.unwrapped.control_dt)
self.assertTrue(np.all(u_log[:2] == 0.0))
self.assertNotEqual(u_log[1], u_log[2])
Expand Down
2 changes: 1 addition & 1 deletion python/jiminy_py/examples/double_pendulum.py
Original file line number Diff line number Diff line change
Expand Up @@ -102,7 +102,7 @@ def internal_dynamics(self, t, q, v, u_custom):

# Plot some data using standard tools only
plt.figure()
plt.plot(log_vars['Global.Time'], log_vars['HighLevelController.energy'])
plt.plot(log_vars['Global.Time'], log_vars['controller.energy'])
plt.title('Double pendulum energy')
plt.grid()
plt.show()
Expand Down
2 changes: 1 addition & 1 deletion python/jiminy_py/examples/tutorial.ipynb
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,7 @@
"# Let's plot the joint position to see the pendulum fall.\n",
"import matplotlib.pyplot as plt\n",
"\n",
"plt.plot(log_vars['Global.Time'], log_vars['HighLevelController.currentPositionPendulum'])\n",
"plt.plot(log_vars['Global.Time'], log_vars['controller.currentPositionPendulum'])\n",
"plt.title('Pendulum angle (rad)')\n",
"plt.grid()\n",
"plt.show()"
Expand Down
12 changes: 6 additions & 6 deletions python/jiminy_py/src/jiminy_py/log.py
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@
@overload
def extract_variables_from_log(log_vars: Dict[str, np.ndarray],
fieldnames: FieldNested,
namespace: str = "HighLevelController",
namespace: str = "controller",
*, as_dict: Literal[False] = False
) -> List[np.ndarray]:
...
Expand All @@ -60,15 +60,15 @@ def extract_variables_from_log(log_vars: Dict[str, np.ndarray],
@overload
def extract_variables_from_log(log_vars: Dict[str, np.ndarray],
fieldnames: FieldNested,
namespace: str = "HighLevelController",
namespace: str = "controller",
*, as_dict: Literal[True]
) -> Dict[str, np.ndarray]:
...


def extract_variables_from_log(log_vars: Dict[str, np.ndarray],
fieldnames: FieldNested,
namespace: str = "HighLevelController",
namespace: str = "controller",
*, as_dict: bool = False
) -> Union[
List[np.ndarray], Dict[str, np.ndarray]]:
Expand All @@ -78,7 +78,7 @@ def extract_variables_from_log(log_vars: Dict[str, np.ndarray],
:param log_vars: Logged variables as a dictionary.
:param fieldnames: Structured fieldnames.
:param namespace: Namespace of the fieldnames. Empty string to disable.
Optional: "HighLevelController" by default.
Optional: "controller" by default.
:param keep_structure: Whether to return a dictionary mapping flattened
fieldnames to values.
Optional: True by default.
Expand Down Expand Up @@ -253,10 +253,10 @@ def build_robots_from_log(

robots_names = []
for key in log_constants.keys():
if key == "HighLevelController.has_freeflyer":
if key == "controller.has_freeflyer":
robots_names.append("")
else:
m = re.findall(r"HighLevelController.(\w+).has_freeflyer", key)
m = re.findall(r"controller.(\w+).has_freeflyer", key)
if len(m) == 1:
robots_names.append(m[0])

Expand Down
4 changes: 2 additions & 2 deletions python/jiminy_py/unit_py/test_dense_pole.py
Original file line number Diff line number Diff line change
Expand Up @@ -123,7 +123,7 @@ def test_flex_model(self):
q_flex = np.stack(extract_variables_from_log(log_vars, [
f"currentPosition{self.flex_joint_name}Quat{e}"
for e in ('X', 'Y', 'Z', 'W')
], 'HighLevelController'), axis=0)
], 'controller'), axis=0)
twist_flex = 2 * np.arctan2(q_flex[2], q_flex[3])
twist_flex_all.append(twist_flex)

Expand Down Expand Up @@ -210,7 +210,7 @@ def test_joint_position_limits(self):
(theta,) = extract_variables_from_log(
log_vars,
(f"currentPosition{self.robot.pinocchio_model.names[-1]}",),
'HighLevelController')
'controller')
theta_all.append(theta)

assert np.allclose(*theta_all, atol=1e-4)
Expand Down
6 changes: 3 additions & 3 deletions python/jiminy_py/unit_py/test_simple_mass.py
Original file line number Diff line number Diff line change
Expand Up @@ -144,7 +144,7 @@ def _test_collision_and_contact_dynamics(self, shape):

# Total energy and derivative
log_vars = engine.log_data["variables"]
E_robot = log_vars['HighLevelController.energy']
E_robot = log_vars['controller.energy']
E_contact = 1 / 2 * self.k_contact * penetration_depth ** 2
E_tot = E_robot + E_contact
E_diff_robot = np.concatenate((
Expand Down Expand Up @@ -279,7 +279,7 @@ def _test_friction_model(self, shape):
# viscous friction because of stiction phenomena.
log_vars = engine.log_data["variables"]
acceleration = log_vars[
'HighLevelController.currentFreeflyerAccelerationLinX']
'controller.currentFreeflyerAccelerationLinX']
jerk = np.diff(acceleration) / np.diff(time)
snap = np.diff(jerk) / np.diff(time[1:])
snap_rel = np.abs(snap / np.max(snap))
Expand All @@ -303,7 +303,7 @@ def _test_friction_model(self, shape):
# Check that the energy increases only when the force is applied
tolerance_E = 1e-9

E_robot = log_vars['HighLevelController.energy']
E_robot = log_vars['controller.energy']
E_diff_robot = np.concatenate((
np.diff(E_robot) / np.diff(time),
np.zeros((1,), dtype=E_robot.dtype)))
Expand Down
8 changes: 4 additions & 4 deletions python/jiminy_py/unit_py/utilities.py
Original file line number Diff line number Diff line change
Expand Up @@ -209,10 +209,10 @@ def simulate_and_get_state_evolution(
time = log_vars['Global.Time']
if isinstance(x0, np.ndarray):
q_jiminy = np.stack([
log_vars['.'.join(['HighLevelController', s])]
log_vars['.'.join(['controller', s])]
for s in engine.robots[0].log_position_fieldnames], axis=-1)
v_jiminy = np.stack([
log_vars['.'.join(['HighLevelController', s])]
log_vars['.'.join(['controller', s])]
for s in engine.robots[0].log_velocity_fieldnames], axis=-1)
if split:
return time, q_jiminy, v_jiminy
Expand All @@ -221,11 +221,11 @@ def simulate_and_get_state_evolution(
return time, x_jiminy
else:
q_jiminy = [np.stack([
log_vars['.'.join(['HighLevelController', robot.name, s])]
log_vars['.'.join(['controller', robot.name, s])]
for s in robot.log_position_fieldnames
], axis=-1) for robot in engine.robots]
v_jiminy = [np.stack([
log_vars['.'.join(['HighLevelController', robot.name, s])]
log_vars['.'.join(['controller', robot.name, s])]
for s in robot.log_velocity_fieldnames
], axis=-1) for robot in engine.robots]
if split:
Expand Down

0 comments on commit 82c97fb

Please sign in to comment.