Skip to content

Commit

Permalink
[python|gym] Add contraint lambda multipliers to state and extracted …
Browse files Browse the repository at this point in the history 
…trajectory. (#809)

* [core] Add option to log constraint lambda multipliers.
* [core] Replace constraint tree node terminolgy for registry for clarity. 
* [core] Enforce unique constraint name per registry not globally, then remove now irrelevant helpers.
* [core] Make sure no constraint is added/removed if simulation is running.
* [python|gym] Add contraint lambda multipliers to state and extracted trajectory.
* [python/dynamics] Fix trajectory interpolation preferring t+ over t-.
* [gym/common] Refresh state quantities only if needed.
  • Loading branch information
@duburcqa
duburcqa authored Jun 17, 2024
1 parent 2f1a834 commit 1e9499c
Show file tree
Hide file tree
Showing 32 changed files with 1,072 additions and 699 deletions.
1 change: 1 addition & 0 deletions .pylintrc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -71,6 +71,7 @@ disable =
useless-parent-delegation,
use-dict-literal,
unspecified-encoding,
undefined-loop-variable,
cyclic-import

# Enable the message, report, category or checker with the given id(s)
Expand Down
2 changes: 1 addition & 1 deletion core/include/jiminy/core/constraints/abstract_constraint.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ namespace jiminy
virtual const std::string & getType() const = 0;

/// \brief Dimension of the constraint.
uint64_t getDim() const;
uint64_t getSize() const;

/// \brief Jacobian of the constraint.
const Eigen::MatrixXd & getJacobian() const;
Expand Down
4 changes: 4 additions & 0 deletions core/include/jiminy/core/engine/engine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -201,6 +201,7 @@ namespace jiminy
std::vector<std::string> logAccelerationFieldnames{};
std::vector<std::string> logEffortFieldnames{};
std::vector<std::string> logForceExternalFieldnames{};
std::vector<std::string> logConstraintFieldnames{};
std::vector<std::string> logCommandFieldnames{};
std::string logEnergyFieldname{};

Expand Down Expand Up @@ -329,6 +330,7 @@ namespace jiminy
config["enableVelocity"] = true;
config["enableAcceleration"] = true;
config["enableForceExternal"] = false;
config["enableConstraint"] = false;
config["enableCommand"] = false;
config["enableEffort"] = false;
config["enableEnergy"] = false;
Expand Down Expand Up @@ -438,6 +440,7 @@ namespace jiminy
const bool enableVelocity;
const bool enableAcceleration;
const bool enableForceExternal;
const bool enableConstraint;
const bool enableCommand;
const bool enableEffort;
const bool enableEnergy;
Expand All @@ -448,6 +451,7 @@ namespace jiminy
enableVelocity{boost::get<bool>(options.at("enableVelocity"))},
enableAcceleration{boost::get<bool>(options.at("enableAcceleration"))},
enableForceExternal{boost::get<bool>(options.at("enableForceExternal"))},
enableConstraint{boost::get<bool>(options.at("enableConstraint"))},
enableCommand{boost::get<bool>(options.at("enableCommand"))},
enableEffort{boost::get<bool>(options.at("enableEffort"))},
enableEnergy{boost::get<bool>(options.at("enableEnergy"))}
Expand Down
87 changes: 41 additions & 46 deletions core/include/jiminy/core/robot/model.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -21,12 +21,14 @@ namespace jiminy
class AbstractConstraintBase;
class FrameConstraint;
class JointConstraint;
class MutexLocal;
class LockGuardLocal;

// ************************************** Constraints ************************************** //

using ConstraintMap = static_map_t<std::string, std::shared_ptr<AbstractConstraintBase>>;

enum class ConstraintNodeType : uint8_t
enum class ConstraintRegistryType : uint8_t
{
CONTACT_FRAMES = 0,
COLLISION_BODIES = 1,
Expand All @@ -37,77 +39,74 @@ namespace jiminy
/* Note that the following ordering plays a critical role as it determines in which order
`foreach` iterates over all the constraints. This has a directly effect on the solution
found by 'PGS' constraint solvers. */
inline constexpr std::array constraintNodeTypesAll{ConstraintNodeType::BOUNDS_JOINTS,
ConstraintNodeType::CONTACT_FRAMES,
ConstraintNodeType::COLLISION_BODIES,
ConstraintNodeType::USER};
inline constexpr std::array constraintNodeTypesAll{ConstraintRegistryType::BOUNDS_JOINTS,
ConstraintRegistryType::CONTACT_FRAMES,
ConstraintRegistryType::COLLISION_BODIES,
ConstraintRegistryType::USER};

struct JIMINY_DLLAPI ConstraintTree
{
public:
void clear() noexcept;

std::pair<ConstraintMap *, ConstraintMap::iterator> find(const std::string & key,
ConstraintNodeType node);
ConstraintRegistryType type);
std::pair<const ConstraintMap *, ConstraintMap::const_iterator> find(
const std::string & key, ConstraintNodeType node) const;
const std::string & key, ConstraintRegistryType type) const;

bool exist(const std::string & key) const;
bool exist(const std::string & key, ConstraintNodeType node) const;
bool exist(const std::string & key, ConstraintRegistryType type) const;

std::shared_ptr<AbstractConstraintBase> get(const std::string & key) const;
std::shared_ptr<AbstractConstraintBase> get(const std::string & key,
ConstraintNodeType node) const;
ConstraintRegistryType type) const;

void insert(const ConstraintMap & constraintMap, ConstraintNodeType node);
void insert(const ConstraintMap & constraintMap, ConstraintRegistryType type);

ConstraintMap::iterator erase(const std::string & key, ConstraintNodeType node);
ConstraintMap::iterator erase(const std::string & key, ConstraintRegistryType type);

void clear() noexcept;

template<typename Function>
void foreach(ConstraintNodeType node, Function && func) const
void foreach(ConstraintRegistryType type, Function && func) const
{
if (node == ConstraintNodeType::COLLISION_BODIES)
if (type == ConstraintRegistryType::COLLISION_BODIES)
{
for (auto & constraintMap : collisionBodies)
{
for (auto & constraintItem : constraintMap)
{
std::invoke(std::forward<Function>(func), constraintItem.second, node);
std::invoke(std::forward<Function>(func), constraintItem.second, type);
}
}
}
else
{
const ConstraintMap * constraintMapPtr;
switch (node)
switch (type)
{
case ConstraintNodeType::BOUNDS_JOINTS:
case ConstraintRegistryType::BOUNDS_JOINTS:
constraintMapPtr = &boundJoints;
break;
case ConstraintNodeType::CONTACT_FRAMES:
case ConstraintRegistryType::CONTACT_FRAMES:
constraintMapPtr = &contactFrames;
break;
case ConstraintNodeType::USER:
constraintMapPtr = &registry;
case ConstraintRegistryType::USER:
constraintMapPtr = &user;
break;
case ConstraintNodeType::COLLISION_BODIES:
case ConstraintRegistryType::COLLISION_BODIES:
default:
constraintMapPtr = nullptr;
}
for (const auto & constraintItem : *constraintMapPtr)
{
std::invoke(std::forward<Function>(func), constraintItem.second, node);
std::invoke(std::forward<Function>(func), constraintItem.second, type);
}
}
}

template<typename Function, std::size_t N>
void foreach(const std::array<ConstraintNodeType, N> & constraintsHolderTypes,
Function && func) const
void foreach(const std::array<ConstraintRegistryType, N> & types, Function && func) const
{
for (ConstraintNodeType node : constraintsHolderTypes)
for (ConstraintRegistryType type : types)
{
foreach(node, std::forward<Function>(func));
foreach(type, std::forward<Function>(func));
}
}

Expand All @@ -125,7 +124,7 @@ namespace jiminy
/// \brief Constraints registered by the engine to handle collision bounds.
std::vector<ConstraintMap> collisionBodies{};
/// \brief Constraints explicitly registered by user.
ConstraintMap registry{};
ConstraintMap user{};
};

// ***************************************** Model ***************************************** //
Expand Down Expand Up @@ -306,18 +305,8 @@ namespace jiminy
/// \param[in] constraintName Unique name identifying the kinematic constraint.
void removeConstraint(const std::string & constraintName);

/// \brief Pointer to the constraint referenced by constraintName
///
/// \param[in] constraintName Name of the constraint to get.
std::shared_ptr<AbstractConstraintBase> getConstraint(const std::string & constraintName);

std::weak_ptr<const AbstractConstraintBase> getConstraint(
const std::string & constraintName) const;

const ConstraintTree & getConstraints() const;

bool existConstraint(const std::string & constraintName) const;

/// \brief Returns true if at least one constraint is active on the robot.
bool hasConstraints() const;

Expand Down Expand Up @@ -371,6 +360,7 @@ namespace jiminy
const std::vector<std::string> & getLogAccelerationFieldnames() const;
const std::vector<std::string> & getLogEffortFieldnames() const;
const std::vector<std::string> & getLogForceExternalFieldnames() const;
const std::vector<std::string> & getLogConstraintFieldnames() const;

void getExtendedPositionFromTheoretical(const Eigen::VectorXd & qTheoretical,
Eigen::VectorXd & qExtended) const;
Expand All @@ -381,6 +371,9 @@ namespace jiminy
void getTheoreticalVelocityFromExtended(const Eigen::VectorXd & vExtended,
Eigen::VectorXd & vTheoretical) const;

virtual std::unique_ptr<LockGuardLocal> getLock();
bool getIsLocked() const;

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

Expand All @@ -397,16 +390,14 @@ namespace jiminy

void addConstraint(const std::string & constraintName,
const std::shared_ptr<AbstractConstraintBase> & constraint,
ConstraintNodeType node);
void addConstraints(const ConstraintMap & constraintMap, ConstraintNodeType node);
void removeConstraint(const std::string & constraintName, ConstraintNodeType node);
ConstraintRegistryType type);
void addConstraints(const ConstraintMap & constraintMap, ConstraintRegistryType type);
void removeConstraint(const std::string & constraintName, ConstraintRegistryType type);
void removeConstraints(const std::vector<std::string> & constraintNames,
ConstraintNodeType node);
ConstraintRegistryType type);

void refreshGeometryProxies();
void refreshContactProxies();
/// \brief Refresh the proxies of the kinematics constraints.
void refreshConstraintProxies();
virtual void refreshProxies();

public:
Expand Down Expand Up @@ -476,8 +467,12 @@ namespace jiminy
/// \brief Concatenated fieldnames of the external force applied at each joint of the
/// model, 'universe' excluded.
std::vector<std::string> logForceExternalFieldnames_{};
/// \brief Concatenated fieldnames of all the constraints.
std::vector<std::string> logConstraintFieldnames_{};

private:
std::unique_ptr<MutexLocal> mutexLocal_{std::make_unique<MutexLocal>()};

/// \brief Vector of joints acceleration corresponding to a copy of data.a.
// Used for computing constraints as a temporary buffer.
MotionVector jointSpatialAccelerations_{};
Expand Down
4 changes: 2 additions & 2 deletions core/include/jiminy/core/robot/pinocchio_overload_algorithms.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,9 +46,9 @@ namespace jiminy::pinocchio_overload
pinocchio::DataTpl<Scalar, Options, JointCollectionTpl> & data,
const Eigen::MatrixBase<ConfigVectorType> & q,
const Eigen::MatrixBase<TangentVectorType> & v,
bool update_kinematics = true)
bool updateKinematics = true)
{
if (update_kinematics)
if (updateKinematics)
{
pinocchio::forwardKinematics(model, data, q, v);
}
Expand Down
8 changes: 2 additions & 6 deletions core/include/jiminy/core/robot/robot.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,6 @@ namespace jiminy
class AbstractSensorBase;
class AbstractController;
class TelemetryData;
class MutexLocal;
class LockGuardLocal;

class JIMINY_DLLAPI Robot : public Model
{
Expand Down Expand Up @@ -115,15 +113,14 @@ namespace jiminy

/// \remark This method does not have to be called manually before running a simulation.
/// The Engine is taking care of it.
virtual void reset(const uniform_random_bit_generator_ref<uint32_t> & g) override;
void reset(const uniform_random_bit_generator_ref<uint32_t> & g) override;
virtual void configureTelemetry(std::shared_ptr<TelemetryData> telemetryData);
void updateTelemetry();
bool getIsTelemetryConfigured() const;

const std::vector<std::string> & getLogCommandFieldnames() const;

std::unique_ptr<LockGuardLocal> getLock();
bool getIsLocked() const;
std::unique_ptr<LockGuardLocal> getLock() override;

// Getters without 'get' prefix for consistency with pinocchio C++ API
Eigen::Index nmotors() const;
Expand Down Expand Up @@ -161,7 +158,6 @@ namespace jiminy
Eigen::VectorXd motorEffortLimit_;

private:
std::unique_ptr<MutexLocal> mutexLocal_{std::make_unique<MutexLocal>()};
std::shared_ptr<MotorSharedStorage> motorSharedStorage_;
std::unordered_map<std::string, std::shared_ptr<SensorSharedStorage>>
sensorSharedStorageMap_{};
Expand Down
2 changes: 1 addition & 1 deletion core/src/constraints/abstract_constraint.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -109,7 +109,7 @@ namespace jiminy
return zeta / (2.0 * M_PI);
}

uint64_t AbstractConstraintBase::getDim() const
uint64_t AbstractConstraintBase::getSize() const
{
return static_cast<uint64_t>(drift_.size());
}
Expand Down
Loading

0 comments on commit 1e9499c

Please sign in to comment.