Skip to content

Commit

Permalink
[core] Add methods to get / set all simulation options at once. (#760)
Browse files Browse the repository at this point in the history 
* [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 <alexis.duburcq@wandercraft.eu>
  • Loading branch information
@duburcqa
duburcqa and Alexis Duburcq authored Apr 13, 2024
1 parent f7714a2 commit d4e753c
Show file tree
Hide file tree
Showing 41 changed files with 354 additions and 268 deletions.
1 change: 1 addition & 0 deletions core/examples/double_pendulum/double_pendulum.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ===============
Expand Down
1 change: 1 addition & 0 deletions core/examples/external_project/double_pendulum.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ===============
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/control/abstract_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@
namespace jiminy
{
/// \brief Namespace of the telemetry object.
inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"HighLevelController"};
inline constexpr std::string_view CONTROLLER_TELEMETRY_NAMESPACE{"controller"};

class TelemetrySender;
class TelemetryData;
Expand Down
14 changes: 13 additions & 1 deletion core/include/jiminy/core/engine/engine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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<Robot> getRobot(const std::string & robotName);
std::ptrdiff_t getRobotIndex(const std::string & robotName) const;
Expand Down
1 change: 0 additions & 1 deletion core/include/jiminy/core/hardware/abstract_sensor.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> JIMINY_STATIC_MEMBER_DLLAPI fieldnames_;
static const bool JIMINY_STATIC_MEMBER_DLLAPI areFieldnamesGrouped_;

protected:
std::size_t sensorIndex_{0};
Expand Down
9 changes: 1 addition & 8 deletions core/include/jiminy/core/hardware/abstract_sensor.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -259,14 +259,7 @@ namespace jiminy
template<typename T>
std::string AbstractSensorTpl<T>::getTelemetryName() const
{
if (areFieldnamesGrouped_)
{
return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER);
}
else
{
return name_;
}
return addCircumfix(name_, getType(), {}, TELEMETRY_FIELDNAME_DELIMITER);
}

template<typename T>
Expand Down
10 changes: 0 additions & 10 deletions core/include/jiminy/core/hardware/basic_sensors.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -16,8 +16,6 @@ namespace jiminy
const std::string JIMINY_DLLAPI AbstractSensorTpl<ImuSensor>::type_;
template<>
const std::vector<std::string> JIMINY_DLLAPI AbstractSensorTpl<ImuSensor>::fieldnames_;
template<>
const bool JIMINY_DLLAPI AbstractSensorTpl<ImuSensor>::areFieldnamesGrouped_;
#endif
template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl<ImuSensor>;

Expand Down Expand Up @@ -58,8 +56,6 @@ namespace jiminy
const std::string JIMINY_DLLAPI AbstractSensorTpl<ContactSensor>::type_;
template<>
const std::vector<std::string> JIMINY_DLLAPI AbstractSensorTpl<ContactSensor>::fieldnames_;
template<>
const bool JIMINY_DLLAPI AbstractSensorTpl<ContactSensor>::areFieldnamesGrouped_;
#endif
template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl<ContactSensor>;

Expand Down Expand Up @@ -98,8 +94,6 @@ namespace jiminy
const std::string JIMINY_DLLAPI AbstractSensorTpl<ForceSensor>::type_;
template<>
const std::vector<std::string> JIMINY_DLLAPI AbstractSensorTpl<ForceSensor>::fieldnames_;
template<>
const bool JIMINY_DLLAPI AbstractSensorTpl<ForceSensor>::areFieldnamesGrouped_;
#endif
template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl<ForceSensor>;

Expand Down Expand Up @@ -141,8 +135,6 @@ namespace jiminy
const std::string JIMINY_DLLAPI AbstractSensorTpl<EncoderSensor>::type_;
template<>
const std::vector<std::string> JIMINY_DLLAPI AbstractSensorTpl<EncoderSensor>::fieldnames_;
template<>
const bool JIMINY_DLLAPI AbstractSensorTpl<EncoderSensor>::areFieldnamesGrouped_;
#endif
template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl<EncoderSensor>;

Expand Down Expand Up @@ -182,8 +174,6 @@ namespace jiminy
const std::string JIMINY_DLLAPI AbstractSensorTpl<EffortSensor>::type_;
template<>
const std::vector<std::string> JIMINY_DLLAPI AbstractSensorTpl<EffortSensor>::fieldnames_;
template<>
const bool JIMINY_DLLAPI AbstractSensorTpl<EffortSensor>::areFieldnamesGrouped_;
#endif
template class JIMINY_TEMPLATE_INSTANTIATION_DLLAPI AbstractSensorTpl<EffortSensor>;

Expand Down
12 changes: 6 additions & 6 deletions core/include/jiminy/core/utilities/helpers.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string> JIMINY_DLLAPI addCircumfix(
const std::vector<std::string> & 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);
Expand Down
62 changes: 42 additions & 20 deletions core/src/engine/engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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<const GenericConfig>(allOptions.at("engine")));
for (auto & robot : robots_)
{
std::string robotOptionName = robot->getName();
if (robotOptionName.empty())
{
robotOptionName = "robot";
}
robot->setOptions(boost::get<const GenericConfig>(allOptions.at(robotOptionName)));
}
}

std::ptrdiff_t Engine::getRobotIndex(const std::string & robotName) const
{
auto robotIt = std::find_if(robots_.begin(),
Expand Down
12 changes: 1 addition & 11 deletions core/src/hardware/basic_sensors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,9 +66,7 @@ namespace jiminy
const std::string AbstractSensorTpl<ImuSensor>::type_{"ImuSensor"};
template<>
const std::vector<std::string> AbstractSensorTpl<ImuSensor>::fieldnames_{
"Gyrox", "Gyroy", "Gyroz", "Accelx", "Accely", "Accelz"};
template<>
const bool AbstractSensorTpl<ImuSensor>::areFieldnamesGrouped_{false};
"GyroX", "GyroY", "GyroZ", "AccelX", "AccelY", "AccelZ"};

void ImuSensor::initialize(const std::string & frameName)
{
Expand Down Expand Up @@ -200,8 +198,6 @@ namespace jiminy
const std::string AbstractSensorTpl<ContactSensor>::type_{"ContactSensor"};
template<>
const std::vector<std::string> AbstractSensorTpl<ContactSensor>::fieldnames_{"FX", "FY", "FZ"};
template<>
const bool AbstractSensorTpl<ContactSensor>::areFieldnamesGrouped_{false};

void ContactSensor::initialize(const std::string & frameName)
{
Expand Down Expand Up @@ -297,8 +293,6 @@ namespace jiminy
template<>
const std::vector<std::string> AbstractSensorTpl<ForceSensor>::fieldnames_{
"FX", "FY", "FZ", "MX", "MY", "MZ"};
template<>
const bool AbstractSensorTpl<ForceSensor>::areFieldnamesGrouped_{false};

void ForceSensor::initialize(const std::string & frameName)
{
Expand Down Expand Up @@ -412,8 +406,6 @@ namespace jiminy
const std::string AbstractSensorTpl<EncoderSensor>::type_{"EncoderSensor"};
template<>
const std::vector<std::string> AbstractSensorTpl<EncoderSensor>::fieldnames_{"Q", "V"};
template<>
const bool AbstractSensorTpl<EncoderSensor>::areFieldnamesGrouped_{true};

void EncoderSensor::initialize(const std::string & jointName)
{
Expand Down Expand Up @@ -525,8 +517,6 @@ namespace jiminy
const std::string AbstractSensorTpl<EffortSensor>::type_{"EffortSensor"};
template<>
const std::vector<std::string> AbstractSensorTpl<EffortSensor>::fieldnames_{"U"};
template<>
const bool AbstractSensorTpl<EffortSensor>::areFieldnamesGrouped_{true};

void EffortSensor::initialize(const std::string & motorName)
{
Expand Down
22 changes: 10 additions & 12 deletions core/src/robot/robot.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
4 changes: 2 additions & 2 deletions core/unit/engine_sanity_check.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,7 +112,7 @@ TEST(EngineSanity, EnergyConservation)
std::shared_ptr<const LogData> logDataPtr = engine.getLog();
const Eigen::VectorXd timesCont = getLogVariable(*logDataPtr, "Global.Time");
ASSERT_DOUBLE_EQ(timesCont[timesCont.size() - 1], tf);
const Eigen::VectorXd energyCont = getLogVariable(*logDataPtr, "HighLevelController.energy");
const Eigen::VectorXd energyCont = getLogVariable(*logDataPtr, "energy");
ASSERT_GT(energyCont.size(), 0);

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

// Check that energy is constant
Expand Down
4 changes: 2 additions & 2 deletions docs/spec/src/tlmc_format_specification.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -58,7 +58,7 @@ GROUP "/" {
(0): 1
}
}
DATASET "HighLevelController.controlOffsetTimestamp" {
DATASET "controlOffsetTimestamp" {
DATATYPE H5T_STRING {
STRSIZE 8;
STRPAD H5T_STR_NULLPAD;
Expand All @@ -73,7 +73,7 @@ GROUP "/" {
...
}
GROUP "variables" {
GROUP "HighLevelController.currentPositionLeftSagittalHip" {
GROUP "currentPositionLeftSagittalHip" {
DATASET "time" {
DATATYPE H5T_STD_I64LE
DATASPACE SIMPLE { ( 338623 ) / ( 338623 ) }
Expand Down
Loading

0 comments on commit d4e753c

Please sign in to comment.