diff --git a/core/include/jiminy/core/io/serialization.hxx b/core/include/jiminy/core/io/serialization.hxx index 2465cb24cf..656c3b2d00 100644 --- a/core/include/jiminy/core/io/serialization.hxx +++ b/core/include/jiminy/core/io/serialization.hxx @@ -44,6 +44,8 @@ namespace jiminy namespace boost::serialization { + // *********************************** pinocchio geometry ********************************** // + template void load_construct_data( Archive & /* ar */, pinocchio::GeometryObject * geomPtr, const unsigned int /* version */) @@ -57,12 +59,7 @@ namespace boost::serialization template void - serialize(Archive & ar, pinocchio::GeometryModel & model, const unsigned int /* version */) - { - ar & make_nvp("ngeoms", model.ngeoms); - ar & make_nvp("geometryObjects", model.geometryObjects); - ar & make_nvp("collisionPairs", model.collisionPairs); - } + serialize(Archive & ar, pinocchio::GeometryModel & model, const unsigned int /* version */); } #endif // JIMINY_SERIALIZATION_HXX diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index 336f435d1b..fbfd596d20 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -912,26 +912,14 @@ namespace jiminy { return value; } - else if constexpr (is_vector_v) - { - std::ostringstream sstr; - const std::size_t size = value.size(); - for (std::size_t i = 0; i < size; ++i) - { - serialize(value[i]); - if (i < size) - { - sstr << ";"; - } - } - return sstr.str(); - } else if constexpr (std::is_arithmetic_v>) { return toString(value); } else { + /* Note that boost::serialization module natively supports passing raw pointers, + `std::shard_ptr`, and `std::vector`. */ return ::jiminy::saveToBinary(value); } } diff --git a/core/src/io/serialization.cc b/core/src/io/serialization.cc index 0c5808dc7c..8b687a19d1 100644 --- a/core/src/io/serialization.cc +++ b/core/src/io/serialization.cc @@ -8,6 +8,9 @@ # pragma warning(disable : 4267) /* conversion from 'size_t' to 'unsigned int' */ #endif +#include + + // Explicit template instantiation for serialization #define EXPL_TPL_INST_SERIALIZE_IMPL(A, ...) \ template void serialize(A &, __VA_ARGS__ &, const unsigned int); @@ -17,6 +20,10 @@ EXPL_TPL_INST_SERIALIZE_IMPL(boost::archive::binary_oarchive, __VA_ARGS__) +BOOST_CLASS_EXPORT(pinocchio::GeometryObject) +BOOST_CLASS_EXPORT(pinocchio::GeometryModel) + + namespace boost::serialization { template @@ -88,4 +95,15 @@ namespace boost::serialization } EXPLICIT_TEMPLATE_INSTANTIATION_SERIALIZE(pinocchio::GeometryObject) + + template + void + serialize(Archive & ar, pinocchio::GeometryModel & model, const unsigned int /* version */) + { + ar & make_nvp("ngeoms", model.ngeoms); + ar & make_nvp("geometryObjects", model.geometryObjects); + ar & make_nvp("collisionPairs", model.collisionPairs); + } + + EXPLICIT_TEMPLATE_INSTANTIATION_SERIALIZE(pinocchio::GeometryModel) } diff --git a/python/jiminy_pywrap/src/engine.cc b/python/jiminy_pywrap/src/engine.cc index 25d476c750..3bca0ddc02 100644 --- a/python/jiminy_pywrap/src/engine.cc +++ b/python/jiminy_pywrap/src/engine.cc @@ -358,6 +358,26 @@ namespace jiminy::python return self.registerProfileForce(robotName, frameName, forceFunc, updatePeriod); } + template + static void populatePythonDictFromBinary( + const std::string & key, const std::string & data, bp::dict & dict) + { + T obj; + try + { + ::jiminy::loadFromBinary(obj, data); + } + catch (const std::exception & e) + { + JIMINY_THROW(std::ios_base::failure, + "Failed to deserialize constant '", + key, + "' from log: ", + e.what()); + } + dict[key] = convertToPython(obj, true); + } + static bp::dict formatLogData(const LogData & logData) { // Early return if empty @@ -380,7 +400,15 @@ namespace jiminy::python // Get constants for (const auto & [key, value] : logData.constants) { - if (endsWith(key, "options")) + // Skip all constant that has been registered by the user from a controller + bool isUnknown = false; + if (key.find(CONTROLLER_TELEMETRY_NAMESPACE) != std::string::npos) + { + isUnknown = true; + } + + // Loop over all "special" constant that will be used to build the robot + else if (endsWith(key, "options")) { std::vector jsonStringVec(value.begin(), value.end()); std::shared_ptr device = @@ -391,44 +419,15 @@ namespace jiminy::python } else if (key.find("pinocchio_model") != std::string::npos) { - try - { - pinocchio::Model model; - ::jiminy::loadFromBinary(model, value); - constants[key] = model; - } - catch (const std::exception & e) - { - JIMINY_THROW(std::ios_base::failure, - "Failed to load pinocchio model from log: ", - e.what()); - } + populatePythonDictFromBinary(key, value, constants); } else if (endsWith(key, "visual_model") || endsWith(key, "collision_model")) { - try - { - pinocchio::GeometryModel geometryModel; - ::jiminy::loadFromBinary(geometryModel, value); - constants[key] = geometryModel; - } - catch (const std::exception & e) - { - JIMINY_THROW(std::ios_base::failure, - "Failed to load collision and/or visual model from log: ", - e.what()); - } + populatePythonDictFromBinary(key, value, constants); } else if (endsWith(key, "mesh_package_dirs")) { - bp::list meshPackageDirs; - std::stringstream ss(value); - std::string item; - while (getline(ss, item, ';')) - { - meshPackageDirs.append(item); - } - constants[key] = meshPackageDirs; + populatePythonDictFromBinary>(key, value, constants); } else if (key == NUM_INTS || key == NUM_FLOATS) { @@ -439,6 +438,12 @@ namespace jiminy::python constants[key] = std::stod(value); } else + { + isUnknown = true; + } + + // Fallback to simple forwarding the constant to Python as a bytes array + if (isUnknown) { constants[key] = bp::object( bp::handle<>(PyBytes_FromStringAndSize(value.c_str(), value.size())));