From 1e9499cd78eca0f6779e240498f3307a671b2ab1 Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Mon, 17 Jun 2024 07:40:58 +0200 Subject: [PATCH] [python|gym] Add contraint lambda multipliers to state and extracted 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. --- .pylintrc | 1 + .../core/constraints/abstract_constraint.h | 2 +- core/include/jiminy/core/engine/engine.h | 4 + core/include/jiminy/core/robot/model.h | 87 ++- .../robot/pinocchio_overload_algorithms.h | 4 +- core/include/jiminy/core/robot/robot.h | 8 +- core/src/constraints/abstract_constraint.cc | 2 +- core/src/engine/engine.cc | 72 ++- core/src/io/serialization.cc | 24 +- core/src/robot/model.cc | 585 ++++++++++-------- core/src/robot/robot.cc | 32 +- core/src/solver/constraint_solvers.cc | 35 +- core/src/utilities/random.cc | 17 +- .../gym_jiminy/common/bases/quantities.py | 250 ++++++-- .../common/gym_jiminy/common/envs/generic.py | 2 +- .../gym_jiminy/common/quantities/__init__.py | 8 +- .../gym_jiminy/common/quantities/generic.py | 11 +- .../common/quantities/locomotion.py | 24 +- .../common/wrappers/observation_stack.py | 2 +- .../unit_py/data/anymal_pipeline.json | 4 +- .../unit_py/data/anymal_pipeline.toml | 4 +- python/gym_jiminy/unit_py/test_misc.py | 6 +- .../unit_py/test_pipeline_design.py | 249 ++++---- python/gym_jiminy/unit_py/test_quantities.py | 7 +- python/gym_jiminy/unit_py/test_rewards.py | 13 +- python/jiminy_py/src/jiminy_py/dynamics.py | 119 ++-- python/jiminy_py/src/jiminy_py/log.py | 13 +- .../jiminy_py/unit_py/test_simple_pendulum.py | 2 +- python/jiminy_py/unit_py/utilities.py | 1 + python/jiminy_pywrap/src/constraints.cc | 13 +- python/jiminy_pywrap/src/helpers.cc | 146 +++-- python/jiminy_pywrap/src/robot.cc | 24 +- 32 files changed, 1072 insertions(+), 699 deletions(-) diff --git a/.pylintrc b/.pylintrc index a8239b7ec..1a3ffd901 100644 --- a/.pylintrc +++ b/.pylintrc @@ -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) diff --git a/core/include/jiminy/core/constraints/abstract_constraint.h b/core/include/jiminy/core/constraints/abstract_constraint.h index 26ea20952..76f542eec 100644 --- a/core/include/jiminy/core/constraints/abstract_constraint.h +++ b/core/include/jiminy/core/constraints/abstract_constraint.h @@ -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; diff --git a/core/include/jiminy/core/engine/engine.h b/core/include/jiminy/core/engine/engine.h index ea3716658..3aa1fa410 100644 --- a/core/include/jiminy/core/engine/engine.h +++ b/core/include/jiminy/core/engine/engine.h @@ -201,6 +201,7 @@ namespace jiminy std::vector logAccelerationFieldnames{}; std::vector logEffortFieldnames{}; std::vector logForceExternalFieldnames{}; + std::vector logConstraintFieldnames{}; std::vector logCommandFieldnames{}; std::string logEnergyFieldname{}; @@ -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; @@ -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; @@ -448,6 +451,7 @@ namespace jiminy enableVelocity{boost::get(options.at("enableVelocity"))}, enableAcceleration{boost::get(options.at("enableAcceleration"))}, enableForceExternal{boost::get(options.at("enableForceExternal"))}, + enableConstraint{boost::get(options.at("enableConstraint"))}, enableCommand{boost::get(options.at("enableCommand"))}, enableEffort{boost::get(options.at("enableEffort"))}, enableEnergy{boost::get(options.at("enableEnergy"))} diff --git a/core/include/jiminy/core/robot/model.h b/core/include/jiminy/core/robot/model.h index da35bd22b..8efd64c94 100644 --- a/core/include/jiminy/core/robot/model.h +++ b/core/include/jiminy/core/robot/model.h @@ -21,12 +21,14 @@ namespace jiminy class AbstractConstraintBase; class FrameConstraint; class JointConstraint; + class MutexLocal; + class LockGuardLocal; // ************************************** Constraints ************************************** // using ConstraintMap = static_map_t>; - enum class ConstraintNodeType : uint8_t + enum class ConstraintRegistryType : uint8_t { CONTACT_FRAMES = 0, COLLISION_BODIES = 1, @@ -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 find(const std::string & key, - ConstraintNodeType node); + ConstraintRegistryType type); std::pair 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 get(const std::string & key) const; std::shared_ptr 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 - 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(func), constraintItem.second, node); + std::invoke(std::forward(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 = ®istry; + 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(func), constraintItem.second, node); + std::invoke(std::forward(func), constraintItem.second, type); } } } template - void foreach(const std::array & constraintsHolderTypes, - Function && func) const + void foreach(const std::array & types, Function && func) const { - for (ConstraintNodeType node : constraintsHolderTypes) + for (ConstraintRegistryType type : types) { - foreach(node, std::forward(func)); + foreach(type, std::forward(func)); } } @@ -125,7 +124,7 @@ namespace jiminy /// \brief Constraints registered by the engine to handle collision bounds. std::vector collisionBodies{}; /// \brief Constraints explicitly registered by user. - ConstraintMap registry{}; + ConstraintMap user{}; }; // ***************************************** Model ***************************************** // @@ -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 getConstraint(const std::string & constraintName); - - std::weak_ptr 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; @@ -371,6 +360,7 @@ namespace jiminy const std::vector & getLogAccelerationFieldnames() const; const std::vector & getLogEffortFieldnames() const; const std::vector & getLogForceExternalFieldnames() const; + const std::vector & getLogConstraintFieldnames() const; void getExtendedPositionFromTheoretical(const Eigen::VectorXd & qTheoretical, Eigen::VectorXd & qExtended) const; @@ -381,6 +371,9 @@ namespace jiminy void getTheoreticalVelocityFromExtended(const Eigen::VectorXd & vExtended, Eigen::VectorXd & vTheoretical) const; + virtual std::unique_ptr getLock(); + bool getIsLocked() const; + protected: void generateModelExtended(const uniform_random_bit_generator_ref & g); @@ -397,16 +390,14 @@ namespace jiminy void addConstraint(const std::string & constraintName, const std::shared_ptr & 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 & constraintNames, - ConstraintNodeType node); + ConstraintRegistryType type); void refreshGeometryProxies(); void refreshContactProxies(); - /// \brief Refresh the proxies of the kinematics constraints. - void refreshConstraintProxies(); virtual void refreshProxies(); public: @@ -476,8 +467,12 @@ namespace jiminy /// \brief Concatenated fieldnames of the external force applied at each joint of the /// model, 'universe' excluded. std::vector logForceExternalFieldnames_{}; + /// \brief Concatenated fieldnames of all the constraints. + std::vector logConstraintFieldnames_{}; private: + std::unique_ptr mutexLocal_{std::make_unique()}; + /// \brief Vector of joints acceleration corresponding to a copy of data.a. // Used for computing constraints as a temporary buffer. MotionVector jointSpatialAccelerations_{}; diff --git a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h index 4148a8e78..62488012d 100644 --- a/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h +++ b/core/include/jiminy/core/robot/pinocchio_overload_algorithms.h @@ -46,9 +46,9 @@ namespace jiminy::pinocchio_overload pinocchio::DataTpl & data, const Eigen::MatrixBase & q, const Eigen::MatrixBase & v, - bool update_kinematics = true) + bool updateKinematics = true) { - if (update_kinematics) + if (updateKinematics) { pinocchio::forwardKinematics(model, data, q, v); } diff --git a/core/include/jiminy/core/robot/robot.h b/core/include/jiminy/core/robot/robot.h index ad4cae0d0..cfab49cb9 100644 --- a/core/include/jiminy/core/robot/robot.h +++ b/core/include/jiminy/core/robot/robot.h @@ -15,8 +15,6 @@ namespace jiminy class AbstractSensorBase; class AbstractController; class TelemetryData; - class MutexLocal; - class LockGuardLocal; class JIMINY_DLLAPI Robot : public Model { @@ -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 & g) override; + void reset(const uniform_random_bit_generator_ref & g) override; virtual void configureTelemetry(std::shared_ptr telemetryData); void updateTelemetry(); bool getIsTelemetryConfigured() const; const std::vector & getLogCommandFieldnames() const; - std::unique_ptr getLock(); - bool getIsLocked() const; + std::unique_ptr getLock() override; // Getters without 'get' prefix for consistency with pinocchio C++ API Eigen::Index nmotors() const; @@ -161,7 +158,6 @@ namespace jiminy Eigen::VectorXd motorEffortLimit_; private: - std::unique_ptr mutexLocal_{std::make_unique()}; std::shared_ptr motorSharedStorage_; std::unordered_map> sensorSharedStorageMap_{}; diff --git a/core/src/constraints/abstract_constraint.cc b/core/src/constraints/abstract_constraint.cc index fc8941232..d389bf2e1 100644 --- a/core/src/constraints/abstract_constraint.cc +++ b/core/src/constraints/abstract_constraint.cc @@ -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(drift_.size()); } diff --git a/core/src/engine/engine.cc b/core/src/engine/engine.cc index a71b4943d..c7d5adb59 100644 --- a/core/src/engine/engine.cc +++ b/core/src/engine/engine.cc @@ -611,6 +611,11 @@ namespace jiminy robotName, {}, TELEMETRY_FIELDNAME_DELIMITER); + robotDataIt->logConstraintFieldnames = + addCircumfix((*robotIt)->getLogConstraintFieldnames(), + robotName, + {}, + TELEMETRY_FIELDNAME_DELIMITER); robotDataIt->logCommandFieldnames = addCircumfix((*robotIt)->getLogCommandFieldnames(), robotName, @@ -653,6 +658,23 @@ namespace jiminy } } } + if (engineOptions_->telemetry.enableConstraint) + { + const ConstraintTree & constraints = (*robotIt)->getConstraints(); + // FIXME: Remove explicit `telemetrySender` capture when moving to C++20 + constraints.foreach( + [&telemetrySender = telemetrySender_, &robotData = *robotDataIt, i = 0]( + const std::shared_ptr & constraint, + ConstraintRegistryType /* type */) mutable + { + for (uint8_t j = 0; j < constraint->getSize(); ++j) + { + telemetrySender->registerVariable( + robotData.logConstraintFieldnames[i++], + &constraint->lambda_[j]); + } + }); + } if (engineOptions_->telemetry.enableCommand) { telemetrySender_->registerVariable(robotDataIt->logCommandFieldnames, @@ -771,6 +793,7 @@ namespace jiminy void computeExtraTerms( std::shared_ptr & robot, const RobotData & robotData, ForceVector & fExt) { + // Define some proxies for convenience const pinocchio::Model & model = robot->pinocchioModel_; pinocchio::Data & data = robot->pinocchioData_; @@ -779,7 +802,7 @@ namespace jiminy model, data, robotData.state.q, robotData.state.v, false); pinocchio::computePotentialEnergy(model, data); - /* Update manually the subtree (apparent) inertia, since it is only computed by crba, which + /* Update manually the subtree (apparent) inertia, since it is only computed by CRBA, which is doing more computation than necessary. It will be used here for computing the centroidal kinematics, and used later for joint bounds dynamics. Note that, by doing all the computations here instead of 'computeForwardKinematics', we are doing the assumption @@ -801,13 +824,25 @@ namespace jiminy } } - /* Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint - accelerations, joint forces and body forces, so it must be done separately: - - 1st step: computing the accelerations based on ForwardKinematic algorithm - - 2nd step: computing the forces based on RNEA algorithm */ + /* The objective here is to compute the actual joint accelerations and the joint internal + forces. The latter are not involved in dynamic computations, but are useful for analysis + of the mechanical design. Indeed, it brings information about stresses and strains + applied to the mechanical structure, which may cause unexpected fatigue wear. In + addition, the body external forces are also evaluated, as an intermediate quantity for + computing the centroidal dynamics, ie the spatial momentum of the whole robot at the + Center of Mass along with its temporal derivative. + + Neither 'aba' nor 'forwardDynamics' are computing simultaneously the actual joint + accelerations, the joint internal forces and the body external forces. Hence, it is done + manually, following a two computation steps procedure: + * joint accelerations based on ForwardKinematic algorithm + * joint internal forces and body external forces based on RNEA algorithm */ /* Compute the true joint acceleration and the one due to the lone gravity field, then - the spatial momenta and the total internal and external forces acting on each body. */ + the spatial momenta and the total internal and external forces acting on each body. + * `fExt` is used as a buffer for storing the total body external forces. It serves + no purpose other than being an intermediate quantity for other computations. + * `data.f` stores the joint internal forces */ data.h[0].setZero(); fExt[0].setZero(); data.f[0].setZero(); @@ -1234,10 +1269,10 @@ namespace jiminy [&contactModel = contactModel_, &freq = engineOptions_->contacts.stabilizationFreq]( const std::shared_ptr & constraint, - ConstraintNodeType node) + ConstraintRegistryType type) { // Set baumgarte freq for all contact constraints - if (node != ConstraintNodeType::USER) + if (type != ConstraintRegistryType::USER) { constraint->setBaumgarteFreq(freq); // It cannot fail } @@ -1245,20 +1280,20 @@ namespace jiminy // Enable constraints by default if (contactModel == ContactModelType::CONSTRAINT) { - switch (node) + switch (type) { - case ConstraintNodeType::BOUNDS_JOINTS: + case ConstraintRegistryType::BOUNDS_JOINTS: { auto & jointConstraint = static_cast(*constraint.get()); jointConstraint.setRotationDir(false); } [[fallthrough]]; - case ConstraintNodeType::CONTACT_FRAMES: - case ConstraintNodeType::COLLISION_BODIES: + case ConstraintRegistryType::CONTACT_FRAMES: + case ConstraintRegistryType::COLLISION_BODIES: constraint->enable(); break; - case ConstraintNodeType::USER: + case ConstraintRegistryType::USER: default: break; } @@ -1449,7 +1484,8 @@ namespace jiminy robotData.statePrev = robotData.state; } - // Lock the telemetry. At this point it is not possible to register new variables. + /* Register all engine and robots variables, then lock the telemetry. + At this point it is not possible for the user to register new variables. */ configureTelemetry(); // Log robots @@ -3715,12 +3751,12 @@ namespace jiminy // Restore contact frame forces and bounds internal efforts const ConstraintTree & constraints = robot->getConstraints(); constraints.foreach( - ConstraintNodeType::BOUNDS_JOINTS, + ConstraintRegistryType::BOUNDS_JOINTS, [&u = robotData.state.u, &uInternal = robotData.state.uInternal, &joints = const_cast(model.joints)]( const std::shared_ptr & constraint, - ConstraintNodeType /* node */) + ConstraintRegistryType /* type */) { if (!constraint->getIsEnabled()) { @@ -3771,9 +3807,9 @@ namespace jiminy } constraints.foreach( - ConstraintNodeType::COLLISION_BODIES, + ConstraintRegistryType::COLLISION_BODIES, [&fext, &model, &data](const std::shared_ptr & constraint, - ConstraintNodeType /* node */) + ConstraintRegistryType /* type */) { if (!constraint->getIsEnabled()) { diff --git a/core/src/io/serialization.cc b/core/src/io/serialization.cc index 2f7bcfb56..1e8db48e4 100644 --- a/core/src/io/serialization.cc +++ b/core/src/io/serialization.cc @@ -675,14 +675,15 @@ namespace boost::serialization /* Copy the theoretical model then remove extra collision frames. Note that `removeCollisionBodies` is not called to avoid altering the robot. */ - pinocchio::Model pinocchioModelTh = model.pinocchioModelTh_; std::vector collisionConstraintNames; + const ConstraintTree & constraints = model.getConstraints(); + pinocchio::Model pinocchioModelTh = model.pinocchioModelTh_; for (const std::string & name : model.getCollisionBodyNames()) { for (const pinocchio::GeometryObject & geom : model.collisionModelTh_.geometryObjects) { if (model.pinocchioModel_.frames[geom.parentFrame].name == name && - model.getConstraints().exist(geom.name, ConstraintNodeType::COLLISION_BODIES)) + constraints.exist(geom.name, ConstraintRegistryType::COLLISION_BODIES)) { const pinocchio::FrameIndex frameIndex = getFrameIndex(pinocchioModelTh, geom.name); @@ -749,15 +750,14 @@ namespace boost::serialization all constraints manually in order to serialize the one for which it works, and just print warning and move on when it is not. */ const std::shared_ptr dummyConstraintPtr{}; - const ConstraintMap & constraintRegistry = model.getConstraints().registry; - std::size_t constraintRegistryLength = constraintRegistry.size(); - ar << make_nvp("user_constraints_length", constraintRegistryLength); - for (std::size_t i = 0; i < constraintRegistryLength; ++i) + std::size_t numUserConstraints = constraints.user.size(); + ar << make_nvp("num_user_constraints", numUserConstraints); + for (std::size_t i = 0; i < numUserConstraints; ++i) { /* Note that it is not possible to serialize `std::pair` directly because serialization failure of the motor would corrupt the whole archive state with half-backed data. */ const std::string name = toString("constraint_", i); - const auto & [constraintName, constraintPtr] = constraintRegistry[i]; + const auto & [constraintName, constraintPtr] = constraints.user[i]; ar << make_nvp(toString(name, "_name").c_str(), constraintName); try { @@ -767,7 +767,7 @@ namespace boost::serialization { ar << make_nvp(toString(name, "_ptr").c_str(), dummyConstraintPtr); JIMINY_WARNING("Failed to serialize constraint '", - constraintRegistry[i].first, + constraints.user[i].first, "'. It will be missing when loading the robot from log." "\nRaised from exception: ", e.what()); @@ -871,7 +871,7 @@ namespace boost::serialization ++meshPathIt) { const std::string meshPath = meshPathIt->str(); - if (meshPath.compare(0, packagePrefix.size(), packagePrefix) != 0) + if (!meshPath.compare(0, packagePrefix.size(), packagePrefix)) { meshPaths.emplace_back(meshPath); } @@ -984,9 +984,9 @@ namespace boost::serialization model.addContactPoints(contactFrameNames); // Restore user-specified constraints - std::size_t constraintRegistryLength; - ar >> make_nvp("user_constraints_length", constraintRegistryLength); - for (std::size_t i = 0; i < constraintRegistryLength; ++i) + std::size_t numUserConstraints; + ar >> make_nvp("num_user_constraints", numUserConstraints); + for (std::size_t i = 0; i < numUserConstraints; ++i) { std::string constraintName; std::shared_ptr constraintPtr; diff --git a/core/src/robot/model.cc b/core/src/robot/model.cc index 36de4b91a..694d99540 100644 --- a/core/src/robot/model.cc +++ b/core/src/robot/model.cc @@ -1,5 +1,6 @@ #include #include +#include #include "pinocchio/spatial/symmetric3.hpp" // `pinocchio::Symmetric3 ` #include "pinocchio/spatial/explog.hpp" // `pinocchio::exp3` @@ -33,8 +34,8 @@ #include "jiminy/core/constraints/frame_constraint.h" #include "jiminy/core/hardware/basic_sensors.h" #include "jiminy/core/io/serialization.h" -#include "jiminy/core/utilities/pinocchio.h" #include "jiminy/core/utilities/helpers.h" +#include "jiminy/core/utilities/pinocchio.h" #include "jiminy/core/robot/model.h" @@ -43,16 +44,24 @@ namespace jiminy { // ************************************** Constraints ************************************** // - void ConstraintTree::clear() noexcept + constexpr std::string_view getConstraintNodeName(ConstraintRegistryType type) { - boundJoints.clear(); - contactFrames.clear(); - collisionBodies.clear(); - registry.clear(); + switch (type) + { + case ConstraintRegistryType::BOUNDS_JOINTS: + return "BoundJoints"; + case ConstraintRegistryType::CONTACT_FRAMES: + return "ContactFrames"; + case ConstraintRegistryType::COLLISION_BODIES: + return "CollisionBodies"; + case ConstraintRegistryType::USER: + default: + return "User"; + } } template - static auto findImpl(T && constraints, const std::string & key, ConstraintNodeType node) + static auto findImpl(T && constraints, const std::string & key, ConstraintRegistryType type) { // Determine return types based on argument constness constexpr bool isConst = std::is_const_v>; @@ -63,7 +72,7 @@ namespace jiminy // Pointers are NOT initialized to nullptr by default constraintMapT * constraintMapPtr{nullptr}; constraintIteratorT constraintIt{}; - if (node == ConstraintNodeType::COLLISION_BODIES) + if (type == ConstraintRegistryType::COLLISION_BODIES) { for (auto & collisionBody : constraints.collisionBodies) { @@ -80,18 +89,18 @@ namespace jiminy } else { - switch (node) + switch (type) { - case ConstraintNodeType::BOUNDS_JOINTS: + case ConstraintRegistryType::BOUNDS_JOINTS: constraintMapPtr = &constraints.boundJoints; break; - case ConstraintNodeType::CONTACT_FRAMES: + case ConstraintRegistryType::CONTACT_FRAMES: constraintMapPtr = &constraints.contactFrames; break; - case ConstraintNodeType::USER: - case ConstraintNodeType::COLLISION_BODIES: + case ConstraintRegistryType::USER: + case ConstraintRegistryType::COLLISION_BODIES: default: - constraintMapPtr = &constraints.registry; + constraintMapPtr = &constraints.user; } constraintIt = std::find_if(constraintMapPtr->begin(), constraintMapPtr->end(), @@ -103,88 +112,112 @@ namespace jiminy } std::pair ConstraintTree::find( - const std::string & key, ConstraintNodeType node) + const std::string & key, ConstraintRegistryType type) { - return findImpl(*this, key, node); + return findImpl(*this, key, type); } std::pair ConstraintTree::find( - const std::string & key, ConstraintNodeType node) const + const std::string & key, ConstraintRegistryType type) const { - return findImpl(*this, key, node); + return findImpl(*this, key, type); } - bool ConstraintTree::exist(const std::string & key, ConstraintNodeType node) const + bool ConstraintTree::exist(const std::string & key, ConstraintRegistryType type) const { - const auto [constraintMapPtr, constraintIt] = - const_cast(this)->find(key, node); + const auto [constraintMapPtr, constraintIt] = find(key, type); return (constraintMapPtr && constraintIt != constraintMapPtr->cend()); } - bool ConstraintTree::exist(const std::string & key) const - { - for (ConstraintNodeType node : constraintNodeTypesAll) - { - if (exist(key, node)) - { - return true; - } - } - return false; - } - std::shared_ptr ConstraintTree::get(const std::string & key, - ConstraintNodeType node) const + ConstraintRegistryType type) const { - auto [constraintMapPtr, constraintIt] = find(key, node); - if (constraintMapPtr && constraintIt != constraintMapPtr->cend()) + auto [constraintMapPtr, constraintIt] = find(key, type); + if (constraintMapPtr && constraintIt == constraintMapPtr->end()) { - return constraintIt->second; + JIMINY_THROW(std::invalid_argument, + "No constraint named '", + key, + "' exists in registry '", + getConstraintNodeName(type), + "'."); } - return {}; + return constraintIt->second; } - std::shared_ptr ConstraintTree::get(const std::string & key) const + void ConstraintTree::insert(const ConstraintMap & constraintMap, ConstraintRegistryType type) { - std::shared_ptr constraint; - for (ConstraintNodeType node : constraintNodeTypesAll) + // Make sure that all constraints are properly defined and their names do not already exist + std::unordered_set keys; + for (const auto & [constraintName, constraintPtr] : constraintMap) { - constraint = get(key, node); - if (constraint) + if (!constraintPtr) { - break; + JIMINY_THROW(std::invalid_argument, + "Constraint named '", + constraintName, + "' is undefined."); } + + if (exist(constraintName, type)) + { + JIMINY_THROW(std::invalid_argument, + "A constraint named '", + constraintName, + "' already exists in registry '", + getConstraintNodeName(type), + "'."); + } + + keys.insert(constraintName); } - return constraint; - } - void ConstraintTree::insert(const ConstraintMap & constraintMap, ConstraintNodeType node) - { - switch (node) + // Make sure that all constraints have a unique name + if (keys.size() != constraintMap.size()) + { + JIMINY_THROW(std::invalid_argument, + "Impossible to insert constraints with duplicate names."); + } + + switch (type) { - case ConstraintNodeType::BOUNDS_JOINTS: + case ConstraintRegistryType::BOUNDS_JOINTS: boundJoints.insert(boundJoints.end(), constraintMap.begin(), constraintMap.end()); break; - case ConstraintNodeType::CONTACT_FRAMES: + case ConstraintRegistryType::CONTACT_FRAMES: contactFrames.insert(contactFrames.end(), constraintMap.begin(), constraintMap.end()); break; - case ConstraintNodeType::COLLISION_BODIES: + case ConstraintRegistryType::COLLISION_BODIES: collisionBodies.push_back(constraintMap); break; - case ConstraintNodeType::USER: + case ConstraintRegistryType::USER: default: - registry.insert(registry.end(), constraintMap.begin(), constraintMap.end()); + user.insert(user.end(), constraintMap.begin(), constraintMap.end()); } } - ConstraintMap::iterator ConstraintTree::erase(const std::string & key, ConstraintNodeType node) + ConstraintMap::iterator ConstraintTree::erase(const std::string & key, + ConstraintRegistryType type) { - auto [constraintMapPtr, constraintIt] = find(key, node); - if (constraintMapPtr && constraintIt != constraintMapPtr->end()) + auto [constraintMapPtr, constraintIt] = find(key, type); + if (constraintMapPtr && constraintIt == constraintMapPtr->end()) { - return constraintMapPtr->erase(constraintIt); + JIMINY_THROW(std::invalid_argument, + "No constraint named '", + key, + "' exists in registry '", + getConstraintNodeName(type), + "'."); } - return constraintMapPtr->end(); + return constraintMapPtr->erase(constraintIt); + } + + void ConstraintTree::clear() noexcept + { + boundJoints.clear(); + contactFrames.clear(); + collisionBodies.clear(); + user.clear(); } // ***************************************** Model ***************************************** // @@ -324,7 +357,7 @@ namespace jiminy jointConstraintsMap.emplace_back(jointName, std::make_shared(jointName)); } - addConstraints(jointConstraintsMap, ConstraintNodeType::BOUNDS_JOINTS); + addConstraints(jointConstraintsMap, ConstraintRegistryType::BOUNDS_JOINTS); } catch (...) { @@ -391,6 +424,14 @@ namespace jiminy frame to another frame. This means that the relative transform of the frame wrt the parent joint must be computed. */ + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before adding frames."); + } + // Check that no frame with the same name already exists if (pinocchioModelTh_.existFrame(frameName)) { @@ -440,6 +481,14 @@ namespace jiminy void Model::removeFrames(const std::vector & frameNames, const std::vector & filter) { + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before removing frames."); + } + /* Check that the frame can be safely removed from the theoretical model. If so, then it holds true for the extended model. */ if (!filter.empty()) @@ -500,11 +549,20 @@ namespace jiminy void Model::addCollisionBodies(const std::vector & bodyNames, bool ignoreMeshes) { + // Make sure that the model is initialized if (!isInitialized_) { JIMINY_THROW(bad_control_flow, "Model not initialized."); } + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before adding collision bodies."); + } + // Returning early if nothing to do if (bodyNames.empty()) { @@ -541,95 +599,114 @@ namespace jiminy } } - // Make sure that at least one geometry is associated with each body - for (const std::string & name : bodyNames) + /* Create the collision pairs and add them to the geometry model of the robot, + after checking first that at least one geometry is associated with each body. */ + for (bool isDryRun : std::array{true, false}) { - bool hasGeometry = false; - for (const pinocchio::GeometryObject & geom : collisionModelTh_.geometryObjects) + const pinocchio::GeomIndex & groundIndex = collisionModelTh_.getGeometryId("ground"); + for (const std::string & name : bodyNames) { - const bool isGeomMesh = (geom.meshPath.find('/') != std::string::npos || - geom.meshPath.find('\\') != std::string::npos); - // geom.meshPath is the geometry type if it is not an actual mesh - if (!(ignoreMeshes && isGeomMesh) && - pinocchioModel_.frames[geom.parentFrame].name == name) - { - hasGeometry = true; - break; - } - } - if (!hasGeometry) - { - JIMINY_THROW(std::invalid_argument, - "At least one of the bodies not associated with any collision " - "geometry of requested type."); - } - } - - // Add the list of bodies to the set of collision bodies - collisionBodyNames_.insert(collisionBodyNames_.end(), bodyNames.begin(), bodyNames.end()); + bool hasGeometry = false; + ConstraintMap collisionConstraintsMap; - // Create the collision pairs and add them to the geometry model of the robot - const pinocchio::GeomIndex & groundIndex = collisionModelTh_.getGeometryId("ground"); - for (const std::string & name : bodyNames) - { - // Add a collision pair for all geometries having the body as parent - ConstraintMap collisionConstraintsMap; - for (std::size_t i = 0; i < collisionModelTh_.geometryObjects.size(); ++i) - { - const pinocchio::GeometryObject & geom = collisionModelTh_.geometryObjects[i]; - const bool isGeomMesh = (geom.meshPath.find('/') != std::string::npos || - geom.meshPath.find('\\') != std::string::npos); - const std::string & frameName = pinocchioModel_.frames[geom.parentFrame].name; - if (!(ignoreMeshes && isGeomMesh) && frameName == name) + // Add a collision pair for all geometries having the body as parent + for (std::size_t i = 0; i < collisionModelTh_.geometryObjects.size(); ++i) { - // Add constraint associated with contact frame only if it is a sphere - const hpp::fcl::CollisionGeometry & shape = *geom.geometry; - if (shape.getNodeType() == hpp::fcl::GEOM_SPHERE) + const pinocchio::GeometryObject & geom = collisionModelTh_.geometryObjects[i]; + // geom.meshPath is the geometry type if it is not an actual mesh + const bool isGeomMesh = (geom.meshPath.find('/') != std::string::npos || + geom.meshPath.find('\\') != std::string::npos); + const std::string & frameName = pinocchioModel_.frames[geom.parentFrame].name; + if (!(ignoreMeshes && isGeomMesh) && frameName == name) { - /* Create and add the collision pair with the ground. - Note that the ground always comes second for the normal to be - consistently compute wrt the ground instead of the body. */ - const pinocchio::CollisionPair collisionPair(i, groundIndex); - collisionModelTh_.addCollisionPair(collisionPair); - - /* Add dedicated frame. - Note that 'FIXED_JOINT' type is used instead of default 'OP_FRAME' to - avoid considering it as manually added to the model, and therefore - prevent its deletion by the user. */ - const pinocchio::FrameType frameType = pinocchio::FrameType::FIXED_JOINT; - addFrame(geom.name, frameName, geom.placement, frameType); - - // Add fixed frame constraint of bounded sphere - // const hpp::fcl::Sphere & sphere = - // static_cast(shape); - // collisionConstraintsMap.emplace_back( - // geom.name, - // std::make_shared(geom.name, sphere.radius)); - collisionConstraintsMap.emplace_back( - geom.name, - std::make_shared( - geom.name, std::array{true, true, true, false, false, true})); + // Add constraint associated with contact frame only if it is a sphere + const hpp::fcl::CollisionGeometry & shape = *geom.geometry; + if (shape.getNodeType() == hpp::fcl::GEOM_SPHERE) + { + // A valid geometry has been found + hasGeometry = true; + + // Early return if dry run + if (isDryRun) + { + break; + } + + /* Create and add the collision pair with the ground. + Note that the ground always comes second for the normal to be + consistently compute wrt the ground instead of the body. */ + const pinocchio::CollisionPair collisionPair(i, groundIndex); + collisionModelTh_.addCollisionPair(collisionPair); + + /* Add dedicated frame. + Note that 'FIXED_JOINT' type is used instead of default 'OP_FRAME' to + avoid considering it as manually added to the model, and therefore + prevent its deletion by the user. */ + const pinocchio::FrameType frameType = + pinocchio::FrameType::FIXED_JOINT; + addFrame(geom.name, frameName, geom.placement, frameType); + + // Add fixed frame constraint of bounded sphere + // const hpp::fcl::Sphere & sphere = + // static_cast(shape); + // collisionConstraintsMap.emplace_back( + // geom.name, + // std::make_shared(geom.name, sphere.radius)); + collisionConstraintsMap.emplace_back( + geom.name, + std::make_shared( + geom.name, std::array{true, true, true, false, false, true})); + } + else + { + JIMINY_WARNING("Geometry object has been ignored because its type is " + "unsupported for now."); + } } + } - // TODO: Add warning or error to notify that a geometry has been ignored + // Make sure that at least one geometry is associated with each body + if (!hasGeometry) + { + JIMINY_THROW(std::invalid_argument, + "Body '", + name, + "' not associated with any collision " + "geometry of a supported type."); } - } - // Add constraints map - addConstraints(collisionConstraintsMap, ConstraintNodeType::COLLISION_BODIES); + // Add constraints map if not dry run + if (!isDryRun) + { + addConstraints(collisionConstraintsMap, + ConstraintRegistryType::COLLISION_BODIES); + } + } } + // Add the list of bodies to the set of collision bodies + collisionBodyNames_.insert(collisionBodyNames_.end(), bodyNames.begin(), bodyNames.end()); + // Refresh proxies associated with the collisions only refreshGeometryProxies(); } void Model::removeCollisionBodies(std::vector bodyNames) { + // Make sure that the model is initialized if (!isInitialized_) { JIMINY_THROW(bad_control_flow, "Model not initialized."); } + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before removing collision bodies."); + } + // Make sure that no body are duplicates if (checkDuplicates(bodyNames)) { @@ -676,7 +753,7 @@ namespace jiminy collisionModelTh_.removeCollisionPair(collisionPair); // Append collision geometry to the list of constraints to remove - if (constraints_.exist(geom.name, ConstraintNodeType::COLLISION_BODIES)) + if (constraints_.exist(geom.name, ConstraintRegistryType::COLLISION_BODIES)) { collisionConstraintNames.emplace_back(geom.name); } @@ -685,7 +762,7 @@ namespace jiminy } // Remove the constraints and associated frames - removeConstraints(collisionConstraintNames, ConstraintNodeType::COLLISION_BODIES); + removeConstraints(collisionConstraintNames, ConstraintRegistryType::COLLISION_BODIES); removeFrames(collisionConstraintNames, {pinocchio::FrameType::FIXED_JOINT}); // Refresh proxies associated with the collisions only @@ -694,11 +771,20 @@ namespace jiminy void Model::addContactPoints(const std::vector & frameNames) { + // Make sure that the model is initialized if (!isInitialized_) { JIMINY_THROW(bad_control_flow, "Model not initialized."); } + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before adding contact points."); + } + // Make sure that no frame are duplicates if (checkDuplicates(frameNames)) { @@ -734,7 +820,7 @@ namespace jiminy std::make_shared( frameName, std::array{true, true, true, false, false, true})); } - addConstraints(frameConstraintsMap, ConstraintNodeType::CONTACT_FRAMES); + addConstraints(frameConstraintsMap, ConstraintRegistryType::CONTACT_FRAMES); // Refresh proxies associated with contacts and constraints refreshContactProxies(); @@ -742,11 +828,20 @@ namespace jiminy void Model::removeContactPoints(const std::vector & frameNames) { + // Make sure that the model is initialized if (!isInitialized_) { JIMINY_THROW(bad_control_flow, "Model not initialized."); } + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before removing contact points."); + } + // Make sure that no frame are duplicates if (checkDuplicates(frameNames)) { @@ -764,12 +859,12 @@ namespace jiminy the set of contact frames. */ if (!frameNames.empty()) { - removeConstraints(frameNames, ConstraintNodeType::CONTACT_FRAMES); + removeConstraints(frameNames, ConstraintRegistryType::CONTACT_FRAMES); eraseVector(contactFrameNames_, frameNames); } else { - removeConstraints(contactFrameNames_, ConstraintNodeType::CONTACT_FRAMES); + removeConstraints(contactFrameNames_, ConstraintRegistryType::CONTACT_FRAMES); contactFrameNames_.clear(); } @@ -777,78 +872,92 @@ namespace jiminy refreshContactProxies(); } - void Model::addConstraints(const ConstraintMap & constraintMap, ConstraintNodeType node) + void Model::addConstraints(const ConstraintMap & constraintMap, ConstraintRegistryType type) { - // Check if constraint is properly defined and not already exists - for (const auto & [constraintName, constraintPtr] : constraintMap) + // Make sure that no simulation is already running + if (getIsLocked()) { - if (!constraintPtr) - { - JIMINY_THROW(std::invalid_argument, - "Constraint named '", - constraintName, - "' is undefined."); - } - if (constraints_.exist(constraintName)) - { - JIMINY_THROW(std::invalid_argument, - "A constraint named '", - constraintName, - "' already exists."); - } + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before adding constraints."); } - // Attach constraint if not already exist + // Add constraints to registry + constraints_.insert(constraintMap, type); + + // Attach constraint for (auto & constraintPair : constraintMap) { constraintPair.second->attach(shared_from_this()); } - // Add them to constraints holder - constraints_.insert(constraintMap, node); - - // Disable internal constraint by default if internal - if (node != ConstraintNodeType::USER) + // Loop over all constraints + Eigen::VectorXd qNeutral = pinocchio::neutral(pinocchioModel_); + for (auto & [constraintName, constraint] : constraintMap) { - for (auto & constraintItem : constraintMap) + // Reset constraint using neutral configuration and zero velocity + constraint->reset(qNeutral, Eigen::VectorXd::Zero(nv_)); + + // Call constraint on neutral position and zero velocity. + auto J = constraint->getJacobian(); + + // Check dimensions consistency + if (J.cols() != pinocchioModel_.nv) { - constraintItem.second->disable(); + JIMINY_THROW(std::logic_error, + "Constraint has inconsistent jacobian and drift (size mismatch)."); + } + + // Append log telemetry constraint fieldnames + for (uint32_t j = 0; j < constraint->getSize(); ++j) + { + logConstraintFieldnames_.push_back( + toString("Constraint", getConstraintNodeName(type), constraintName, j)); + } + + // Disable constraint by default if internal + if (type != ConstraintRegistryType::USER) + { + constraint->disable(); } } } void Model::addConstraint(const std::string & constraintName, const std::shared_ptr & constraint, - ConstraintNodeType node) + ConstraintRegistryType type) { - return addConstraints({{constraintName, constraint}}, node); + return addConstraints({{constraintName, constraint}}, type); } void Model::addConstraint(const std::string & constraintName, const std::shared_ptr & constraint) { - return addConstraint(constraintName, constraint, ConstraintNodeType::USER); + return addConstraint(constraintName, constraint, ConstraintRegistryType::USER); } void Model::removeConstraints(const std::vector & constraintNames, - ConstraintNodeType node) + ConstraintRegistryType type) { - // Make sure the constraints exists + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before removing constraints."); + } + + // Make sure all constraints exist for (const std::string & constraintName : constraintNames) { - if (!constraints_.exist(constraintName, node)) + if (!constraints_.exist(constraintName, type)) { - if (node == ConstraintNodeType::USER) - { - JIMINY_THROW(std::invalid_argument, - "No user-registered constraint with name '", - constraintName, - "' exists."); - } JIMINY_THROW(std::invalid_argument, - "No internal constraint with name '", + "No constraint named '", constraintName, - "' exists."); + "' exists in registry '", + getConstraintNodeName(type), + "'."); } } @@ -856,50 +965,40 @@ namespace jiminy for (const std::string & constraintName : constraintNames) { // Lookup constraint - auto [constraintMapPtr, constraintIt] = constraints_.find(constraintName, node); + auto [constraintMapPtr, constraintIt] = constraints_.find(constraintName, type); // Detach the constraint constraintIt->second->detach(); + // Remove log telemetry constraint fieldnames + const std::string logConstraintFieldnameFirst = + toString("Constraint", getConstraintNodeName(type), constraintName, 0); + for (auto logConstraintFieldnamesIt = logConstraintFieldnames_.begin(); + logConstraintFieldnamesIt != logConstraintFieldnames_.end(); + ++logConstraintFieldnamesIt) + { + if (*logConstraintFieldnamesIt == logConstraintFieldnameFirst) + { + logConstraintFieldnames_.erase(logConstraintFieldnamesIt, + logConstraintFieldnamesIt + + constraintIt->second->getSize()); + break; + } + } + // Remove the constraint from the holder constraintMapPtr->erase(constraintIt); } } - void Model::removeConstraint(const std::string & constraintName, ConstraintNodeType node) + void Model::removeConstraint(const std::string & constraintName, ConstraintRegistryType type) { - return removeConstraints({constraintName}, node); + return removeConstraints({constraintName}, type); } void Model::removeConstraint(const std::string & constraintName) { - return removeConstraint(constraintName, ConstraintNodeType::USER); - } - - std::shared_ptr Model::getConstraint( - const std::string & constraintName) - { - std::shared_ptr constraint = constraints_.get(constraintName); - if (!constraint) - { - JIMINY_THROW( - std::invalid_argument, "No constraint with name '", constraintName, "' exists."); - } - return constraint; - } - - std::weak_ptr Model::getConstraint( - const std::string & constraintName) const - { - std::weak_ptr constraint = - std::const_pointer_cast( - const_cast(constraints_).get(constraintName)); - if (!constraint.lock()) - { - JIMINY_THROW( - std::invalid_argument, "No constraint with name '", constraintName, "' exists."); - } - return constraint; + return removeConstraint(constraintName, ConstraintRegistryType::USER); } const ConstraintTree & Model::getConstraints() const @@ -907,11 +1006,6 @@ namespace jiminy return constraints_; } - bool Model::existConstraint(const std::string & constraintName) const - { - return constraints_.exist(constraintName); - } - bool Model::hasConstraints() const { bool hasConstraintsEnabled = false; @@ -919,7 +1013,7 @@ namespace jiminy .foreach( [&hasConstraintsEnabled]( const std::shared_ptr & constraint, - ConstraintNodeType /* node */) + ConstraintRegistryType /* type */) { if (constraint->getIsEnabled()) { @@ -931,14 +1025,23 @@ namespace jiminy void Model::resetConstraints(const Eigen::VectorXd & q, const Eigen::VectorXd & v) { + // Make sure that no simulation is already running + if (getIsLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked, probably because a simulation is running. " + "Please stop it before resetting constraints."); + } + constraints_.foreach([&q, &v](const std::shared_ptr & constraint, - ConstraintNodeType /* node */) { constraint->reset(q, v); }); + ConstraintRegistryType /* type */) + { constraint->reset(q, v); }); - constraints_.foreach(std::array{ConstraintNodeType::BOUNDS_JOINTS, - ConstraintNodeType::CONTACT_FRAMES, - ConstraintNodeType::COLLISION_BODIES}, + constraints_.foreach(std::array{ConstraintRegistryType::BOUNDS_JOINTS, + ConstraintRegistryType::CONTACT_FRAMES, + ConstraintRegistryType::COLLISION_BODIES}, [](const std::shared_ptr & constraint, - ConstraintNodeType /* node */) { constraint->disable(); }); + ConstraintRegistryType /* type */) { constraint->disable(); }); } void Model::generateModelExtended(const uniform_random_bit_generator_ref & g) @@ -1166,7 +1269,7 @@ namespace jiminy // Compute sequentially the jacobian and drift of each enabled constraint constraints_.foreach( [&](const std::shared_ptr & constraint, - ConstraintNodeType /* node */) + ConstraintRegistryType /* type */) { // Skip constraint if disabled if (!constraint || !constraint->getIsEnabled()) @@ -1335,10 +1438,13 @@ namespace jiminy pinocchioModel_.lowerPositionLimit = positionLimitMin; pinocchioModel_.upperPositionLimit = positionLimitMax; + // Initialize backup joint space acceleration + jointSpatialAccelerations_ = + MotionVector(pinocchioData_.a.size(), pinocchio::Motion::Zero()); + // Refresh all other proxies refreshGeometryProxies(); refreshContactProxies(); - refreshConstraintProxies(); } void Model::refreshGeometryProxies() @@ -1436,32 +1542,6 @@ namespace jiminy contactFrameIndices_ = getFrameIndices(pinocchioModel_, contactFrameNames_); } - void Model::refreshConstraintProxies() - { - // Initialize backup joint space acceleration - jointSpatialAccelerations_ = - MotionVector(pinocchioData_.a.size(), pinocchio::Motion::Zero()); - - constraints_.foreach( - [&](const std::shared_ptr & constraint, - ConstraintNodeType /* node */) - { - // Reset constraint using neutral configuration and zero velocity - constraint->reset(pinocchio::neutral(pinocchioModel_), Eigen::VectorXd::Zero(nv_)); - - // Call constraint on neutral position and zero velocity. - auto J = constraint->getJacobian(); - - // Check dimensions consistency - if (J.cols() != pinocchioModel_.nv) - { - JIMINY_THROW( - std::logic_error, - "Constraint has inconsistent jacobian and drift (size mismatch)."); - } - }); - } - void Model::setOptions(const GenericConfig & modelOptions) { bool internalBuffersMustBeUpdated = false; @@ -1895,4 +1975,27 @@ namespace jiminy { return logForceExternalFieldnames_; } + + const std::vector & Model::getLogConstraintFieldnames() const + { + return logConstraintFieldnames_; + } + + std::unique_ptr Model::getLock() + { + // Make sure that the robot is not already locked + if (mutexLocal_->isLocked()) + { + JIMINY_THROW(bad_control_flow, + "Model already locked. Please release it first prior requesting lock."); + } + + // Return lock + return std::make_unique(*mutexLocal_); + } + + bool Model::getIsLocked() const + { + return mutexLocal_->isLocked(); + } } diff --git a/core/src/robot/robot.cc b/core/src/robot/robot.cc index 941e07774..2c03270f9 100644 --- a/core/src/robot/robot.cc +++ b/core/src/robot/robot.cc @@ -592,9 +592,9 @@ namespace jiminy std::string backlashName = jointName; backlashName += BACKLASH_JOINT_SUFFIX; - // Check if constraint a joint bounds constraint exist + // Check if the corresponding joint bound constraint already exists const bool hasConstraint = - constraints_.exist(backlashName, ConstraintNodeType::BOUNDS_JOINTS); + constraints_.exist(backlashName, ConstraintRegistryType::BOUNDS_JOINTS); // Skip adding joint if no backlash const double backlash = motor->getBacklash(); @@ -603,24 +603,23 @@ namespace jiminy // Remove joint bounds constraint if any if (hasConstraint) { - removeConstraint(backlashName, ConstraintNodeType::BOUNDS_JOINTS); + removeConstraint(backlashName, ConstraintRegistryType::BOUNDS_JOINTS); } - continue; } + // Add backlash joint to the model + addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName); + backlashJointNames_.push_back(backlashName); + // Add joint bounds constraint if necessary if (!hasConstraint) { std::shared_ptr constraint = std::make_shared(backlashName); - addConstraint(backlashName, constraint, ConstraintNodeType::BOUNDS_JOINTS); + addConstraint(backlashName, constraint, ConstraintRegistryType::BOUNDS_JOINTS); } - // Add backlash joint to the model - addBacklashJointAfterMechanicalJoint(pinocchioModel_, jointName, backlashName); - backlashJointNames_.push_back(backlashName); - // Update position limits in model const Eigen::Index positionIndex = getJointPositionFirstIndex(pinocchioModel_, backlashName); @@ -1050,24 +1049,15 @@ namespace jiminy std::unique_ptr Robot::getLock() { - // Make sure that the robot is not already locked - if (mutexLocal_->isLocked()) - { - JIMINY_THROW(bad_control_flow, - "Robot already locked. Please release it first prior requesting lock."); - } + // Get lock + std::unique_ptr lock = Model::getLock(); // Make sure that the options are not already considered valid as it was impossible to // guarantee it before locking the robot. areRobotOptionsRefreshed_ = false; // Return lock - return std::make_unique(*mutexLocal_); - } - - bool Robot::getIsLocked() const - { - return mutexLocal_->isLocked(); + return lock; } Eigen::Index Robot::nmotors() const diff --git a/core/src/solver/constraint_solvers.cc b/core/src/solver/constraint_solvers.cc index 24d97f258..90eaf56cb 100644 --- a/core/src/solver/constraint_solvers.cc +++ b/core/src/solver/constraint_solvers.cc @@ -38,15 +38,16 @@ namespace jiminy Eigen::Index constraintsRowsMax = 0U; constraints->foreach( [&](const std::shared_ptr & constraint, - ConstraintNodeType node) + ConstraintRegistryType type) { // Define constraint blocks - const Eigen::Index constraintDim = static_cast(constraint->getDim()); + const Eigen::Index constraintSize = + static_cast(constraint->getSize()); ConstraintBlock block{}; ConstraintData constraintData{}; - switch (node) + switch (type) { - case ConstraintNodeType::BOUNDS_JOINTS: + case ConstraintRegistryType::BOUNDS_JOINTS: // The joint is blocked in only one direction block.lo = 0.0; block.hi = INF; @@ -55,8 +56,8 @@ namespace jiminy constraintData.blocks[0] = block; constraintData.nBlocks = 1; break; - case ConstraintNodeType::CONTACT_FRAMES: - case ConstraintNodeType::COLLISION_BODIES: + case ConstraintRegistryType::CONTACT_FRAMES: + case ConstraintRegistryType::COLLISION_BODIES: // Non-penetration normal force block.lo = 0.0; block.hi = INF; @@ -85,14 +86,14 @@ namespace jiminy constraintData.nBlocks = 3; break; - case ConstraintNodeType::USER: + case ConstraintRegistryType::USER: default: break; } - constraintData.dim = constraintDim; + constraintData.dim = constraintSize; constraintData.constraint = constraint.get(); constraintsData_.emplace_back(std::move(constraintData)); - constraintsRowsMax += constraintDim; + constraintsRowsMax += constraintSize; }); // Resize buffers @@ -338,15 +339,15 @@ namespace jiminy { continue; } - const Eigen::Index constraintDim = constraintData.dim; + const Eigen::Index constraintSize = constraintData.dim; if (!isStateUpToDate) { - J_.middleRows(constraintRows, constraintDim) = constraint->getJacobian(); - gamma_.segment(constraintRows, constraintDim) = constraint->getDrift(); - lambda_.segment(constraintRows, constraintDim) = constraint->lambda_; + J_.middleRows(constraintRows, constraintSize) = constraint->getJacobian(); + gamma_.segment(constraintRows, constraintSize) = constraint->getDrift(); + lambda_.segment(constraintRows, constraintSize) = constraint->lambda_; } constraintData.startIndex = constraintRows; - constraintRows += constraintDim; + constraintRows += constraintSize; }; // Extract active rows @@ -433,9 +434,9 @@ namespace jiminy { continue; } - const Eigen::Index constraintDim = static_cast(constraint->getDim()); - constraint->lambda_ = lambda_.segment(constraintRows, constraintDim); - constraintRows += constraintDim; + const Eigen::Index constraintSize = static_cast(constraint->getSize()); + constraint->lambda_ = lambda_.segment(constraintRows, constraintSize); + constraintRows += constraintSize; }; /* Compute resulting acceleration, no matter if computing forces was successful. diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index cf241349b..5d5b84345 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -347,7 +347,10 @@ namespace jiminy AbstractPerlinNoiseOctave::AbstractPerlinNoiseOctave(double wavelength) : wavelength_{wavelength} { - assert(wavelength_ > 0 && "wavelength must be strictly larger than 0.0."); + if (wavelength_ <= 0.0) + { + JIMINY_THROW(std::invalid_argument, "'wavelength' must be strictly larger than 0.0."); + } shift_ = uniform(std::random_device{}); } @@ -447,8 +450,11 @@ namespace jiminy period_{period} { // Make sure the wavelength is multiple of the period - assert(std::abs(std::round(period / wavelength) * wavelength - period) < 1e-6 && - "wavelength must be multiple of period."); + if (std::abs(std::round(period / wavelength) * wavelength - period) > + std::numeric_limits::epsilon()) + { + JIMINY_THROW(std::invalid_argument, "'wavelength' must be multiple of 'period'."); + } // Initialize the permutation vector with values from 0 to 255 and shuffle it internal::randomizePermutationVector(std::random_device{}, perm_); @@ -569,7 +575,10 @@ namespace jiminy period_{period} { // Make sure the period is larger than the wavelength - assert(period_ >= wavelength && "Period must be larger than wavelength."); + if (period_ < wavelength) + { + JIMINY_THROW(std::invalid_argument, "'period' must be larger than 'wavelength'."); + } } double PeriodicPerlinProcess::getPeriod() const noexcept diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py b/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py index cb16a255d..37876ae65 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/quantities.py @@ -29,7 +29,7 @@ import jiminy_py.core as jiminy from jiminy_py.core import ( # pylint: disable=no-name-in-module - multi_array_copyto) + array_copyto, multi_array_copyto) from jiminy_py.dynamics import State, Trajectory, update_quantities import pinocchio as pin @@ -296,6 +296,14 @@ class and all their constructor keyword-arguments name: cls(env, self, **kwargs) for name, (cls, kwargs) in requirements.items()} + # Update the state explicitly if available but auto-refresh not enabled + self._state: Optional[StateQuantity] = None + if isinstance(self, AbstractQuantity): + quantity = self.requirements["state"] + if not quantity.auto_refresh: + assert isinstance(quantity, StateQuantity) + self._state = quantity + # Shared cache handling self._cache: Optional[SharedCache[ValueT]] = None self.has_cache = False @@ -423,10 +431,17 @@ def get(self) -> ValueT: # Evaluate quantity try: + # Initialize quantity if not self._is_initialized: self.initialize() assert (self._is_initialized and self._is_active) # type: ignore[unreachable] + + # Make sure that the state has been refreshed + if self._state is not None: + self._state.get() + + # Refresh quantity value = self.refresh() except RecursionError as e: raise LookupError( @@ -471,7 +486,7 @@ def reset(self, if (not ignore_auto_refresh and self.auto_refresh and not self.has_cache): raise RuntimeError( - "Automatic refresh enabled but no shared cache available. " + "Automatic refresh enabled but no shared cache is available. " "Please add one before calling this method.") # Reset all requirements first @@ -611,13 +626,23 @@ class and all their constructor keyword-arguments # Make sure that no user-specified requirement is named 'trajectory' requirement_names = requirements.keys() - if any(name in requirement_names for name in ("state", "trajectory")): + if "trajectory" in requirement_names: raise ValueError( - "No user-specified requirement can be named 'state' nor " - "'trajectory' as these keys are reserved.") - - # Add state quantity as requirement - requirements["state"] = (StateQuantity, dict(mode=mode)) + "Key 'trajectory' is reserved and cannot be used for " + "user-specified requirements.") + + # Make sure that state requirement is valid if any or use default + quantity = requirements.get("state") + if quantity is not None: + cls, kwargs = quantity + if (not issubclass(cls, StateQuantity) or + kwargs.setdefault("mode", mode) != mode): + raise ValueError( + "Key 'state' is reserved and can only be used to specify " + "a `StateQuantity` requirement, as a way to give the " + "opportunity to overwrite 'update_*' default arguments.") + else: + requirements["state"] = (StateQuantity, dict(mode=mode)) # Call base implementation super().__init__(env, parent, requirements, auto_refresh=auto_refresh) @@ -905,7 +930,12 @@ def __init__(self, env: InterfaceJiminyEnv, parent: Optional[InterfaceQuantity], *, - mode: QuantityEvalMode = QuantityEvalMode.TRUE) -> None: + mode: QuantityEvalMode = QuantityEvalMode.TRUE, + update_kinematics: bool = True, + update_dynamics: bool = False, + update_centroidal: bool = False, + update_energy: bool = False, + update_jacobian: bool = False) -> None: """ :param env: Base or wrapped jiminy environment. :param parent: Higher-level quantity from which this quantity is a @@ -917,12 +947,66 @@ def __init__(self, some reference trajectory at the current simulation time will be used instead. Optional: 'QuantityEvalMode.TRUE' by default. + :param update_kinematics: Whether to update body and frame transforms, + spatial velocities and accelerations stored + in `self.pinocchio_data` if necessary to be + consistent with the current state of the + robot. This argument has no effect if mode is + set to `QuantityEvalMode.TRUE` because this + property is already guarantee. + Optional: False by default. + :param update_dynamics: Whether to update the non-linear effects and + the joint internal forces stored in + `self.pinocchio_data` if necessary. + Optional: False by default. + :param update_centroidal: Whether to update the centroidal dynamics + (incl. CoM) stored in `self.pinocchio_data` + if necessary. + Optional: True by default. + :param update_energy: Whether to update the potential and kinematic + energy stored in `self.pinocchio_data` if + necessary. + Optional: False by default. + :param update_jacobian: Whether to update the joint Jacobian matrices + stored in `self.pinocchio_data` if necessary. + Optional: False by default. """ + # Make sure that the input arguments are valid + update_kinematics = ( + update_kinematics or update_dynamics or update_centroidal or + update_energy or update_jacobian) + # Backup user argument(s) self.mode = mode + self.update_kinematics = update_kinematics + self.update_dynamics = update_dynamics + self.update_centroidal = update_centroidal + self.update_energy = update_energy + self.update_jacobian = update_jacobian + + # Enable auto-refresh based on the evaluation mode + # Note that it is necessary to auto-refresh this quantity, as it is the + # one responsible for making sure that dynamics quantities are always + # up-to-date when refreshing quantities. The latter are involved one + # way of the other in the computation of any quantity, which means that + # pre-computing it does not induce any unnecessary computations as long + # as the user fetches the value of at least one quantity. Although this + # assumption is very likely to be true at the step update period, it is + # not the case at the observer update period. It sounds more efficient + # refresh to the state the first time any quantity gets computed. + # However, systematically checking if the state must be refreshed for + # all quantities adds overheat and may be fairly costly overall. The + # optimal trade-off is to rely on auto-refresh if the evaluation mode + # is TRUE, since refreshing the state only consists in copying some + # data, which is very cheap. On the contrary, it is more efficient to + # only refresh the state when needed if the evaluation mode is TRAJ. + # * Update state: 500ns (TRUE) | 5.0us (TRAJ) + # * Check cache state: 70ns + auto_refresh = mode == QuantityEvalMode.TRUE - # Call base implementation - super().__init__(env, parent, requirements={}, auto_refresh=True) + # Call base implementation. + super().__init__( + env, parent, requirements={}, auto_refresh=auto_refresh) # Create empty trajectory database, manually added as a requirement. # Note that it must be done after base initialization, otherwise a @@ -936,11 +1020,47 @@ def __init__(self, self.pinocchio_data = env.robot.pinocchio_data # State for which the quantity must be evaluated - self._f_external_slices: Tuple[np.ndarray, ...] = () - self._f_external_list: Tuple[np.ndarray, ...] = () self.state = State(t=np.nan, q=np.array([])) + # Persistent buffer for storing body external forces if necessary + self._f_external_vec = pin.StdVec_Force() + self._f_external_list: List[np.ndarray] = [] + self._f_external_batch = np.array([]) + self._f_external_slices: List[np.ndarray] = [] + + # Persistent buffer storing all lambda multipliers for efficiency + self._constraint_lambda_batch = np.array([]) + + # Slices in stacked lambda multiplier flat vector + self._constraint_lambda_slices: List[np.ndarray] = [] + + # Lambda multipliers of all the constraints individually + self._constraint_lambda_list: List[np.ndarray] = [] + + # Whether to update kinematic and dynamic data to be consistent with + # the current state of the robot, based on the requirement of all the + # co-owners of shared cache. + self._update_kinematics = False + self._update_dynamics = False + self._update_centroidal = False + self._update_energy = False + self._update_jacobian = False + def initialize(self) -> None: + # Determine which data must be update based on shared cache co-owners + owners = self.cache.owners if self.has_cache else (self,) + self._update_kinematics = False + self._update_dynamics = False + self._update_centroidal = False + self._update_energy = False + self._update_jacobian = False + for owner in owners: + self._update_kinematics |= owner.update_kinematics + self._update_dynamics |= owner.update_dynamics + self._update_centroidal |= owner.update_centroidal + self._update_energy |= owner.update_energy + self._update_jacobian |= owner.update_jacobian + # Refresh robot and pinocchio proxies for co-owners of shared cache. # Note that automatic refresh is not sufficient to guarantee that # `initialize` will be called unconditionally, because it will be @@ -950,7 +1070,6 @@ def initialize(self) -> None: # instance to found the cache empty. Only the necessary bits are # synchronized instead of the whole method, to avoid messing up with # computation graph tracking. - owners = self.cache.owners if self.has_cache else (self,) for owner in owners: assert isinstance(owner, StateQuantity) if owner._is_initialized: @@ -972,26 +1091,61 @@ def initialize(self) -> None: # The quantity will be considered initialized and active at this point. super().initialize() - # State for which the quantity must be evaluated + # Refresh proxies and allocate memory for storing external forces + if self.mode == QuantityEvalMode.TRUE: + self._f_external_vec = self.env.robot_state.f_external + else: + self._f_external_vec = pin.StdVec_Force() + self._f_external_vec.extend([ + pin.Force() for _ in range(self.pinocchio_model.njoints)]) + self._f_external_list = [ + f_ext.vector for f_ext in self._f_external_vec] + self._f_external_batch = np.zeros((self.pinocchio_model.njoints, 6)) + self._f_external_slices = list(self._f_external_batch) + + # Allocate memory for lambda vector + self._constraint_lambda_batch = np.zeros( + (len(self.robot.log_constraint_fieldnames),)) + + # Refresh mapping from lambda multipliers to corresponding slice + self._constraint_lambda_list.clear() + self._constraint_lambda_slices.clear() + constraint_lookup_pairs = tuple( + (f"Constraint{registry_type}", registry) + for registry_type, registry in ( + ("BoundJoints", self.robot.constraints.bounds_joints), + ("ContactFrames", self.robot.constraints.contact_frames), + ("CollisionBodies", { + name: constraint for constraints in ( + self.robot.constraints.collision_bodies) + for name, constraint in constraints.items()}), + ("User", self.robot.constraints.user))) + i = 0 + while i < len(self.robot.log_constraint_fieldnames): + fieldname = self.robot.log_constraint_fieldnames[i] + for registry_type, registry in constraint_lookup_pairs: + if fieldname.startswith(registry_type): + break + constraint_name = fieldname[len(registry_type):-1] + constraint = registry[constraint_name] + self._constraint_lambda_list.append(constraint.lambda_c) + self._constraint_lambda_slices.append( + self._constraint_lambda_batch[i:(i + constraint.size)]) + i += constraint.size + + # Allocate state for which the quantity must be evaluated if needed if self.mode == QuantityEvalMode.TRUE: if not self.env.is_simulation_running: raise RuntimeError("No simulation running. Impossible to " "initialize this quantity.") - self._f_external_list = tuple( - f_ext.vector for f_ext in self.env.robot_state.f_external) - if self._f_external_list: - f_external_batch = np.stack(self._f_external_list, axis=0) - else: - f_external_batch = np.array([]) self.state = State( - self.env.stepper_state.t, + 0.0, self.env.robot_state.q, self.env.robot_state.v, self.env.robot_state.a, self.env.robot_state.u, self.env.robot_state.command, - f_external_batch) - self._f_external_slices = tuple(f_external_batch) + self._f_external_batch) def refresh(self) -> State: """Compute the current state depending on the mode of evaluation, and @@ -999,23 +1153,47 @@ def refresh(self) -> State: """ # Update state at which the quantity must be evaluated if self.mode == QuantityEvalMode.TRUE: + # Update the current simulation time + self.state.t = self.env.stepper_state.t + + # Update external forces and constraint multipliers in state buffer multi_array_copyto(self._f_external_slices, self._f_external_list) + multi_array_copyto( + self._constraint_lambda_slices, self._constraint_lambda_list) else: self.state = self.trajectory.get() - # Update all the dynamical quantities that can be given available data if self.mode == QuantityEvalMode.REFERENCE: - update_quantities( - self.robot, - self.state.q, - self.state.v, - self.state.a, - update_physics=True, - update_centroidal=True, - update_energy=True, - update_jacobian=False, - update_collisions=True, - use_theoretical_model=self.trajectory.use_theoretical_model) + # Copy body external forces from stacked buffer to force vector + has_forces = self.state.f_external is not None + if has_forces: + array_copyto(self._f_external_batch, self.state.f_external) + multi_array_copyto(self._f_external_list, + self._f_external_slices) + + # Update all dynamical quantities that can be given available data + if self.update_kinematics: + update_quantities( + self.robot, + self.state.q, + self.state.v, + self.state.a, + self._f_external_vec if has_forces else None, + update_dynamics=self._update_dynamics, + update_centroidal=self._update_centroidal, + update_energy=self._update_energy, + update_jacobian=self._update_jacobian, + update_collisions=False, + use_theoretical_model=( + self.trajectory.use_theoretical_model)) + + # Restore lagrangian multipliers of the constraints if available + has_constraints = self.state.lambda_c is not None + if has_constraints: + array_copyto( + self._constraint_lambda_batch, self.state.lambda_c) + multi_array_copyto(self._constraint_lambda_list, + self._constraint_lambda_slices) # Return state return self.state diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py index 03bf129b7..7e3db84a7 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/generic.py @@ -904,7 +904,7 @@ def step(self, # type: ignore[override] if terminated or truncated: self._num_steps_beyond_terminate = 0 else: - if not self.is_training and self._num_steps_beyond_terminate == 0: + if self.is_training and self._num_steps_beyond_terminate == 0: LOGGER.error( "Calling `step` after termination causes the reward to be " "'nan' systematically and is strongly discouraged in " diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py index d22168ed0..416a5836c 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/__init__.py @@ -13,7 +13,7 @@ MultiFramesPosition, MultiFramesXYZQuat, MultiFramesMeanXYZQuat, - AverageFramePose, + AverageFrameXYZQuat, AverageFrameRollPitch, AverageFrameSpatialVelocity, ActuatedJointsPosition) @@ -24,10 +24,10 @@ AverageBaseMomentum, MultiFootMeanXYZQuat, MultiFootRelativeXYZQuat, + MultiContactRelativeForceTangential, CenterOfMass, CapturePoint, ZeroMomentPoint, - MultiContactRelativeForceTangential, translate_position_odom) @@ -48,7 +48,8 @@ 'MultiFootMeanXYZQuat', 'MultiFootRelativeXYZQuat', 'MultiFootMeanOdometryPose', - 'AverageFramePose', + 'MultiContactRelativeForceTangential', + 'AverageFrameXYZQuat', 'AverageFrameRollPitch', 'AverageFrameSpatialVelocity', 'AverageBaseSpatialVelocity', @@ -59,6 +60,5 @@ 'CenterOfMass', 'CapturePoint', 'ZeroMomentPoint', - 'MultiContactRelativeForceTangential', 'translate_position_odom' ] diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py index 73e94c2d9..6970034b9 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/generic.py @@ -1126,8 +1126,9 @@ def refresh(self) -> np.ndarray: @dataclass(unsafe_hash=True) -class AverageFramePose(InterfaceQuantity[np.ndarray]): - """Average pose of a given frame over the whole agent step. +class AverageFrameXYZQuat(InterfaceQuantity[np.ndarray]): + """Spatial vector representation (X, Y, Z, QuatX, QuatY, QuatZ, QuatW) of + the average pose of a given frame over the whole agent step. The average frame pose is obtained by integration of the average velocity over the whole agent step, backward in time from the state at the end of @@ -1203,7 +1204,7 @@ class AverageFrameRollPitch(InterfaceQuantity[np.ndarray]): Roll-Pitch_yaw decomposition of a given frame over the whole agent step. .. seealso:: - See `remove_yaw_from_quat` and `AverageFramePose` for details about + See `remove_yaw_from_quat` and `AverageFrameXYZQuat` for details about the Roll-Pitch-Yaw decomposition and how the average frame pose is defined respectively. """ @@ -1245,7 +1246,7 @@ def __init__(self, parent, requirements=dict( quat_mean=(MaskedQuantity, dict( - quantity=(AverageFramePose, dict( + quantity=(AverageFrameXYZQuat, dict( frame_name=frame_name, mode=mode)), axis=0, @@ -1340,7 +1341,7 @@ def __init__(self, frame_name=frame_name, mode=mode)), quat_mean=(MaskedQuantity, dict( - quantity=(AverageFramePose, dict( + quantity=(AverageFrameXYZQuat, dict( frame_name=frame_name, mode=mode)), axis=0, diff --git a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py index 493122016..0ba1d3b20 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/quantities/locomotion.py @@ -14,7 +14,8 @@ import pinocchio as pin from ..bases import ( - InterfaceJiminyEnv, InterfaceQuantity, AbstractQuantity, QuantityEvalMode) + InterfaceJiminyEnv, InterfaceQuantity, AbstractQuantity, StateQuantity, + QuantityEvalMode) from ..utils import ( matrix_to_yaw, quat_to_yaw, quat_to_matrix, quat_multiply, quat_apply) @@ -660,7 +661,13 @@ def __init__( # Call base implementation super().__init__( - env, parent, requirements={}, mode=mode, auto_refresh=False) + env, + parent, + requirements=dict( + state=(StateQuantity, dict( + update_centroidal=True))), + mode=mode, + auto_refresh=False) # Pre-allocate memory for the CoM quantity self._com_data: np.ndarray = np.array([]) @@ -747,8 +754,7 @@ def __init__(self, com_position=(CenterOfMass, dict( kinematic_level=pin.POSITION, mode=mode)), - odom_pose=(BaseOdometryPose, dict(mode=mode)) - ), + odom_pose=(BaseOdometryPose, dict(mode=mode))), mode=mode, auto_refresh=False) @@ -855,8 +861,7 @@ def __init__(self, com_velocity=(CenterOfMass, dict( kinematic_level=pin.VELOCITY, mode=mode)), - odom_pose=(BaseOdometryPose, dict(mode=mode)) - ), + odom_pose=(BaseOdometryPose, dict(mode=mode))), mode=mode, auto_refresh=False) @@ -881,7 +886,7 @@ def initialize(self) -> None: update_quantities( self.robot, pin.neutral(self.robot.pinocchio_model_th), - update_physics=True, + update_dynamics=False, update_centroidal=True, update_energy=False, update_jacobian=False, @@ -933,10 +938,7 @@ def __init__(self, """ # Call base implementation super().__init__( - env, - parent, - requirements={}, - auto_refresh=False) + env, parent, requirements={}, auto_refresh=False) # Weight of the robot self._robot_weight: float = -1 diff --git a/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py b/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py index 6499d0c4b..594c0a113 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py +++ b/python/gym_jiminy/common/gym_jiminy/common/wrappers/observation_stack.py @@ -198,7 +198,7 @@ def _setup(self) -> None: if (step_ratio // frame_ratio) * frame_ratio != step_ratio: LOGGER.warning( "Beware `step_dt // observe_dt` is not a multiple of " - "`skip_frame_ratio + 1`.") + "`skip_frames_ratio + 1`.") # Copy observe and control update periods from wrapped environment self.observe_dt = self.env.observe_dt diff --git a/python/gym_jiminy/unit_py/data/anymal_pipeline.json b/python/gym_jiminy/unit_py/data/anymal_pipeline.json index 6ca97568d..641c5940f 100644 --- a/python/gym_jiminy/unit_py/data/anymal_pipeline.json +++ b/python/gym_jiminy/unit_py/data/anymal_pipeline.json @@ -58,8 +58,8 @@ ["measurements", "ImuSensor"], ["actions"] ], - "num_stack": 3, - "skip_frames_ratio": 2 + "num_stack": 4, + "skip_frames_ratio": 3 } } } diff --git a/python/gym_jiminy/unit_py/data/anymal_pipeline.toml b/python/gym_jiminy/unit_py/data/anymal_pipeline.toml index 111118f55..053d2beef 100644 --- a/python/gym_jiminy/unit_py/data/anymal_pipeline.toml +++ b/python/gym_jiminy/unit_py/data/anymal_pipeline.toml @@ -54,5 +54,5 @@ nested_filter_keys = [ ["measurements", "ImuSensor"], ["actions"], ] -num_stack = 3 -skip_frames_ratio = 2 +num_stack = 4 +skip_frames_ratio = 3 diff --git a/python/gym_jiminy/unit_py/test_misc.py b/python/gym_jiminy/unit_py/test_misc.py index 8baac24de..14dd440de 100644 --- a/python/gym_jiminy/unit_py/test_misc.py +++ b/python/gym_jiminy/unit_py/test_misc.py @@ -29,7 +29,8 @@ def test_play_log_data(self): """ TODO: Write documentation. """ # Instantiate the environment - env = gym.make("gym_jiminy.envs:atlas", debug=True) + env = gym.make("gym_jiminy.envs:atlas", debug=False) + env.eval() # Run a few steps env.reset(seed=0) @@ -50,7 +51,8 @@ def test_play_log_data(self): backend="panda3d-sync", record_video_path=video_path, display_contacts=True, - display_f_external=True) + display_f_external=True, + verbose=False) Viewer.close() # Check the external forces has been updated diff --git a/python/gym_jiminy/unit_py/test_pipeline_design.py b/python/gym_jiminy/unit_py/test_pipeline_design.py index 973a18112..4ea10cc24 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -6,7 +6,6 @@ import unittest import numpy as np -import gymnasium as gym from jiminy_py.robot import _gcd from jiminy_py.log import extract_trajectory_from_log @@ -16,87 +15,69 @@ from gym_jiminy.common.blocks import PDController +STEP_DT = 0.04 +PID_KP = np.full((12,), fill_value=1500) +PID_KD = np.full((12,), fill_value=0.01) + TOLERANCE = 1.0e-6 -class PipelineDesign(unittest.TestCase): +def _create_pipeline(num_stack: int, + skip_frames_ratio: int) -> InterfaceJiminyEnv: """ TODO: Write documentation """ - def setUp(self): - """ TODO: Write documentation - """ - self.step_dt = 0.04 - self.pid_kp = np.full((12,), fill_value=1500) - self.pid_kd = np.full((12,), fill_value=0.01) - self.num_stack = 3 - self.skip_frames_ratio = 2 - - self.ANYmalPipelineEnv = build_pipeline( - env_config=dict( - cls='gym_jiminy.envs.ANYmalJiminyEnv', - kwargs=dict( - step_dt=self.step_dt, - debug=False - ) - ), - layers_config=[ - dict( - block=dict( - cls='gym_jiminy.common.blocks.PDController', - kwargs=dict( - update_ratio=2, - kp=self.pid_kp, - kd=self.pid_kd, - joint_position_margin=0.0, - joint_velocity_limit=float("inf"), - joint_acceleration_limit=float("inf") - ) - ), - wrapper=dict( - kwargs=dict( - augment_observation=True - ) - ) - ), dict( - block=dict( - cls='gym_jiminy.common.blocks.PDAdapter', - kwargs=dict( - update_ratio=-1, - order=1, - ) - ), - wrapper=dict( - kwargs=dict( - augment_observation=False - ) - ) - ), dict( - block=dict( - cls='gym_jiminy.common.blocks.MahonyFilter', - kwargs=dict( - update_ratio=1, - exact_init=True, - kp=1.0, - ki=0.1 - ) - ) - ), dict( - wrapper=dict( - cls='gym_jiminy.common.wrappers.StackObservation', - kwargs=dict( - nested_filter_keys=[ - ('t',), - ('measurements', 'ImuSensor'), - ('actions',) - ], - num_stack=self.num_stack, - skip_frames_ratio=self.skip_frames_ratio - ) - ) - ) - ] - ) + return build_pipeline( + env_config=dict( + cls='gym_jiminy.envs.ANYmalJiminyEnv', + kwargs=dict( + step_dt=STEP_DT, + debug=False)), + layers_config=[ + dict( + block=dict( + cls='gym_jiminy.common.blocks.PDController', + kwargs=dict( + update_ratio=2, + kp=PID_KP, + kd=PID_KD, + joint_position_margin=0.0, + joint_velocity_limit=float("inf"), + joint_acceleration_limit=float("inf"))), + wrapper=dict( + kwargs=dict( + augment_observation=True)) + ), dict( + block=dict( + cls='gym_jiminy.common.blocks.PDAdapter', + kwargs=dict( + update_ratio=-1, + order=1)), + wrapper=dict( + kwargs=dict( + augment_observation=False)) + ), dict( + block=dict( + cls='gym_jiminy.common.blocks.MahonyFilter', + kwargs=dict( + update_ratio=1, + exact_init=True, + kp=1.0, + ki=0.1)) + ), dict( + wrapper=dict( + cls='gym_jiminy.common.wrappers.StackObservation', + kwargs=dict( + nested_filter_keys=[ + ('t',), + ('measurements', 'ImuSensor'), + ('actions',)], + num_stack=num_stack, + skip_frames_ratio=skip_frames_ratio)))]) + +class PipelineDesign(unittest.TestCase): + """ TODO: Write documentation + """ def test_load_files(self): """ TODO: Write documentation """ @@ -104,7 +85,8 @@ def test_load_files(self): data_dir = os.path.join(os.path.dirname(__file__), "data") # Generate machine-dependent reference trajectory - env = self.ANYmalPipelineEnv() + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + env = ANYmalPipelineEnv() env.reset(seed=0) for _ in range(10): env.step(env.action) @@ -130,14 +112,16 @@ def test_load_files(self): def test_override_default(self): """ TODO: Write documentation """ + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + # Override default environment arguments - step_dt_2 = 2 * self.step_dt - env = self.ANYmalPipelineEnv(step_dt=step_dt_2) + step_dt_2 = 2 * STEP_DT + env = ANYmalPipelineEnv(step_dt=step_dt_2) self.assertEqual(env.unwrapped.step_dt, step_dt_2) # It does not override the default persistently - env = self.ANYmalPipelineEnv() - self.assertEqual(env.unwrapped.step_dt, self.step_dt) + env = ANYmalPipelineEnv() + self.assertEqual(env.unwrapped.step_dt, STEP_DT) def test_memory_leak(self): """Check that memory is freed when environment goes out of scope. @@ -146,7 +130,8 @@ def test_memory_leak(self): objects that cannot be tracked by Python, which would make it impossible for the garbage collector to release memory. """ - env = self.ANYmalPipelineEnv() + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + env = ANYmalPipelineEnv() env.reset(seed=0) proxy = weakref.proxy(env) env = None @@ -157,7 +142,10 @@ def test_initial_state(self): """ TODO: Write documentation """ # Get initial observation - env = self.ANYmalPipelineEnv() + num_stack = 4 + skip_frames_ratio = 3 + ANYmalPipelineEnv = _create_pipeline(num_stack, skip_frames_ratio) + env = ANYmalPipelineEnv() obs, _ = env.reset(seed=0) # Controller target is observed, and has right name @@ -165,11 +153,11 @@ def test_initial_state(self): # Target, time, and Imu data are stacked self.assertEqual(obs['t'].ndim, 1) - self.assertEqual(len(obs['t']), self.num_stack) + self.assertEqual(len(obs['t']), num_stack) self.assertEqual(obs['measurements']['ImuSensor'].ndim, 3) - self.assertEqual(len(obs['measurements']['ImuSensor']), self.num_stack) + self.assertEqual(len(obs['measurements']['ImuSensor']), num_stack) controller_target_obs = obs['actions']['pd_controller'] - self.assertEqual(len(controller_target_obs), self.num_stack) + self.assertEqual(len(controller_target_obs), num_stack) self.assertEqual(obs['measurements']['EffortSensor'].ndim, 2) # Stacked obs are zeroed @@ -192,50 +180,59 @@ def test_initial_state(self): def test_stacked_obs(self): """ TODO: Write documentation """ - # Perform a single step - env = self.ANYmalPipelineEnv() - env.reset(seed=0) - action = env.action + 1.0e-3 - obs, *_ = env.step(action) - - # Extract PD controller wrapper env - env_ctrl = env.env.env.env - assert isinstance(env_ctrl.controller, PDController) - - # Observation stacking is skipping the required number of frames - stack_dt = (self.skip_frames_ratio + 1) * env.observe_dt - t_obs_last = env.step_dt - env.step_dt % stack_dt - self.assertTrue(np.isclose( - obs['t'][-1], env.stepper_state.t, TOLERANCE)) - for i in range(1, self.num_stack): - self.assertTrue(np.isclose( - obs['t'][::-1][i], t_obs_last - (i - 1) * stack_dt, TOLERANCE)) - - # Initial observation is consistent with internal simulator state - controller_target_obs = obs['actions']['pd_controller'] - self.assertTrue(np.all(controller_target_obs[-1] == env_ctrl.action)) - imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] - imu_data_obs = obs['measurements']['ImuSensor'][-1] - self.assertTrue(np.all(imu_data_ref == imu_data_obs)) - state_ref = {'q': env.robot_state.q, 'v': env.robot_state.v} - state_obs = obs['states']['agent'] - self.assertTrue(np.all(state_ref['q'] == state_obs['q'])) - self.assertTrue(np.all(state_ref['v'] == state_obs['v'])) - - # Step until to reach the next stacking breakpoint - n_steps_breakpoint = int(stack_dt // _gcd(env.step_dt, stack_dt)) - for _ in range(1, n_steps_breakpoint): + for num_stack, skip_frames_ratio in ((3, 2), (4, 3)): + # Perform a single step + ANYmalPipelineEnv = _create_pipeline(num_stack, skip_frames_ratio) + env = ANYmalPipelineEnv() + env.reset(seed=0) + action = env.action + 1.0e-3 obs, *_ = env.step(action) - for i, t in enumerate(np.flip(obs['t'])): - self.assertTrue(np.isclose( - t, n_steps_breakpoint * env.step_dt - i * stack_dt, TOLERANCE)) - imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] - imu_data_obs = obs['measurements']['ImuSensor'][-1] - self.assertTrue(np.all(imu_data_ref == imu_data_obs)) + + # Extract PD controller wrapper env + env_ctrl = env.env.env.env + assert isinstance(env_ctrl.controller, PDController) + + # Observation stacking is skipping the required number of frames + stack_dt = (skip_frames_ratio + 1) * env.observe_dt + t_obs_last = env.step_dt - env.step_dt % stack_dt + is_shifted = env.step_dt % stack_dt > 0.0 + np.testing.assert_allclose( + obs['t'][-1], env.stepper_state.t, atol=TOLERANCE) + for i in range(1, num_stack): + t_ref = max(t_obs_last - (i - is_shifted) * stack_dt, 0.0) + np.testing.assert_allclose( + obs['t'][::-1][i], t_ref, atol=TOLERANCE) + + # Initial observation is consistent with internal simulator state + controller_target = obs['actions']['pd_controller'] + np.testing.assert_allclose(controller_target[-1], env_ctrl.action) + imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] + imu_data_obs = obs['measurements']['ImuSensor'][-1] + np.testing.assert_allclose(imu_data_ref, imu_data_obs) + state_ref = {'q': env.robot_state.q, 'v': env.robot_state.v} + state_obs = obs['states']['agent'] + np.testing.assert_allclose(state_ref['q'], state_obs['q']) + np.testing.assert_allclose(state_ref['v'], state_obs['v']) + + # Step until to next stacking breakpoint and filling buffer + n_steps_breakpoint = int(stack_dt // _gcd(env.step_dt, stack_dt)) + n_steps_full = int(np.ceil( + num_stack / int(np.ceil(env.step_dt / stack_dt) + 1))) + n_steps = n_steps_breakpoint * int( + np.ceil(n_steps_full / n_steps_breakpoint)) + for _ in range(1, n_steps): + obs, *_ = env.step(action) + for i, t in enumerate(np.flip(obs['t'])): + t_ref = n_steps * env.step_dt - i * stack_dt + np.testing.assert_allclose(t, t_ref, atol=TOLERANCE) + imu_data_ref = env.simulator.robot.sensor_measurements['ImuSensor'] + imu_data_obs = obs['measurements']['ImuSensor'][-1] + np.testing.assert_allclose(imu_data_ref, imu_data_obs) def test_update_periods(self): # Perform a single step and get log data - env = self.ANYmalPipelineEnv() + ANYmalPipelineEnv = _create_pipeline(num_stack=4, skip_frames_ratio=3) + env = ANYmalPipelineEnv() def configure_telemetry() -> InterfaceJiminyEnv: engine_options = env.simulator.get_options() diff --git a/python/gym_jiminy/unit_py/test_quantities.py b/python/gym_jiminy/unit_py/test_quantities.py index b57175047..81a1f9ce5 100644 --- a/python/gym_jiminy/unit_py/test_quantities.py +++ b/python/gym_jiminy/unit_py/test_quantities.py @@ -262,7 +262,8 @@ def test_masked(self): env.quantities["v_masked"], quantity.data[[0, 2, 4]]) def test_true_vs_reference(self): - env = gym.make("gym_jiminy.envs:atlas") + env = gym.make("gym_jiminy.envs:atlas", debug=False) + env.eval() frame_names = [ frame.name for frame in env.robot.pinocchio_model.frames] @@ -309,8 +310,10 @@ def test_true_vs_reference(self): env.quantities["ref"] = quantity_creator( QuantityEvalMode.REFERENCE) + # No trajectory has been selected with self.assertRaises(RuntimeError): env.reset(seed=0) + env.quantities["ref"] env.quantities.add_trajectory("reference", trajectory) env.quantities.select_trajectory("reference") @@ -407,7 +410,7 @@ def test_capture_point(self): update_quantities( env.robot, pin.neutral(env.robot.pinocchio_model_th), - update_physics=True, + update_dynamics=True, update_centroidal=True, update_energy=False, update_jacobian=False, diff --git a/python/gym_jiminy/unit_py/test_rewards.py b/python/gym_jiminy/unit_py/test_rewards.py index 096311b1f..04b4158ee 100644 --- a/python/gym_jiminy/unit_py/test_rewards.py +++ b/python/gym_jiminy/unit_py/test_rewards.py @@ -28,15 +28,17 @@ class Rewards(unittest.TestCase): """ TODO: Write documentation """ def setUp(self): - self.env = gym.make("gym_jiminy.envs:atlas") + self.env = gym.make("gym_jiminy.envs:atlas-pid", debug=False) + self.env.eval() self.env.reset(seed=1) action = self.env.action_space.sample() for _ in range(10): self.env.step(action) self.env.stop() - trajectory = extract_trajectory_from_log(self.env.log_data) + self.env.train() + self.env.quantities.add_trajectory("reference", trajectory) self.env.quantities.select_trajectory("reference") @@ -130,12 +132,11 @@ def test_stability(self): def test_friction(self): CUTOFF = 0.5 - env = gym.make("gym_jiminy.envs:atlas-pid", debug=True) - reward_friction = MinimizeFrictionReward(env, cutoff=CUTOFF) + reward_friction = MinimizeFrictionReward(self.env, cutoff=CUTOFF) quantity = reward_friction.quantity - env.reset(seed=0) - _, _, terminated, _, _ = env.step(env.action) + self.env.reset(seed=0) + _, _, terminated, _, _ = self.env.step(self.env.action) force_tangential_rel = quantity.get() force_tangential_rel_norm = np.sum(np.square(force_tangential_rel)) diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py index 373655a6a..e3cfd4b92 100644 --- a/python/jiminy_py/src/jiminy_py/dynamics.py +++ b/python/jiminy_py/src/jiminy_py/dynamics.py @@ -7,9 +7,9 @@ """ # pylint: disable=invalid-name,no-member import logging -from bisect import bisect_right +from bisect import bisect_left from dataclasses import dataclass -from typing import Optional, Tuple, Sequence, Callable, Literal +from typing import List, Union, Optional, Tuple, Sequence, Callable, Literal import numpy as np @@ -138,6 +138,13 @@ class State: coordinates (Fx, Fy, Fz, Mx, My, Mz). """ + lambda_c: Optional[np.ndarray] = None + """Lambda multipliers associated with all the constraints as a 2D array. + + The first dimension corresponds to the N individual constraints applied on + the robot, while the second gathers the lambda multipliers. + """ + @dataclass(unsafe_hash=True) class Trajectory: @@ -198,15 +205,15 @@ def __init__(self, # Keep track of last request to speed up nearest neighbors search self._t_prev = 0.0 - self._index_prev = 0 + self._index_prev = 1 # List of optional state fields that are provided - self._fields: Tuple[str, ...] = () self._has_velocity = False self._has_acceleration = False self._has_effort = False self._has_command = False self._has_external_forces = False + self._has_constraints = False if states: state = states[0] self._has_velocity = state.v is not None @@ -214,8 +221,10 @@ def __init__(self, self._has_effort = state.u is not None self._has_command = state.command is not None self._has_external_forces = state.f_external is not None + self._has_constraints = state.lambda_c is not None self._fields = tuple( - field for field in ("v", "a", "u", "command", "f_external") + field for field in ( + "v", "a", "u", "command", "f_external", "lambda_c") if getattr(state, field) is not None) @property @@ -254,6 +263,12 @@ def has_external_forces(self) -> bool: """ return self._has_external_forces + @property + def has_constraints(self) -> bool: + """Whether the trajectory contains lambda multipliers of constraints. + """ + return self._has_constraints + @property def time_interval(self) -> Tuple[float, float]: """Time interval of the trajectory. @@ -303,14 +318,20 @@ def get(self, else: t = max(t, t_start) # Clipping right it is sufficient - # Get nearest neighbors timesteps for linear interpolation + # Get nearest neighbors timesteps for linear interpolation. + # Note that the left and right data points may be associated with the + # same timestamp, corresponding respectively t- and t+. These values + # are different for quantities that may change discontinuously such as + # the acceleration. If the state at such a timestamp is requested, then + # returning the left value is preferred, because it corresponds to the + # only state that was accessible to the user, ie after call `step`. if t < self._t_prev: - self._index_prev = 0 - self._index_prev = bisect_right( + self._index_prev = 1 + self._index_prev = bisect_left( self._times, t, self._index_prev, len(self._times) - 1) self._t_prev = t - # Skip interpolation if not necessary + # Skip interpolation if not necessary. index_left, index_right = self._index_prev - 1, self._index_prev t_left, s_left = self._times[index_left], self.states[index_left] if t - t_left < TRAJ_INTERP_TOL: @@ -338,11 +359,13 @@ def update_quantities(robot: jiminy.Model, position: np.ndarray, velocity: Optional[np.ndarray] = None, acceleration: Optional[np.ndarray] = None, - update_physics: bool = True, + f_external: Optional[ + Union[List[np.ndarray], pin.StdVec_Force]] = None, + update_dynamics: bool = True, update_centroidal: bool = True, update_energy: bool = True, update_jacobian: bool = False, - update_collisions: bool = True, + update_collisions: bool = False, use_theoretical_model: bool = True) -> None: """Compute all quantities using position, velocity and acceleration configurations. @@ -351,11 +374,11 @@ def update_quantities(robot: jiminy.Model, the model position, velocity and acceleration. This includes: - - body spatial transforms, + - body and frame spatial transforms, - body spatial velocities, - body spatial drifts, - - body transform acceleration, - - body transform jacobians, + - body spatial acceleration, + - joint transform jacobian matrices, - center-of-mass position, - center-of-mass velocity, - center-of-mass drift, @@ -366,34 +389,31 @@ def update_quantities(robot: jiminy.Model, - collisions and distances .. note:: - Computation results are stored internally in the robot, and can - be retrieved with associated getters. + Computation results are stored internally in the robot, and can be + retrieved with associated getters. .. warning:: This function modifies the internal robot data. - .. warning:: - It does not called overloaded pinocchio methods provided by - `jiminy_py.core` but the original pinocchio methods instead. As a - result, it does not take into account the rotor inertias / armatures. - One is responsible of calling the appropriate methods manually instead - of this one if necessary. This behavior is expected to change in the - future. - :param robot: Jiminy robot. - :param position: Robot position vector. - :param velocity: Robot velocity vector. - :param acceleration: Robot acceleration vector. - :param update_physics: Whether to compute the non-linear effects and - internal/external forces. - Optional: True by default. + :param position: Configuration vector. + :param velocity: Joint velocity vector. + :param acceleration: Joint acceleration vector. + :param f_external: External forces applied on each joints. + :param update_dynamics: Whether to compute the non-linear effects and the + joint internal forces. + Optional: True by default. :param update_centroidal: Whether to compute the centroidal dynamics (incl. CoM) of the robot. Optional: False by default. :param update_energy: Whether to compute the energy of the robot. Optional: False by default - :param update_jacobian: Whether to compute the jacobians. + :param update_jacobian: Whether to compute the Jacobian matrices of the + joint transforms. Optional: False by default. + :param update_collisions: Whether to detect collisions and compute + distances between all the geometry objects. + Optional: False by default. :param use_theoretical_model: Whether the state corresponds to the theoretical model when updating and fetching the state of the robot. @@ -406,7 +426,7 @@ def update_quantities(robot: jiminy.Model, model = robot.pinocchio_model data = robot.pinocchio_data - if (update_physics and update_centroidal and + if (update_dynamics and update_centroidal and update_energy and update_jacobian and velocity is not None and acceleration is None): pin.computeAllTerms(model, data, position, velocity) @@ -419,36 +439,37 @@ def update_quantities(robot: jiminy.Model, pin.forwardKinematics( model, data, position, velocity, acceleration) - if update_centroidal: - if velocity is None: - kinematic_level = pin.POSITION - elif acceleration is None: - kinematic_level = pin.VELOCITY - else: - kinematic_level = pin.ACCELERATION - pin.centerOfMass(model, data, kinematic_level, False) - pin.computeCentroidalMomentumTimeVariation(model, data) - if update_jacobian: if update_centroidal: pin.jacobianCenterOfMass(model, data) - pin.computeJointJacobians(model, data) + if not update_dynamics: + pin.computeJointJacobians(model, data) - if update_physics: + if update_dynamics: if velocity is not None: pin.nonLinearEffects(model, data, position, velocity) - pin.crba(model, data, position) + jiminy.crba(model, data, position) if update_energy: if velocity is not None: - pin.computeKineticEnergy(model, data) + jiminy.computeKineticEnergy( + model, data, position, velocity, update_kinematics=False) pin.computePotentialEnergy(model, data) + if update_centroidal: + pin.computeCentroidalMomentumTimeVariation(model, data) + if acceleration is not None: + pin.centerOfMass(model, data, pin.ACCELERATION, False) + + if (update_dynamics and velocity is not None and + acceleration is not None and f_external is not None): + jiminy.rnea(model, data, position, velocity, acceleration, f_external) + pin.updateFramePlacements(model, data) + pin.updateGeometryPlacements( + model, data, robot.collision_model, robot.collision_data) if update_collisions: - pin.updateGeometryPlacements( - model, data, robot.collision_model, robot.collision_data) pin.computeCollisions( robot.collision_model, robot.collision_data, stop_at_first_collision=False) @@ -776,7 +797,7 @@ def compute_freeflyer_state_from_fixed_body( position, velocity, acceleration, - update_physics=False, + update_dynamics=False, update_centroidal=False, update_energy=False, use_theoretical_model=use_theoretical_model) diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index fc8f2a472..db8a64d59 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -54,13 +54,9 @@ def extract_variables_from_log(log_vars: Dict[str, np.ndarray], :param fieldnames: Structured fieldnames. :param namespace: Namespace of the fieldnames. Empty string to disable. Optional: Empty by default. - :param keep_structure: Whether to return a dictionary mapping flattened - fieldnames to values. - Optional: True by default. - - :returns: - `np.ndarray` or None for each fieldname individually depending if it is - found or not. + :param as_dict: Whether to return a dictionary mapping flattened fieldnames + to values. + Optional: True by default. """ # Extract values from log if it exists if as_dict: @@ -238,7 +234,8 @@ def extract_trajectory_from_log(log_data: Dict[str, Any], "acceleration", "effort", "command", - "f_external"): + "f_external", + "constraint"): fieldnames = getattr(robot, f"log_{name}_fieldnames") try: data[name] = extract_variables_from_log( diff --git a/python/jiminy_py/unit_py/test_simple_pendulum.py b/python/jiminy_py/unit_py/test_simple_pendulum.py index 2b77878e7..7995a3201 100644 --- a/python/jiminy_py/unit_py/test_simple_pendulum.py +++ b/python/jiminy_py/unit_py/test_simple_pendulum.py @@ -230,7 +230,7 @@ def test_extract_trajectory(self): assert np.abs(state_0.t) < 1e-12 assert np.abs(state_f.t - T_END) < 1e-12 for key, value in asdict(state_f).items(): - if key == "t": + if key in ("t", "lambda_c"): continue data = getattr(robot_state, key) if key == "f_external": diff --git a/python/jiminy_py/unit_py/utilities.py b/python/jiminy_py/unit_py/utilities.py index 6c7a182c8..b187acead 100644 --- a/python/jiminy_py/unit_py/utilities.py +++ b/python/jiminy_py/unit_py/utilities.py @@ -119,6 +119,7 @@ def setup_controller_and_engine( telemetry_options["enableEffort"] = True telemetry_options["enableCommand"] = True telemetry_options["enableForceExternal"] = True + telemetry_options["enableConstraint"] = True telemetry_options["enableEnergy"] = True engine.set_options(engine_options) diff --git a/python/jiminy_pywrap/src/constraints.cc b/python/jiminy_pywrap/src/constraints.cc index 7144f6bba..622cfea87 100644 --- a/python/jiminy_pywrap/src/constraints.cc +++ b/python/jiminy_pywrap/src/constraints.cc @@ -104,6 +104,7 @@ namespace jiminy::python .ADD_PROPERTY_GET_WITH_POLICY("type", &AbstractConstraintBase::getType, bp::return_value_policy()) + .ADD_PROPERTY_GET("size", &AbstractConstraintBase::getSize) .ADD_PROPERTY_GET_SET("is_enabled", &AbstractConstraintBase::getIsEnabled, &internal::constraints::setIsEnable) @@ -283,14 +284,14 @@ namespace jiminy::python return constraintCollisionBodies; } - static bp::dict getRegistry(ConstraintTree & self) + static bp::dict getUser(ConstraintTree & self) { - bp::dict constraintRegistryPy; - for (auto & [name, constraint] : self.registry) + bp::dict constraintUserPy; + for (auto & [name, constraint] : self.user) { - constraintRegistryPy[name] = constraint; + constraintUserPy[name] = constraint; } - return constraintRegistryPy; + return constraintUserPy; } } @@ -301,6 +302,6 @@ namespace jiminy::python .ADD_PROPERTY_GET("bounds_joints", &internal::constraint_tree::getBoundJoints) .ADD_PROPERTY_GET("contact_frames", &internal::constraint_tree::getContactFrames) .ADD_PROPERTY_GET("collision_bodies", &internal::constraint_tree::getCollisionBodies) - .ADD_PROPERTY_GET("registry", &internal::constraint_tree::getRegistry); + .ADD_PROPERTY_GET("user", &internal::constraint_tree::getUser); } } diff --git a/python/jiminy_pywrap/src/helpers.cc b/python/jiminy_pywrap/src/helpers.cc index 2789db25a..90ed42f3c 100644 --- a/python/jiminy_pywrap/src/helpers.cc +++ b/python/jiminy_pywrap/src/helpers.cc @@ -321,80 +321,113 @@ namespace jiminy::python void exposeHelpers() { - // clang-format off - bp::def("build_geom_from_urdf", &buildGeometryModelFromUrdf, - (bp::arg("pinocchio_model"), "urdf_filename", "geom_type", - bp::arg("mesh_package_dirs") = bp::list(), - bp::arg("load_meshes") = true, - bp::arg("make_meshes_convex") = false)); - - bp::def("build_models_from_urdf", &buildMultipleModelsFromUrdf, - (bp::arg("urdf_path"), "has_freeflyer", - bp::arg("mesh_package_dirs") = bp::list(), - bp::arg("build_visual_model") = false, - bp::arg("load_visual_meshes") = false)); + bp::def("build_geom_from_urdf", + &buildGeometryModelFromUrdf, + (bp::arg("pinocchio_model"), + "urdf_filename", + "geom_type", + bp::arg("mesh_package_dirs") = bp::list(), + bp::arg("load_meshes") = true, + bp::arg("make_meshes_convex") = false)); + + bp::def("build_models_from_urdf", + &buildMultipleModelsFromUrdf, + (bp::arg("urdf_path"), + "has_freeflyer", + bp::arg("mesh_package_dirs") = bp::list(), + bp::arg("build_visual_model") = false, + bp::arg("load_visual_meshes") = false)); bp::def("save_to_binary", &saveRobotToBinary, (bp::arg("robot"))); - bp::def("load_from_binary", &buildRobotFromBinary, - (bp::arg("data"), - bp::arg("mesh_path_dir") = bp::object(), - bp::arg("mesh_package_dirs") = bp::list())); + bp::def("load_from_binary", + &buildRobotFromBinary, + (bp::arg("data"), + bp::arg("mesh_path_dir") = bp::object(), + bp::arg("mesh_package_dirs") = bp::list())); bp::def("get_joint_type", &getJointType, bp::arg("joint_model")); - bp::def("get_joint_type", &getJointTypeFromIndex, - (bp::arg("pinocchio_model"), "joint_index")); - bp::def("get_joint_indices", &getJointIndices, - (bp::arg("pinocchio_model"), "joint_names")); - bp::def("get_joint_position_first_index", &getJointPositionFirstIndex, - (bp::arg("pinocchio_model"), "joint_name")); - bp::def("is_position_valid", &isPositionValid, - (bp::arg("pinocchio_model"), "position", - bp::arg("tol_abs") = std::numeric_limits::epsilon())); - - bp::def("get_frame_indices", &getFrameIndices, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "frame_names")); - bp::def("get_joint_indices", &getFrameIndices, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "joint_names")); + bp::def( + "get_joint_type", &getJointTypeFromIndex, (bp::arg("pinocchio_model"), "joint_index")); + bp::def( + "get_joint_indices", &getJointIndices, (bp::arg("pinocchio_model"), "joint_names")); + bp::def("get_joint_position_first_index", + &getJointPositionFirstIndex, + (bp::arg("pinocchio_model"), "joint_name")); + bp::def("is_position_valid", + &isPositionValid, + (bp::arg("pinocchio_model"), + "position", + bp::arg("tol_abs") = std::numeric_limits::epsilon())); + + bp::def("get_frame_indices", + &getFrameIndices, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "frame_names")); + bp::def("get_joint_indices", + &getFrameIndices, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "joint_names")); bp::def("array_copyto", &arrayCopyTo, (bp::arg("dst"), "src")); bp::def("multi_array_copyto", &multiArrayCopyTo, (bp::arg("dst"), "src")); - // Do not use EigenPy To-Python converter because it considers matrices with 1 column as vectors - bp::def("interpolate_positions", &interpolatePositions, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out")); + // Do NOT use EigenPy to-python converter as it considers arrays with 1 column as vectors + bp::def("interpolate_positions", + &interpolatePositions, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "times_in", "positions_in", "times_out")); bp::def("aba", - &pinocchio_overload::aba< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>, + &pinocchio_overload::aba, bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "u", "fext"), "Compute ABA with external forces, store the result in Data::ddq and return it."); + bp::def( + "rnea", + &pinocchio_overload::rnea, + bp::return_value_policy>(), + (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"), + "Compute the RNEA without external forces, store the result in Data and return it."); bp::def("rnea", - &pinocchio_overload::rnea< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd>, - bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a"), - "Compute the RNEA without external forces, store the result in Data and return it."); - bp::def("rnea", - &pinocchio_overload::rnea< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd, Eigen::VectorXd, pinocchio::Force>, + &pinocchio_overload::rnea, bp::return_value_policy>(), (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v", "a", "fext"), "Compute the RNEA with external forces, store the result in Data and return it."); bp::def("crba", - &pinocchio_overload::crba< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd>, + &pinocchio_overload:: + crba, bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fast_math") = false), + (bp::arg("pinocchio_model"), "pinocchio_data", "q", bp::arg("fastmath") = false), "Computes CRBA, store the result in Data and return it."); bp::def("computeKineticEnergy", - &pinocchio_overload::computeKineticEnergy< - double, 0, pinocchio::JointCollectionDefaultTpl, Eigen::VectorXd, Eigen::VectorXd>, - (bp::arg("pinocchio_model"), "pinocchio_data", "q", "v"), + &pinocchio_overload::computeKineticEnergy, + (bp::arg("pinocchio_model"), + "pinocchio_data", + "q", + "v", + bp::arg("update_kinematics") = true), "Computes the forward kinematics and the kinematic energy of the model for the " "given joint configuration and velocity given as input. " "The result is accessible through data.kinetic_energy."); @@ -402,9 +435,12 @@ namespace jiminy::python bp::def("computeJMinvJt", &pinocchio_overload::computeJMinvJt, bp::return_value_policy>(), - (bp::arg("pinocchio_model"), "pinocchio_data", "J", bp::arg("update_decomposition") = true)); - bp::def("solveJMinvJtv", &solveJMinvJtv, + (bp::arg("pinocchio_model"), + "pinocchio_data", + "J", + bp::arg("update_decomposition") = true)); + bp::def("solveJMinvJtv", + &solveJMinvJtv, (bp::arg("pinocchio_data"), "v", bp::arg("update_decomposition") = true)); - // clang-format on } } diff --git a/python/jiminy_pywrap/src/robot.cc b/python/jiminy_pywrap/src/robot.cc index 34a2a8798..e55f67027 100644 --- a/python/jiminy_pywrap/src/robot.cc +++ b/python/jiminy_pywrap/src/robot.cc @@ -98,30 +98,30 @@ namespace jiminy::python ConstraintTree constraints = self.getConstraints(); constraints.foreach( [&constraintsRows](const std::shared_ptr & constraint, - ConstraintNodeType /* node */) + ConstraintRegistryType /* type */) { if (!constraint->getIsEnabled()) { return; } - constraintsRows += static_cast(constraint->getDim()); + constraintsRows += static_cast(constraint->getSize()); }); Eigen::MatrixXd J(constraintsRows, self.nv()); Eigen::VectorXd gamma(constraintsRows); constraints.foreach( [&J, &gamma, &constraintRow]( const std::shared_ptr & constraint, - ConstraintNodeType /* node */) + ConstraintRegistryType /* type */) { if (!constraint->getIsEnabled()) { return; } - const Eigen::Index constraintDim = - static_cast(constraint->getDim()); - J.middleRows(constraintRow, constraintDim) = constraint->getJacobian(); - gamma.segment(constraintRow, constraintDim) = constraint->getDrift(); - constraintRow += constraintDim; + const Eigen::Index constraintSize = + static_cast(constraint->getSize()); + J.middleRows(constraintRow, constraintSize) = constraint->getJacobian(); + gamma.segment(constraintRow, constraintSize) = constraint->getDrift(); + constraintRow += constraintSize; }); return bp::make_tuple(J, gamma); } @@ -221,11 +221,6 @@ namespace jiminy::python .def("remove_constraint", static_cast(&Model::removeConstraint), (bp::arg("self"), "name")) - .def("get_constraint", - static_cast (Model::*)( - const std::string &)>(&Model::getConstraint), - (bp::arg("self"), "constraint_name")) - .def("exist_constraint", &Model::existConstraint, (bp::arg("self"), "constraint_name")) .ADD_PROPERTY_GET("has_constraints", &Model::hasConstraints) .ADD_PROPERTY_GET_WITH_POLICY("constraints", &Model::getConstraints, @@ -333,6 +328,9 @@ namespace jiminy::python bp::return_value_policy>()) .ADD_PROPERTY_GET_WITH_POLICY("log_f_external_fieldnames", &Model::getLogForceExternalFieldnames, + bp::return_value_policy>()) + .ADD_PROPERTY_GET_WITH_POLICY("log_constraint_fieldnames", + &Model::getLogConstraintFieldnames, bp::return_value_policy>()); }