From f1168c4b73d7264cde3d2cb1200f7146655c0220 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Fri, 12 Apr 2024 19:20:49 +0200 Subject: [PATCH] [core] Remove irrelevant telemetry prefix. Replace controller telemetry prefix by 'controller'. --- .../double_pendulum/double_pendulum.cc | 1 + .../external_project/double_pendulum.cc | 1 + .../jiminy/core/control/abstract_controller.h | 2 +- core/include/jiminy/core/engine/engine.h | 2 +- core/include/jiminy/core/utilities/helpers.h | 12 +-- core/src/engine/engine.cc | 19 +++-- core/src/robot/robot.cc | 22 +++--- core/unit/engine_sanity_check.cc | 4 +- docs/spec/src/tlmc_format_specification.md | 4 +- .../common/gym_jiminy/common/envs/generic.py | 20 ++--- .../gym_jiminy/common/envs/locomotion.py | 4 +- .../unit_py/test_pipeline_control.py | 11 ++- .../unit_py/test_pipeline_design.py | 2 +- python/jiminy_py/examples/double_pendulum.py | 2 +- python/jiminy_py/examples/tutorial.ipynb | 2 +- python/jiminy_py/src/jiminy_py/log.py | 75 +++++++++---------- python/jiminy_py/src/jiminy_py/plot.py | 7 +- python/jiminy_py/unit_py/test_dense_pole.py | 5 +- python/jiminy_py/unit_py/test_simple_mass.py | 9 +-- python/jiminy_py/unit_py/utilities.py | 37 +++++---- python/jiminy_pywrap/src/engine.cc | 8 +- 21 files changed, 127 insertions(+), 122 deletions(-) diff --git a/core/examples/double_pendulum/double_pendulum.cc b/core/examples/double_pendulum/double_pendulum.cc index 818ba35ba..ea11d4c04 100644 --- a/core/examples/double_pendulum/double_pendulum.cc +++ b/core/examples/double_pendulum/double_pendulum.cc @@ -55,6 +55,7 @@ int main(int /* argc */, char * /* argv */[]) const auto dataPath = jiminySrcPath / "data/toys_models"; const auto urdfPath = dataPath / "double_pendulum/double_pendulum.urdf"; const auto outputDirPath = std::filesystem::temp_directory_path(); + std::cout << "Output directory: " << outputDirPath << std::endl; // ===================================================================== // ============ Instantiate and configure the simulation =============== diff --git a/core/examples/external_project/double_pendulum.cc b/core/examples/external_project/double_pendulum.cc index a11b6c485..fa40433db 100644 --- a/core/examples/external_project/double_pendulum.cc +++ b/core/examples/external_project/double_pendulum.cc @@ -53,6 +53,7 @@ int main(int argc, char * argv[]) const std::filesystem::path filePath(__FILE__); const auto urdfPath = filePath.parent_path() / "double_pendulum.urdf"; const auto outputDirPath = std::filesystem::temp_directory_path(); + std::cout << "Output directory: " << outputDirPath << std::endl; // ===================================================================== // ============ Instantiate and configure the simulation =============== diff --git a/core/include/jiminy/core/control/abstract_controller.h b/core/include/jiminy/core/control/abstract_controller.h index d80b90956..96fc42d13 100644 --- a/core/include/jiminy/core/control/abstract_controller.h +++ b/core/include/jiminy/core/control/abstract_controller.h @@ -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; diff --git a/core/include/jiminy/core/engine/engine.h b/core/include/jiminy/core/engine/engine.h index da8cd0cd0..c74bd6900 100644 --- a/core/include/jiminy/core/engine/engine.h +++ b/core/include/jiminy/core/engine/engine.h @@ -12,7 +12,7 @@ namespace jiminy { - inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{"HighLevelController"}; + inline constexpr std::string_view ENGINE_TELEMETRY_NAMESPACE{""}; enum class ContactModelType : uint8_t { diff --git a/core/include/jiminy/core/utilities/helpers.h b/core/include/jiminy/core/utilities/helpers.h index 5b701fe1b..3537d8714 100644 --- a/core/include/jiminy/core/utilities/helpers.h +++ b/core/include/jiminy/core/utilities/helpers.h @@ -100,14 +100,14 @@ namespace jiminy bool JIMINY_DLLAPI endsWith(const std::string & str, const std::string & substr); std::string JIMINY_DLLAPI addCircumfix(std::string fieldname, // Make a copy - const std::string_view & prefix = {}, - const std::string_view & suffix = {}, - const std::string_view & delimiter = {}); + const std::string_view & prefix, + const std::string_view & suffix, + const std::string_view & delimiter); std::vector JIMINY_DLLAPI addCircumfix( const std::vector & fieldnames, - const std::string_view & prefix = {}, - const std::string_view & suffix = {}, - const std::string_view & delimiter = {}); + const std::string_view & prefix, + const std::string_view & suffix, + const std::string_view & delimiter); std::string JIMINY_DLLAPI removeSuffix(std::string fieldname, // Make a copy const std::string & suffix); diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index da3b95c2e..884b3025a 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -571,39 +571,42 @@ namespace jiminy auto energyIt = energy_.begin(); for (; robotIt != robots_.end(); ++robotIt, ++robotDataIt, ++energyIt) { + // Define proxy for convenience + const std::string & robotName = (*robotIt)->getName(); + // Generate the log fieldnames robotDataIt->logPositionFieldnames = addCircumfix((*robotIt)->getLogPositionFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logVelocityFieldnames = addCircumfix((*robotIt)->getLogVelocityFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logAccelerationFieldnames = addCircumfix((*robotIt)->getLogAccelerationFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logForceExternalFieldnames = addCircumfix((*robotIt)->getLogForceExternalFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logCommandFieldnames = addCircumfix((*robotIt)->getLogCommandFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logMotorEffortFieldnames = addCircumfix((*robotIt)->getLogMotorEffortFieldnames(), - (*robotIt)->getName(), + robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); - robotDataIt->logEnergyFieldname = addCircumfix( - "energy", (*robotIt)->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logEnergyFieldname = + addCircumfix("energy", robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); // Register variables to the telemetry senders if (engineOptions_->telemetry.enableConfiguration) diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 77057cbf9..9febb5b4e 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -525,22 +525,20 @@ namespace jiminy // Generate the fieldnames associated with command logCommandFieldnames_.clear(); logCommandFieldnames_.reserve(nmotors_); - std::transform( - motors_.begin(), - motors_.end(), - std::back_inserter(logCommandFieldnames_), - [](const auto & elem) -> std::string - { return addCircumfix(elem->getName(), toString(JOINT_PREFIX_BASE, "Command")); }); + std::transform(motors_.begin(), + motors_.end(), + std::back_inserter(logCommandFieldnames_), + [](const auto & elem) -> std::string + { return toString(JOINT_PREFIX_BASE, "Command", elem->getName()); }); // Generate the fieldnames associated with motor efforts logMotorEffortFieldnames_.clear(); logMotorEffortFieldnames_.reserve(nmotors_); - std::transform( - motors_.begin(), - motors_.end(), - std::back_inserter(logMotorEffortFieldnames_), - [](const auto & elem) -> std::string - { return addCircumfix(elem->getName(), toString(JOINT_PREFIX_BASE, "Effort")); }); + std::transform(motors_.begin(), + motors_.end(), + std::back_inserter(logMotorEffortFieldnames_), + [](const auto & elem) -> std::string + { return toString(JOINT_PREFIX_BASE, "Effort", elem->getName()); }); } void Robot::refreshSensorProxies() diff --git a/core/unit/engine_sanity_check.cc b/core/unit/engine_sanity_check.cc index 4de859b50..d33724822 100755 --- a/core/unit/engine_sanity_check.cc +++ b/core/unit/engine_sanity_check.cc @@ -112,7 +112,7 @@ TEST(EngineSanity, EnergyConservation) std::shared_ptr 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, "energy"); ASSERT_GT(energyCont.size(), 0); // Check that energy is constant @@ -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, "energy"); ASSERT_GT(energyDisc.size(), 0); // Check that energy is constant diff --git a/docs/spec/src/tlmc_format_specification.md b/docs/spec/src/tlmc_format_specification.md index 027d68eb4..ffee01fac 100644 --- a/docs/spec/src/tlmc_format_specification.md +++ b/docs/spec/src/tlmc_format_specification.md @@ -58,7 +58,7 @@ GROUP "/" { (0): 1 } } - DATASET "HighLevelController.controlOffsetTimestamp" { + DATASET "controlOffsetTimestamp" { DATATYPE H5T_STRING { STRSIZE 8; STRPAD H5T_STR_NULLPAD; @@ -73,7 +73,7 @@ GROUP "/" { ... } GROUP "variables" { - GROUP "HighLevelController.currentPositionLeftSagittalHip" { + GROUP "currentPositionLeftSagittalHip" { DATASET "time" { DATATYPE H5T_STD_I64LE DATASPACE SIMPLE { ( 338623 ) / ( 338623 ) } diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py index 0403637f0..3cfcbdd1d 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py @@ -1031,7 +1031,7 @@ def plot(self, "`plot` method.") # Plot all registered variables - for key, fielnames in self.log_fieldnames.items(): + for key, fieldnames in self.log_fieldnames.items(): # Filter state if requested if not enable_block_states and key.endswith(".state"): continue @@ -1042,21 +1042,23 @@ def plot(self, # scalar data over time to be displayed to the same subplot. t = log_vars["Global.Time"] tab_data: Dict[str, Union[np.ndarray, Dict[str, np.ndarray]]] = {} - if isinstance(fielnames, dict): - for group, fieldnames in fielnames.items(): - if not isinstance(fieldnames, list): + if isinstance(fieldnames, dict): + for group, subfieldnames in fieldnames.items(): + if not isinstance(subfieldnames, list): LOGGER.error( "Action space not supported by this method.") return figure + value_map = extract_variables_from_log( + log_vars, subfieldnames, "controller", as_dict=True) tab_data[group] = { key.split(".", 2)[2]: value - for key, value in extract_variables_from_log( - log_vars, fieldnames, as_dict=True).items()} - elif isinstance(fielnames, list): + for key, value in value_map.items()} + elif isinstance(fieldnames, list): + value_map = extract_variables_from_log( + log_vars, fieldnames, "controller", as_dict=True) tab_data.update({ key.split(".", 2)[2]: value - for key, value in extract_variables_from_log( - log_vars, fielnames, as_dict=True).items()}) + for key, value in value_map.items()}) # Add action tab figure.add_tab(key.replace(".", " "), t, tab_data) diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py index f33ef0b3b..2af385360 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py @@ -424,8 +424,8 @@ def compute_reward(self, # Y-axis. It is equal to 0.0 if the frontal displacement is # 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'])) + frontal_displacement = abs(np.mean( + self.log_data['currentFreeflyerPositionTransY'])) reward_dict['direction'] = - frontal_displacement # Compute the total reward diff --git a/python/gym_jiminy/unit_py/test_pipeline_control.py b/python/gym_jiminy/unit_py/test_pipeline_control.py index 433495e13..1e534bcf4 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_control.py +++ b/python/gym_jiminy/unit_py/test_pipeline_control.py @@ -92,16 +92,15 @@ def _test_pid_standing(self): # Check that the joint velocity target is zero time = log_vars["Global.Time"] velocity_target = np.stack([ - log_vars['.'.join(( - 'HighLevelController', self.env.controller.name, name))] + log_vars['.'.join(("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))] - for name in self.env.robot.log_velocity_fieldnames], axis=-1) + log_vars[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)) @@ -202,9 +201,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( diff --git a/python/gym_jiminy/unit_py/test_pipeline_design.py b/python/gym_jiminy/unit_py/test_pipeline_design.py index 533cba0ee..193c608d8 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -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['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]) diff --git a/python/jiminy_py/examples/double_pendulum.py b/python/jiminy_py/examples/double_pendulum.py index e080b92e4..788fd03ac 100644 --- a/python/jiminy_py/examples/double_pendulum.py +++ b/python/jiminy_py/examples/double_pendulum.py @@ -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['energy']) plt.title('Double pendulum energy') plt.grid() plt.show() diff --git a/python/jiminy_py/examples/tutorial.ipynb b/python/jiminy_py/examples/tutorial.ipynb index 01cdfbb6e..c29b9ae36 100644 --- a/python/jiminy_py/examples/tutorial.ipynb +++ b/python/jiminy_py/examples/tutorial.ipynb @@ -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['currentPositionPendulum'])\n", "plt.title('Pendulum angle (rad)')\n", "plt.grid()\n", "plt.show()" diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index 6daf2782a..ac1ccb82a 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -25,7 +25,6 @@ from .dynamics import State, TrajectoryDataType -ENGINE_NAMESPACE = "HighLevelController" SENSORS_FIELDS: Dict[ Type[jiminy.AbstractSensor], Union[List[str], Dict[str, List[str]]] ] = { @@ -52,8 +51,8 @@ @overload def extract_variables_from_log(log_vars: Dict[str, np.ndarray], fieldnames: FieldNested, - namespace: Optional[str] = ENGINE_NAMESPACE, *, - as_dict: Literal[False] = False + namespace: str = "", + *, as_dict: Literal[False] = False ) -> List[np.ndarray]: ... @@ -61,24 +60,25 @@ 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: Optional[str] = ENGINE_NAMESPACE, *, - as_dict: Literal[True] + namespace: str = "", + *, as_dict: Literal[True] ) -> Dict[str, np.ndarray]: ... def extract_variables_from_log(log_vars: Dict[str, np.ndarray], fieldnames: FieldNested, - namespace: Optional[str] = ENGINE_NAMESPACE, *, - as_dict: bool = False) -> Union[ + namespace: str = "", + *, as_dict: bool = False + ) -> Union[ List[np.ndarray], Dict[str, np.ndarray]]: """Extract values associated with a set of variables in a specific namespace. :param log_vars: Logged variables as a dictionary. :param fieldnames: Structured fieldnames. - :param namespace: Namespace of the fieldnames. None to disable. - Optional: ENGINE_TELEMETRY_NAMESPACE by default. + :param namespace: Namespace of the fieldnames. Empty string to disable. + Optional: Empty by default. :param keep_structure: Whether to return a dictionary mapping flattened fieldnames to values. Optional: True by default. @@ -93,9 +93,9 @@ def extract_variables_from_log(log_vars: Dict[str, np.ndarray], values: List[np.ndarray] = [] for fieldname_path, fieldname in tree.flatten_with_path(fieldnames): # A key is the concatenation of namespace and full path of fieldname - key = ".".join(map(str, filter( - lambda key: isinstance(key, str), - (namespace, *fieldname_path, fieldname)))) + key = ".".join(filter( + lambda key: isinstance(key, str) and key, + (namespace, *fieldname_path, fieldname))) # Raise an exception if the key does not exists and not fail safe if key not in log_vars: @@ -151,9 +151,6 @@ def build_robot_from_log( :returns: Reconstructed robot, and parsed log data as returned by `jiminy_py.log.read_log` method. """ - # Prepare robot namespace - robot_namespace = ".".join(filter(None, (ENGINE_NAMESPACE, robot_name))) - # Instantiate empty robot robot = jiminy.Robot(robot_name) @@ -162,7 +159,7 @@ def build_robot_from_log( try: pinocchio_model = log_constants[ - ".".join((robot_namespace, "pinocchio_model"))] + ".".join(filter(None, (robot_name, "pinocchio_model")))] except KeyError as e: if robot_name == "": raise ValueError( @@ -172,20 +169,20 @@ def build_robot_from_log( try: # Extract geometry models collision_model = log_constants[ - ".".join((robot_namespace, "collision_model"))] + ".".join(filter(None, (robot_name, "collision_model")))] visual_model = log_constants[ - ".".join((robot_namespace, "visual_model"))] + ".".join(filter(None, (robot_name, "visual_model")))] # Initialize the model robot.initialize(pinocchio_model, collision_model, visual_model) except KeyError as e: # Extract initialization arguments urdf_data = log_constants[ - ".".join((robot_namespace, "urdf_file"))] + ".".join(filter(None, (robot_name, "urdf_file")))] has_freeflyer = int(log_constants[ - ".".join((robot_namespace, "has_freeflyer"))]) + ".".join(filter(None, (robot_name, "has_freeflyer")))]) mesh_package_dirs = [*mesh_package_dirs, *log_constants.get( - ".".join((robot_namespace, "mesh_package_dirs")), ())] + ".".join(filter(None, (robot_name, "mesh_package_dirs"))), ())] # Make sure urdf data is available if len(urdf_data) <= 1: @@ -198,7 +195,7 @@ def build_robot_from_log( tempfile.gettempdir(), f"{next(tempfile._get_candidate_names())}.urdf") with open(urdf_path, "xb") as f: - f.write(urdf_data.encode()) + f.write(urdf_data) # Fix the mesh paths in the URDF model if requested if mesh_path_dir is not None: @@ -213,7 +210,7 @@ def build_robot_from_log( os.remove(urdf_path) # Load the options - all_options = log_constants[".".join((ENGINE_NAMESPACE, "options"))] + all_options = log_constants["options"] robot.set_options(all_options[robot_name or "robot"]) # Update model in-place. @@ -253,23 +250,20 @@ def build_robots_from_log( :returns: Sequence of reconstructed robots. """ - # Extract log constants - log_constants = log_data["constants"] - - robots_names = [] - for key in log_constants.keys(): - if key == "HighLevelController.has_freeflyer": - robots_names.append("") - else: - m = re.findall(r"HighLevelController.(\w+).has_freeflyer", key) - if len(m) == 1: - robots_names.append(m[0]) - + # Try to infer robot names from log constants + robot_names = [] + for key in log_data["constants"].keys(): + try: + groups = re.match(r"^(\w*?)\.?has_freeflyer$", key) + robot_names.append(groups[1]) + except TypeError: + pass + + # Build all the robots sequentially robots = [] - for robot_name in robots_names: + for robot_name in robot_names: robot = build_robot_from_log( log_data, mesh_path_dir, mesh_package_dirs, robot_name=robot_name) - robots.append(robot) return robots @@ -310,7 +304,6 @@ def extract_trajectory_from_log(log_data: Dict[str, Any], elif robot_name is None: robot_name = "" - robot_namespace = ".".join(filter(None, (ENGINE_NAMESPACE, robot_name))) # Handling of default argument(s) if robot is None: robot = build_robot_from_log(log_data, robot_name=robot_name) @@ -320,13 +313,13 @@ def extract_trajectory_from_log(log_data: Dict[str, Any], # Extract the joint positions, velocities and external forces over time positions = np.stack([ - log_vars.get(".".join((robot_namespace, field)), []) + log_vars.get(".".join(filter(None, (robot_name, field))), []) for field in robot.log_position_fieldnames], axis=-1) velocities = np.stack([ - log_vars.get(".".join((robot_namespace, field)), []) + log_vars.get(".".join(filter(None, (robot_name, field))), []) for field in robot.log_velocity_fieldnames], axis=-1) forces = np.stack([ - log_vars.get(".".join((robot_namespace, field)), []) + log_vars.get(".".join(filter(None, (robot_name, field))), []) for field in robot.log_f_external_fieldnames], axis=-1) # Determine which optional data are available diff --git a/python/jiminy_py/src/jiminy_py/plot.py b/python/jiminy_py/src/jiminy_py/plot.py index 087e87a47..ed4480570 100644 --- a/python/jiminy_py/src/jiminy_py/plot.py +++ b/python/jiminy_py/src/jiminy_py/plot.py @@ -627,6 +627,11 @@ def plot_log(log_data: Dict[str, Any], if robot is None: robot = build_robot_from_log(log_data) + # Make sure tha the simulation was single-robot + if robot.name: + raise NotImplementedError( + "This method only support single-robot simulations.") + # Figures data structure as a dictionary tabs_data: Dict[ str, Dict[str, Union[np.ndarray, Dict[str, np.ndarray]]] @@ -648,7 +653,7 @@ def plot_log(log_data: Dict[str, Any], values = extract_variables_from_log( log_vars, fieldnames, as_dict=True) tabs_data[' '.join(("State", fields_type))] = OrderedDict( - (field.split(".", 1)[1][7:].replace(fields_type, ""), elem) + (field[7:].replace(fields_type, ""), elem) for field, elem in values.items()) except ValueError: # Variable has not been recorded and is missing in log file diff --git a/python/jiminy_py/unit_py/test_dense_pole.py b/python/jiminy_py/unit_py/test_dense_pole.py index 57691f29d..692ac57fd 100644 --- a/python/jiminy_py/unit_py/test_dense_pole.py +++ b/python/jiminy_py/unit_py/test_dense_pole.py @@ -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) + ]), axis=0) twist_flex = 2 * np.arctan2(q_flex[2], q_flex[3]) twist_flex_all.append(twist_flex) @@ -209,8 +209,7 @@ def test_joint_position_limits(self): log_vars = self.simulator.log_data["variables"] (theta,) = extract_variables_from_log( log_vars, - (f"currentPosition{self.robot.pinocchio_model.names[-1]}",), - 'HighLevelController') + (f"currentPosition{self.robot.pinocchio_model.names[-1]}",)) theta_all.append(theta) assert np.allclose(*theta_all, atol=1e-4) diff --git a/python/jiminy_py/unit_py/test_simple_mass.py b/python/jiminy_py/unit_py/test_simple_mass.py index dc4d4beb8..85ce421e1 100644 --- a/python/jiminy_py/unit_py/test_simple_mass.py +++ b/python/jiminy_py/unit_py/test_simple_mass.py @@ -128,7 +128,7 @@ def _test_collision_and_contact_dynamics(self, shape): # Set some extra options of the engine, to avoid assertion failure # because of problem regularization and outliers engine_options = engine.get_options() - engine_options['contacts']['transitionEps'] = 1.0e-6 + engine_options["contacts"]["transitionEps"] = 1.0e-6 engine_options["stepper"]["controllerUpdatePeriod"] = self.dtMax engine.set_options(engine_options) @@ -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["energy"] E_contact = 1 / 2 * self.k_contact * penetration_depth ** 2 E_tot = E_robot + E_contact E_diff_robot = np.concatenate(( @@ -278,8 +278,7 @@ def _test_friction_model(self, shape): # Validate the stiction model: check the transition between dry and # viscous friction because of stiction phenomena. log_vars = engine.log_data["variables"] - acceleration = log_vars[ - 'HighLevelController.currentFreeflyerAccelerationLinX'] + acceleration = log_vars["currentFreeflyerAccelerationLinX"] jerk = np.diff(acceleration) / np.diff(time) snap = np.diff(jerk) / np.diff(time[1:]) snap_rel = np.abs(snap / np.max(snap)) @@ -303,7 +302,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["energy"] E_diff_robot = np.concatenate(( np.diff(E_robot) / np.diff(time), np.zeros((1,), dtype=E_robot.dtype))) diff --git a/python/jiminy_py/unit_py/utilities.py b/python/jiminy_py/unit_py/utilities.py index a141113ee..5c46387d5 100644 --- a/python/jiminy_py/unit_py/utilities.py +++ b/python/jiminy_py/unit_py/utilities.py @@ -194,7 +194,8 @@ def simulate_and_get_state_evolution( """ # Run simulation if isinstance(x0, np.ndarray): - q0, v0 = x0[:engine.robots[0].nq], x0[-engine.robots[0].nv:] + robot, = engine.robots + q0, v0 = x0[:robot.nq], x0[-robot.nv:] else: q0, v0 = {}, {} for robot in engine.robots: @@ -208,29 +209,33 @@ def simulate_and_get_state_evolution( # Extract state evolution over time time = log_vars['Global.Time'] if isinstance(x0, np.ndarray): + robot, = engine.robots + q_jiminy = np.stack([ - log_vars['.'.join(['HighLevelController', s])] - for s in engine.robots[0].log_position_fieldnames], axis=-1) + log_vars[field] for field in robot.log_position_fieldnames + ], axis=-1) v_jiminy = np.stack([ - log_vars['.'.join(['HighLevelController', s])] - for s in engine.robots[0].log_velocity_fieldnames], axis=-1) + log_vars[field] for field in robot.log_velocity_fieldnames + ], axis=-1) + if split: return time, q_jiminy, v_jiminy - else: - x_jiminy = np.concatenate((q_jiminy, v_jiminy), axis=-1) - return time, x_jiminy + + x_jiminy = np.concatenate((q_jiminy, v_jiminy), axis=-1) + return time, x_jiminy else: q_jiminy = [np.stack([ - log_vars['.'.join(['HighLevelController', robot.name, s])] - for s in robot.log_position_fieldnames + log_vars['.'.join((robot.name, field))] + for field in robot.log_position_fieldnames ], axis=-1) for robot in engine.robots] v_jiminy = [np.stack([ - log_vars['.'.join(['HighLevelController', robot.name, s])] - for s in robot.log_velocity_fieldnames + log_vars['.'.join((robot.name, field))] + for field in robot.log_velocity_fieldnames ], axis=-1) for robot in engine.robots] + if split: return time, q_jiminy, v_jiminy - else: - x_jiminy = [np.concatenate((q, v), axis=-1) - for q, v in zip(q_jiminy, v_jiminy)] - return time, x_jiminy + + x_jiminy = [np.concatenate((q, v), axis=-1) + for q, v in zip(q_jiminy, v_jiminy)] + return time, x_jiminy diff --git a/python/jiminy_pywrap/src/engine.cc b/python/jiminy_pywrap/src/engine.cc index 9ac506771..4ca5e8054 100644 --- a/python/jiminy_pywrap/src/engine.cc +++ b/python/jiminy_pywrap/src/engine.cc @@ -380,7 +380,7 @@ namespace jiminy::python // Get constants for (const auto & [key, value] : logData.constants) { - if (endsWith(key, ".options")) + if (endsWith(key, "options")) { std::vector jsonStringVec(value.begin(), value.end()); std::shared_ptr device = @@ -389,7 +389,7 @@ namespace jiminy::python jsonLoad(robotOptions, device); constants[key] = robotOptions; } - else if (key.find(".pinocchio_model") != std::string::npos) + else if (key.find("pinocchio_model") != std::string::npos) { try { @@ -404,7 +404,7 @@ namespace jiminy::python e.what()); } } - else if (endsWith(key, ".visual_model") || endsWith(key, ".collision_model")) + else if (endsWith(key, "visual_model") || endsWith(key, "collision_model")) { try { @@ -419,7 +419,7 @@ namespace jiminy::python e.what()); } } - else if (endsWith(key, ".mesh_package_dirs")) + else if (endsWith(key, "mesh_package_dirs")) { bp::list meshPackageDirs; std::stringstream ss(value);