Skip to content

Commit

Permalink
[core] More robust serialization/deserialization from log.
Browse files Browse the repository at this point in the history
  • Loading branch information
@duburcqa
duburcqa committed Apr 22, 2024
1 parent 1cf65cb commit df7914c
Show file tree
Hide file tree
Showing 4 changed files with 61 additions and 53 deletions.
9 changes: 3 additions & 6 deletions core/include/jiminy/core/io/serialization.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,8 @@ namespace jiminy

namespace boost::serialization
{
// *********************************** pinocchio geometry ********************************** //

template<class Archive>
void load_construct_data(
Archive & /* ar */, pinocchio::GeometryObject * geomPtr, const unsigned int /* version */)
Expand All @@ -57,12 +59,7 @@ namespace boost::serialization

template<class Archive>
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
16 changes: 2 additions & 14 deletions core/src/engine/engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -912,26 +912,14 @@ namespace jiminy
{
return value;
}
else if constexpr (is_vector_v<T>)
{
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<std::decay_t<T>>)
{
return toString(value);
}
else
{
/* Note that boost::serialization module natively supports passing raw pointers,
`std::shard_ptr`, and `std::vector`. */
return ::jiminy::saveToBinary(value);
}
}
Expand Down
18 changes: 18 additions & 0 deletions core/src/io/serialization.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,9 @@
# pragma warning(disable : 4267) /* conversion from 'size_t' to 'unsigned int' */
#endif

#include <boost/serialization/export.hpp>


// Explicit template instantiation for serialization
#define EXPL_TPL_INST_SERIALIZE_IMPL(A, ...) \
template void serialize(A &, __VA_ARGS__ &, const unsigned int);
Expand All @@ -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<class Archive>
Expand Down Expand Up @@ -88,4 +95,15 @@ namespace boost::serialization
}

EXPLICIT_TEMPLATE_INSTANTIATION_SERIALIZE(pinocchio::GeometryObject)

template<class Archive>
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)
}
71 changes: 38 additions & 33 deletions python/jiminy_pywrap/src/engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -358,6 +358,26 @@ namespace jiminy::python
return self.registerProfileForce(robotName, frameName, forceFunc, updatePeriod);
}

template<typename T>
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
Expand All @@ -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<uint8_t> jsonStringVec(value.begin(), value.end());
std::shared_ptr<AbstractIODevice> device =
Expand All @@ -391,44 +419,15 @@ namespace jiminy::python
}
else if (key.find("pinocchio_model") != std::string::npos)
{
try
{
pinocchio::Model model;
::jiminy::loadFromBinary<pinocchio::Model>(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<pinocchio::Model>(key, value, constants);
}
else if (endsWith(key, "visual_model") || endsWith(key, "collision_model"))
{
try
{
pinocchio::GeometryModel geometryModel;
::jiminy::loadFromBinary<pinocchio::GeometryModel>(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<pinocchio::GeometryModel>(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<std::vector<std::string>>(key, value, constants);
}
else if (key == NUM_INTS || key == NUM_FLOATS)
{
Expand All @@ -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())));
Expand Down

0 comments on commit df7914c

Please sign in to comment.