Skip to content

Commit

Permalink
[core] Various cleanup and minor improvements.
Browse files Browse the repository at this point in the history
  • Loading branch information
@duburcqa
duburcqa committed Apr 7, 2024
1 parent 32874fe commit e9da859
Show file tree
Hide file tree
Showing 18 changed files with 231 additions and 256 deletions.
2 changes: 1 addition & 1 deletion core/include/jiminy/core/control/abstract_controller.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,7 +23,7 @@ namespace jiminy
{ return element.first == name; });
if (variableIt != variableRegistry_.end())
{
THROW_ERROR(bad_control_flow, "Variable already registered.");
THROW_ERROR(bad_control_flow, "Variable '", name, "' already registered.");
}
variableRegistry_.emplace_back(name, &value);
}
Expand Down
8 changes: 4 additions & 4 deletions core/include/jiminy/core/fwd.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -234,20 +234,20 @@ namespace jiminy
std::string toString(Args &&... args)
{
std::ostringstream sstr;
auto format = [](auto && var)
auto format = [&sstr](auto && var)
{
if constexpr (is_eigen_any_v<decltype(var)>)
{
static const Eigen::IOFormat k_heavy_fmt(
Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]");
return var.format(k_heavy_fmt);
sstr << var.format(k_heavy_fmt);
}
else
{
return var;
sstr << var;
}
};
((sstr << format(std::forward<Args>(args))), ...);
(format(std::forward<Args>(args)), ...);
return sstr.str();
}
}
Expand Down
5 changes: 3 additions & 2 deletions core/include/jiminy/core/robot/model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -372,15 +372,16 @@ namespace jiminy

protected:
void generateModelExtended(const uniform_random_bit_generator_ref<uint32_t> & g);
void generateModelBiased(const uniform_random_bit_generator_ref<uint32_t> & g);

void addFlexibilityJointsToExtendedModel();
void addBiasedToExtendedModel(const uniform_random_bit_generator_ref<uint32_t> & g);

void addFrame(const std::string & frameName,
const std::string & parentBodyName,
const pinocchio::SE3 & framePlacement,
const pinocchio::FrameType & frameType);
void removeFrames(const std::vector<std::string> & frameNames);
void removeFrames(const std::vector<std::string> & frameNames,
const std::vector<pinocchio::FrameType> & filter);

void addConstraint(const std::string & constraintName,
const std::shared_ptr<AbstractConstraintBase> & constraint,
Expand Down
9 changes: 4 additions & 5 deletions core/include/jiminy/core/utilities/pinocchio.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,12 +72,11 @@ namespace jiminy
pinocchio::JointIndex jointIndex1,
pinocchio::JointIndex jointIndex2);

void insertFlexibilityBeforeJointInModel(pinocchio::Model & model,
const std::string & childJointName,
const std::string & newJointName);
void addFlexibilityJointBeforeMechanicalJoint(pinocchio::Model & model,
const std::string & childJointName,
const std::string & newJointName);

void insertFlexibilityAtFixedFrameInModel(pinocchio::Model & model,
const std::string & frameName);
void addFlexibilityJointAtFixedFrame(pinocchio::Model & model, const std::string & frameName);

Eigen::MatrixXd JIMINY_DLLAPI interpolatePositions(const pinocchio::Model & model,
const Eigen::VectorXd & timesIn,
Expand Down
107 changes: 64 additions & 43 deletions core/src/engine/engine.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -890,6 +890,37 @@ namespace jiminy
}
}

template<typename T>
static std::string serialize(T && value)
{
if constexpr (std::is_same_v<std::string, std::decay_t<T>>)
{
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
{
return ::jiminy::saveToBinary(value);
}
};

void Engine::start(const std::map<std::string, Eigen::VectorXd> & qInit,
const std::map<std::string, Eigen::VectorXd> & vInit,
const std::optional<std::map<std::string, Eigen::VectorXd>> & aInit)
Expand Down Expand Up @@ -1442,66 +1473,56 @@ namespace jiminy
// Log robots data
for (const auto & robot : robots_)
{
// Define helper to make it easy to register a robot member as a constant
auto registerRobotMember = [&](const std::string & name, auto && member)
{
// Prepend robot name to variable name
const std::string key =
addCircumfix(name, robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);

// Dump serialized constant
if constexpr (std::is_member_function_pointer_v<std::decay_t<decltype(member)>>)
{
telemetrySender_->registerConstant(key, serialize((robot.get()->*member)()));
}
else
{
telemetrySender_->registerConstant(key, serialize(robot.get()->*member));
}
};

// Backup URDF file
const std::string telemetryUrdfFile =
addCircumfix("urdf_file", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);
const std::string & urdfFileString = robot->getUrdfAsString();
telemetrySender_->registerConstant(telemetryUrdfFile, urdfFileString);
registerRobotMember("urdf_file", &Robot::getUrdfAsString);

// Backup 'has_freeflyer' option
const std::string telemetrHasFreeflyer =
addCircumfix("has_freeflyer", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);
telemetrySender_->registerConstant(telemetrHasFreeflyer,
toString(robot->getHasFreeflyer()));
registerRobotMember("has_freeflyer", &Robot::getHasFreeflyer);

// Backup mesh package lookup directories
const std::string telemetryMeshPackageDirs = addCircumfix(
"mesh_package_dirs", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);
std::string meshPackageDirsString;
std::stringstream meshPackageDirsStream;
const std::vector<std::string> & meshPackageDirs = robot->getMeshPackageDirs();
copy(meshPackageDirs.begin(),
meshPackageDirs.end(),
std::ostream_iterator<std::string>(meshPackageDirsStream, ";"));
if (meshPackageDirsStream.peek() !=
decltype(meshPackageDirsStream)::traits_type::eof())
{
meshPackageDirsString = meshPackageDirsStream.str();
meshPackageDirsString.pop_back();
}
telemetrySender_->registerConstant(telemetryMeshPackageDirs, meshPackageDirsString);
registerRobotMember("mesh_package_dirs", &Robot::getMeshPackageDirs);

// Backup the true and theoretical Pinocchio::Model
std::string key = addCircumfix(
"pinocchio_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);
std::string value = saveToBinary(robot->pinocchioModel_);
telemetrySender_->registerConstant(key, value);
// Backup the theoretical model and current extended model
registerRobotMember("pinocchio_model_th", &Robot::pinocchioModelTh_);
registerRobotMember("pinocchio_model", &Robot::pinocchioModel_);

/* Backup the Pinocchio GeometryModel for collisions and visuals.
It may fail because of missing serialization methods for convex, or because it
cannot fit into memory (return code). Persistent mode is automatically enforced
if no URDF is associated with the robot.*/
if (engineOptions_->telemetry.isPersistent || urdfFileString.empty())
cannot fit into memory.
Persistent mode is automatically enabled if no URDF is associated with the robot. */
const bool hasUrdfFile = !robot->getUrdfAsString().empty();
if (engineOptions_->telemetry.isPersistent || !hasUrdfFile)
{
try
{
key = addCircumfix(
"collision_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);
value = saveToBinary(robot->collisionModel_);
telemetrySender_->registerConstant(key, value);

key = addCircumfix(
"visual_model", robot->getName(), {}, TELEMETRY_FIELDNAME_DELIMITER);
value = saveToBinary(robot->visualModel_);
telemetrySender_->registerConstant(key, value);
registerRobotMember("collision_model", &Robot::collisionModel_);
registerRobotMember("visual_model", &Robot::visualModel_);
}
catch (const std::exception & e)
{
std::string msg{"Failed to log the collision and/or visual model."};
if (urdfFileString.empty())
if (!hasUrdfFile)
{
msg += " It will be impossible to replay log files because no URDF "
"file is available as fallback.";
msg += "\nIt will be impossible to replay log files because no URDF file "
"is available as fallback.";
}
msg += "\nRaised from exception: ";
PRINT_WARNING(msg, e.what());
Expand Down
Loading

0 comments on commit e9da859

Please sign in to comment.