Skip to content

Commit

Permalink
[core] Introduce new terminology: 'Mechanical', 'Theoretical', 'Exten…
Browse files Browse the repository at this point in the history 
…ded'. (#753)

* [core] Clarify terminology: 'Rigid' by 'Mechanical', 'Original' by 'Theoretical', 'Actual' by 'Extended'.
* [core] More appropriate tolerance when checking GCD to avoid false positive.
* [core] User is now systematically responsible for serializing constants before telemetry registration.
* [core] Various cleanup and minor improvements.
  • Loading branch information
@duburcqa
duburcqa committed Apr 7, 2024
1 parent c954a3d commit 8101a68
Show file tree
Hide file tree
Showing 40 changed files with 767 additions and 843 deletions.
16 changes: 9 additions & 7 deletions core/include/jiminy/core/control/abstract_controller.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,15 @@ namespace jiminy
/// \param[in] robot Robot
virtual void initialize(std::weak_ptr<const Robot> robot);

/// \brief Register a constant (so-called invariant) to the telemetry.
///
/// \details The user is responsible to convert it as a byte array (eg `std::string`),
/// either using `toString` for arithmetic types or `saveToBinary` complex types.
///
/// \param[in] name Name of the constant.
/// \param[in] value Constant to add to the telemetry.
void registerConstant(const std::string_view & name, const std::string & value);

/// \brief Dynamically registered a scalar variable to the telemetry. It is the main entry
/// point for a user to log custom variables.
///
Expand Down Expand Up @@ -88,13 +97,6 @@ namespace jiminy
const Eigen::Ref<VectorX<int64_t>, 0, Eigen::Stride<Eigen::Dynamic, Eigen::Dynamic>> &
values);

/// \brief Register a constant float64 to the telemetry.
///
/// \param[in] name Name of the variable.
/// \param[in] values Variable to add to the telemetry
template<typename T>
void registerConstant(const std::string_view & name, const T & value);

/// \brief Remove all variables dynamically registered to the telemetry.
///
/// \details Note that one must reset Jiminy Engine for this to take effect.
Expand Down
29 changes: 1 addition & 28 deletions core/include/jiminy/core/control/abstract_controller.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,33 +7,6 @@

namespace jiminy
{
/// \brief Register a constant to the telemetry.
///
/// \param[in] fieldnames Name of the variable.
/// \param[in] values Variable to add to the telemetry.
template<typename T>
void AbstractController::registerConstant(const std::string_view & fieldname, const T & value)
{
// Delayed variable registration (Taken into account by 'configureTelemetry')

if (isTelemetryConfigured_)
{
THROW_ERROR(bad_control_flow,
"Telemetry already initialized. Impossible to register new variables.");
}

// Check in local cache before.
auto constantIt = std::find_if(constantRegistry_.begin(),
constantRegistry_.end(),
[&fieldname](const auto & element)
{ return element.first == fieldname; });
if (constantIt != constantRegistry_.end())
{
THROW_ERROR(bad_control_flow, "Constant already registered.");
}
constantRegistry_.emplace_back(fieldname, toString(value));
}

template<typename T>
void AbstractController::registerVariable(const std::string_view & name, const T & value)
{
Expand All @@ -50,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
14 changes: 7 additions & 7 deletions core/include/jiminy/core/fwd.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -194,10 +194,10 @@ namespace jiminy
const Eigen::Vector2d & /* xy */, double & /* height */, Eigen::Vector3d & /* normal */)>;

// Flexible joints
struct FlexibleJointData
struct FlexibilityJointConfig
{
// FIXME: Replace by default spaceship operator `<=>` when moving to C++20.
inline bool operator==(const FlexibleJointData & other) const noexcept
inline bool operator==(const FlexibilityJointConfig & other) const noexcept
{
return (this->frameName == other.frameName && this->stiffness == other.stiffness &&
this->damping == other.damping && this->inertia == other.inertia);
Expand All @@ -209,7 +209,7 @@ namespace jiminy
Eigen::Vector3d inertia{};
};

using FlexibilityConfig = std::vector<FlexibleJointData>;
using FlexibilityConfig = std::vector<FlexibilityJointConfig>;

using GenericConfig =
std::unordered_map<std::string,
Expand All @@ -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
87 changes: 45 additions & 42 deletions core/include/jiminy/core/robot/model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -154,7 +154,7 @@ namespace jiminy
config["massBodiesBiasStd"] = 0.0;
config["centerOfMassPositionBodiesBiasStd"] = 0.0;
config["relativePositionBodiesBiasStd"] = 0.0;
config["enableFlexibleModel"] = true;
config["enableFlexibility"] = true;
config["flexibilityConfig"] = FlexibilityConfig{};

return config;
Expand Down Expand Up @@ -184,8 +184,7 @@ namespace jiminy
{
const bool enablePositionLimit;
const bool positionLimitFromUrdf;
/// \brief Min position limit of all the rigid joints, ie without freeflyer and
/// flexibility joints if any.
/// \brief Min position limit of all the mechanical joints of the theoretical model.
const Eigen::VectorXd positionLimitMin;
const Eigen::VectorXd positionLimitMax;
const bool enableVelocityLimit;
Expand All @@ -210,7 +209,7 @@ namespace jiminy
const double massBodiesBiasStd;
const double centerOfMassPositionBodiesBiasStd;
const double relativePositionBodiesBiasStd;
const bool enableFlexibleModel;
const bool enableFlexibility;
const FlexibilityConfig flexibilityConfig;

DynamicsOptions(const GenericConfig & options) :
Expand All @@ -220,7 +219,7 @@ namespace jiminy
boost::get<double>(options.at("centerOfMassPositionBodiesBiasStd"))},
relativePositionBodiesBiasStd{
boost::get<double>(options.at("relativePositionBodiesBiasStd"))},
enableFlexibleModel{boost::get<bool>(options.at("enableFlexibleModel"))},
enableFlexibility{boost::get<bool>(options.at("enableFlexibility"))},
flexibilityConfig{boost::get<FlexibilityConfig>(options.at("flexibilityConfig"))}
{
}
Expand Down Expand Up @@ -274,7 +273,8 @@ namespace jiminy
void addFrame(const std::string & frameName,
const std::string & parentBodyName,
const pinocchio::SE3 & framePlacement);
void removeFrame(const std::string & frameName);
void removeFrames(const std::vector<std::string> & frameNames);

void addCollisionBodies(const std::vector<std::string> & bodyNames,
bool ignoreMeshes = false);
void removeCollisionBodies(std::vector<std::string> frameNames = {}); // Copy on purpose
Expand Down Expand Up @@ -345,12 +345,12 @@ namespace jiminy
const std::vector<std::vector<pinocchio::PairIndex>> & getCollisionPairIndices() const;
const std::vector<pinocchio::FrameIndex> & getContactFrameIndices() const;

const std::vector<std::string> & getRigidJointNames() const;
const std::vector<pinocchio::JointIndex> & getRigidJointIndices() const;
const std::vector<Eigen::Index> & getRigidJointPositionIndices() const;
const std::vector<Eigen::Index> & getRigidJointVelocityIndices() const;
const std::vector<std::string> & getFlexibleJointNames() const;
const std::vector<pinocchio::JointIndex> & getFlexibleJointIndices() const;
const std::vector<std::string> & getMechanicalJointNames() const;
const std::vector<pinocchio::JointIndex> & getMechanicalJointIndices() const;
const std::vector<Eigen::Index> & getMechanicalJointPositionIndices() const;
const std::vector<Eigen::Index> & getMechanicalJointVelocityIndices() const;
const std::vector<std::string> & getFlexibilityJointNames() const;
const std::vector<pinocchio::JointIndex> & getFlexibilityJointIndices() const;

const Eigen::VectorXd & getPositionLimitMin() const;
const Eigen::VectorXd & getPositionLimitMax() const;
Expand All @@ -361,24 +361,27 @@ namespace jiminy
const std::vector<std::string> & getLogAccelerationFieldnames() const;
const std::vector<std::string> & getLogForceExternalFieldnames() const;

void getFlexiblePositionFromRigid(const Eigen::VectorXd & qRigid,
Eigen::VectorXd & qFlex) const;
void getRigidPositionFromFlexible(const Eigen::VectorXd & qFlex,
Eigen::VectorXd & qRigid) const;
void getFlexibleVelocityFromRigid(const Eigen::VectorXd & vRigid,
Eigen::VectorXd & vFlex) const;
void getRigidVelocityFromFlexible(const Eigen::VectorXd & vFlex,
Eigen::VectorXd & vRigid) const;
void getExtendedPositionFromTheoretical(const Eigen::VectorXd & qTheoretical,
Eigen::VectorXd & qExtended) const;
void getExtendedVelocityFromTheoretical(const Eigen::VectorXd & vTheoretical,
Eigen::VectorXd & vExtended) const;
void getTheoreticalPositionFromExtended(const Eigen::VectorXd & qExtended,
Eigen::VectorXd & qTheoretical) const;
void getTheoreticalVelocityFromExtended(const Eigen::VectorXd & vExtended,
Eigen::VectorXd & vTheoretical) const;

protected:
void generateModelFlexible();
void generateModelBiased(const uniform_random_bit_generator_ref<uint32_t> & g);
void generateModelExtended(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 All @@ -395,13 +398,13 @@ namespace jiminy
virtual void refreshProxies();

public:
pinocchio::Model pinocchioModelOrig_{};
pinocchio::Model pinocchioModelTh_{};
pinocchio::Model pinocchioModel_{};
pinocchio::GeometryModel collisionModelOrig_{};
pinocchio::GeometryModel collisionModelTh_{};
pinocchio::GeometryModel collisionModel_{};
pinocchio::GeometryModel visualModelOrig_{};
pinocchio::GeometryModel visualModelTh_{};
pinocchio::GeometryModel visualModel_{};
mutable pinocchio::Data pinocchioDataOrig_{};
mutable pinocchio::Data pinocchioDataTh_{};
mutable pinocchio::Data pinocchioData_{};
mutable pinocchio::GeometryData collisionData_{};
mutable pinocchio::GeometryData visualData_{};
Expand All @@ -427,22 +430,23 @@ namespace jiminy
std::vector<std::vector<pinocchio::PairIndex>> collisionPairIndices_{};
/// \brief Indices of the contact frames in the frame list of the robot.
std::vector<pinocchio::FrameIndex> contactFrameIndices_{};
/// \brief Name of the actual joints of the robot, not taking into account the freeflyer.
std::vector<std::string> rigidJointNames_{};
/// \brief Index of the actual joints in the pinocchio robot.
std::vector<pinocchio::JointIndex> rigidJointIndices_{};
/// \brief All the indices of the actual joints in the configuration vector of the robot
/// (ie including all the degrees of freedom).
std::vector<Eigen::Index> rigidJointPositionIndices_{};
/// \brief All the indices of the actual joints in the velocity vector of the robot (ie
/// including all the degrees of freedom).
std::vector<Eigen::Index> rigidJointVelocityIndices_{};
/// \brief Name of the mechanical joints of the robot, ie all joints of the theoretical
/// excluding freeflyer if any.
std::vector<std::string> mechanicalJointNames_{};
/// \brief Index of the mechanical joints in the pinocchio robot.
std::vector<pinocchio::JointIndex> mechanicalJointIndices_{};
/// \brief All the indices of the mechanical joints in the configuration vector of the
/// robot, ie including all their respective degrees of freedom.
std::vector<Eigen::Index> mechanicalJointPositionIndices_{};
/// \brief All the indices of the mechanical joints in the velocity vector of the robot,
/// ie including all their respective degrees of freedom.
std::vector<Eigen::Index> mechanicalJointVelocityIndices_{};
/// \brief Name of the flexibility joints of the robot regardless of whether the
/// flexibilities are enabled.
std::vector<std::string> flexibleJointNames_{};
std::vector<std::string> flexibilityJointNames_{};
/// \brief Index of the flexibility joints in the pinocchio robot regardless of whether the
/// flexibilities are enabled.
std::vector<pinocchio::JointIndex> flexibleJointIndices_{};
std::vector<pinocchio::JointIndex> flexibilityJointIndices_{};

/// \brief Store constraints.
ConstraintTree constraints_{};
Expand All @@ -466,9 +470,8 @@ namespace jiminy
std::vector<std::string> logForceExternalFieldnames_{};

private:
pinocchio::Model pncModelFlexibleOrig_{};
/// \brief Vector of joints acceleration corresponding to a copy of data.a - temporary
/// buffer for computing constraints.
/// \brief Vector of joints acceleration corresponding to a copy of data.a.
// Used for computing constraints as a temporary buffer.
MotionVector jointSpatialAccelerations_{};

Eigen::Index nq_{0};
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/stepper/abstract_stepper.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,7 +33,7 @@ namespace jiminy

/// \brief Generic interface for steppers used to integrate the dynamic equations of motion.
///
/// \details The generalized position of a mechanical system evolves not in a vector space, but
/// \details The generalized position of a dynamical system evolves not in a vector space, but
/// in a Lie group: this is visible for instance in the fact that quaternions have 4
/// components, but only 3 are degrees of freedom, as quaternion must remain unitary
/// to represent a rotation. As such, the velocity vector v is not the term-by-term
Expand Down
8 changes: 5 additions & 3 deletions core/include/jiminy/core/telemetry/telemetry_data.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -28,10 +28,12 @@ namespace jiminy
template<typename T>
T * registerVariable(const std::string & name);

/// \brief Register a constant for the telemetry.
/// \brief Register a constant (so-called invariant) to the telemetry.
///
/// \param[in] name Name of the invariant.
/// \param[in] value Value of the invariant.
/// \details The user is responsible to convert it as a byte array (eg `std::string`).
///
/// \param[in] name Name of the constant.
/// \param[in] value Value of the constant.
void registerConstant(const std::string & name, const std::string & value);

/// \brief Format the telemetry header with the current recorded informations.
Expand Down
4 changes: 3 additions & 1 deletion core/include/jiminy/core/telemetry/telemetry_data.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,9 @@ namespace jiminy
if (!isRegisteringAvailable_)
{
THROW_ERROR(std::invalid_argument,
"Entry not found and registration is not available.");
"Entry '",
name,
"' not found and registration is not available.");
}

// Create new variable in registry
Expand Down
10 changes: 6 additions & 4 deletions core/include/jiminy/core/telemetry/telemetry_sender.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ namespace jiminy
/// \brief Register a new variable to the telemetry.
///
/// \details A variable must be registered to be taken into account by the telemetry. The
/// user is responsible for managing its lifetime and updating it in-place. The
/// user is responsible for managing its lifetime and updating it in-place. The
/// telemetry sender will fetch its value when calling 'updateValues'.
///
/// \param[in] name Name of the field to record in the telemetry.
Expand All @@ -55,10 +55,12 @@ namespace jiminy
void registerVariable(const std::vector<KeyType> & fieldnames,
const Eigen::MatrixBase<Derived> & values);

/// \brief Add an invariant header entry in the log file.
/// \brief Register a constant (so-called invariant) to the telemetry.
///
/// \param[in] name Name of the invariant.
/// \param[in] value Value of the invariant.
/// \details The user is responsible to convert it as a byte array (eg `std::string`).
///
/// \param[in] name Name of the constant.
/// \param[in] value Value of the constant.
void registerConstant(const std::string & name, const std::string & value);

/// \brief Update all registered variables in the telemetry buffer.
Expand Down
7 changes: 2 additions & 5 deletions core/include/jiminy/core/utilities/helpers.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,11 +109,8 @@ namespace jiminy
bool isIncluded = (
[&valueMin](double value)
{
if (value < EPS)
{
return true;
}
return std::fmod(value, valueMin) < EPS;
// No need to be more accurate than the minimum stepper update period
return std::fmod(value, valueMin) < STEPPER_MIN_TIMESTEP;
}(values) && ...);
return {isIncluded, valueMin};
}
Expand Down
8 changes: 4 additions & 4 deletions core/include/jiminy/core/utilities/json.hxx
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ namespace jiminy
}

template<>
Json::Value convertToJson<FlexibleJointData>(const FlexibleJointData & value);
Json::Value convertToJson<FlexibilityJointConfig>(const FlexibilityJointConfig & value);

template<>
Json::Value convertToJson<HeightmapFunction>(const HeightmapFunction & value);
Expand All @@ -76,8 +76,8 @@ namespace jiminy
}

template<>
constexpr const char *
getJsonVectorType<FlexibleJointData>(const std::vector<FlexibleJointData> & /* value */)
constexpr const char * getJsonVectorType<FlexibilityJointConfig>(
const std::vector<FlexibilityJointConfig> & /* value */)
{
return "list(flexibility)";
}
Expand Down Expand Up @@ -139,7 +139,7 @@ namespace jiminy
Eigen::MatrixXd convertFromJson<Eigen::MatrixXd>(const Json::Value & value);

template<>
FlexibleJointData convertFromJson<FlexibleJointData>(const Json::Value & value);
FlexibilityJointConfig convertFromJson<FlexibilityJointConfig>(const Json::Value & value);

template<>
HeightmapFunction convertFromJson<HeightmapFunction>(const Json::Value & value);
Expand Down
Loading

0 comments on commit 8101a68

Please sign in to comment.