From d4e753cae2bbd2ae482ff46db234e2ff2ed8f005 Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Sat, 13 Apr 2024 07:34:16 +0200 Subject: [PATCH] [core] Add methods to get / set all simulation options at once. (#760) * [core] Add methods to get / set all simulation options at once. * [core] Stop supporting flat sensor data hierarchy in telemetry. * [core] Remove irrelevant engine telemetry prefix. * [core] Replace controller telemetry prefix by 'controller'. * [core/python] Do not try decoding telemetry constants automatically anymore. * [python/viewer] Fix ground profile rendering. * [misc] Add initialization python unit test. --------- Co-authored-by: Alexis Duburcq --- .../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 | 14 ++- .../jiminy/core/hardware/abstract_sensor.h | 1 - .../jiminy/core/hardware/abstract_sensor.hxx | 9 +- .../jiminy/core/hardware/basic_sensors.h | 10 --- core/include/jiminy/core/utilities/helpers.h | 12 +-- core/src/engine/engine.cc | 62 ++++++++----- core/src/hardware/basic_sensors.cc | 12 +-- 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 | 28 +++--- .../gym_jiminy/common/envs/locomotion.py | 8 +- .../envs/gym_jiminy/envs/acrobot.py | 4 +- python/gym_jiminy/envs/gym_jiminy/envs/ant.py | 2 +- .../envs/gym_jiminy/envs/cartpole.py | 4 +- .../unit_py/test_deformation_estimator.py | 8 +- .../unit_py/test_pipeline_control.py | 11 ++- .../unit_py/test_pipeline_design.py | 6 +- .../box_uneven_ground_impulse_contact.py | 6 +- .../examples/constraint_fixed_frame.py | 8 +- python/jiminy_py/examples/constraint_wheel.py | 4 +- python/jiminy_py/examples/double_pendulum.py | 2 +- python/jiminy_py/examples/sphere_rolling.py | 4 +- python/jiminy_py/examples/tutorial.ipynb | 2 +- python/jiminy_py/src/jiminy_py/log.py | 75 +++++++--------- python/jiminy_py/src/jiminy_py/plot.py | 12 ++- python/jiminy_py/src/jiminy_py/simulator.py | 40 +++------ .../viewer/meshcat/meshcat_visualizer.py | 4 +- .../viewer/panda3d/panda3d_visualizer.py | 19 ++-- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 24 ++--- python/jiminy_py/unit_py/test_dense_pole.py | 17 ++-- python/jiminy_py/unit_py/test_flexible_arm.py | 4 +- .../jiminy_py/unit_py/test_foot_pendulum.py | 90 +++++++++++++++++++ python/jiminy_py/unit_py/test_simple_mass.py | 9 +- .../jiminy_py/unit_py/test_simple_pendulum.py | 19 ++-- python/jiminy_py/unit_py/utilities.py | 37 ++++---- python/jiminy_pywrap/src/engine.cc | 20 +++-- python/jiminy_pywrap/src/sensors.cc | 1 - 41 files changed, 354 insertions(+), 268 deletions(-) create mode 100644 python/jiminy_py/unit_py/test_foot_pendulum.py 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 c92527140..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 { @@ -663,6 +663,18 @@ namespace jiminy GenericConfig getOptions() const noexcept; void setOptions(const GenericConfig & engineOptions); + /// \brief Get the options of the engine and all the robots. + /// + /// \details The key 'engine' maps to the engine options, whereas `robot.name` maps to the + /// invididual options of each robot for multi-robot simulations, 'robot' for + /// single-robot simulations. + GenericConfig getAllOptions() const noexcept; + /// \brief Set the options of the engine and all the robots. + /// + /// \param[in] allOptions Dictionary gathering all the options. See `getAllOptions` for + /// details about the hierarchy. + void setAllOptions(const GenericConfig & allOptions); + bool getIsTelemetryConfigured() const; std::shared_ptr getRobot(const std::string & robotName); std::ptrdiff_t getRobotIndex(const std::string & robotName) const; diff --git a/core/include/jiminy/core/hardware/abstract_sensor.h b/core/include/jiminy/core/hardware/abstract_sensor.h index 9c289b23a..b3a5e6eb9 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.h +++ b/core/include/jiminy/core/hardware/abstract_sensor.h @@ -360,7 +360,6 @@ namespace jiminy sensors together, even if they are associated to complete separated robots. */ static const std::string JIMINY_STATIC_MEMBER_DLLAPI type_; static const std::vector JIMINY_STATIC_MEMBER_DLLAPI fieldnames_; - static const bool JIMINY_STATIC_MEMBER_DLLAPI areFieldnamesGrouped_; protected: std::size_t sensorIndex_{0}; diff --git a/core/include/jiminy/core/hardware/abstract_sensor.hxx b/core/include/jiminy/core/hardware/abstract_sensor.hxx index 4441c64b4..3ca952024 100644 --- a/core/include/jiminy/core/hardware/abstract_sensor.hxx +++ b/core/include/jiminy/core/hardware/abstract_sensor.hxx @@ -259,14 +259,7 @@ namespace jiminy template std::string AbstractSensorTpl::getTelemetryName() const { - if (areFieldnamesGrouped_) - { - return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER); - } - else - { - return name_; - } + return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER); } template diff --git a/core/include/jiminy/core/hardware/basic_sensors.h b/core/include/jiminy/core/hardware/basic_sensors.h index f6346b254..a3a216b34 100644 --- a/core/include/jiminy/core/hardware/basic_sensors.h +++ b/core/include/jiminy/core/hardware/basic_sensors.h @@ -16,8 +16,6 @@ namespace jiminy const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; - template<> - const bool JIMINY_DLLAPI AbstractSensorTpl::areFieldnamesGrouped_; #endif template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; @@ -58,8 +56,6 @@ namespace jiminy const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; - template<> - const bool JIMINY_DLLAPI AbstractSensorTpl::areFieldnamesGrouped_; #endif template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; @@ -98,8 +94,6 @@ namespace jiminy const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; - template<> - const bool JIMINY_DLLAPI AbstractSensorTpl::areFieldnamesGrouped_; #endif template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; @@ -141,8 +135,6 @@ namespace jiminy const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; - template<> - const bool JIMINY_DLLAPI AbstractSensorTpl::areFieldnamesGrouped_; #endif template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; @@ -182,8 +174,6 @@ namespace jiminy const std::string JIMINY_DLLAPI AbstractSensorTpl::type_; template<> const std::vector JIMINY_DLLAPI AbstractSensorTpl::fieldnames_; - template<> - const bool JIMINY_DLLAPI AbstractSensorTpl::areFieldnamesGrouped_; #endif template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl; 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 cbecb7302..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) @@ -1534,18 +1537,7 @@ namespace jiminy } // Log all options - GenericConfig allOptions; - for (const auto & robot : robots_) - { - std::string telemetryRobotOptions = robot->getName(); - if (telemetryRobotOptions.empty()) - { - telemetryRobotOptions = "robot"; - } - allOptions[telemetryRobotOptions] = robot->getOptions(); - } - allOptions["engine"] = engineOptionsGeneric_; - Json::Value allOptionsJson = convertToJson(allOptions); + Json::Value allOptionsJson = convertToJson(getAllOptions()); Json::StreamWriterBuilder jsonWriter; jsonWriter["indentation"] = ""; const std::string allOptionsString = Json::writeString(jsonWriter, allOptionsJson); @@ -2790,6 +2782,36 @@ namespace jiminy stepperUpdatePeriod_ = updatePeriodMin; } + GenericConfig Engine::getAllOptions() const noexcept + { + GenericConfig allOptions; + allOptions["engine"] = engineOptionsGeneric_; + for (const auto & robot : robots_) + { + std::string robotOptionName = robot->getName(); + if (robotOptionName.empty()) + { + robotOptionName = "robot"; + } + allOptions[robotOptionName] = robot->getOptions(); + } + return allOptions; + } + + void Engine::setAllOptions(const GenericConfig & allOptions) + { + setOptions(boost::get(allOptions.at("engine"))); + for (auto & robot : robots_) + { + std::string robotOptionName = robot->getName(); + if (robotOptionName.empty()) + { + robotOptionName = "robot"; + } + robot->setOptions(boost::get(allOptions.at(robotOptionName))); + } + } + std::ptrdiff_t Engine::getRobotIndex(const std::string & robotName) const { auto robotIt = std::find_if(robots_.begin(), diff --git a/core/src/hardware/basic_sensors.cc b/core/src/hardware/basic_sensors.cc index 38495ede8..8bbcc0d7a 100644 --- a/core/src/hardware/basic_sensors.cc +++ b/core/src/hardware/basic_sensors.cc @@ -66,9 +66,7 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"ImuSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{ - "Gyrox", "Gyroy", "Gyroz", "Accelx", "Accely", "Accelz"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{false}; + "GyroX", "GyroY", "GyroZ", "AccelX", "AccelY", "AccelZ"}; void ImuSensor::initialize(const std::string & frameName) { @@ -200,8 +198,6 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"ContactSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{"FX", "FY", "FZ"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{false}; void ContactSensor::initialize(const std::string & frameName) { @@ -297,8 +293,6 @@ namespace jiminy template<> const std::vector AbstractSensorTpl::fieldnames_{ "FX", "FY", "FZ", "MX", "MY", "MZ"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{false}; void ForceSensor::initialize(const std::string & frameName) { @@ -412,8 +406,6 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"EncoderSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{"Q", "V"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{true}; void EncoderSensor::initialize(const std::string & jointName) { @@ -525,8 +517,6 @@ namespace jiminy const std::string AbstractSensorTpl::type_{"EffortSensor"}; template<> const std::vector AbstractSensorTpl::fieldnames_{"U"}; - template<> - const bool AbstractSensorTpl::areFieldnamesGrouped_{true}; void EffortSensor::initialize(const std::string & motorName) { 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 30dd57af2..3cfcbdd1d 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py @@ -709,7 +709,7 @@ def reset(self, # type: ignore[override] # Extract the observer/controller update period. # The controller update period is used by default for the observer if # it was not specify by the user in `_setup`. - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() self.control_dt = float( engine_options['stepper']['controllerUpdatePeriod']) if self.observe_dt < 0.0: @@ -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) @@ -1318,7 +1320,7 @@ def _setup(self) -> None: super()._setup() # Configure the low-level integrator - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options["stepper"]["iterMax"] = 0 if self.debug: engine_options["stepper"]["verbose"] = True @@ -1334,7 +1336,7 @@ def _setup(self) -> None: engine_options["telemetry"]["isPersistent"] = False # Update engine options - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _initialize_observation_space(self) -> None: """Configure the observation of the environment. @@ -1410,7 +1412,7 @@ def _sample_state(self) -> Tuple[np.ndarray, np.ndarray]: # Make sure the robot impacts the ground if self.robot.has_freeflyer: - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() ground_fun = engine_options['world']['groundProfile'] compute_freeflyer_state_from_fixed_body( self.robot, q, ground_profile=ground_fun) 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 2baf77204..2af385360 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/locomotion.py @@ -247,7 +247,7 @@ def _setup(self) -> None: # Get the options of robot and engine robot_options = self.robot.get_options() - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() # Make sure to log at least the required data for terminal reward # computation and log replay. @@ -334,7 +334,7 @@ def _setup(self) -> None: # Set the options, finally self.robot.set_options(robot_options) - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _force_external_profile(self, t: float, @@ -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/envs/gym_jiminy/envs/acrobot.py b/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py index eef3f9449..17f4cd678 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/acrobot.py @@ -150,10 +150,10 @@ def _setup(self) -> None: # Enforce fixed-timestep integrator. # It ensures calling 'step' always takes the same amount of time. - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options["stepper"]["odeSolver"] = "runge_kutta_4" engine_options["stepper"]["dtMax"] = CONTROL_DT - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _initialize_observation_space(self) -> None: """Configure the observation of the environment. diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py index 891e70dec..54d37b7aa 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/ant.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/ant.py @@ -158,7 +158,7 @@ def _initialize_observation_space(self) -> None: def _initialize_buffers(self) -> None: # Extract observation from the robot state. # Note that this is only reliable with using a fixed step integrator. - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() if engine_options['stepper']['odeSolver'] in ('runge_kutta_dopri5',): raise ValueError( "This environment does not support adaptive step integrators. " diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py b/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py index 225c13c12..39b82afd3 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py @@ -155,10 +155,10 @@ def _setup(self) -> None: super()._setup() # OpenAI Gym implementation uses euler explicit integration scheme - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options["stepper"]["odeSolver"] = "euler_explicit" engine_options["stepper"]["dtMax"] = CONTROL_DT - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def _initialize_observation_space(self) -> None: """Configure the observation of the environment. diff --git a/python/gym_jiminy/unit_py/test_deformation_estimator.py b/python/gym_jiminy/unit_py/test_deformation_estimator.py index 659fb9c90..c3bec0681 100644 --- a/python/gym_jiminy/unit_py/test_deformation_estimator.py +++ b/python/gym_jiminy/unit_py/test_deformation_estimator.py @@ -99,10 +99,10 @@ def test_arm(self): simulator = Simulator(robot) # Configure the controller and sensor update periods - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options['stepper']['controllerUpdatePeriod'] = 0.0001 engine_options['stepper']['sensorsUpdatePeriod'] = 0.0001 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Instantiate the environment env = BaseJiminyEnv(simulator, step_dt=0.01, debug=DEBUG) @@ -183,10 +183,10 @@ def _setup(self) -> None: super()._setup() # Configure the controller and sensor update periods - engine_options = self.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['controllerUpdatePeriod'] = 0.0001 engine_options['stepper']['sensorsUpdatePeriod'] = 0.0001 - self.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Add flexibility frames model_options = self.robot.get_model_options() 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 e2b8d07bc..193c608d8 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -206,10 +206,10 @@ def test_update_periods(self): env = self.ANYmalPipelineEnv() def configure_telemetry() -> InterfaceJiminyEnv: - engine_options = env.simulator.engine.get_options() + engine_options = env.simulator.get_options() engine_options['telemetry']['enableCommand'] = True engine_options['stepper']['logInternalStepperSteps'] = False - env.simulator.engine.set_options(engine_options) + env.simulator.set_options(engine_options) return env env.reset(seed=0, options=dict(reset_hook=configure_telemetry)) @@ -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/box_uneven_ground_impulse_contact.py b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py index 6d5ee38dd..84444fe11 100644 --- a/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py +++ b/python/jiminy_py/examples/box_uneven_ground_impulse_contact.py @@ -30,7 +30,7 @@ simulator = Simulator.build(urdf_path, has_freeflyer=True) # Enable constraint contact model - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options['contacts']['model'] = 'constraint' engine_options['contacts']['stabilizationFreq'] = 20.0 engine_options["constraints"]['regularization'] = 0.0 @@ -43,7 +43,7 @@ # Set the ground contact options engine_options['contacts']['friction'] = 1.0 engine_options['contacts']['torsion'] = 0.0 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Generate random ground profile ground_params = list(starmap(random_tile_ground, zip( @@ -51,7 +51,7 @@ TILE_ORIENTATION, TILE_SEED))) engine_options["world"]["groundProfile"] = sum_heightmaps([ ground_params[0], merge_heightmaps(ground_params[1:])]) - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Sample the initial state qpos = pin.neutral(simulator.robot.pinocchio_model) diff --git a/python/jiminy_py/examples/constraint_fixed_frame.py b/python/jiminy_py/examples/constraint_fixed_frame.py index 87cd7e87b..3b7a29343 100644 --- a/python/jiminy_py/examples/constraint_fixed_frame.py +++ b/python/jiminy_py/examples/constraint_fixed_frame.py @@ -21,15 +21,15 @@ robot = simulator.robot # Disable constraint solver regularization - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["constraints"]["regularization"] = 0.0 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Continuous sensor and controller update - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["stepper"]["controllerUpdatePeriod"] = 0.0 engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Add fixed frame constraint constraint = jiminy.FrameConstraint( diff --git a/python/jiminy_py/examples/constraint_wheel.py b/python/jiminy_py/examples/constraint_wheel.py index 54c7a17b6..e1c2bec1c 100644 --- a/python/jiminy_py/examples/constraint_wheel.py +++ b/python/jiminy_py/examples/constraint_wheel.py @@ -20,7 +20,7 @@ urdf_path, has_freeflyer=True, hardware_path="") # Disable constraint solver regularization - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["constraints"]["regularization"] = 0.0 # Continuous sensor and controller update @@ -30,7 +30,7 @@ # Configure integrator engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5' engine_options['stepper']['dtMax'] = 1.0e-3 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Add fixed frame constraint constraint = jiminy.WheelConstraint( 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/sphere_rolling.py b/python/jiminy_py/examples/sphere_rolling.py index 85e2645a1..2f51155dc 100644 --- a/python/jiminy_py/examples/sphere_rolling.py +++ b/python/jiminy_py/examples/sphere_rolling.py @@ -18,7 +18,7 @@ simulator = Simulator.build(urdf_path, has_freeflyer=True, hardware_path="") # Disable constraint solver regularization - engine_options = simulator.engine.get_options() + engine_options = simulator.get_options() engine_options["constraints"]["regularization"] = 0.0 # Continuous sensor and controller update @@ -28,7 +28,7 @@ # Configure integrator engine_options['stepper']['odeSolver'] = 'runge_kutta_dopri5' engine_options['stepper']['dtMax'] = 1.0e-3 - simulator.engine.set_options(engine_options) + simulator.set_options(engine_options) # Add fixed frame constraint constraint = jiminy.SphereConstraint("MassBody", 0.5) 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 997e5c974..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 @@ -679,7 +684,6 @@ def plot_log(log_data: Dict[str, Any], sensor_names = robot.sensor_names.get(sensors_type, []) if not sensor_names: continue - namespace = sensors_type if sensors_class.has_prefix else None if isinstance(sensors_fields, dict): for fields_prefix, fieldnames in sensors_fields.items(): try: @@ -687,7 +691,7 @@ def plot_log(log_data: Dict[str, Any], data_nested = [ extract_variables_from_log(log_vars, [ '.'.join((name, fields_prefix + field)) - for name in sensor_names], namespace) + for name in sensor_names], sensors_type) for field in fieldnames] tabs_data[type_name] = OrderedDict( (field, OrderedDict(zip(sensor_names, data))) @@ -701,7 +705,7 @@ def plot_log(log_data: Dict[str, Any], type_name = ' '.join((sensors_type, field)) data = extract_variables_from_log(log_vars, [ '.'.join((name, field)) for name in sensor_names - ], namespace) + ], sensors_type) tabs_data[type_name] = OrderedDict(zip( sensor_names, data)) except ValueError: diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index ff611d6ad..b872acb49 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -789,7 +789,8 @@ def render(self, if update_ground_profile: engine_options = self.engine.get_options() ground_profile = engine_options["world"]["groundProfile"] - Viewer.update_floor(ground_profile, show_meshes=False) + Viewer.update_floor( + ground_profile, simplify_mesh=True, show_vertices=False) # Set the camera pose if requested if camera_pose is not None: @@ -944,31 +945,9 @@ def plot(self, return self._figure - def get_options(self) -> Dict[str, Dict[str, Dict[str, Any]]]: - """Get the options of the engine and all the robots. - - The key 'engine' maps to the engine options, whereas `robot.name` maps - to the invididual options of each robot for multi-robot simulations, - 'robot' for single-robot simulations. - """ - return {'engine': self.engine.get_options(), **{ - robot.name or 'robot': robot.get_options() - for robot in self.engine.robots}} - - def set_options(self, - options: Dict[str, Dict[str, Dict[str, Any]]]) -> None: - """Set the options of the engine and all the robots. - - :param options: Dictionary gathering all the options. See `get_options` - for details about the hierarchy. - """ - self.engine.set_options(options['engine']) - for robot in self.engine.robots: - robot.set_options(options[robot.name or 'robot']) - def export_options(self, config_path: Union[str, os.PathLike]) -> None: - """Export the complete configuration of the simulator, ie the options - of the engine and all the robots. + """Export in a single configuration file all the options of the + simulator, ie the engine and all the robots. .. note:: The generated configuration file can be imported thereafter using @@ -981,11 +960,11 @@ def export_options(self, config_path: Union[str, os.PathLike]) -> None: config_path = pathlib.Path(config_path).with_suffix('.toml') with open(config_path, 'w') as f: toml.dump( - self.get_options(), f, encoder=toml.TomlNumpyEncoder()) + self.get_all_options(), f, encoder=toml.TomlNumpyEncoder()) def import_options(self, config_path: Union[str, os.PathLike]) -> None: - """Import the complete configuration of the simulator, ie the options - of the engine and all the robots. + """Import all the options of the simulator at once, ie the engine + and all the robots. .. note:: A full configuration file can be exported beforehand using @@ -1018,5 +997,6 @@ def deep_update(original: Dict[str, Any], original[key] = new_dict[key] return original - options = deep_update(self.get_options(), toml.load(str(config_path))) - self.set_options(options) + options = deep_update( + self.get_all_options(), toml.load(str(config_path))) + self.set_all_options(options) diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py index 340b3c069..8ab567ff3 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py @@ -194,8 +194,8 @@ def update_floor(viewer: meshcat.Visualizer, # Convert provided geometry in Meshcat-specific triangle-based geometry vertices, faces = extract_vertices_and_faces_from_geometry(geom) obj = TriangularMeshGeometry(vertices, faces) - material = meshcat.geometry.MeshLambertMaterial() - material.color = (255 << 16) + (255 << 8) + 255 + material = meshcat.geometry.MeshPhongMaterial() + material.color = (155 << 16) + (155 << 8) + 180 # Disable newly created custom ground profile and hide original flat ground viewer["Ground"].set_object(obj, material) diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py index 023d3dfd1..78e5e8f67 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py @@ -925,7 +925,7 @@ def _make_axes(self) -> NodePath: def _make_floor(self, geom: Optional[Geom] = None, - show_meshes: bool = False) -> NodePath: + show_vertices: bool = False) -> NodePath: model = GeomNode('floor') node = self.render.attach_new_node(model) @@ -943,7 +943,7 @@ def _make_floor(self, else: model.add_geom(geom) node.set_color((0.75, 0.75, 0.85, 1.0)) - if show_meshes: + if show_vertices: render_attrib = node.get_state().get_attrib_def( RenderModeAttrib.get_class_slot()) node.set_attrib(RenderModeAttrib.make( @@ -982,7 +982,7 @@ def show_floor(self, show: bool) -> None: def update_floor(self, geom: Optional[Geom] = None, - show_meshes: bool = False) -> NodePath: + show_vertices: bool = False) -> NodePath: """Update the floor. :param geom: Ground profile as a generic geometry object. If None, then @@ -994,7 +994,7 @@ def update_floor(self, # Remove existing floor and create a new one self._floor.remove_node() - self._floor = self._make_floor(geom, show_meshes) + self._floor = self._make_floor(geom, show_vertices) # Hide the floor if is was previously hidden if is_hidden: @@ -1985,14 +1985,17 @@ def convert_bvh_collision_geometry_to_primitive(geom: hppfcl.CollisionGeometry if len(faces) == 0: return None - # Define normal to vertices as the average normal of adjacent triangles + # Define normal to vertices as the average normal of adjacent triangles, + # weigthed by their surface area: https://iquilezles.org/articles/normals/ fnormals = np.cross(vertices[faces[:, 2]] - vertices[faces[:, 1]], vertices[faces[:, 0]] - vertices[faces[:, 1]]) - fnormals /= np.linalg.norm(fnormals, axis=0) normals = np.zeros((len(vertices), 3), dtype=np.float32) for i in range(3): - normals[faces[:, i]] += fnormals - normals /= np.linalg.norm(normals, axis=0) + # Must use `np.add.at` which is unbuffered unlike `+=`, otherwise + # accumulation will not work properly as there are repeated indices. + np.add.at(normals, faces[:, i], fnormals) + scale = np.linalg.norm(normals, axis=1) + normals[scale > 0.0] /= scale[scale > 0.0, np.newaxis] # Create primitive triangle geometry vformat = GeomVertexFormat() diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 3ea489863..615c760de 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -1783,8 +1783,8 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, x_range: Tuple[float, float] = (-10.0, 10.0), y_range: Tuple[float, float] = (-10.0, 10.0), grid_unit: Tuple[float, float] = (0.04, 0.04), - simplify_meshes: bool = False, - show_meshes: bool = False) -> None: + simplify_mesh: bool = False, + show_vertices: bool = False) -> None: """Display a custom ground profile as a height map or the original tile ground floor. @@ -1801,14 +1801,14 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, :param grid_unit: Tuple gathering X and Y discretization steps for the generation of the heightmap mesh. Optional: 4cm by default. - :param simplify_meshes: Whether the generated heightmap mesh should be - decimated before final rendering. This option - must be enabled for the ratio between grid size - and unit is very large to avoid a prohibitive - slowdown of the viewer. - Optional: False by default - :param show_meshes: Whether to highlight the mesh vertices. - Optional: disabled by default. + :param simplify_mesh: Whether the generated heightmap mesh should be + decimated before final rendering. This option + must be enabled for the ratio between grid size + and unit is very large to avoid a prohibitive + slowdown of the viewer. + Optional: False by default + :param show_vertices: Whether to highlight the mesh vertices. + Optional: disabled by default. """ # pylint: disable=import-outside-toplevel @@ -1821,7 +1821,7 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, if ground_profile is not None: geom = discretize_heightmap( ground_profile, *x_range, grid_unit[0], *y_range, grid_unit[1], - must_simplify=simplify_meshes) + must_simplify=simplify_mesh) # Render original flat tile ground if possible. # TODO: Improve this check using LocalAABB box geometry instead. @@ -1836,7 +1836,7 @@ def update_floor(ground_profile: Optional[jiminy.HeightmapFunction] = None, obj = None if geom is not None: obj = convert_bvh_collision_geometry_to_primitive(geom) - Viewer._backend_obj.gui.update_floor(obj, show_meshes) + Viewer._backend_obj.gui.update_floor(obj, show_vertices) else: from .meshcat.meshcat_visualizer import ( update_floor as meshcat_update_floor) diff --git a/python/jiminy_py/unit_py/test_dense_pole.py b/python/jiminy_py/unit_py/test_dense_pole.py index aef55ba35..692ac57fd 100644 --- a/python/jiminy_py/unit_py/test_dense_pole.py +++ b/python/jiminy_py/unit_py/test_dense_pole.py @@ -46,12 +46,12 @@ def setUp(self): self.robot.set_model_options(model_options) # Configure the integrator - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['tolAbs'] = 1e-9 engine_options['stepper']['tolRel'] = 1e-8 engine_options['constraints']['regularization'] = 0.0 engine_options['contacts']['transitionEps'] = self.transition_eps - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) def test_flex_model(self): """Test if the result is the same with true and virtual inertia in @@ -67,9 +67,9 @@ def test_flex_model(self): self.robot.add_constraint("fixed_joint", const) # Configure the engine - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['sensorsUpdatePeriod'] = step_dt - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Extract some proxies for convenience pinocchio_model_th = self.robot.pinocchio_model_th @@ -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) @@ -165,11 +165,11 @@ def test_joint_position_limits(self): theta_all = [] for contact_model in ('constraint', 'spring_damper'): # Configure the engine - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['stepper']['odeSolver'] = 'euler_explicit' engine_options['stepper']['dtMax'] = step_dt engine_options['contacts']['model'] = contact_model - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Start the simulation self.simulator.start(np.array((0.0,)), np.array((1.0,))) @@ -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_flexible_arm.py b/python/jiminy_py/unit_py/test_flexible_arm.py index 0c84a9f2b..3d5b2964c 100644 --- a/python/jiminy_py/unit_py/test_flexible_arm.py +++ b/python/jiminy_py/unit_py/test_flexible_arm.py @@ -152,9 +152,9 @@ def test_write_replay_standalone_log(self): """Check if reading/writing standalone log file is working. """ # Configure log file to be standalone - engine_options = self.simulator.engine.get_options() + engine_options = self.simulator.get_options() engine_options['telemetry']['isPersistent'] = True - self.simulator.engine.set_options(engine_options) + self.simulator.set_options(engine_options) # Specify joint flexibility parameters model_options = self.simulator.robot.get_model_options() diff --git a/python/jiminy_py/unit_py/test_foot_pendulum.py b/python/jiminy_py/unit_py/test_foot_pendulum.py new file mode 100644 index 000000000..cfbdcafdd --- /dev/null +++ b/python/jiminy_py/unit_py/test_foot_pendulum.py @@ -0,0 +1,90 @@ +""" +@brief This file aims at verifying the sanity of the physics and the + integration method of jiminy on simple models. +""" +import os +import unittest + +import numpy as np + + +from jiminy_py.core import ( # pylint: disable=no-name-in-module + ForceSensor as force, ImuSensor as imu, ContactSensor as contact) +from jiminy_py.simulator import Simulator + + +# Small tolerance for numerical equality. +# The integration error is supposed to be bounded. +TOLERANCE = 1.0e-5 + + +class SimulateFootedPendulum(unittest.TestCase): + """Simulate the motion of a pendulum with a square foot having contact + points at each corner. + """ + def test_init_and_consistency(self): + """Verify that the pendulum holds straight without falling when + perfectly initialized at the unstable equilibrium point. + + This test also checks that there is no discontinuity at initialization + and that the various sensors are working properly. + """ + # Create the jiminy robot + current_dir = os.path.dirname(os.path.realpath(__file__)) + data_root_dir = os.path.join(current_dir, "data") + urdf_path = os.path.join(data_root_dir, "foot_pendulum.urdf") + hardware_path = os.path.join(data_root_dir, "foot_pendulum.toml") + simulator = Simulator.build( + urdf_path, hardware_path=hardware_path, has_freeflyer=True) + + # Set options + engine_options = simulator.get_options() + engine_options["telemetry"]["enableConfiguration"] = True + engine_options["stepper"]["odeSolver"] = "runge_kutta_4" + engine_options["stepper"]["dtMax"] = 1.0e-5 + engine_options["stepper"]["sensorsUpdatePeriod"] = 0.0 + engine_options["stepper"]["controllerUpdatePeriod"] = 1.0e-3 + engine_options["stepper"]["logInternalStepperSteps"] = True + engine_options['contacts']['model'] = "constraint" + engine_options['contacts']['stabilizationFreq'] = 0.0 + engine_options['constraints']['regularization'] = 1e-9 + simulator.set_options(engine_options) + + # Initialize the simulation + q0 = np.array([0.0, 0.0, 0.005, 0.0, 0.0, 0.0, 1.0, 0.0]) + v0 = np.zeros((simulator.robot.pinocchio_model.nv,)) + mass = simulator.robot.pinocchio_data.mass[0] + gravity = engine_options["world"]["gravity"] + simulator.start(q0, v0) + + self.assertTrue(np.all(np.abs(simulator.stepper_state.a) < TOLERANCE)) + imu_data = simulator.robot.sensor_measurements[imu.type, 'Foot'] + gyro_data, accel_data = np.split(imu_data, 2) + self.assertTrue(np.allclose(gyro_data, 0.0, atol=TOLERANCE)) + self.assertTrue(np.allclose(accel_data, -gravity[:3], atol=TOLERANCE)) + self.assertTrue(np.allclose( + simulator.robot.sensor_measurements[force.type, 'Foot'], + -gravity * mass, + atol=TOLERANCE)) + for i in range(3): + self.assertTrue(np.allclose( + simulator.robot.sensor_measurements[ + contact.type, f'Foot_CollisionBox_0_{2 * i}'], + simulator.robot.sensor_measurements[ + contact.type, f'Foot_CollisionBox_0_{2 * (i + 1)}'], + atol=TOLERANCE)) + with self.assertRaises(KeyError): + simulator.robot.sensor_measurements[contact.type, 'NA'] + with self.assertRaises(KeyError): + simulator.robot.sensor_measurements['NA', 'Foot_CollisionBox_0_1'] + + # Simulate for a few seconds + simulator.step(1.0) + simulator.stop() + + self.assertTrue(np.all(np.abs(simulator.stepper_state.v) < TOLERANCE)) + self.assertTrue(np.all(np.abs(simulator.stepper_state.a) < TOLERANCE)) + + +if __name__ == '__main__': + unittest.main() 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/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py index 11887c598..dbe474f4a 100644 --- a/python/jiminy_py/unit_py/test_simple_pendulum.py +++ b/python/jiminy_py/unit_py/test_simple_pendulum.py @@ -83,19 +83,16 @@ def _simulate_and_get_imu_data_evolution( # Extract state evolution over time time = log_vars['Global.Time'] + imu_jiminy = np.stack([ + log_vars['.'.join((jiminy.ImuSensor.type, 'PendulumLink', field))] + for field in jiminy.ImuSensor.fieldnames], axis=-1) + + # Split IMU data if requested if split: - gyro_jiminy = np.stack([ - log_vars['PendulumLink.Gyro' + s] for s in ['x', 'y', 'z'] - ], axis=-1) - accel_jiminy = np.stack([ - log_vars['PendulumLink.Accel' + s] for s in ['x', 'y', 'z'] - ], axis=-1) + gyro_jiminy, accel_jiminy = np.split(imu_jiminy, 2, axis=-1) return time, gyro_jiminy, accel_jiminy - else: - imu_jiminy = np.stack([ - log_vars['PendulumLink.' + f] - for f in jiminy.ImuSensor.fieldnames], axis=-1) - return time, imu_jiminy + + return time, imu_jiminy def test_armature(self): """Verify the dynamics of the system when adding rotor inertia. 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 107d39911..09608615a 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); @@ -440,7 +440,8 @@ namespace jiminy::python } else { - constants[key] = value; // convertToPython(value, false); + constants[key] = bp::object( + bp::handle<>(PyBytes_FromStringAndSize(value.c_str(), value.size()))); } } @@ -596,6 +597,13 @@ namespace jiminy::python convertFromPython(configPy, config); return self.setOptions(config); } + + static void setAllOptions(Engine & self, const bp::dict & configPy) + { + GenericConfig config = self.getAllOptions(); + convertFromPython(configPy, config); + return self.setAllOptions(config); + } } void exposeEngine() @@ -776,6 +784,8 @@ namespace jiminy::python .def("set_options", &internal::engine::setOptions) .def("get_options", &Engine::getOptions) + .def("set_all_options", &internal::engine::setAllOptions) + .def("get_all_options", &Engine::getAllOptions) .DEF_READONLY_WITH_POLICY( "robots", &Engine::robots_, bp::return_value_policy>()) diff --git a/python/jiminy_pywrap/src/sensors.cc b/python/jiminy_pywrap/src/sensors.cc index 725ad443c..9f14176ec 100644 --- a/python/jiminy_pywrap/src/sensors.cc +++ b/python/jiminy_pywrap/src/sensors.cc @@ -291,7 +291,6 @@ namespace jiminy::python // clang-format off cl .def_readonly("type", &DerivedSensor::type_) - .def_readonly("has_prefix", &DerivedSensor::areFieldnamesGrouped_) .add_static_property("fieldnames", bp::make_getter(&DerivedSensor::fieldnames_, bp::return_value_policy>())) ;