Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[core] Introduce new terminology: 'Mechanical', 'Theoretical', 'Extended'. #753

Merged
merged 4 commits into from
Apr 7, 2024
Merged

[core] Introduce new terminology: 'Mechanical', 'Theoretical', 'Extended'. #753

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f04df76
[core] Clarify terminology: 'Rigid' by 'Mechanical', 'Original' by 'T…
duburcqa Apr 6, 2024
638d30b
[core] More appropriate tolerance when checking GCD to avoid false po…
duburcqa Apr 6, 2024
32874fe
[core] User is now systematically responsible for serializing constan…
duburcqa Apr 7, 2024
7228ae8
[core] Various cleanup and minor improvements.
duburcqa Apr 7, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 9 additions & 7 deletions core/include/jiminy/core/control/abstract_controller.h
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading