From a92209e7cdb087a8f5806e2a1e38dc11825b55fd Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 16:04:20 +0200 Subject: [PATCH 01/16] [core] Minor cleanup. --- .../jiminy/core/control/ControllerFunctor.h | 16 ++++++++-------- core/src/robot/Model.cc | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/core/include/jiminy/core/control/ControllerFunctor.h b/core/include/jiminy/core/control/ControllerFunctor.h index dac2f2376..db03ecad1 100644 --- a/core/include/jiminy/core/control/ControllerFunctor.h +++ b/core/include/jiminy/core/control/ControllerFunctor.h @@ -73,10 +73,10 @@ namespace jiminy /// \return Return code to determine whether the execution of the method was successful. /// /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t computeCommand(float64_t const & t, - vectorN_t const & q, - vectorN_t const & v, - vectorN_t & command) override; + virtual hresult_t computeCommand(float64_t const & t, + vectorN_t const & q, + vectorN_t const & v, + vectorN_t & command) override; /////////////////////////////////////////////////////////////////////////////////////////////// /// @@ -91,10 +91,10 @@ namespace jiminy /// \return Return code to determine whether the execution of the method was successful. /// /////////////////////////////////////////////////////////////////////////////////////////////// - hresult_t internalDynamics(float64_t const & t, - vectorN_t const & q, - vectorN_t const & v, - vectorN_t & uCustom) override; + virtual hresult_t internalDynamics(float64_t const & t, + vectorN_t const & q, + vectorN_t const & v, + vectorN_t & uCustom) override; private: // std::conditional_t enables to use both functors and lambdas diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index 5bfeda31e..0ffba7ac9 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -1458,7 +1458,7 @@ namespace jiminy // Set the max number of contact points per collision pairs // Only a global collisionRequest is available for Pinocchio < 2.4.4, instead of one for each collision pair. - # if PINOCCHIO_MINOR_VERSION >= 4 || PINOCCHIO_PATCH_VERSION >= 4 + # if PINOCCHIO_MINOR_VERSION >= 4 && PINOCCHIO_PATCH_VERSION >= 4 for (hpp::fcl::CollisionRequest & collisionRequest : pncGeometryData_->collisionRequests) { collisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody; From b0bab806263fc14fa7145db6fd5658f91c3299d5 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 17:17:35 +0200 Subject: [PATCH 02/16] [core] Compute velocity of the center of mass of each subtree systematically. --- core/src/engine/EngineMultiRobot.cc | 33 +++++++++++++++++++---------- core/src/robot/Model.cc | 2 +- 2 files changed, 23 insertions(+), 12 deletions(-) diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 4a5601f54..e890f1871 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -857,6 +857,14 @@ namespace jiminy void computeExtraTerms(systemHolder_t & system, systemDataHolder_t const & systemData) { + /// This method is optimized to avoid redundant computations. + /// See `pinocchio::computeAllTerms` for reference: + /// + /// Based on https://github.com/stack-of-tasks/pinocchio/blob/a1df23c2f183d84febdc2099e5fbfdbd1fc8018b/src/algorithm/compute-all-terms.hxx + /// + /// Copyright (c) 2014-2020, CNRS + /// Copyright (c) 2018-2020, INRIA + pinocchio::Model const & model = system.robot->pncModel_; pinocchio::Data & data = system.robot->pncData_; @@ -887,16 +895,6 @@ namespace jiminy } } - // Now that Ycrb is available, it is possible to infer directly the subtree center of masses - pinocchio::getComFromCrba(model, data); - data.Ig.mass() = data.oYcrb[0].mass(); - data.Ig.lever().setZero(); - data.Ig.inertia() = data.oYcrb[0].inertia(); - for (int32_t i = 1; i < model.njoints; ++i) - { - data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever()); - } - /* Neither 'aba' nor 'forwardDynamics' are computed the actual joints acceleration and forces, so it must be done separately: - 1st step: computing the forces based on rnea algorithm @@ -928,7 +926,20 @@ namespace jiminy } } - pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq); + + /* Now that `data.oYcrb` and `data.h` are available, one can get directly + the position and velocity of the center of mass of each subtrees. */ + data.Ig.mass() = data.oYcrb[0].mass(); + data.Ig.lever().setZero(); + data.Ig.inertia() = data.oYcrb[0].inertia(); + data.com[0] = data.oYcrb[0].lever(); + for (int32_t i = 1; i < model.njoints; ++i) + { + data.com[i] = data.oMi[i].actInv(data.oYcrb[i].lever()); + data.vcom[i] = data.h[i].linear() / data.mass[i]; + } + + pinocchio_overload::forwardKinematicsAcceleration(model, data, data.ddq); } void computeAllExtraTerms(std::vector & systems, diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index 0ffba7ac9..17bb29b1c 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -1458,7 +1458,7 @@ namespace jiminy // Set the max number of contact points per collision pairs // Only a global collisionRequest is available for Pinocchio < 2.4.4, instead of one for each collision pair. - # if PINOCCHIO_MINOR_VERSION >= 4 && PINOCCHIO_PATCH_VERSION >= 4 + #if PINOCCHIO_MAJOR_VERSION > 2 || (PINOCCHIO_MAJOR_VERSION == 2 && (PINOCCHIO_MINOR_VERSION > 4 || (PINOCCHIO_MINOR_VERSION == 4 && PINOCCHIO_PATCH_VERSION >= 4))) for (hpp::fcl::CollisionRequest & collisionRequest : pncGeometryData_->collisionRequests) { collisionRequest.num_max_contacts = mdlOptions_->collisions.maxContactPointsPerBody; From 47692bc73b519344e10629ec4294fbb15c56f04e Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Mon, 17 May 2021 10:03:17 +0200 Subject: [PATCH 03/16] [core] Fix internal update period computation when defined control and sensor update period of the engine without robot. --- core/src/engine/EngineMultiRobot.cc | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index e890f1871..4c0a685ae 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -926,7 +926,6 @@ namespace jiminy } } - /* Now that `data.oYcrb` and `data.h` are available, one can get directly the position and velocity of the center of mass of each subtrees. */ data.Ig.mass() = data.oYcrb[0].mass(); @@ -2227,6 +2226,11 @@ namespace jiminy template std::tuple isGcdIncluded(std::vector const & systemsDataHolder, Args... values) { + if (systemsDataHolder.empty()) + { + return isGcdIncluded(std::forward(values)...); + } + float64_t minValue = INF; auto lambda = [&minValue, &values...](systemDataHolder_t const & systemData) { From 0b137ea59944e9dd8d9f0feb317811a71401c50f Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 23:11:53 +0200 Subject: [PATCH 04/16] [core] Add option to register external forces to the telemetry. --- .../jiminy/core/engine/EngineMultiRobot.h | 3 + core/include/jiminy/core/engine/System.h | 1 + core/include/jiminy/core/robot/Model.h | 14 +- core/src/engine/EngineMultiRobot.cc | 31 +++- core/src/engine/System.cc | 1 + core/src/robot/Model.cc | 141 ++++++++++-------- examples/double_pendulum/double_pendulum.cc | 1 + python/jiminy_pywrap/src/Robot.cc | 2 + 8 files changed, 122 insertions(+), 72 deletions(-) diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h index d38b1b3b8..381e170de 100644 --- a/core/include/jiminy/core/engine/EngineMultiRobot.h +++ b/core/include/jiminy/core/engine/EngineMultiRobot.h @@ -172,6 +172,7 @@ namespace jiminy config["enableConfiguration"] = true; config["enableVelocity"] = true; config["enableAcceleration"] = true; + config["enableForceExternal"] = false; config["enableCommand"] = true; config["enableMotorEffort"] = true; config["enableEnergy"] = true; @@ -284,6 +285,7 @@ namespace jiminy bool_t const enableConfiguration; bool_t const enableVelocity; bool_t const enableAcceleration; + bool_t const enableForceExternal; bool_t const enableCommand; bool_t const enableMotorEffort; bool_t const enableEnergy; @@ -293,6 +295,7 @@ namespace jiminy enableConfiguration(boost::get(options.at("enableConfiguration"))), enableVelocity(boost::get(options.at("enableVelocity"))), enableAcceleration(boost::get(options.at("enableAcceleration"))), + enableForceExternal(boost::get(options.at("enableForceExternal"))), enableCommand(boost::get(options.at("enableCommand"))), enableMotorEffort(boost::get(options.at("enableMotorEffort"))), enableEnergy(boost::get(options.at("enableEnergy"))), diff --git a/core/include/jiminy/core/engine/System.h b/core/include/jiminy/core/engine/System.h index 57708b775..b93e0b31e 100644 --- a/core/include/jiminy/core/engine/System.h +++ b/core/include/jiminy/core/engine/System.h @@ -161,6 +161,7 @@ namespace jiminy std::vector positionFieldnames; std::vector velocityFieldnames; std::vector accelerationFieldnames; + std::vector forceExternalFieldnames; std::vector commandFieldnames; std::vector motorEffortFieldnames; std::string energyFieldname; diff --git a/core/include/jiminy/core/robot/Model.h b/core/include/jiminy/core/robot/Model.h index 0c36995b7..9a0976721 100644 --- a/core/include/jiminy/core/robot/Model.h +++ b/core/include/jiminy/core/robot/Model.h @@ -353,6 +353,7 @@ namespace jiminy std::vector const & getPositionFieldnames(void) const; std::vector const & getVelocityFieldnames(void) const; std::vector const & getAccelerationFieldnames(void) const; + std::vector const & getForceExternalFieldnames(void) const; hresult_t getFlexibleConfigurationFromRigid(vectorN_t const & qRigid, vectorN_t & qFlex) const; @@ -427,16 +428,17 @@ namespace jiminy vectorN_t constraintsDrift_; ///< Vector holding the drift of the constraints vectorN_t positionLimitMin_; ///< Upper position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) - vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) - vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector (INF for non-physical joints, ie flexibility joints and freeflyer, if any) + vectorN_t positionLimitMax_; ///< Lower position limit of the whole configuration vector + vectorN_t velocityLimit_; ///< Maximum absolute velocity of the whole velocity vector - std::vector positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the rigid robot - std::vector velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the rigid robot - std::vector accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the rigid robot + std::vector positionFieldnames_; ///< Fieldnames of the elements in the configuration vector of the model + std::vector velocityFieldnames_; ///< Fieldnames of the elements in the velocity vector of the model + std::vector accelerationFieldnames_; ///< Fieldnames of the elements in the acceleration vector of the model + std::vector forceExternalFieldnames_; ///< Concatenated fieldnames of the external force applied at each joint of the model, 'universe' excluded private: pinocchio::Model pncModelFlexibleOrig_; - motionVector_t jointsAcceleration_; ///< Vector of joints acceleration corresponding to a copy of data.a - temporary buffer for computing constraints. + motionVector_t jointsAcceleration_; ///< Vector of joints acceleration corresponding to a copy of data.a - temporary buffer for computing constraints. int32_t nq_; int32_t nv_; diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index 4c0a685ae..1ea6e8fda 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -665,6 +665,9 @@ namespace jiminy systemDataIt->accelerationFieldnames = addCircumfix(systemIt->robot->getAccelerationFieldnames(), systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER); + systemDataIt->forceExternalFieldnames = + addCircumfix(systemIt->robot->getForceExternalFieldnames(), + systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER); systemDataIt->commandFieldnames = addCircumfix(systemIt->robot->getCommandFieldnames(), systemIt->name, "", TELEMETRY_FIELDNAME_DELIMITER); @@ -703,6 +706,19 @@ namespace jiminy systemDataIt->state.a); } } + if (engineOptions_->telemetry.enableForceExternal) + { + for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i) + { + auto const & fext = systemDataIt->state.fExternal[i].toVector(); + for (uint8_t j = 0; j < 6U; ++j) + { + returnCode = telemetrySender_.registerVariable( + systemDataIt->forceExternalFieldnames[(i - 1) * 6U + j], + fext[j]); + } + } + } if (returnCode == hresult_t::SUCCESS) { if (engineOptions_->telemetry.enableCommand) @@ -767,7 +783,7 @@ namespace jiminy systemIt->robot->pncModel_, systemIt->robot->pncData_); - // Update the telemetry internal state + // Update telemetry values if (engineOptions_->telemetry.enableConfiguration) { telemetrySender_.updateValue(systemDataIt->positionFieldnames, @@ -783,6 +799,19 @@ namespace jiminy telemetrySender_.updateValue(systemDataIt->accelerationFieldnames, systemDataIt->state.a); } + if (engineOptions_->telemetry.enableForceExternal) + { + for (std::size_t i = 1; i < systemDataIt->state.fExternal.size(); ++i) + { + auto const & fext = systemDataIt->state.fExternal[i].toVector(); + for (uint8_t j = 0; j < 6U; ++j) + { + telemetrySender_.updateValue( + systemDataIt->forceExternalFieldnames[(i - 1) * 6U + j], + fext[j]); + } + } + } if (engineOptions_->telemetry.enableCommand) { telemetrySender_.updateValue(systemDataIt->commandFieldnames, diff --git a/core/src/engine/System.cc b/core/src/engine/System.cc index 167949115..6a426968c 100644 --- a/core/src/engine/System.cc +++ b/core/src/engine/System.cc @@ -182,6 +182,7 @@ namespace jiminy positionFieldnames(), velocityFieldnames(), accelerationFieldnames(), + forceExternalFieldnames(), commandFieldnames(), motorEffortFieldnames(), energyFieldname(), diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index 17bb29b1c..c07c70c0e 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -26,6 +26,7 @@ #include "urdf_parser/urdf_parser.h" +#include "jiminy/core/robot/BasicSensors.h" #include "jiminy/core/robot/PinocchioOverloadAlgorithms.h" #include "jiminy/core/robot/AbstractConstraint.h" #include "jiminy/core/robot/JointConstraint.h" @@ -221,6 +222,7 @@ namespace jiminy positionFieldnames_(), velocityFieldnames_(), accelerationFieldnames_(), + forceExternalFieldnames_(), pncModelFlexibleOrig_(), jointsAcceleration_(), nq_(0), @@ -1239,96 +1241,100 @@ namespace jiminy getJointsPositionIdx(pncModel_, rigidJointsNames_, rigidJointsPositionIdx_, false); getJointsVelocityIdx(pncModel_, rigidJointsNames_, rigidJointsVelocityIdx_, false); - /* Generate the fieldnames associated with the configuration, - velocity, and acceleration vectors. */ + /* Generate the fieldnames associated with the configuration vector, + velocity, acceleration and external force vectors. */ positionFieldnames_.clear(); positionFieldnames_.resize(nq_); velocityFieldnames_.clear(); velocityFieldnames_.resize(nv_); accelerationFieldnames_.clear(); accelerationFieldnames_.resize(nv_); - std::vector const & jointNames = pncModel_.names; - std::vector jointShortNames = removeSuffix(jointNames, "Joint"); - for (std::size_t i = 0; i < jointNames.size(); ++i) + forceExternalFieldnames_.clear(); + forceExternalFieldnames_.resize(6U * pncModel_.njoints); + for (std::size_t i = 1; i < pncModel_.joints.size(); ++i) { - std::string const & jointName = jointNames[i]; - jointIndex_t const & jointIdx = pncModel_.getJointId(jointName); + // Get joint name without "Joint" suffix, if any + std::string jointShortName = removeSuffix(pncModel_.names[i], "Joint"); - int32_t const idx_q = pncModel_.joints[jointIdx].idx_q(); + // Get joint postion and velocity starting indices + int32_t const idx_q = pncModel_.joints[i].idx_q(); + int32_t const idx_v = pncModel_.joints[i].idx_v(); - if (idx_q >= 0) // Otherwise the joint is not part of the vectorial representation + // Get joint prefix depending on its type + joint_t jointType; + std::string jointPrefix; + if (returnCode == hresult_t::SUCCESS) { - int32_t const idx_v = pncModel_.joints[jointIdx].idx_v(); - - joint_t jointType; - std::string jointPrefix; - if (returnCode == hresult_t::SUCCESS) + returnCode = getJointTypeFromIdx(pncModel_, i, jointType); + } + if (returnCode == hresult_t::SUCCESS) + { + if (jointType == joint_t::FREE) { - returnCode = getJointTypeFromIdx(pncModel_, jointIdx, jointType); + // Discard the joint name for FREE joint type since it is unique if any + jointPrefix = FREE_FLYER_PREFIX_BASE_NAME; + jointShortName = ""; } - if (returnCode == hresult_t::SUCCESS) + else { - if (jointType == joint_t::FREE) - { - // Discard the joint name for FREE joint type since it is unique if any - jointPrefix = FREE_FLYER_PREFIX_BASE_NAME; - jointShortNames[i] = ""; - } - else - { - jointPrefix = JOINT_PREFIX_BASE; - } + jointPrefix = JOINT_PREFIX_BASE; } + } + + // Get joint position and velocity suffices depending on its type + std::vector jointTypePositionSuffixes; + std::vector jointTypeVelocitySuffixes; + if (returnCode == hresult_t::SUCCESS) + { + returnCode = getJointTypePositionSuffixes(jointType, + jointTypePositionSuffixes); + } + if (returnCode == hresult_t::SUCCESS) + { + returnCode = getJointTypeVelocitySuffixes(jointType, + jointTypeVelocitySuffixes); + } - std::vector jointTypePositionSuffixes; + if (returnCode == hresult_t::SUCCESS) + { + // Define complete position fieldnames and backup them std::vector jointPositionFieldnames; - if (returnCode == hresult_t::SUCCESS) - { - returnCode = getJointTypePositionSuffixes(jointType, - jointTypePositionSuffixes); - } - if (returnCode == hresult_t::SUCCESS) - { - for (std::string const & suffix : jointTypePositionSuffixes) - { - jointPositionFieldnames.emplace_back( - jointPrefix + "Position" + jointShortNames[i] + suffix); - } - } - if (returnCode == hresult_t::SUCCESS) + for (std::string const & suffix : jointTypePositionSuffixes) { - std::copy(jointPositionFieldnames.begin(), - jointPositionFieldnames.end(), - positionFieldnames_.begin() + idx_q); + jointPositionFieldnames.emplace_back( + jointPrefix + "Position" + jointShortName + suffix); } + std::copy(jointPositionFieldnames.begin(), + jointPositionFieldnames.end(), + positionFieldnames_.begin() + idx_q); - std::vector jointTypeVelocitySuffixes; + // Define complete velocity and acceleration fieldnames and backup them std::vector jointVelocityFieldnames; std::vector jointAccelerationFieldnames; - if (returnCode == hresult_t::SUCCESS) + for (std::string const & suffix : jointTypeVelocitySuffixes) { - returnCode = getJointTypeVelocitySuffixes(jointType, - jointTypeVelocitySuffixes); - } - if (returnCode == hresult_t::SUCCESS) - { - for (std::string const & suffix : jointTypeVelocitySuffixes) - { - jointVelocityFieldnames.emplace_back( - jointPrefix + "Velocity" + jointShortNames[i] + suffix); - jointAccelerationFieldnames.emplace_back( - jointPrefix + "Acceleration" + jointShortNames[i] + suffix); - } + jointVelocityFieldnames.emplace_back( + jointPrefix + "Velocity" + jointShortName + suffix); + jointAccelerationFieldnames.emplace_back( + jointPrefix + "Acceleration" + jointShortName + suffix); } - if (returnCode == hresult_t::SUCCESS) + std::copy(jointVelocityFieldnames.begin(), + jointVelocityFieldnames.end(), + velocityFieldnames_.begin() + idx_v); + std::copy(jointAccelerationFieldnames.begin(), + jointAccelerationFieldnames.end(), + accelerationFieldnames_.begin() + idx_v); + + // Define complete external force fieldnames and backup them + std::vector jointForceExternalFieldnames; + for (std::string const & suffix : ForceSensor::fieldNames_) { - std::copy(jointVelocityFieldnames.begin(), - jointVelocityFieldnames.end(), - velocityFieldnames_.begin() + idx_v); - std::copy(jointAccelerationFieldnames.begin(), - jointAccelerationFieldnames.end(), - accelerationFieldnames_.begin() + idx_v); + jointForceExternalFieldnames.emplace_back( + jointPrefix + "ForceExternal" + jointShortName + suffix); } + std::copy(jointForceExternalFieldnames.begin(), + jointForceExternalFieldnames.end(), + forceExternalFieldnames_.begin() + 6U); } } } @@ -1942,6 +1948,11 @@ namespace jiminy return accelerationFieldnames_; } + std::vector const & Model::getForceExternalFieldnames(void) const + { + return forceExternalFieldnames_; + } + std::vector const & Model::getRigidJointsNames(void) const { return rigidJointsNames_; diff --git a/examples/double_pendulum/double_pendulum.cc b/examples/double_pendulum/double_pendulum.cc index 57805c77b..acfa020dc 100644 --- a/examples/double_pendulum/double_pendulum.cc +++ b/examples/double_pendulum/double_pendulum.cc @@ -91,6 +91,7 @@ int main(int /* argc */, char_t * /* argv */[]) boost::get(boost::get(simuOptions.at("telemetry")).at("enableConfiguration")) = true; boost::get(boost::get(simuOptions.at("telemetry")).at("enableVelocity")) = true; boost::get(boost::get(simuOptions.at("telemetry")).at("enableAcceleration")) = true; + boost::get(boost::get(simuOptions.at("telemetry")).at("enableForceExternal")) = false; boost::get(boost::get(simuOptions.at("telemetry")).at("enableCommand")) = true; boost::get(boost::get(simuOptions.at("telemetry")).at("enableMotorEffort")) = true; boost::get(boost::get(simuOptions.at("telemetry")).at("enableEnergy")) = true; diff --git a/python/jiminy_pywrap/src/Robot.cc b/python/jiminy_pywrap/src/Robot.cc index 913dc4487..82e188f7f 100644 --- a/python/jiminy_pywrap/src/Robot.cc +++ b/python/jiminy_pywrap/src/Robot.cc @@ -139,6 +139,8 @@ namespace python bp::return_value_policy >())) .add_property("logfile_acceleration_headers", bp::make_function(&Model::getAccelerationFieldnames, bp::return_value_policy >())) + .add_property("logfile_f_external_headers", bp::make_function(&Model::getForceExternalFieldnames, + bp::return_value_policy >())) ; } From 09261a1ae90c113d826b38d0cbb28fd146c84b52 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 15:59:31 +0200 Subject: [PATCH 05/16] [python] Minor cleanup. --- .../gym_jiminy/common/envs/env_generic.py | 21 ++++++----- .../common/gym_jiminy/common/utils/helpers.py | 2 +- .../gym_jiminy/common/wrappers/frame_stack.py | 10 +++--- .../envs/gym_jiminy/envs/__init__.py | 36 +++++++++---------- .../envs/gym_jiminy/envs/cartpole.py | 2 +- .../rllib/double_table_cart_py/fcnet.py | 2 +- .../gym_jiminy/examples/rllib/tools/fcnet.py | 2 +- .../examples/rllib/tools/utilities.py | 1 - .../unit_py/test_pipeline_design.py | 4 +-- .../viewer/meshcat/meshcat_visualizer.py | 8 ++--- .../src/jiminy_py/viewer/meshcat/recorder.py | 4 +-- .../src/jiminy_py/viewer/meshcat/server.py | 2 +- .../viewer/panda3d/panda3d_visualizer.py | 7 ++-- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 2 +- unit_py/test_simple_mass.py | 4 +-- 15 files changed, 51 insertions(+), 56 deletions(-) diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py index d096458ce..3870dd6dd 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py @@ -716,11 +716,6 @@ def step(self, # Add terminal reward to current reward reward += self.compute_reward_terminal(info=self._info) - # Check if the observation is out-of-bounds, in debug mode only - if not done and self.debug and \ - not self.observation_space.contains(obs): - logger.warn("The observation is out-of-bounds.") - # Update number of (successful) steps self.num_steps += 1 @@ -851,7 +846,7 @@ def play_interactive(self, key: Optional[str] = None) -> bool: action = self._key_to_action(key) else: action = None - _, _, done, _ = self.step(action) + *_, done, _ = self.step(action) self.render() sleep(self.step_dt - (time.time() - t_init)) return done @@ -982,16 +977,20 @@ def refresh_observation(self) -> None: # type: ignore[override] """Compute the observation based on the current state of the robot. .. note:: - There is no way in the current implementation to discriminate the - initialization of the observation buffer from the next one. A - workaround is to check if the simulation already stated. Even - though it is not the same rigorousely speaking, it does the job of - preserving efficiency. + This method is called right after step, so it is the right place to + update shared data between `refresh_observation`, `is_done`, and + `compute_reward` methods. .. warning:: In practice, it updates the internal buffer directly for the sake of efficiency. + As a side note, there is no way in the current implementation to + discriminate the initialization of the observation buffer from the + next one. The workaround is to check if the simulation already + started. Even though it is not the same rigorously speaking, it + does the job here since it is only about preserving efficiency. + :param full_refresh: Whether or not to do a full refresh. This is usually done once, when calling `reset` method. """ diff --git a/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py b/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py index 1a99b0f37..bbf2dfb79 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py @@ -296,7 +296,7 @@ def register_variables(controller: jiminy.AbstractController, :param data: Data from `Gym.spaces.Space` to register. Note that the telemetry stores pointers to the underlying memory, so it only supports np.float64, and make sure to reassign data - using `np.core.umath.copyto` for `[:]` operator. + using `np.core.multiarray.copyto` for `[:]` operator. :param namespace: Namespace used to prepend fields, using '.' delimiter. Empty string to disable. Optional: Disable by default. diff --git a/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py b/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py index 7f41493c6..8371936f9 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py +++ b/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py @@ -3,7 +3,7 @@ from copy import deepcopy from functools import reduce from collections import deque -from typing import Tuple, Dict, Sequence, List, Any, Iterator, Union +from typing import Optional, Tuple, Dict, Sequence, List, Any, Iterator, Union import numpy as np @@ -26,8 +26,9 @@ class FilteredFrameStack(gym.Wrapper): """ def __init__(self, # pylint: disable=unused-argument env: gym.Env, - nested_filter_keys: Sequence[Union[Sequence[str], str]], num_stack: int, + nested_filter_keys: Optional[ + Sequence[Union[Sequence[str], str]]] = None, **kwargs: Any): """ :param env: Environment to wrap. @@ -40,9 +41,8 @@ def __init__(self, # pylint: disable=unused-argument wrapper generation. """ # Sanitize user arguments if necessary - nested_filter_keys = [ - [field] if isinstance(field, str) else field - for field in nested_filter_keys] + if nested_filter_keys is None: + nested_filter_keys = self.env.observation_space.spaces.keys() # Define helper that will be used to determine the leaf fields to stack def _get_branches(root: Any) -> Iterator[List[str]]: diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/__init__.py b/python/gym_jiminy/envs/gym_jiminy/envs/__init__.py index 222656aa2..01154ff5f 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/__init__.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/__init__.py @@ -3,10 +3,10 @@ from .cartpole import CartPoleJiminyEnv from .acrobot import AcrobotJiminyEnv, AcrobotJiminyGoalEnv from .ant import AntEnv -from .anymal import ANYmalJiminyEnv, ANYmalPDControlJiminyEnv -from .atlas import AtlasJiminyEnv, AtlasPDControlJiminyEnv from .spotmicro import SpotmicroJiminyEnv from .cassie import CassieJiminyEnv, CassiePDControlJiminyEnv +from .anymal import ANYmalJiminyEnv, ANYmalPDControlJiminyEnv +from .atlas import AtlasJiminyEnv, AtlasPDControlJiminyEnv __all__ = [ @@ -14,13 +14,13 @@ 'AcrobotJiminyEnv', 'AcrobotJiminyGoalEnv', 'AntEnv', + 'SpotmicroJiminyEnv', + 'CassieJiminyEnv', + 'CassiePDControlJiminyEnv', 'ANYmalJiminyEnv', 'ANYmalPDControlJiminyEnv', 'AtlasJiminyEnv', - 'AtlasPDControlJiminyEnv', - 'SpotmicroJiminyEnv', - 'CassieJiminyEnv', - 'CassiePDControlJiminyEnv' + 'AtlasPDControlJiminyEnv' ] register( @@ -41,6 +41,18 @@ max_episode_steps=1000, reward_threshold=6000.0 ) +register( + id='spotmicro-v0', + entry_point='gym_jiminy.envs:SpotmicroJiminyEnv' +) +register( + id='cassie-v0', + entry_point='gym_jiminy.envs:CassieJiminyEnv' +) +register( + id='cassie-pid-v0', + entry_point='gym_jiminy.envs:CassiePDControlJiminyEnv' +) register( id='anymal-v0', entry_point='gym_jiminy.envs:ANYmalJiminyEnv' @@ -57,15 +69,3 @@ id='atlas-pid-v0', entry_point='gym_jiminy.envs:AtlasPDControlJiminyEnv' ) -register( - id='spotmicro-v0', - entry_point='gym_jiminy.envs:SpotmicroJiminyEnv' -) -register( - id='cassie-v0', - entry_point='gym_jiminy.envs:CassieJiminyEnv' -) -register( - id='cassie-pid-v0', - entry_point='gym_jiminy.envs:CassiePDControlJiminyEnv' -) diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py b/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py index 66535a7e5..93d6da49b 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/cartpole.py @@ -194,7 +194,7 @@ def refresh_observation(self) -> None: def is_done(self) -> bool: """ TODO: Write documentation. """ - x, theta, _, _ = self.get_observation() + x, theta, *_ = self.get_observation() return (abs(x) > X_THRESHOLD) or (abs(theta) > THETA_THRESHOLD) def compute_command(self, diff --git a/python/gym_jiminy/examples/rllib/double_table_cart_py/fcnet.py b/python/gym_jiminy/examples/rllib/double_table_cart_py/fcnet.py index 990adb76e..c3fa95d2e 100644 --- a/python/gym_jiminy/examples/rllib/double_table_cart_py/fcnet.py +++ b/python/gym_jiminy/examples/rllib/double_table_cart_py/fcnet.py @@ -178,7 +178,7 @@ def __init__(self, name="value_out", activation=None, kernel_initializer=normc_initializer(1.0))( - last_vf_layer if last_vf_layer is not None else last_layer) + last_vf_layer or last_layer) # Finish definition of the model self.base_model = tf.keras.Model(inputs, [logits_out, value_out]) diff --git a/python/gym_jiminy/examples/rllib/tools/fcnet.py b/python/gym_jiminy/examples/rllib/tools/fcnet.py index 4cdead92b..e66cbaf80 100644 --- a/python/gym_jiminy/examples/rllib/tools/fcnet.py +++ b/python/gym_jiminy/examples/rllib/tools/fcnet.py @@ -295,7 +295,7 @@ def __init__(self, name="value_out", activation=None, kernel_initializer=normc_initializer_tf(1.0))( - last_vf_layer if last_vf_layer is not None else last_layer) + last_vf_layer or last_layer) # Finish definition of the model self.base_model = tf.keras.Model(inputs, [logits_out, value_out]) diff --git a/python/gym_jiminy/examples/rllib/tools/utilities.py b/python/gym_jiminy/examples/rllib/tools/utilities.py index f35cffd55..20aa4f6e0 100644 --- a/python/gym_jiminy/examples/rllib/tools/utilities.py +++ b/python/gym_jiminy/examples/rllib/tools/utilities.py @@ -21,7 +21,6 @@ from ray.rllib.utils.filter import NoFilter from ray.rllib.agents.trainer import Trainer from ray.rllib.agents.callbacks import DefaultCallbacks -from ray.rllib.models.action_dist import ActionDistribution from gym_jiminy.common.utils import clip diff --git a/python/gym_jiminy/unit_py/test_pipeline_design.py b/python/gym_jiminy/unit_py/test_pipeline_design.py index 78aafe200..fb1434a89 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_design.py +++ b/python/gym_jiminy/unit_py/test_pipeline_design.py @@ -142,7 +142,7 @@ def test_step_state(self): env.reset() action = env.env.get_observation()['targets']['controller_0'] action['Q'] += 1.0e-3 - obs, _, _, _ = env.step(action) + obs, *_ = env.step(action) # Observation stacking is skipping the required number of frames stack_dt = (self.skip_frames_ratio + 1) * env.observe_dt @@ -163,7 +163,7 @@ def test_step_state(self): # 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): - obs, _, _, _ = env.step(action) + 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, 1.0e-6)) diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py index 36f3fbf96..3c07549cb 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/meshcat_visualizer.py @@ -173,14 +173,14 @@ def loadPrimitive(self, geometry_object: hppfcl.CollisionGeometry): elif isinstance(geom, (hppfcl.Convex, hppfcl.BVHModelOBBRSS)): # Extract vertices and faces from geometry if isinstance(geom, hppfcl.Convex): - vertices = np.vstack([geom.points(i) - for i in range(geom.num_points)]) - faces = np.vstack([np.array(list(geom.polygons(i))) + vertices = np.vstack([ + geom.points(i) for i in range(geom.num_points)]) + faces = np.vstack([np.array(geom.polygons(i)) for i in range(geom.num_polygons)]) else: vertices = np.vstack([geom.vertices(i) for i in range(geom.num_vertices)]) - faces = np.vstack([np.array(list(geom.tri_indices(i))) + faces = np.vstack([np.array(geom.tri_indices(i)) for i in range(geom.num_tris)]) # Create primitive triangle geometry diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py index 666709dd6..5a948b851 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/recorder.py @@ -355,9 +355,7 @@ def capture_frame(self, width: Optional[int] = None, height: Optional[int] = None) -> str: self._send_request( - "take_snapshot", message=( - f"{width if width is not None else -1}|" - f"{height if height is not None else -1}")) + "take_snapshot", message=f"{width or -1}|{height or -1}") return self.__shm['message'].value def start_video_recording(self, diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py index db40f3bd3..c058bdb5d 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py @@ -173,7 +173,7 @@ def handle_comm(self, frames: Sequence[bytes]) -> None: def forward_to_websockets(self, frames: Sequence[bytes]) -> None: super().forward_to_websockets(frames) - _, _, data = frames + *_, data = frames for comm_id in self.comm_pool: self.forward_to_comm(comm_id, data) diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py index 81893e131..0f553ac59 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py @@ -891,8 +891,7 @@ def set_legend(self, # Create empty figure with the legend color_default = (0.0, 0.0, 0.0, 1.0) - handles = [Patch(color=c if c is not None else color_default, label=t) - for t, c in items] + handles = [Patch(color=c or color_default, label=t) for t, c in items] fig = plt.figure() legend = fig.gca().legend(handles=handles, framealpha=1, frameon=True) fig.gca().set_axis_off() @@ -1306,12 +1305,12 @@ def loadViewerGeometryObject(self, # Extract vertices and faces from geometry if isinstance(geom, hppfcl.Convex): vertices = [geom.points(i) for i in range(geom.num_points)] - faces = [np.array(list(geom.polygons(i))) + faces = [np.array(geom.polygons(i)) for i in range(geom.num_polygons)] else: vertices = [geom.vertices(i) for i in range(geom.num_vertices)] - faces = [np.array(list(geom.tri_indices(i))) + faces = [np.array(geom.tri_indices(i)) for i in range(geom.num_tris)] # Create primitive triangle geometry diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 5d4f28c35..de9506591 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -354,7 +354,7 @@ def __init__(self, self.scene_name = scene_name self.use_theoretical_model = use_theoretical_model self.delete_robot_on_close = delete_robot_on_close - self._lock = lock if lock is not None else Viewer._lock + self._lock = lock or Viewer._lock self._display_com = display_com self._display_contacts = display_contacts diff --git a/unit_py/test_simple_mass.py b/unit_py/test_simple_mass.py index 0ab022260..03d5c19eb 100644 --- a/unit_py/test_simple_mass.py +++ b/unit_py/test_simple_mass.py @@ -183,7 +183,7 @@ def test_contact_sensor(self): the friction model is not assessed here. """ # Create the robot - robot, _, _, joint_idx, frame_pose = self._setup(ShapeType.POINT) + robot, *_, joint_idx, frame_pose = self._setup(ShapeType.POINT) # Create the engine engine = jiminy.Engine() @@ -226,7 +226,7 @@ def _test_friction_model(self, shape): also compared to the theoretical model. """ # Create the robot and engine - robot, weight, height, _, _ = self._setup(shape) + robot, weight, height, *_ = self._setup(shape) # Create, initialize, and configure the engine engine = jiminy.Engine() From c62291f3ee37deeddc1d39762fffe775eabbde7a Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 16:06:22 +0200 Subject: [PATCH 06/16] [python/robot] Add fallback if 'dynamics' fields 'damping'/'friction' are partially defined in URDF file. --- python/jiminy_py/src/jiminy_py/robot.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index 48b4eec07..8514681f6 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -303,11 +303,10 @@ def generate_hardware_description_file( joint_name = joint_descr.get('name') dyn_descr = joint_descr.find('./dynamics') if dyn_descr is not None: - damping = float(dyn_descr.get('damping')) - friction = float(dyn_descr.get('friction')) + damping = float(dyn_descr.get('damping') or 0.0) + friction = float(dyn_descr.get('friction') or 0.0) else: - damping = 0.0 - friction = 0.0 + damping, friction = 0.0, 0.0 joints_options[joint_name] = OrderedDict( frictionViscousPositive=-damping, frictionViscousNegative=-damping, From 3750c95de6711b62e3f4b2be13b433e080f12325 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 16:03:25 +0200 Subject: [PATCH 07/16] [python/processing] Add efficient convex hull tools. Improve efficiency and versatility of 'interpolate_zoh'. --- python/jiminy_py/src/jiminy_py/processing.py | 167 ++++++++++++++++++- 1 file changed, 160 insertions(+), 7 deletions(-) diff --git a/python/jiminy_py/src/jiminy_py/processing.py b/python/jiminy_py/src/jiminy_py/processing.py index 8df964f17..e5c5202c5 100644 --- a/python/jiminy_py/src/jiminy_py/processing.py +++ b/python/jiminy_py/src/jiminy_py/processing.py @@ -1,24 +1,177 @@ -from math import factorial +import math from typing import Optional, Dict, Union, Sequence +import numba as nb import numpy as np -from scipy.linalg import toeplitz +from numpy.lib.stride_tricks import as_strided from scipy.interpolate import UnivariateSpline +from scipy.spatial.qhull import _Qhull + + +FACTORIAL_TABLE = (1, 1, 2, 6, 24, 120, 720) + + +class ConvexHull: + def __init__(self, points: np.ndarray) -> None: + """Compute the convex hull defined by a set of points. + + :param points: N-D points whose to computed the associated convex hull, + as a 2D array whose first dimension corresponds to the + number of points, and the second to the N-D coordinates. + """ + assert len(points) > 0, "The length of 'points' must be at least 1." + + # Backup user argument(s) + self._points = points + + # Create convex full if possible + if len(self._points) > 2: + self._hull = _Qhull(points=self._points, + options=b"", + mode_option=b"i", + required_options=b"Qt", + furthest_site=False, + incremental=False, + interior_point=None) + else: + self._hull = None + + # Buffer to cache center computation + self._center = None + + @property + def center(self) -> np.ndarray: + """Get the center of the convex hull. + + .. note:: + Degenerated convex hulls corresponding to len(points) == 1 or 2 are + handled separately. + + :returns: 1D float vector with N-D coordinates of the center. + """ + if self._center is None: + if len(self._points) > 3: + vertices = self._points[self._hull.get_extremes_2d()] + else: + vertices = self._points + self._center = np.mean(vertices, axis=0) + return self._center + + def get_distance(self, queries: np.ndarray) -> np.ndarray: + """Compute the signed distance of query points from the convex hull. + + Positive distance corresponds to a query point lying outside the convex + hull. + + .. note:: + Degenerated convex hulls corresponding to len(points) == 1 or 2 are + handled separately. The distance from a point and a segment is used + respectevely. + + :param queries: N-D query points for which to compute distance from the + convex hull, as a 2D array. + + :returns: 1D float vector of the same length than `queries`. + """ + if len(self._points) > 2: + equations = self._hull.get_simplex_facet_array()[2].T + return np.max(queries @ equations[:-1] + equations[-1], axis=1) + elif len(self._points) == 2: + vec = self._points[1] - self._points[0] + ratio = (queries - self._points[0]) @ vec / squared_norm_2(vec) + proj = self._points[0] + np.outer(np.clip(ratio, 0.0, 1.0), vec) + return np.linalg.norm(queries - proj, 2, axis=1) + else: + return np.linalg.norm(queries - self._points, 2, axis=1) + + +@nb.jit(nopython=True, nogil=True) +def toeplitz(c: np.ndarray, r: np.ndarray) -> np.ndarray: + """Numba-compatible implementation of `scipy.linalg.toeplitz` method. + + .. note: + It does not handle any special case for efficiency, so the input types + is more respective than originally. + + .. warning: + It returns a strided matrix instead of contiguous copy for efficiency. + + :param c: First column of the matrix. + :param r: First row of the matrix. + + see:: + https://docs.scipy.org/doc/scipy/reference/generated/scipy.linalg.toeplitz.html + """ + vals = np.concatenate((c[::-1], r[1:])) + n = vals.strides[0] + return as_strided( + vals[len(c)-1:], shape=(len(c), len(r)), strides=(-n, n)) + + +@nb.jit(nopython=True, nogil=True) +def _integrate_zoh_impl(state_prev: np.ndarray, + dt: float, + state_min: np.ndarray, + state_max: np.ndarray) -> np.ndarray: + # Compute integration matrix + order = len(state_prev) + integ_coeffs = np.array([ + pow(dt, k) / FACTORIAL_TABLE[k] for k in range(order)]) + integ_matrix = toeplitz(integ_coeffs, np.zeros(order)).T + integ_zero = integ_matrix[:, :-1].copy() @ state_prev[:-1] + integ_drift = np.expand_dims(integ_matrix[:, -1], axis=-1) + + # Propagate derivative bounds to compute highest derivative bounds + deriv = state_prev[-1] + deriv_min_stack = (state_min - integ_zero) / integ_drift + deriv_max_stack = (state_max - integ_zero) / integ_drift + deriv_min = np.full_like(deriv, fill_value=-np.inf) + deriv_max = np.full_like(deriv, fill_value=np.inf) + for deriv_min_i, deriv_max_i in zip(deriv_min_stack, deriv_max_stack): + deriv_min_i_valid = np.logical_and( + deriv_min < deriv_min_i, deriv_min_i < deriv_max) + deriv_min[deriv_min_i_valid] = deriv_min_i[deriv_min_i_valid] + deriv_max_i_valid = np.logical_and( + deriv_min < deriv_max_i, deriv_max_i < deriv_max) + deriv_max[deriv_max_i_valid] = deriv_max_i[deriv_max_i_valid] + + # Clip highest derivative to ensure every derivative are withing bounds + # it possible, lowest orders in priority otherwise. + deriv = np.minimum(np.maximum(deriv, deriv_min), deriv_max) + + # Integrate, taking into account clipped highest derivative + return integ_zero + integ_drift * deriv def integrate_zoh(state_prev: np.ndarray, - dt: float) -> np.ndarray: + dt: float, + state_min: Optional[np.ndarray] = None, + state_max: Optional[np.ndarray] = None) -> np.ndarray: """N-order integration scheme assuming Zero-Order-Hold for highest - derivative. + derivative, taking state bounds into account to make sure integrated state + is wthin bounds. :param state_prev: Previous state update, ordered from lowest to highest derivative order, which means: s[i](t) = s[i](t-1) + integ_{t-1}^{t}(s[i+1](t)) + :param state_min: Lower bounds of the state. + Optional: -Inf by default. + :param state_max: Upper bounds of the state. + Optional: Inf by default. :param dt: Integration delta of time since previous state update. """ - integ_coeffs = [pow(dt, k) / factorial(k) for k in range(len(state_prev))] - integ_matrix = toeplitz(integ_coeffs, np.zeros(len(state_prev))).T - return integ_matrix @ state_prev + # Make sure dt is not zero, otherwise return early + if abs(dt) < 1e-9: + return state_prev.copy() + + # Handling of default arguments + if state_min is None: + state_min = np.full_like(state_prev, fill_value=-np.inf) + if state_max is None: + state_max = np.full_like(state_prev, fill_value=-np.inf) + + # Call internal implementation + return _integrate_zoh_impl(state_prev, dt, state_min, state_max) def smoothing_filter( From 3098dbcaeedde63b160e11f0221324a966f3a690 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 15:55:56 +0200 Subject: [PATCH 08/16] [python/viewer] Add option to display DCM in viewer. --- .../gym_jiminy/common/envs/env_generic.py | 1 + .../gym_jiminy/common/envs/env_locomotion.py | 1 + .../gym_jiminy/common/wrappers/frame_stack.py | 2 +- python/jiminy_py/src/jiminy_py/log.py | 49 ++++---- python/jiminy_py/src/jiminy_py/simulator.py | 1 + python/jiminy_py/src/jiminy_py/state.py | 17 ++- .../viewer/panda3d/panda3d_visualizer.py | 10 +- .../jiminy_py/src/jiminy_py/viewer/replay.py | 29 +++-- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 107 +++++++++++++++--- 9 files changed, 157 insertions(+), 60 deletions(-) diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py index 3870dd6dd..1319d96cc 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_generic.py @@ -877,6 +877,7 @@ def _setup(self) -> None: if field.startswith('enable'): engine_options["telemetry"][field] = self.debug engine_options['telemetry']['enableConfiguration'] = True + engine_options['telemetry']['enableVelocity'] = True # Enable the friction model for motor_name in robot_options["motors"].keys(): diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py index f39fd7894..3d8d6bb84 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py @@ -211,6 +211,7 @@ def _setup(self) -> None: # Make sure to log at least required data for reward # computation and log replay engine_options['telemetry']['enableConfiguration'] = True + engine_options['telemetry']['enableVelocity'] = True # Enable the flexible model robot_options["model"]["dynamics"]["enableFlexibleModel"] = True diff --git a/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py b/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py index 8371936f9..03cc006d1 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py +++ b/python/gym_jiminy/common/gym_jiminy/common/wrappers/frame_stack.py @@ -56,7 +56,7 @@ def _get_branches(root: Any) -> Iterator[List[str]]: # Backup user arguments self.nested_filter_keys: List[List[str]] = list( - map(list, nested_filter_keys)) # type: ignore[arg-type] + map(list, nested_filter_keys)) self.num_stack = num_stack # Initialize base wrapper diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index 228c00057..0d3406f5a 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -138,13 +138,16 @@ def parse_constant(key: str, value: str) -> Any: return data_dict, const_dict -def extract_viewer_data_from_log(log_data: Dict[str, np.ndarray], - robot: jiminy.Model) -> TrajectoryDataType: +def extract_trajectory_data_from_log(log_data: Dict[str, np.ndarray], + robot: jiminy.Model + ) -> TrajectoryDataType: """Extract the minimal required information from raw log data in order to replay the simulation in a viewer. - It extracts only the required data for replay, namely the evolution over - time of the joints positions. + .. note:: + It extracts the required data for replay, namely the evolution over + time of the robot configuration, and the velocities, which is required + for updating markers such as DCM. :param log_data: Data from the log file, in a dictionnary. :param robot: Jiminy robot. @@ -153,35 +156,39 @@ def extract_viewer_data_from_log(log_data: Dict[str, np.ndarray], field "evolution_robot" and it is a list of State object. The other fields are additional information. """ - t = log_data["Global.Time"] + times = log_data["Global.Time"] try: - # Extract the joint positions evolution over time - qe = np.stack([log_data[".".join(("HighLevelController", s))] - for s in robot.logfile_position_headers], axis=-1) + # Extract the joint positions and velocities evolution over time + positions = np.stack([ + log_data.get(".".join(("HighLevelController", field)), None) + for field in robot.logfile_position_headers], axis=-1) + velocities = np.stack([ + log_data.get(".".join(("HighLevelController", field)), None) + for field in robot.logfile_velocity_headers], axis=-1) # Determine whether to use the theoretical or flexible model use_theoretical_model = not robot.is_flexible # Create state sequence evolution_robot = [] - for t_i, q_i in zip(t, qe): - evolution_robot.append(State(t=t_i, q=q_i)) + for t, q, v in zip(times, positions, velocities): + evolution_robot.append(State(t=t, q=q, v=v)) - traj_data = {'evolution_robot': evolution_robot, - 'robot': robot, - 'use_theoretical_model': use_theoretical_model} + traj_data = {"evolution_robot": evolution_robot, + "robot": robot, + "use_theoretical_model": use_theoretical_model} except KeyError: # The current options are inconsistent with log data # Toggle flexibilities model_options = robot.get_model_options() - dyn_options = model_options['dynamics'] - dyn_options['enableFlexibleModel'] = not robot.is_flexible + dyn_options = model_options["dynamics"] + dyn_options["enableFlexibleModel"] = not robot.is_flexible robot.set_model_options(model_options) # Get viewer data - traj_data = extract_viewer_data_from_log(log_data, robot) + traj_data = extract_trajectory_data_from_log(log_data, robot) # Restore back flexibilities - dyn_options['enableFlexibleModel'] = not robot.is_flexible + dyn_options["enableFlexibleModel"] = not robot.is_flexible robot.set_model_options(model_options) return traj_data @@ -256,6 +263,9 @@ def emulate_sensors_data_from_log(log_data: Dict[str, np.ndarray], Note that it does not through an exception if out-of-range, but rather clip to desired time to the available data range. """ + # Extract time from log + times = log_data['Global.Time'] + # Filter sensors whose data is available sensors_set, sensors_log = [], [] for sensor_type, sensors_names in robot.sensors_names.items(): @@ -269,11 +279,10 @@ def emulate_sensors_data_from_log(log_data: Dict[str, np.ndarray], sensors_set.append(sensor) sensors_log.append(sensor_log) - def update_hook(t: float) -> None: - nonlocal sensors_set, sensors_log + def update_hook(t: float, q: np.ndarray, v: np.ndarray) -> None: + nonlocal times, sensors_set, sensors_log # Get current time ratio and surrounding indices in log data - times = log_data['Global.Time'] i = bisect_right(times, t) i_prev, i_next = max(i - 1, 0), min(i, len(times) - 1) ratio = (t - times[i_prev]) / (times[i_next] - times[i_prev]) diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index 9f4733ae9..42a1fdea2 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -485,6 +485,7 @@ def render(self, 'robot_name': robot_name, 'backend': self.viewer_backend, 'display_com': True, + 'display_dcm': True, 'display_contacts': True, 'delete_robot_on_close': True, **kwargs}) diff --git a/python/jiminy_py/src/jiminy_py/state.py b/python/jiminy_py/src/jiminy_py/state.py index 0387a362f..5334a12b2 100644 --- a/python/jiminy_py/src/jiminy_py/state.py +++ b/python/jiminy_py/src/jiminy_py/state.py @@ -1,8 +1,9 @@ -import numpy as np +from copy import deepcopy from collections import defaultdict -from copy import copy as _copy, deepcopy from typing import Optional, Union, Sequence, Dict +import numpy as np + from pinocchio import Force, StdVec_Force @@ -32,19 +33,17 @@ def __init__(self, # Time self.t = t # Configuration vector - self.q = _copy(q) if copy else q + self.q = deepcopy(q) if copy else q # Velocity vector - self.v = _copy(v) if copy else v + self.v = deepcopy(v) if copy else v # Acceleration vector - self.a = _copy(a) if copy else a + self.a = deepcopy(a) if copy else a # Effort vector - self.tau = _copy(tau) if copy else tau + self.tau = deepcopy(tau) if copy else tau # Frame name of the contact point, if nay self.contact_frame = contact_frame # External forces - self.f_ext = None - if f_ext is not None: - self.f_ext = deepcopy(f_ext) if copy else f_ext + self.f_ext = deepcopy(f_ext) if copy else f_ext @staticmethod def todict(state_list: Sequence['State']) -> Dict[ diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py index 0f553ac59..8a3fa2ec9 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py @@ -156,13 +156,13 @@ def make_cone(num_sides: int = 16) -> Geom: # Define vertex format vformat = GeomVertexFormat.get_v3n3t2() vdata = GeomVertexData('vdata', vformat, Geom.UH_static) - vdata.uncleanSetNumRows(num_sides + 2) + vdata.uncleanSetNumRows(num_sides + 3) vertex = GeomVertexWriter(vdata, 'vertex') normal = GeomVertexWriter(vdata, 'normal') tcoord = GeomVertexWriter(vdata, 'texcoord') # Add radial points - for u in np.linspace(0.0, 2 * np.pi, num_sides): + for u in np.linspace(0.0, 2 * np.pi, num_sides + 1): x, y = math.cos(u), math.sin(u) vertex.addData3(x, y, 0.0) normal.addData3(x, y, 0.0) @@ -182,9 +182,9 @@ def make_cone(num_sides: int = 16) -> Geom: # the triangles. For reference, see: # https://discourse.panda3d.org/t/procedurally-generated-geometry-and-the-default-normals/24986/2 prim = GeomTriangles(Geom.UH_static) - for i in range(num_sides - 1): - prim.add_vertices(i, i + 1, num_sides) - prim.add_vertices(i + 1, i, num_sides + 1) + for i in range(num_sides): + prim.add_vertices(i, i + 1, num_sides + 1) + prim.add_vertices(i + 1, i, num_sides + 2) geom = Geom(vdata) geom.add_primitive(prim) diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index 73debe65e..2ce010650 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -17,7 +17,7 @@ from .. import core as jiminy from ..log import (TrajectoryDataType, build_robot_from_log, - extract_viewer_data_from_log, + extract_trajectory_data_from_log, emulate_sensors_data_from_log) from .viewer import ( COLORS, Viewer, Tuple3FType, Tuple4FType, CameraPoseType, CameraMotionType) @@ -297,10 +297,10 @@ def play_trajectories(trajs_data: Union[ # Initialize robot configuration is viewer before any further processing for viewer_i, traj, offset in zip(viewers, trajs_data, xyz_offsets): - evolution_robot = traj['evolution_robot'] - if evolution_robot: - i = bisect_right([s.t for s in evolution_robot], time_interval[0]) - viewer_i.display(evolution_robot[i].q, offset) + data = traj['evolution_robot'] + if data: + i = bisect_right([s.t for s in data], time_interval[0]) + viewer_i.display(data[i].q, data[i].v, offset) if display_com is not None: viewer_i.display_center_of_mass(display_com) if display_contacts is not None: @@ -331,7 +331,7 @@ def play_trajectories(trajs_data: Union[ time_global = np.arange( time_interval[0], time_max, speed_ratio / VIDEO_FRAMERATE) - position_evolutions = [] + position_evolutions, velocity_evolutions = [], [] for traj in trajs_data: if len(traj['evolution_robot']): data_orig = traj['evolution_robot'] @@ -343,8 +343,15 @@ def play_trajectories(trajs_data: Union[ pos_orig = np.stack([s.q for s in data_orig], axis=0) position_evolutions.append(jiminy.interpolate( model, t_orig, pos_orig, time_global)) + if data_orig[0].v is not None: + vel_orig = np.stack([s.v for s in data_orig], axis=0) + velocity_evolutions.append( + np.interp(time_global, t_orig, vel_orig)) + else: + velocity_evolutions = (None,) * len(time_global) else: position_evolutions.append(None) + velocity_evolutions.append(None) # Disable framerate limit of Panda3d for efficiency if Viewer.backend.startswith('panda3d'): @@ -377,14 +384,16 @@ def play_trajectories(trajs_data: Union[ for i, t_cur in enumerate(tqdm( time_global, desc="Rendering frames", disable=(not verbose))): # Update the configurations of the robots - for viewer, positions, xyz_offset, update_hook in zip( - viewers, position_evolutions, xyz_offsets, update_hooks): + for viewer, positions, velocities, xyz_offset, update_hook in zip( + viewers, position_evolutions, velocity_evolutions, + xyz_offsets, update_hooks): if positions is not None: + q, v = positions[i], velocities[i] if update_hook is not None: update_hook_t = partial(update_hook, t_cur) else: update_hook_t = None - viewer.display(positions[i], xyz_offset, update_hook_t) + viewer.display(q, v, xyz_offset, update_hook_t) # Update clock if enabled if enable_clock: @@ -487,7 +496,7 @@ def play_logs_data(robots: Union[Sequence[jiminy.Robot], jiminy.Robot], # For each pair (log, robot), extract a trajectory object for # `play_trajectories` - trajectories = [extract_viewer_data_from_log(log, robot) + trajectories = [extract_trajectory_data_from_log(log, robot) for log, robot in zip(logs_data, robots)] # Define `update_hook` to emulate sensor update diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index de9506591..8e934e522 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -3,6 +3,7 @@ import io import sys import time +import math import shutil import base64 import atexit @@ -293,6 +294,7 @@ def __init__(self, robot_name: Optional[str] = None, scene_name: str = 'world', display_com: bool = False, + display_dcm: bool = False, display_contacts: bool = False, **kwargs: Any): """ @@ -336,6 +338,8 @@ def __init__(self, Optional: 'world' by default. :param display_com: Whether or not to display the center of mass. Optional: Disable by default. + :param display_dcm: Whether or not to display the capture point / DCM. + Optional: Disable by default. :param display_contacts: Whether or not to display the contact forces. Note that the user is responsible for updating sensors data since `Viewer.display` is only @@ -356,6 +360,7 @@ def __init__(self, self.delete_robot_on_close = delete_robot_on_close self._lock = lock or Viewer._lock self._display_com = display_com + self._display_dcm = display_dcm self._display_contacts = display_contacts # Initialize marker register @@ -595,7 +600,7 @@ def _setup(self, self._gui.append_group(self._markers_group, remove_if_exists=False) # Add center of mass - def get_scale() -> Tuple3FType: + def get_com_scale() -> Tuple3FType: return (1.0, 1.0, self._client.data.com[0][2]) self.add_marker(name="com_0_sphere", @@ -607,20 +612,21 @@ def get_scale() -> Tuple3FType: self.add_marker(name="com_0_cylinder", shape="cylinder", pose=[self._client.data.com[0], None], - scale=get_scale, + scale=get_com_scale, remove_if_exists=True, radius=0.004, length=1.0, anchor_bottom=True) # Add contact sensor markers - def get_pose(sensor: contact) -> Tuple[Tuple3FType, Tuple4FType]: + def get_contact_pose( + sensor: contact) -> Tuple[Tuple3FType, Tuple4FType]: oMf = self._client.data.oMf[sensor.frame_idx] frame_position = oMf.translation frame_rotation = pin.Quaternion(oMf.rotation).coeffs() return (frame_position, frame_rotation) - def get_scale(sensor: contact) -> Tuple3FType: + def get_contact_scale(sensor: contact) -> Tuple3FType: length = min(abs(sensor.data[2]) / CONTACT_FORCE_SCALE, 100.0) return (1.0, 1.0, - np.sign(sensor.data[2]) * length) @@ -629,15 +635,41 @@ def get_scale(sensor: contact) -> Tuple3FType: sensor = robot.get_sensor(contact.type, name) self.add_marker(name='_'.join((contact.type, name)), shape="cylinder", - pose=partial(get_pose, sensor), - scale=partial(get_scale, sensor), + pose=partial(get_contact_pose, sensor), + scale=partial(get_contact_scale, sensor), remove_if_exists=True, radius=0.02, length=0.01, anchor_bottom=True) + # Add DCM marker + def get_dcm_pose() -> Tuple[Tuple3FType, Tuple4FType]: + dcm = np.zeros(3) + com_position = self._client.data.com[0] + if com_position[2] > 0.0: + com_velocity = self._client.data.vcom[0] + gravity = abs(self._client.model.gravity.linear[2]) + omega = math.sqrt(abs(gravity / com_position[2])) + dcm[:2] = com_position[:2] + com_velocity[:2] / omega + return (dcm, pin.Quaternion.Identity().coeffs()) + + def get_dcm_scale() -> Tuple3FType: + com_position = self._client.data.com[0] + return np.full((3,), com_position[2] > 0.0, dtype=np.float64) + + self.add_marker(name="dcm", + shape="cone", + color="green", + pose=get_dcm_pose, + scale=get_dcm_scale, + remove_if_exists=True, + radius=0.03, + length=0.03, + num_sides=4) + # Refresh CoM and contacts visibility self.display_center_of_mass(self._display_com) + self.display_capture_point(self._display_dcm) self.display_contact_forces(self._display_contacts) @staticmethod @@ -1639,6 +1671,29 @@ def display_center_of_mass(self, visibility: bool) -> None: raise NotImplementedError( "This method is only supported by Panda3d.") + @__must_be_open + def display_capture_point(self, visibility: bool) -> None: + """Display the position of the capture point,also called divergent + component of motion (DCM), as a sphere. + + .. note:: + Calling `Viewer.display` will update it automatically, while + `Viewer.refresh` will not. + + :param visibility: Whether to enable or disable display of the capture + point. + """ + if Viewer.backend.startswith('panda3d'): + for name in self.markers: + if name.startswith("dcm"): + self._gui.show_node( + self._markers_group, name, visibility, + always_foreground=True) + self._display_dcm = visibility + else: + raise NotImplementedError( + "This method is only supported by Panda3d.") + @__must_be_open def display_contact_forces(self, visibility: bool) -> None: """Display forces associated with the contact sensors attached to the @@ -1785,6 +1840,7 @@ def refresh(self, @__must_be_open def display(self, q: np.ndarray, + v: Optional[np.ndarray] = None, xyz_offset: Optional[np.ndarray] = None, update_hook: Optional[Callable[[], None]] = None, wait: bool = False) -> None: @@ -1795,6 +1851,8 @@ def display(self, `use_theoretical_model` is false. :param q: Configuration of the robot. + :param v: Velocity of the robot. It is used to update velocity + dependent :param xyz_offset: Freeflyer position offset. Note that it does not check for the robot actually have a freeflyer. :param update_hook: Callable that will be called right after updating @@ -1813,7 +1871,11 @@ def display(self, # Update pinocchio data pin.framesForwardKinematics(self._client.model, self._client.data, q) - pin.centerOfMass(self._client.model, self._client.data, False) + if v is None: + pin.centerOfMass(self._client.model, self._client.data, q, False) + else: + pin.centerOfMass( + self._client.model, self._client.data, q, v, False) # Call custom update hook if update_hook is not None: @@ -1829,7 +1891,8 @@ def replay(self, np.ndarray, Tuple[float, float]]] = (0.0, np.inf), speed_ratio: float = 1.0, xyz_offset: Optional[np.ndarray] = None, - update_hook: Optional[Callable[[float], None]] = None, + update_hook: Optional[Callable[ + [float, np.ndarray, np.ndarray], None]] = None, enable_clock: bool = False, wait: bool = False) -> None: """Replay a complete robot trajectory at a given real-time ratio. @@ -1852,8 +1915,10 @@ def replay(self, check for the robot actually have a freeflyer. OPtional: None by default. :param update_hook: Callable that will be called periodically between - every state update. `None` to disable. - Optional: None by default. + every state update. `None` to disable, otherwise it + must have the following signature: + f(t:float, q: ndarray, v: ndarray) -> None + Optional: No update hook by default. :param wait: Whether or not to wait for rendering to finish. """ # Disable display of sensor data if no update hook is provided @@ -1862,7 +1927,15 @@ def replay(self, disable_display_contacts = True self.display_contact_forces(False) + # Disable display of DCM if no velocity data provided + disable_display_dcm = False + has_velocities = evolution_robot[0].v is not None + if not has_velocities and self._display_dcm: + disable_display_dcm = True + self.display_capture_point(False) + # Replay the whole trajectory at constant speed ratio + v = None update_hook_t = None times = [s.t for s in evolution_robot] t_simu = time_interval[0] @@ -1876,11 +1949,13 @@ def replay(self, s = evolution_robot[max(i - 1, 0)] ratio = (t_simu - s.t) / (s_next.t - s.t) q = pin.interpolate(self._client.model, s.q, s_next.q, ratio) + if has_velocities: + v = s.v + ratio * (s_next.v - s.v) if Viewer._camera_motion is not None: Viewer._camera_xyzrpy = Viewer._camera_motion(t_simu) if update_hook is not None: - update_hook_t = partial(update_hook, t_simu) - self.display(q, xyz_offset, update_hook_t, wait) + update_hook_t = partial(update_hook, t_simu, q, v) + self.display(q, v, xyz_offset, update_hook_t, wait) t_simu = time_interval[0] + speed_ratio * ( time.time() - init_time) i = bisect_right(times, t_simu) @@ -1900,7 +1975,9 @@ def replay(self, with self._lock: Viewer.set_clock() - # Restore display of sensor data - if disable_display_contacts: - with self._lock: + # Restore display if necessary + with self._lock: + if disable_display_contacts: self.display_contact_forces(True) + if disable_display_dcm: + self.display_capture_point(True) From 99d9cdec8900e4e1eb3fe4f47f645e14f755eb74 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 16 May 2021 18:04:01 +0200 Subject: [PATCH 09/16] [python/viewer] Fix default display marker value if backend is not Panda3d. --- python/jiminy_py/src/jiminy_py/simulator.py | 24 ++++-- .../jiminy_py/src/jiminy_py/viewer/replay.py | 78 +++++++++++-------- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 8 ++ 3 files changed, 70 insertions(+), 40 deletions(-) diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index 42a1fdea2..d1330cda4 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -477,22 +477,34 @@ def render(self, robot_name = self.viewer.robot_name scene_name = self.viewer.scene_name - # Create a new viewer client + # Handling of default backend + self.viewer_backend = self.viewer_backend or Viewer.backend + + # Create new viewer instance self.viewer = Viewer(self.robot, use_theoretical_model=False, open_gui_if_parent=False, **{'scene_name': scene_name, 'robot_name': robot_name, 'backend': self.viewer_backend, - 'display_com': True, - 'display_dcm': True, - 'display_contacts': True, 'delete_robot_on_close': True, **kwargs}) - self.viewer_backend = Viewer.backend # Just in case it was `None` + + if self.viewer_backend.startswith('panda3d'): + # Enable display of COM, DCM and contact markers by default + if "display_com" not in kwargs: + self.viewer.display_center_of_mass(True) + if "display_dcm" not in kwargs: + self.viewer.display_capture_point(True) + if "display_contacts" not in kwargs: + self.viewer.display_contact_forces(True) + + # Initialize camera pose if self.viewer.is_backend_parent and camera_xyzrpy is None: camera_xyzrpy = [(9.0, 0.0, 2e-5), (np.pi/2, 0.0, np.pi/2)] - self.viewer.wait(require_client=False) # Wait to finish loading + + # Wait for the viewer to finish loading + self.viewer.wait(require_client=False) # Set the camera pose if requested if camera_xyzrpy is not None: diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index 2ce010650..2ab880f70 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -53,7 +53,8 @@ def play_trajectories(trajs_data: Union[ watermark_fullpath: Optional[str] = None, legend: Optional[Union[str, Sequence[str]]] = None, enable_clock: bool = False, - display_com: bool = None, + display_com: Optional[bool] = None, + display_dcm: Optional[bool] = None, display_contacts: Optional[bool] = None, scene_name: str = 'world', record_video_path: Optional[str] = None, @@ -119,18 +120,24 @@ def play_trajectories(trajs_data: Union[ Optional: No legend if no color by default, the robots names otherwise. :param enable_clock: Add clock on bottom right corner of the viewer. - Only available with panda3d rendering backend. + Only available with 'panda3d' rendering backend. Optional: Disable by default. :param display_com: Whether or not to display the center of mass. `None` to keep current viewers' settings, if any. - Optional: Enable by default iif `viewers` is `None`. + Optional: Enable by default iif `viewers` is `None`, + and backend is 'panda3d'. + :param display_dcm: Whether or not to display the capture point (also + called DCM). `None to keep current viewers' settings. + Optional: Enable by default iif `viewers` is `None`, + and backend is 'panda3d'. :param display_contacts: Whether or not to display the contact forces. Note that the user is responsible for updating sensors data since `Viewer.display` is only computing kinematic quantities. `None` to keep - current viewers' settings, if any. + current viewers' settings. Optional: Enable by default iif `update_hooks` is - specified and `viewers` is `None`. + specified, `viewers` is `None`, and backend is + 'panda3d'. :param scene_name: Name of viewer's scene in which to display the robot. Optional: Common default name if omitted. :param record_video_path: Fullpath location where to save generated video. @@ -143,10 +150,10 @@ def play_trajectories(trajs_data: Union[ input before starting to play the trajectories. Only available if `record_video_path` is None. Optional: False by default. - :param backend: Backend, one of 'meshcat' or 'gepetto-gui'. If None, - 'meshcat' is used in notebook environment and 'gepetto-gui' - otherwise. - Optional: None by default. + :param backend: Backend, one of 'panda3d', 'meshcat', or 'gepetto-gui'. If + `None`, the most appropriate backend will be selected + automatically, based on hardware and python environment. + Optional: `None` by default. :param delete_robot_on_close: Whether or not to delete the robot from the viewer when closing it. Optional: True by default. @@ -166,21 +173,20 @@ def play_trajectories(trajs_data: Union[ :returns: List of viewers used to play the trajectories. """ - # Make sure trajectory data and update hook are list or tuple + # Make sure sequence arguments are list or tuple if not isinstance(trajs_data, (list, tuple)): trajs_data = [trajs_data] if update_hooks is None: update_hooks = [None] * len(trajs_data) if not isinstance(update_hooks, (list, tuple)): update_hooks = [update_hooks] + if not isinstance(viewers, (list, tuple)): + viewers = [viewers] + elif not viewers: + viewers = None - # Sanitize user-specified viewers + # Make sure the viewers are still running if specified if viewers is not None: - # Make sure that viewers is a list - if not isinstance(viewers, (list, tuple)): - viewers = [viewers] - - # Make sure the viewers are still running if specified if not Viewer.is_open(): viewers = None else: @@ -189,15 +195,17 @@ def play_trajectories(trajs_data: Union[ viewers = None break - # Handling of default display of CoM and contact forces - if viewers is None: + # Handling of default display of CoM, DCM and contact forces + if viewers is None and Viewer.backend.startswith('panda3d'): if display_com is None: display_com = True + if display_dcm is None: + display_dcm = True if display_contacts is None: display_contacts = all(func is not None for func in update_hooks) # Make sure it is possible to display contacts if requested - if display_contacts is not False: + if display_contacts: if any(traj['robot'].is_locked for traj in trajs_data): logger.debug( "`display_contacts` is not available if robot is locked. " @@ -241,16 +249,15 @@ def play_trajectories(trajs_data: Union[ robot = traj['robot'] robot_name = f"{uniq_id}_robot_{i}" use_theoretical_model = traj['use_theoretical_model'] - viewer = Viewer( - robot, - use_theoretical_model=use_theoretical_model, - robot_color=color, - robot_name=robot_name, - lock=lock, - backend=backend, - scene_name=scene_name, - delete_robot_on_close=delete_robot_on_close, - open_gui_if_parent=(record_video_path is None)) + viewer = Viewer(robot, + use_theoretical_model=use_theoretical_model, + robot_color=color, + robot_name=robot_name, + lock=lock, + backend=backend, + scene_name=scene_name, + delete_robot_on_close=delete_robot_on_close, + open_gui_if_parent=(record_video_path is None)) viewers.append(viewer) else: # Reset robot model in viewer if requested color has changed @@ -268,7 +275,7 @@ def play_trajectories(trajs_data: Union[ viewer = viewers[0] # Make sure clock is only enabled for panda3d backend - if enable_clock and Viewer.backend != 'panda3d': + if enable_clock and not Viewer.backend.startswith('panda3d'): logger.warn( "`enable_clock` is only available with 'panda3d' backend.") enable_clock = False @@ -301,10 +308,13 @@ def play_trajectories(trajs_data: Union[ if data: i = bisect_right([s.t for s in data], time_interval[0]) viewer_i.display(data[i].q, data[i].v, offset) - if display_com is not None: - viewer_i.display_center_of_mass(display_com) - if display_contacts is not None: - viewer_i.display_contact_forces(display_contacts) + if Viewer.backend.startswith('panda3d'): + if display_com is not None: + viewer_i.display_center_of_mass(display_com) + if display_dcm is not None: + viewer_i.display_capture_point(display_dcm) + if display_contacts is not None: + viewer_i.display_contact_forces(display_contacts) # Wait for the meshes to finish loading if video recording is disable if record_video_path is None: diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 8e934e522..a3e922e88 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -352,6 +352,14 @@ def __init__(self, uniq_id = next(tempfile._get_candidate_names()) robot_name = "_".join(("robot", uniq_id)) + # Make sure user arguments are valid + if not Viewer.backend.startswith('panda3d'): + if display_com or display_dcm or display_contacts: + logger.warning( + "Panda3d backend is required to display markers, e.g. " + "CoM, DCM or Contact.") + display_com, display_dcm, display_contacts = False, False, False + # Backup some user arguments self.robot_color = get_color_code(robot_color) self.robot_name = robot_name From 88a52a23d028100624766493ce6798dd89c33a7b Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 16 May 2021 22:46:19 +0200 Subject: [PATCH 10/16] [python/viewer] Add option to display external linear forces at joint level in viewer. Improve efficiency of markers update. --- python/jiminy_py/src/jiminy_py/simulator.py | 23 +- .../viewer/panda3d/panda3d_visualizer.py | 15 +- .../jiminy_py/src/jiminy_py/viewer/replay.py | 17 +- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 286 +++++++++++++----- 4 files changed, 262 insertions(+), 79 deletions(-) diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index d1330cda4..1d489428f 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -364,6 +364,10 @@ def start(self, if hresult != jiminy.hresult_t.SUCCESS: raise RuntimeError("Failed to start the simulation.") + # Share the external force buffer of the viewer with the engine + if self.viewer is not None: + self.viewer.f_external = self.system_state.f_external[1:] + def step(self, step_dt: float = -1) -> None: """Integrate system dynamics from current state for a given duration. @@ -490,6 +494,9 @@ def render(self, 'delete_robot_on_close': True, **kwargs}) + # Share the external force buffer of the viewer with the engine + self.viewer.f_external = self.system_state.f_external[1:] + if self.viewer_backend.startswith('panda3d'): # Enable display of COM, DCM and contact markers by default if "display_com" not in kwargs: @@ -499,6 +506,20 @@ def render(self, if "display_contacts" not in kwargs: self.viewer.display_contact_forces(True) + # Enable display of external forces by default only for + # the joints having an external force registered to it. + if "display_f_external" not in kwargs: + force_frames = set([ + self.robot.pinocchio_model.frames[f_i.frame_idx].parent + for f_i in self.engine.forces_profile]) + force_frames |= set([ + self.robot.pinocchio_model.frames[f_i.frame_idx].parent + for f_i in self.engine.forces_impulse]) + visibility = self.viewer._display_f_external + for i in force_frames: + visibility[i - 1] = True + self.viewer.display_external_forces(visibility) + # Initialize camera pose if self.viewer.is_backend_parent and camera_xyzrpy is None: camera_xyzrpy = [(9.0, 0.0, 2e-5), (np.pi/2, 0.0, np.pi/2)] @@ -540,8 +561,6 @@ def replay(self, **kwargs: Any) -> None: viewers=[self.viewer], **{'verbose': True, 'backend': self.viewer_backend, - 'display_com': True, - 'display_contacts': True, **kwargs}) def close(self) -> None: diff --git a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py index 8a3fa2ec9..7640f112c 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py @@ -1065,7 +1065,7 @@ def show_node(self, root_path: str, name: str, show: bool, - always_foreground: bool = False) -> None: + always_foreground: Optional[bool] = None) -> None: """Turn rendering on or off for a single node. """ node = self._groups[root_path].find(name) @@ -1077,12 +1077,13 @@ def show_node(self, else: node.set_tag("status", "hidden") node.hide() - if always_foreground: - node.set_bin("fixed", 0) - else: - node.clear_bin() - node.set_depth_test(not always_foreground) - node.set_depth_write(not always_foreground) + if always_foreground is not None: + if always_foreground: + node.set_bin("fixed", 0) + else: + node.clear_bin() + node.set_depth_test(not always_foreground) + node.set_depth_write(not always_foreground) def set_camera_transform(self, pos: Tuple3FType, diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index 2ab880f70..874f3fcb6 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -56,6 +56,7 @@ def play_trajectories(trajs_data: Union[ display_com: Optional[bool] = None, display_dcm: Optional[bool] = None, display_contacts: Optional[bool] = None, + display_f_external: Union[Sequence[bool], bool] = False, scene_name: str = 'world', record_video_path: Optional[str] = None, start_paused: bool = False, @@ -132,12 +133,22 @@ def play_trajectories(trajs_data: Union[ and backend is 'panda3d'. :param display_contacts: Whether or not to display the contact forces. Note that the user is responsible for updating - sensors data since `Viewer.display` is only - computing kinematic quantities. `None` to keep + sensors data via `update_hooks`. `None` to keep current viewers' settings. Optional: Enable by default iif `update_hooks` is specified, `viewers` is `None`, and backend is 'panda3d'. + :param display_f_external: Whether or not to display the external external + forces applied at the joints on the robot. If a + boolean is provided, the same visibility will be + set for each joint, alternatively one can + provide a boolean list whose ordering is + consistent with `pinocchio_model.names`. Note + that the user is responsible for updating the + force buffer `viewer.f_external` via + `update_hooks`. `None` to keep current viewers' + settings. + Optional: `None` by default. :param scene_name: Name of viewer's scene in which to display the robot. Optional: Common default name if omitted. :param record_video_path: Fullpath location where to save generated video. @@ -315,6 +326,8 @@ def play_trajectories(trajs_data: Union[ viewer_i.display_capture_point(display_dcm) if display_contacts is not None: viewer_i.display_contact_forces(display_contacts) + if display_f_external is not None: + viewer_i.display_contact_forces(display_f_external) # Wait for the meshes to finish loading if video recording is disable if record_video_path is None: diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index a3e922e88..44181ce50 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -55,8 +55,9 @@ DEFAULT_WATERMARK_MAXSIZE = (150, 150) -# Fz force value corresponding to capsule's length of 1cm -CONTACT_FORCE_SCALE = 100.0 # [N] +# Fz force value corresponding to capsule's length of 1m +CONTACT_FORCE_SCALE = 10000.0 # [N] +EXTERNAL_FORCE_SCALE = 800.0 # [N] COLORS = {'red': (0.9, 0.15, 0.15, 1.0), 'blue': (0.3, 0.3, 1.0, 1.0), @@ -296,6 +297,7 @@ def __init__(self, display_com: bool = False, display_dcm: bool = False, display_contacts: bool = False, + display_f_external: Union[Sequence[bool], bool] = False, **kwargs: Any): """ :param robot: Jiminy.Model to display. @@ -345,6 +347,15 @@ def __init__(self, sensors data since `Viewer.display` is only computing kinematic quantities. Optional: Disable by default. + :param display_f_external: + Whether or not to display the external external forces applied at + the joints on the robot. If a boolean is provided, the same + visibility will be set for each joint, alternatively one can + provide a boolean list whose ordering is consistent with + `pinocchio_model.names`. Note that the user is responsible for + updating the force buffer `viewer.f_external` data since + `Viewer.display` is only computing kinematic quantities. + Optional: Disable by default. :param kwargs: Unused extra keyword arguments to enable forwarding. """ # Handling of default arguments @@ -370,11 +381,22 @@ def __init__(self, self._display_com = display_com self._display_dcm = display_dcm self._display_contacts = display_contacts + self._display_f_external = display_f_external # Initialize marker register self.markers: Dict[str, MarkerDataType] = {} self._markers_group = '/'.join(( self.scene_name, self.robot_name, "markers")) + self._markers_visibility: Dict[str, bool] = {} + + # Initialize external forces + if self.use_theoretical_model: + pinocchio_model = robot.pinocchio_model_th + else: + pinocchio_model = robot.pinocchio_model + self.f_external = pin.StdVec_Force() + self.f_external.extend([ + pin.Force.Zero() for _ in range(pinocchio_model.njoints - 1)]) # Select the desired backend if backend is None: @@ -559,6 +581,12 @@ def _setup(self, pinocchio_model = robot.pinocchio_model pinocchio_data = robot.pinocchio_data + # Reset external force buffer iif it is necessary + if len(self.f_external) != pinocchio_model.njoints - 1: + self.f_external = pin.StdVec_Force() + self.f_external.extend([ + pin.Force.Zero() for _ in range(pinocchio_model.njoints - 1)]) + # Create robot visual model. # Note that it does not actually loads the meshes if possible, since # the rendering backend will reload them anyway. @@ -611,44 +639,24 @@ def _setup(self, def get_com_scale() -> Tuple3FType: return (1.0, 1.0, self._client.data.com[0][2]) - self.add_marker(name="com_0_sphere", + self.add_marker(name="COM_0_sphere", shape="sphere", pose=[self._client.data.com[0], None], remove_if_exists=True, + auto_refresh=False, radius=0.03) - self.add_marker(name="com_0_cylinder", + self.add_marker(name="COM_0_cylinder", shape="cylinder", pose=[self._client.data.com[0], None], scale=get_com_scale, remove_if_exists=True, + auto_refresh=False, radius=0.004, length=1.0, anchor_bottom=True) - # Add contact sensor markers - def get_contact_pose( - sensor: contact) -> Tuple[Tuple3FType, Tuple4FType]: - oMf = self._client.data.oMf[sensor.frame_idx] - frame_position = oMf.translation - frame_rotation = pin.Quaternion(oMf.rotation).coeffs() - return (frame_position, frame_rotation) - - def get_contact_scale(sensor: contact) -> Tuple3FType: - length = min(abs(sensor.data[2]) / CONTACT_FORCE_SCALE, 100.0) - return (1.0, 1.0, - np.sign(sensor.data[2]) * length) - - if contact.type in robot.sensors_names.keys(): - for name in robot.sensors_names[contact.type]: - sensor = robot.get_sensor(contact.type, name) - self.add_marker(name='_'.join((contact.type, name)), - shape="cylinder", - pose=partial(get_contact_pose, sensor), - scale=partial(get_contact_scale, sensor), - remove_if_exists=True, - radius=0.02, - length=0.01, - anchor_bottom=True) + self.display_center_of_mass(self._display_com) # Add DCM marker def get_dcm_pose() -> Tuple[Tuple3FType, Tuple4FType]: @@ -665,21 +673,83 @@ def get_dcm_scale() -> Tuple3FType: com_position = self._client.data.com[0] return np.full((3,), com_position[2] > 0.0, dtype=np.float64) - self.add_marker(name="dcm", + self.add_marker(name="DCM", shape="cone", color="green", pose=get_dcm_pose, scale=get_dcm_scale, remove_if_exists=True, + auto_refresh=False, radius=0.03, length=0.03, num_sides=4) - # Refresh CoM and contacts visibility - self.display_center_of_mass(self._display_com) self.display_capture_point(self._display_dcm) + + # Add contact sensor markers + def get_contact_pose( + sensor: contact) -> Tuple[Tuple3FType, Tuple4FType]: + oMf = self._client.data.oMf[sensor.frame_idx] + frame_position = oMf.translation + frame_rotation = pin.Quaternion(oMf.rotation).coeffs() + return (frame_position, frame_rotation) + + def get_contact_scale(sensor: contact) -> Tuple3FType: + length = min(abs(sensor.data[2]) / CONTACT_FORCE_SCALE, 1.0) + return (1.0, 1.0, - np.sign(sensor.data[2]) * length) + + if contact.type in robot.sensors_names.keys(): + for name in robot.sensors_names[contact.type]: + sensor = robot.get_sensor(contact.type, name) + self.add_marker(name='_'.join((contact.type, name)), + shape="cylinder", + pose=partial(get_contact_pose, sensor), + scale=partial(get_contact_scale, sensor), + remove_if_exists=True, + auto_refresh=False, + radius=0.02, + length=0.5, + anchor_bottom=True) + self.display_contact_forces(self._display_contacts) + # Add external forces + def get_force_pose( + joint_idx: int) -> Tuple[Tuple3FType, Tuple4FType]: + joint_pose = self._client.data.oMi[joint_idx] + joint_position = joint_pose.translation + if self.f_external: + f_ext = self.f_external[joint_idx - 1].linear + joint_rotation = pin.Quaternion(joint_pose.rotation) + frame_rotation = (joint_rotation * pin.Quaternion( + np.array([0.0, 0.0, 1.0]), f_ext)).coeffs() + else: + frame_rotation = pin.Quaternion.Identity().coeffs() + return (joint_position, frame_rotation) + + def get_force_scale(joint_idx: int) -> Tuple[float, float, float]: + if self.f_external: + f_ext = self.f_external[joint_idx - 1].linear + f_ext_norm = np.linalg.norm(f_ext, 2) + length = min(f_ext_norm / EXTERNAL_FORCE_SCALE, 1.0) + else: + length = 0.0 + return (1.0, 1.0, length) + + for i in range(1, pinocchio_model.njoints): + frame_name = self._client.model.names[i] + self.add_marker(name=f"ForceExternal_{frame_name}", + shape="arrow", + color="red", + pose=partial(get_force_pose, i), + scale=partial(get_force_scale, i), + remove_if_exists=True, + auto_refresh=False, + radius=0.015, + length=0.7) + + self.display_external_forces(self._display_f_external) + @staticmethod def open_gui(start_if_needed: bool = False) -> bool: """Open a new viewer graphical interface. @@ -1618,6 +1688,11 @@ def add_marker(self, reference to `viewer.markers[name]`. Any modification of it will take effect at next `refresh` call. """ + # Make sure the backend supports this method + if not Viewer.backend.startswith('panda3d'): + raise NotImplementedError( + "This method is only supported by Panda3d.") + # Handling of user arguments if pose is None: pose = [None, None] @@ -1635,20 +1710,22 @@ def add_marker(self, color = 'white' color = np.asarray(get_color_code(color)) - # Make sure no marker with this name already exists + # Remove marker is one already exists and requested if name in self.markers.keys(): if not remove_if_exists: raise ValueError(f"marker's name '{name}' already exists.") self.remove_marker(name) - if Viewer.backend.startswith('panda3d'): - create_shape = getattr(self._gui, f"append_{shape}") - create_shape(self._markers_group, name, **shape_kwargs) - marker_data = {"pose": pose, "scale": scale, "color": color} - self.markers[name] = marker_data - else: - raise NotImplementedError( - "This method is only supported by Panda3d.") + # Add new marker + create_shape = getattr(self._gui, f"append_{shape}") + create_shape(self._markers_group, name, **shape_kwargs) + marker_data = {"pose": pose, "scale": scale, "color": color} + self.markers[name] = marker_data + self._markers_visibility[name] = True + + # Make sure the marker always display in front of the model + self._gui.show_node( + self._markers_group, name, True, always_foreground=True) # Refresh the scene if desired if auto_refresh: @@ -1668,17 +1745,16 @@ def display_center_of_mass(self, visibility: bool) -> None: :param visibility: Whether to enable or disable display of the center of mass. """ - if Viewer.backend.startswith('panda3d'): - for name in self.markers: - if name.startswith("com_0"): - self._gui.show_node( - self._markers_group, name, visibility, - always_foreground=True) - self._display_com = visibility - else: + if not Viewer.backend.startswith('panda3d'): raise NotImplementedError( "This method is only supported by Panda3d.") + for name in self.markers: + if name.startswith("COM_0"): + self._gui.show_node(self._markers_group, name, visibility) + self._markers_visibility[name] = visibility + self._display_com = visibility + @__must_be_open def display_capture_point(self, visibility: bool) -> None: """Display the position of the capture point,also called divergent @@ -1691,17 +1767,22 @@ def display_capture_point(self, visibility: bool) -> None: :param visibility: Whether to enable or disable display of the capture point. """ - if Viewer.backend.startswith('panda3d'): - for name in self.markers: - if name.startswith("dcm"): - self._gui.show_node( - self._markers_group, name, visibility, - always_foreground=True) - self._display_dcm = visibility - else: + # Make sure the current backend is supported by this method + if not Viewer.backend.startswith('panda3d'): raise NotImplementedError( "This method is only supported by Panda3d.") + # Update visibility + for name in self.markers: + if name == "DCM": + self._gui.show_node(self._markers_group, name, visibility) + self._markers_visibility[name] = visibility + self._display_dcm = visibility + + # Must refresh the scene + if visibility: + self.refresh() + @__must_be_open def display_contact_forces(self, visibility: bool) -> None: """Display forces associated with the contact sensors attached to the @@ -1713,26 +1794,79 @@ def display_contact_forces(self, visibility: bool) -> None: .. warning:: It corresponds to the attribute `data` of `jiminy.ContactSensor`. Calling `Viewer.display` will NOT update its value automatically. - It is up to the user to keep it up-to-date. It will always be the - case during a simulation, but not when replaying a log file - a-posteriori. In such a case, the user is responsible of specifying - a custom `update_hook` to `Viewer.display` and `Viewer.replay` - methods to emulate sensor update based on log data. + It is up to the user to keep it up-to-date. .. warning:: This method is only supported by Panda3d. + + :param visibility: Whether or not to display the contact forces. """ - if Viewer.backend.startswith('panda3d'): - for name in self.markers: - if name.startswith(contact.type): - self._gui.show_node( - self._markers_group, name, visibility, - always_foreground=True) - self._display_contacts = visibility - else: + # Make sure the current backend is supported by this method + if not Viewer.backend.startswith('panda3d'): raise NotImplementedError( "This method is only supported by Panda3d.") + # Update visibility + for name in self.markers: + if name.startswith(contact.type): + self._gui.show_node(self._markers_group, name, visibility) + self._markers_visibility[name] = visibility + self._display_contacts = visibility + + # Must refresh the scene + if visibility: + self.refresh() + + @__must_be_open + def display_external_forces(self, + visibility: Union[Sequence[bool], bool]) -> None: + """Display external forces applied on the joints the robot, as an + arrow of variable length depending of magnitude of the force. + + .. warning:: + It only display the linear component of the force, while ignoring + the angular part for now. + + .. warning:: + It corresponds to the attribute ``viewer.f_external`. Calling + `Viewer.display` will NOT update its value automatically. It is up + to the user to keep it up-to-date. + + .. warning:: + This method is only supported by Panda3d. + + :param visibility: Whether or not to display the external force applied + at each joint selectively. If a boolean is provided, + the same visibility will be set for each joint, + alternatively, one can provide a boolean list whose + ordering is consistent with pinocchio model (i.e. + `pinocchio_model.names`). + """ + # Make sure the current backend is supported by this method + if not Viewer.backend.startswith('panda3d'): + raise NotImplementedError( + "This method is only supported by Panda3d.") + + # Convert boolean visiblity to mask if necessary + if isinstance(visibility, bool): + visibility = [visibility,] * (self._client.model.njoints - 1) + + # Check that the length of the mask is consistent with the model + assert len(visibility) == self._client.model.njoints - 1, ( + "The length of the visibility mask must be equal to the number of " + "joints of the model, 'universe' excluded.") + + # Update visibility + for i in range(self._client.model.njoints - 1): + name = f"ForceExternal_{self._client.model.names[i + 1]}" + self._gui.show_node(self._markers_group, name, visibility[i]) + self._markers_visibility[name] = visibility[i] + self._display_f_external = list(visibility) + + # Must refresh the scene + if any(visibility): + self.refresh() + @__must_be_open def remove_marker(self, name: str) -> None: """Remove a marker, based on its name. @@ -1824,6 +1958,8 @@ def refresh(self, if Viewer.backend.startswith('panda3d'): pose_dict, material_dict, scale_dict = {}, {}, {} for marker_name, marker_data in self.markers.items(): + if not self._markers_visibility[marker_name]: + continue marker_data = {key: value() if callable(value) else value for key, value in marker_data.items()} (x, y, z), (qx, qy, qz, qw) = marker_data["pose"] @@ -1942,6 +2078,13 @@ def replay(self, disable_display_dcm = True self.display_capture_point(False) + # Disable display of external forces if no force data provided + disable_display_f_external = False + has_forces = evolution_robot[0].f_ext is not None + if not has_forces and self._display_f_external: + disable_display_f_external = True + self.display_external_forces(False) + # Replay the whole trajectory at constant speed ratio v = None update_hook_t = None @@ -1959,6 +2102,11 @@ def replay(self, q = pin.interpolate(self._client.model, s.q, s_next.q, ratio) if has_velocities: v = s.v + ratio * (s_next.v - s.v) + if has_forces: + for i, (f_ext, f_ext_next) in enumerate(zip( + s.f_ext, s_next.f_ext)): + self.f_external[i].vector[:] = \ + f_ext + ratio * (f_ext_next - f_ext) if Viewer._camera_motion is not None: Viewer._camera_xyzrpy = Viewer._camera_motion(t_simu) if update_hook is not None: @@ -1989,3 +2137,5 @@ def replay(self, self.display_contact_forces(True) if disable_display_dcm: self.display_capture_point(True) + if disable_display_f_external: + self.display_external_forces(True) From 7dddf4b4c9b8a5e156eea23998ac84eeabfaf622 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 16 May 2021 23:34:45 +0200 Subject: [PATCH 11/16] [python/viewer] 'jiminy_replay' entrypoint can now be used to record video. --- python/jiminy_py/src/jiminy_py/viewer/replay.py | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index 874f3fcb6..fc9e8cc27 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -56,7 +56,8 @@ def play_trajectories(trajs_data: Union[ display_com: Optional[bool] = None, display_dcm: Optional[bool] = None, display_contacts: Optional[bool] = None, - display_f_external: Union[Sequence[bool], bool] = False, + display_f_external: Optional[ + Union[Sequence[bool], bool]] = None, scene_name: str = 'world', record_video_path: Optional[str] = None, start_paused: bool = False, @@ -575,11 +576,14 @@ def _play_logs_files_entrypoint() -> None: '-p', '--start_paused', action='store_true', help="Start in pause, waiting for keyboard input.") parser.add_argument( - '-s', '--speed_ratio', type=float, default=0.5, + '-s', '--speed_ratio', type=float, default=1.0, help="Real time to simulation time factor.") parser.add_argument( '-b', '--backend', default='panda3d', help="Display backend (panda3d, meshcat, or gepetto-gui).") + parser.add_argument( + '-v', '--record_video_path', default=None, + help="Fullpath location where to save generated video.") options, files = parser.parse_known_args() kwargs = vars(options) kwargs['logs_files'] = files @@ -588,5 +592,5 @@ def _play_logs_files_entrypoint() -> None: play_logs_files(**{"remove_widgets_overlay": False, **kwargs}) # Do not exit method as long as Jiminy viewer is open - while Viewer.is_alive(): + while Viewer.is_alive() and not kwargs['record_video_path']: time.sleep(0.5) From 86a8eb63002cbaa1bfc9bf17b501795693d8c45e Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 16 May 2021 01:08:23 +0200 Subject: [PATCH 12/16] [python/log] Extract external forces rom log if recorded. --- core/src/robot/Model.cc | 4 +- .../double_pendulum_py/test_simulation.py | 1 + .../gym_jiminy/common/envs/env_locomotion.py | 2 + .../unit_py/test_pipeline_control.py | 8 +-- python/jiminy_py/src/jiminy_py/log.py | 33 ++++++--- python/jiminy_py/src/jiminy_py/state.py | 69 ++++--------------- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 4 +- 7 files changed, 46 insertions(+), 75 deletions(-) diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index c07c70c0e..f3edb637d 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -1250,7 +1250,7 @@ namespace jiminy accelerationFieldnames_.clear(); accelerationFieldnames_.resize(nv_); forceExternalFieldnames_.clear(); - forceExternalFieldnames_.resize(6U * pncModel_.njoints); + forceExternalFieldnames_.resize(6U * (pncModel_.njoints - 1)); for (std::size_t i = 1; i < pncModel_.joints.size(); ++i) { // Get joint name without "Joint" suffix, if any @@ -1334,7 +1334,7 @@ namespace jiminy } std::copy(jointForceExternalFieldnames.begin(), jointForceExternalFieldnames.end(), - forceExternalFieldnames_.begin() + 6U); + forceExternalFieldnames_.begin() + 6U * (i - 1)); } } } diff --git a/examples/double_pendulum_py/test_simulation.py b/examples/double_pendulum_py/test_simulation.py index 2fcf70479..0009c6ca7 100644 --- a/examples/double_pendulum_py/test_simulation.py +++ b/examples/double_pendulum_py/test_simulation.py @@ -49,6 +49,7 @@ def internalDynamics(t, q, v, sensors_data, u_custom): engine_options["telemetry"]["enableConfiguration"] = True engine_options["telemetry"]["enableVelocity"] = True engine_options["telemetry"]["enableAcceleration"] = True +engine_options["telemetry"]["enableForceExternal"] = False engine_options["telemetry"]["enableCommand"] = True engine_options["telemetry"]["enableMotorEffort"] = True engine_options["telemetry"]["enableEnergy"] = True diff --git a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py index 3d8d6bb84..4fd6ed319 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py +++ b/python/gym_jiminy/common/gym_jiminy/common/envs/env_locomotion.py @@ -212,6 +212,8 @@ def _setup(self) -> None: # computation and log replay engine_options['telemetry']['enableConfiguration'] = True engine_options['telemetry']['enableVelocity'] = True + engine_options['telemetry']['enableForceExternal'] = \ + 'disturbance' in self.std_ratio.keys() # Enable the flexible model robot_options["model"]["dynamics"]["enableFlexibleModel"] = True diff --git a/python/gym_jiminy/unit_py/test_pipeline_control.py b/python/gym_jiminy/unit_py/test_pipeline_control.py index 062adfdf4..f19ccce45 100644 --- a/python/gym_jiminy/unit_py/test_pipeline_control.py +++ b/python/gym_jiminy/unit_py/test_pipeline_control.py @@ -30,13 +30,7 @@ def _test_pid_standing(self): """ TODO: Write documentation """ # Reset the environment - def configure_telemetry() -> None: - nonlocal self - engine_options = self.env.simulator.engine.get_options() - engine_options['telemetry']['enableVelocity'] = True - self.env.simulator.engine.set_options(engine_options) - - obs_init = self.env.reset(controller_hook=configure_telemetry) + obs_init = self.env.reset() # Compute the initial target, so that the robot stand-still. # In practice, it corresponds to the initial joints state. diff --git a/python/jiminy_py/src/jiminy_py/log.py b/python/jiminy_py/src/jiminy_py/log.py index 0d3406f5a..95b9c67fc 100644 --- a/python/jiminy_py/src/jiminy_py/log.py +++ b/python/jiminy_py/src/jiminy_py/log.py @@ -145,9 +145,10 @@ def extract_trajectory_data_from_log(log_data: Dict[str, np.ndarray], replay the simulation in a viewer. .. note:: - It extracts the required data for replay, namely the evolution over - time of the robot configuration, and the velocities, which is required - for updating markers such as DCM. + It extracts the required data for replay, namely temporal evolution of: + - robot configuration: to display of the robot on the scene, + - robot velocity: to update velocity-dependent markers such as DCM, + - external forces: to update force-dependent markers. :param log_data: Data from the log file, in a dictionnary. :param robot: Jiminy robot. @@ -158,21 +159,37 @@ def extract_trajectory_data_from_log(log_data: Dict[str, np.ndarray], """ times = log_data["Global.Time"] try: - # Extract the joint positions and velocities evolution over time + # Extract the joint positions, velocities and external forces over time positions = np.stack([ - log_data.get(".".join(("HighLevelController", field)), None) + log_data.get(".".join(("HighLevelController", field)), []) for field in robot.logfile_position_headers], axis=-1) velocities = np.stack([ - log_data.get(".".join(("HighLevelController", field)), None) + log_data.get(".".join(("HighLevelController", field)), []) for field in robot.logfile_velocity_headers], axis=-1) + forces = np.stack([ + log_data.get(".".join(("HighLevelController", field)), []) + for field in robot.logfile_f_external_headers], axis=-1) + + # Determine available data + has_positions = len(positions) > 0 + has_velocities = len(velocities) > 0 + has_forces = len(forces) > 0 # Determine whether to use the theoretical or flexible model use_theoretical_model = not robot.is_flexible # Create state sequence evolution_robot = [] - for t, q, v in zip(times, positions, velocities): - evolution_robot.append(State(t=t, q=q, v=v)) + q, v, f_ext = None, None, None + for i, t in enumerate(times): + if has_positions: + q = positions[i] + if has_velocities: + v = velocities[i] + if has_forces: + f_ext = [forces[i, (6 * (j - 1)):(6 * j)] + for j in range(1, robot.pinocchio_model.njoints)] + evolution_robot.append(State(t=t, q=q, v=v, f_ext=f_ext)) traj_data = {"evolution_robot": evolution_robot, "robot": robot, diff --git a/python/jiminy_py/src/jiminy_py/state.py b/python/jiminy_py/src/jiminy_py/state.py index 5334a12b2..9e70e80a3 100644 --- a/python/jiminy_py/src/jiminy_py/state.py +++ b/python/jiminy_py/src/jiminy_py/state.py @@ -12,12 +12,12 @@ class State: """ def __init__(self, t: float, - q: Union[np.ndarray, Sequence[float]], - v: Optional[Union[np.ndarray, Sequence[float]]] = None, - a: Optional[Union[np.ndarray, Sequence[float]]] = None, - tau: Optional[Union[np.ndarray, Sequence[float]]] = None, - contact_frame: str = None, - f_ext: Optional[Union[Sequence[Force], StdVec_Force]] = None, + q: np.ndarray, + v: Optional[np.ndarray] = None, + a: Optional[np.ndarray] = None, + tau: Optional[np.ndarray] = None, + contact_frames: Optional[Sequence[str]] = None, + f_ext: Optional[Sequence[np.ndarray]] = None, copy: bool = False, **kwargs): """ @@ -26,8 +26,8 @@ def __init__(self, :param v: Velocity vector. :param a: Acceleration vector. :param tau: Joint efforts. - :param contact_frame: Name of the contact frame. - :param f_ext: External forces in the contact frame. + :param contact_frames: Name of the contact frames. + :param f_ext: Joint external forces. :param copy: Force to copy the arguments. """ # Time @@ -40,57 +40,14 @@ def __init__(self, self.a = deepcopy(a) if copy else a # Effort vector self.tau = deepcopy(tau) if copy else tau - # Frame name of the contact point, if nay - self.contact_frame = contact_frame + # Frame names of the contact points + if copy: + self.contact_frames = deepcopy(contact_frames) + else: + self.contact_frames = contact_frames # External forces self.f_ext = deepcopy(f_ext) if copy else f_ext - @staticmethod - def todict(state_list: Sequence['State']) -> Dict[ - str, Union[np.ndarray, Sequence[ - Union[Sequence[Force], StdVec_Force]]]]: - """Get the dictionary whose keys are the kinematics and dynamics - data at several time steps from a list of State objects. - - :param state_list: Sequence of State objects - - :returns: Kinematics and dynamics data as a dictionary. - Each property is a 2D numpy array (row: state, column: time). - """ - state_dict = {} - state_dict['t'] = np.array([s.t for s in state_list]) - state_dict['q'] = np.stack([s.q for s in state_list], axis=-1) - state_dict['v'] = np.stack([s.v for s in state_list], axis=-1) - state_dict['a'] = np.stack([s.a for s in state_list], axis=-1) - state_dict['tau'] = [s.tau for s in state_list] - state_dict['contact_frame'] = [s.contact_frame for s in state_list] - state_dict['f_ext'] = [s.f_ext for s in state_list] - return state_dict - - @classmethod - def fromdict(cls, - state_dict: Dict[str, Union[ - np.ndarray, Sequence[ - Union[Sequence[Force], StdVec_Force]]]] - ) -> Sequence['State']: - """Get a list of State objects from a dictionary whose keys are the - kinematics and dynamics data at several time steps. - - :param state_dict: Dictionary whose keys are the kinematics and - dynamics data. Each property is a 2D numpy - array (row: state, column: time). - - :returns: Sequence of State. - """ - _state_dict = defaultdict( - lambda: [None for _ in state_dict['t']], state_dict) - state_list = [] - for i, _ in enumerate(state_dict['t']): - state_list.append(cls(**{ - k: v[..., i] if isinstance(v, np.ndarray) else v[i] - for k, v in _state_dict.items()})) - return state_list - def __repr__(self): """Convert the kinematics and dynamics data into string. diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 44181ce50..f4f9f44a2 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -1995,8 +1995,8 @@ def display(self, `use_theoretical_model` is false. :param q: Configuration of the robot. - :param v: Velocity of the robot. It is used to update velocity - dependent + :param v: Velocity of the robot. Used only to update velocity + dependent markers such as DCM. `None` if undefined. :param xyz_offset: Freeflyer position offset. Note that it does not check for the robot actually have a freeflyer. :param update_hook: Callable that will be called right after updating From 592892f54dc06ee99cac8869ba66deafb1e922cd Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sat, 15 May 2021 16:01:16 +0200 Subject: [PATCH 13/16] [gym/examples] Improve Rllib utilities to be more flexible. Fix logging performance issues. --- .../examples/rllib/tools/utilities.py | 158 +++++++++++++++--- python/jiminy_py/src/jiminy_py/processing.py | 9 +- .../jiminy_py/src/jiminy_py/viewer/replay.py | 54 ++++-- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 13 +- 4 files changed, 190 insertions(+), 44 deletions(-) diff --git a/python/gym_jiminy/examples/rllib/tools/utilities.py b/python/gym_jiminy/examples/rllib/tools/utilities.py index 20aa4f6e0..3f489b58b 100644 --- a/python/gym_jiminy/examples/rllib/tools/utilities.py +++ b/python/gym_jiminy/examples/rllib/tools/utilities.py @@ -1,10 +1,14 @@ import os +import copy import math +import json +import shutil import socket import pathlib import logging +import inspect from datetime import datetime -from typing import Optional, Callable, Dict, Any +from typing import Optional, Callable, Dict, Any, Type import numpy as np import torch @@ -15,10 +19,12 @@ import ray from ray.tune.logger import Logger, TBXLogger +from ray.tune.utils.util import SafeFallbackEncoder from ray.rllib.env import BaseEnv from ray.rllib.evaluation import MultiAgentEpisode, RolloutWorker from ray.rllib.policy import Policy from ray.rllib.utils.filter import NoFilter +from ray.rllib.utils.typing import PolicyID from ray.rllib.agents.trainer import Trainer from ray.rllib.agents.callbacks import DefaultCallbacks @@ -36,28 +42,94 @@ ] -class MonitorInfoCallbacks(DefaultCallbacks): +logger = logging.getLogger(__name__) + + +class MonitorInfoMixin: # Base on `rllib/examples/custom_metrics_and_callbacks.py` example. def on_episode_step(self, + *, worker: RolloutWorker, base_env: BaseEnv, episode: MultiAgentEpisode, - **kwargs): + env_index: Optional[int] = None, + **kwargs) -> None: info = episode.last_info_for() if info is not None: for key, value in info.items(): episode.user_data.setdefault(key, []).append(value) def on_episode_end(self, + *, worker: RolloutWorker, base_env: BaseEnv, - policies: Dict[str, Policy], + policies: Dict[PolicyID, Policy], episode: MultiAgentEpisode, - **kwargs): + env_index: Optional[int] = None, + **kwargs) -> None: for key, value in episode.user_data.items(): # episode.custom_metrics[key] = np.mean(value) episode.hist_data[key] = value + episode.user_data.clear() + + +class CurriculumUpdateMixin: + def on_train_result(self, + *, + trainer, + result: dict, + **kwargs) -> None: + trainer.workers.foreach_worker( + lambda worker: worker.foreach_env( + lambda env: env.update(result))) + + +def build_callbacks(*callback_mixins: Type) -> DefaultCallbacks: + """Aggregate several callback mixin together. + + .. note:: + Note that the order is important if several mixin are implementing the + same method. It follows the same precedence roles than usual multiple + inheritence, namely ordered from highest to lowest priority. + + :param callback_mixins: Sequence of callback mixin objects. + """ + return type("UnifiedCallbacks", (*callback_mixins, DefaultCallbacks), {}) + + +def flatten_dict(dt, delimiter="/", prevent_delimiter=False): + """Must be patched to use copy instead of deepcopy to prevent memory + allocation, significantly impeding computational efficiency of `TBXLogger`, + and slowing down the optimization by about 25%. + """ + dt = copy.copy(dt) + if prevent_delimiter and any(delimiter in key for key in dt): + # Raise if delimiter is any of the keys + raise ValueError( + "Found delimiter `{}` in key when trying to flatten array." + "Please avoid using the delimiter in your specification.") + while any(isinstance(v, dict) for v in dt.values()): + remove = [] + add = {} + for key, value in dt.items(): + if isinstance(value, dict): + for subkey, v in value.items(): + if prevent_delimiter and delimiter in subkey: + # Raise if delimiter is in any of the subkeys + raise ValueError( + "Found delimiter `{}` in key when trying to " + "flatten array. Please avoid using the delimiter " + "in your specification.") + add[delimiter.join([key, str(subkey)])] = v + remove.append(key) + dt.update(add) + for k in remove: + del dt[k] + return dt + + +ray.tune.logger.flatten_dict = flatten_dict def initialize(num_cpus: int = 0, @@ -132,7 +204,8 @@ def logger_creator(config): def train(train_agent: Trainer, - max_timesteps: int, + max_timesteps: int = 0, + max_iters: int = 0, evaluation_period: int = 0, verbose: bool = True) -> str: """Train a model on a specific environment using a given agent. @@ -146,7 +219,10 @@ def train(train_agent: Trainer, This function can be terminated early using CTRL+C. :param train_agent: Training agent. - :param max_timesteps: Number of maximum training timesteps. + :param max_timesteps: Maximum number of training timesteps. 0 to disable. + Optional: Disable by default. + :param max_iters: Maximum number of training iterations. 0 to disable. + Optional: Disable by default. :param evaluation_period: Run one simulation without exploration every given number of training steps, and save a video of the esult in log folder. 0 to disable. @@ -157,13 +233,43 @@ def train(train_agent: Trainer, :returns: Fullpath of agent's final state dump. Note that it also contains the trained neural network model. """ - env_spec = [spec for ev in train_agent.workers.foreach_worker( - lambda ev: ev.foreach_env(lambda env: env.spec)) for spec in ev][0] + # Get environment's reward threshold, if any + env_spec, *_ = [val for worker in train_agent.workers.foreach_worker( + lambda worker: worker.foreach_env(lambda env: env.spec)) + for val in worker] if env_spec is None or env_spec.reward_threshold is None: reward_threshold = math.inf else: reward_threshold = env_spec.reward_threshold + # Backup some information + if not train_agent.iteration: + # Make sure log dir exists + os.makedirs(train_agent.logdir, exist_ok=True) + + # Backup environment's source file + env_type, *_ = [val for worker in train_agent.workers.foreach_worker( + lambda worker: worker.foreach_env(lambda env: type(env.unwrapped))) + for val in worker] + env_file = inspect.getfile(env_type) + shutil.copy2(env_file, train_agent.logdir, follow_symlinks=True) + + # Backup main's source file, if any + frame = inspect.stack()[1] # assuming called directly from main script + main_file = inspect.getfile(frame[0]) + main_backup_name = f"{train_agent.logdir}/main.py" + if main_file.endswith(".py"): + shutil.copy2(main_file, main_backup_name, follow_symlinks=True) + + # Backup RLlib config + with open(f"{train_agent.logdir}/params.json", 'w') as f: + json.dump(train_agent.config, + f, + indent=2, + sort_keys=True, + cls=SafeFallbackEncoder) + + # Run several training iterations until terminal condition is reached try: while True: # Perform one iteration of training the policy @@ -176,16 +282,19 @@ def train(train_agent: Trainer, msg_data.append(f"{field}: {result[field]:.5g}") print(" - ".join(msg_data)) - # Record video of the result if requested + # Record video and log data of the result if requested iter = result["training_iteration"] if evaluation_period > 0 and iter % evaluation_period == 0: record_video_path = f"{train_agent.logdir}/iter_{iter}.mp4" - test(train_agent, explore=False, viewer_kwargs={ + env, _ = test(train_agent, explore=False, viewer_kwargs={ "record_video_path": record_video_path, "scene_name": f"iter_{iter}"}) + env.write_log(f"{train_agent.logdir}/iter_{iter}.hdf5") # Check terminal conditions - if result["timesteps_total"] > max_timesteps: + if max_timesteps > 0 and result["timesteps_total"] > max_timesteps: + break + if max_iters > 0 and iter > max_iters: break if result["episode_reward_mean"] > reward_threshold: if verbose: @@ -195,6 +304,7 @@ def train(train_agent: Trainer, if verbose: print("Interrupting training...") + # Backup trained agent and return file location return train_agent.save() @@ -237,7 +347,7 @@ def compute_action(policy: Policy, return action -def evaluate(env_creator: Callable[..., gym.Env], +def evaluate(env: gym.Env, policy: Policy, dist_class: ActionDistribution, obs_filter_fn: Optional[ @@ -255,8 +365,10 @@ def evaluate(env_creator: Callable[..., gym.Env], if viewer_kwargs is None: viewer_kwargs = {} - # Instantiate the environment - env = FlattenObservation(env_creator(debug=True)) + # Wrap the environment to flatten the observation space + env = FlattenObservation(env) + + # Extract some proxies for convenience observation_space, action_space = env.observation_space, env.action_space # Initialize frame stack @@ -302,7 +414,11 @@ def evaluate(env_creator: Callable[..., gym.Env], # Replay the result if requested if enable_replay: - env.replay(**{'speed_ratio': 1.0, **viewer_kwargs}) + try: + env.replay(**{'speed_ratio': 1.0, **viewer_kwargs}) + except Exception as e: + # Do not fail because of replay/recording exception + logger.warning(str(e)) return env, info_episode @@ -312,6 +428,7 @@ def test(test_agent: Trainer, n_frames_stack: int = 1, enable_stats: bool = True, enable_replay: bool = True, + test_env: Optional[gym.Env] = None, viewer_kwargs: Optional[Dict[str, Any]] = None, **kwargs: Any) -> gym.Env: """Test a model on a specific environment using a given agent. It will @@ -320,10 +437,9 @@ def test(test_agent: Trainer, .. note:: This function can be terminated early using CTRL+C. """ - # Define environment creator - def env_creator(**kwargs: Any): - nonlocal test_agent - return test_agent.env_creator( + # Instantiate the environment if not provided + if test_env is None: + test_env = test_agent.env_creator( {**test_agent.config["env_config"], **kwargs}) # Get policy model @@ -340,7 +456,7 @@ def env_creator(**kwargs: Any): if viewer_kwargs is not None: kwargs.update(viewer_kwargs) - return evaluate(env_creator, + return evaluate(test_env, policy, dist_class, obs_filter_fn, diff --git a/python/jiminy_py/src/jiminy_py/processing.py b/python/jiminy_py/src/jiminy_py/processing.py index e5c5202c5..b24d8b9e0 100644 --- a/python/jiminy_py/src/jiminy_py/processing.py +++ b/python/jiminy_py/src/jiminy_py/processing.py @@ -1,4 +1,3 @@ -import math from typing import Optional, Dict, Union, Sequence import numba as nb @@ -85,6 +84,14 @@ def get_distance(self, queries: np.ndarray) -> np.ndarray: return np.linalg.norm(queries - self._points, 2, axis=1) +@nb.jit(nopython=True, nogil=True) +def squared_norm_2(array: np.ndarray) -> float: + """Fast implementation of the sum of squared arrray elements, optimized for + small to medium size 1D arrays. + """ + return np.sum(np.square(array)) + + @nb.jit(nopython=True, nogil=True) def toeplitz(c: np.ndarray, r: np.ndarray) -> np.ndarray: """Numba-compatible implementation of `scipy.linalg.toeplitz` method. diff --git a/python/jiminy_py/src/jiminy_py/viewer/replay.py b/python/jiminy_py/src/jiminy_py/viewer/replay.py index fc9e8cc27..51885cfec 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/replay.py +++ b/python/jiminy_py/src/jiminy_py/viewer/replay.py @@ -13,6 +13,7 @@ import av import numpy as np from tqdm import tqdm +from scipy.interpolate import interp1d from .. import core as jiminy from ..log import (TrajectoryDataType, @@ -328,7 +329,7 @@ def play_trajectories(trajs_data: Union[ if display_contacts is not None: viewer_i.display_contact_forces(display_contacts) if display_f_external is not None: - viewer_i.display_contact_forces(display_f_external) + viewer_i.display_external_forces(display_f_external) # Wait for the meshes to finish loading if video recording is disable if record_video_path is None: @@ -355,7 +356,7 @@ def play_trajectories(trajs_data: Union[ time_global = np.arange( time_interval[0], time_max, speed_ratio / VIDEO_FRAMERATE) - position_evolutions, velocity_evolutions = [], [] + position_evolutions, velocity_evolutions, force_evolutions = [], [], [] for traj in trajs_data: if len(traj['evolution_robot']): data_orig = traj['evolution_robot'] @@ -370,12 +371,25 @@ def play_trajectories(trajs_data: Union[ if data_orig[0].v is not None: vel_orig = np.stack([s.v for s in data_orig], axis=0) velocity_evolutions.append( - np.interp(time_global, t_orig, vel_orig)) + interp1d(t_orig, vel_orig, axis=0)(time_global)) else: velocity_evolutions = (None,) * len(time_global) + if data_orig[0].f_ext is not None: + forces = [] + for i in range(len(data_orig[0].f_ext)): + f_ext_orig = np.stack([ + s.f_ext[i] for s in data_orig], axis=0) + forces.append(interp1d( + t_orig, f_ext_orig, axis=0)(time_global)) + force_evolutions.append([ + [f_ext[i] for f_ext in forces] + for i in range(len(time_global))]) + else: + force_evolutions = (None,) * len(time_global) else: position_evolutions.append(None) velocity_evolutions.append(None) + force_evolutions.append(None) # Disable framerate limit of Panda3d for efficiency if Viewer.backend.startswith('panda3d'): @@ -407,17 +421,21 @@ def play_trajectories(trajs_data: Union[ # Add frames to video sequentially for i, t_cur in enumerate(tqdm( time_global, desc="Rendering frames", disable=(not verbose))): - # Update the configurations of the robots - for viewer, positions, velocities, xyz_offset, update_hook in zip( + # Update 3D view + for viewer, pos, vel, forces, xyz_offset, update_hook in zip( viewers, position_evolutions, velocity_evolutions, - xyz_offsets, update_hooks): - if positions is not None: - q, v = positions[i], velocities[i] - if update_hook is not None: - update_hook_t = partial(update_hook, t_cur) - else: - update_hook_t = None - viewer.display(q, v, xyz_offset, update_hook_t) + force_evolutions, xyz_offsets, update_hooks): + if pos is None: + continue + q, v, f_ext = pos[i], vel[i], forces[i] + if f_ext is not None: + for i, f_ext in enumerate(f_ext): + viewer.f_external[i].vector[:] = f_ext + if update_hook is not None: + update_hook_t = partial(update_hook, t_cur, q, v) + else: + update_hook_t = None + viewer.display(q, v, xyz_offset, update_hook_t) # Update clock if enabled if enable_clock: @@ -523,6 +541,16 @@ def play_logs_data(robots: Union[Sequence[jiminy.Robot], jiminy.Robot], trajectories = [extract_trajectory_data_from_log(log, robot) for log, robot in zip(logs_data, robots)] + # Display external forces on root joint, if any + if robots[0].has_freeflyer: + if "display_f_external" not in kwargs and "viewers" not in kwargs: + if kwargs.get("use_theoretical_model", False): + njoints = robots[0].pinocchio_model_th.njoints + else: + njoints = robots[0].pinocchio_model.njoints + visibility = [True] + [False] * (njoints - 2) + kwargs["display_f_external"] = visibility + # Define `update_hook` to emulate sensor update if not any(robot.is_locked for robot in robots): update_hooks = [emulate_sensors_data_from_log(log, robot) diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index f4f9f44a2..6046c95c7 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -1819,7 +1819,8 @@ def display_contact_forces(self, visibility: bool) -> None: @__must_be_open def display_external_forces(self, - visibility: Union[Sequence[bool], bool]) -> None: + visibility: Union[Sequence[bool], bool] + ) -> None: """Display external forces applied on the joints the robot, as an arrow of variable length depending of magnitude of the force. @@ -1849,7 +1850,7 @@ def display_external_forces(self, # Convert boolean visiblity to mask if necessary if isinstance(visibility, bool): - visibility = [visibility,] * (self._client.model.njoints - 1) + visibility = [visibility] * (self._client.model.njoints - 1) # Check that the length of the mask is consistent with the model assert len(visibility) == self._client.model.njoints - 1, ( @@ -2078,12 +2079,8 @@ def replay(self, disable_display_dcm = True self.display_capture_point(False) - # Disable display of external forces if no force data provided - disable_display_f_external = False + # Check if force data is available has_forces = evolution_robot[0].f_ext is not None - if not has_forces and self._display_f_external: - disable_display_f_external = True - self.display_external_forces(False) # Replay the whole trajectory at constant speed ratio v = None @@ -2137,5 +2134,3 @@ def replay(self, self.display_contact_forces(True) if disable_display_dcm: self.display_capture_point(True) - if disable_display_f_external: - self.display_external_forces(True) From 0d5ac12e669741266a9d4eb7a3d4a671d6a36039 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 16 May 2021 11:21:55 +0200 Subject: [PATCH 14/16] [gym/examples] Improve documentation of RLLib tools. --- .../examples/rllib/tools/utilities.py | 115 +++++++++++++++--- python/jiminy_py/src/jiminy_py/state.py | 5 +- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 1 + 3 files changed, 98 insertions(+), 23 deletions(-) diff --git a/python/gym_jiminy/examples/rllib/tools/utilities.py b/python/gym_jiminy/examples/rllib/tools/utilities.py index 3f489b58b..bffe96e36 100644 --- a/python/gym_jiminy/examples/rllib/tools/utilities.py +++ b/python/gym_jiminy/examples/rllib/tools/utilities.py @@ -98,7 +98,7 @@ def build_callbacks(*callback_mixins: Type) -> DefaultCallbacks: return type("UnifiedCallbacks", (*callback_mixins, DefaultCallbacks), {}) -def flatten_dict(dt, delimiter="/", prevent_delimiter=False): +def _flatten_dict(dt, delimiter="/", prevent_delimiter=False): """Must be patched to use copy instead of deepcopy to prevent memory allocation, significantly impeding computational efficiency of `TBXLogger`, and slowing down the optimization by about 25%. @@ -129,7 +129,7 @@ def flatten_dict(dt, delimiter="/", prevent_delimiter=False): return dt -ray.tune.logger.flatten_dict = flatten_dict +ray.tune.logger.flatten_dict = _flatten_dict def initialize(num_cpus: int = 0, @@ -143,10 +143,24 @@ def initialize(num_cpus: int = 0, It will be used later for almost everything from dashboard, remote/client management, to multithreaded environment. + :param num_cpus: Maximum number of CPU threads that can be executed in + parallel. Note that it does not actually reserve part of + the CPU, so that several processes can reserve the number + of threads available on the system at the same time. 0 for + unlimited. + Optional: Unlimited by default. + :param num_gpu: Maximum number of GPU unit that can be used, which can be + fractional to only allocate part of the resource. Note that + contrary to CPU resource, the memory is likely to actually + be reserve and allocated by the process, in particular + using Tensorflow backend. 0 for unlimited. + Optional: Unlimited by default. :param log_root_path: Fullpath of root log directory. Optional: location of this file / log by default. :param log_name: Name of the subdirectory where to save data. Optional: full date _ hostname by default. + :param debug: Whether or not to display debugging trace. + Optional: Disable by default. :param verbose: Whether or not to print information about what is going on. Optional: True by default. @@ -207,6 +221,7 @@ def train(train_agent: Trainer, max_timesteps: int = 0, max_iters: int = 0, evaluation_period: int = 0, + record_video: bool = True, verbose: bool = True) -> str: """Train a model on a specific environment using a given agent. @@ -223,11 +238,16 @@ def train(train_agent: Trainer, Optional: Disable by default. :param max_iters: Maximum number of training iterations. 0 to disable. Optional: Disable by default. - :param evaluation_period: Run one simulation without exploration every - given number of training steps, and save a video - of the esult in log folder. 0 to disable. + :param evaluation_period: Run one simulation (without exploration) every + given number of training steps, and save the log + file and a video of the result in log folder if + requested. 0 to disable. Optional: Disable by default. - :param verbose: Whether or not to print information about what is going on. + :param record_video: Whether or not to enable video recording during + evaluation. + Optional: True by default. + :param verbose: Whether or not to print high-level information after each + training iteration. Optional: True by default. :returns: Fullpath of agent's final state dump. Note that it also contains @@ -285,7 +305,10 @@ def train(train_agent: Trainer, # Record video and log data of the result if requested iter = result["training_iteration"] if evaluation_period > 0 and iter % evaluation_period == 0: - record_video_path = f"{train_agent.logdir}/iter_{iter}.mp4" + if record_video: + record_video_path = f"{train_agent.logdir}/iter_{iter}.mp4" + else: + record_video_path = None env, _ = test(train_agent, explore=False, viewer_kwargs={ "record_video_path": record_video_path, "scene_name": f"iter_{iter}"}) @@ -309,16 +332,25 @@ def train(train_agent: Trainer, def compute_action(policy: Policy, - dist_class: ActionDistribution, input_dict: Dict[str, np.ndarray], - explore: bool) -> np.ndarray: - """TODO Write documentation. + explore: bool) -> Any: + """Compute predicted action by the policy. + + .. note:: + It supports both Pytorch and Tensorflow backends (both eager and + compiled graph modes). + + :param policy: `rllib.poli.Policy` to use to predict the action, which is + a thin wrapper around the actual policy model. + :param input_dict: Input dictionary for forward as input of the policy. + :param explore: Whether or not to enable exploration during sampling of the + action. """ if policy.framework == 'torch': with torch.no_grad(): input_dict = policy._lazy_tensor_dict(input_dict) action_logits, _ = policy.model(input_dict) - action_dist = dist_class(action_logits, policy.model) + action_dist = policy.dist_class(action_logits, policy.model) if explore: action_torch = action_dist.sample() else: @@ -326,7 +358,7 @@ def compute_action(policy: Policy, action = action_torch.cpu().numpy() elif tf.compat.v1.executing_eagerly(): action_logits, _ = policy.model(input_dict) - action_dist = dist_class(action_logits, policy.model) + action_dist = policy.dist_class(action_logits, policy.model) if explore: action_tf = action_dist.sample() else: @@ -349,7 +381,6 @@ def compute_action(policy: Policy, def evaluate(env: gym.Env, policy: Policy, - dist_class: ActionDistribution, obs_filter_fn: Optional[ Callable[[np.ndarray], np.ndarray]] = None, n_frames_stack: int = 1, @@ -359,7 +390,37 @@ def evaluate(env: gym.Env, enable_stats: bool = True, enable_replay: bool = True, viewer_kwargs: Optional[Dict[str, Any]] = None) -> gym.Env: - """TODO Write documentation. + """Evaluate a policy on a given environment over a complete episode. + + :param env: Environment on which to evaluate the policy. Note that the + environment must be already instantiated and ready-to-use. + :param policy: Policy to evaluate. + :param obs_filter_fn: Observation filter to apply on (flattened) + observation from the environment, usually used + from moving average normalization. `None` to + disable. + Optional: Disable by default. + :param n_frames_stack: Number of frames to stack in the input to provide + to the policy. Note that previous observation, + action, and reward will be stacked. + Optional: 1 by default. + :param horizon: Horizon of the simulation, namely maximum number of steps + before termination. `None` to disable. + Optional: Disable by default. + :param clip_action: Whether or not to clip action to make sure the + prediction by the policy is not out-of-bounds. + Optional: Disable by default. + :param explore: Whether or not to enable exploration during sampling of the + actions predicted by the policy. + Optional: Disable by default. + :param enable_stats: Whether or not to print high-level statistics after + simulation. + Optional: Enable by default. + :param enable_replay: Whether or not to enable replay of the simulation, + and eventually recording through `viewer_kwargs`. + Optional: Enable by default. + :param viewer_kwargs: Extra keyword arguments to forward to the viewer if + replay has been requested. """ # Handling of default arguments if viewer_kwargs is None: @@ -391,7 +452,7 @@ def evaluate(env: gym.Env, if obs_filter_fn is not None: obs = obs_filter_fn(obs) input_dict["obs"][0] = obs - action = compute_action(policy, dist_class, input_dict, explore) + action = compute_action(policy, input_dict, explore) if clip_action: action = clip(action_space, action) input_dict["prev_n_obs"][0, -1] = input_dict["obs"][0] @@ -431,11 +492,29 @@ def test(test_agent: Trainer, test_env: Optional[gym.Env] = None, viewer_kwargs: Optional[Dict[str, Any]] = None, **kwargs: Any) -> gym.Env: - """Test a model on a specific environment using a given agent. It will - render the result in the default viewer. + """Test a model on a specific environment using a given agent. .. note:: This function can be terminated early using CTRL+C. + + :param test_agent: Agent to evaluate on a single simulation. + :param explore: Whether or not to enable exploration during sampling of the + actions predicted by the policy. + Optional: Disable by default. + :param n_frames_stack: Number of frames to stack in the input to provide + to the policy. Note that previous observation, + action, and reward will be stacked. + Optional: 1 by default. + :param enable_stats: Whether or not to print high-level statistics after + simulation. + Optional: Enable by default. + :param enable_replay: Whether or not to enable replay of the simulation, + and eventually recording through `viewer_kwargs`. + Optional: Enable by default. + :param test_env: Environment on which to evaluate the policy. It must be + already instantiated and ready-to-use. + :param viewer_kwargs: Extra keyword arguments to forward to the viewer if + replay has been requested. """ # Instantiate the environment if not provided if test_env is None: @@ -444,7 +523,6 @@ def test(test_agent: Trainer, # Get policy model policy = test_agent.get_policy() - dist_class = policy.dist_class obs_filter = test_agent.workers.local_worker().filters["default_policy"] if isinstance(obs_filter, NoFilter): obs_filter_fn = None @@ -458,7 +536,6 @@ def test(test_agent: Trainer, return evaluate(test_env, policy, - dist_class, obs_filter_fn, n_frames_stack=n_frames_stack, horizon=test_agent.config["horizon"], diff --git a/python/jiminy_py/src/jiminy_py/state.py b/python/jiminy_py/src/jiminy_py/state.py index 9e70e80a3..1e9b3f6d3 100644 --- a/python/jiminy_py/src/jiminy_py/state.py +++ b/python/jiminy_py/src/jiminy_py/state.py @@ -1,11 +1,8 @@ from copy import deepcopy -from collections import defaultdict -from typing import Optional, Union, Sequence, Dict +from typing import Optional, Sequence import numpy as np -from pinocchio import Force, StdVec_Force - class State: """Store the kinematics and dynamics data of the robot at a given time. diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 6046c95c7..8472f2fbf 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -1998,6 +1998,7 @@ def display(self, :param q: Configuration of the robot. :param v: Velocity of the robot. Used only to update velocity dependent markers such as DCM. `None` if undefined. + Optional: `None` by default. :param xyz_offset: Freeflyer position offset. Note that it does not check for the robot actually have a freeflyer. :param update_hook: Callable that will be called right after updating From d00cf27861141b27ecb3fe8197b9ae12ba558c6d Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Sun, 16 May 2021 14:54:58 +0200 Subject: [PATCH 15/16] [gym/examples] Add monitoring of actually episode duration. --- .../gym_jiminy/examples/rllib/tools/utilities.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/python/gym_jiminy/examples/rllib/tools/utilities.py b/python/gym_jiminy/examples/rllib/tools/utilities.py index bffe96e36..2683d08a6 100644 --- a/python/gym_jiminy/examples/rllib/tools/utilities.py +++ b/python/gym_jiminy/examples/rllib/tools/utilities.py @@ -58,7 +58,7 @@ def on_episode_step(self, info = episode.last_info_for() if info is not None: for key, value in info.items(): - episode.user_data.setdefault(key, []).append(value) + episode.hist_data.setdefault(key, []).append(value) def on_episode_end(self, *, @@ -68,10 +68,8 @@ def on_episode_end(self, episode: MultiAgentEpisode, env_index: Optional[int] = None, **kwargs) -> None: - for key, value in episode.user_data.items(): - # episode.custom_metrics[key] = np.mean(value) - episode.hist_data[key] = value - episode.user_data.clear() + episode.custom_metrics["episode_duration"] = \ + base_env.get_unwrapped()[0].step_dt * episode.length class CurriculumUpdateMixin: @@ -238,9 +236,9 @@ def train(train_agent: Trainer, Optional: Disable by default. :param max_iters: Maximum number of training iterations. 0 to disable. Optional: Disable by default. - :param evaluation_period: Run one simulation (without exploration) every - given number of training steps, and save the log - file and a video of the result in log folder if + :param evaluation_period: Run one simulation (with exploration) every given + number of training steps, and save the log file + and a video of the result in log folder if requested. 0 to disable. Optional: Disable by default. :param record_video: Whether or not to enable video recording during @@ -309,7 +307,7 @@ def train(train_agent: Trainer, record_video_path = f"{train_agent.logdir}/iter_{iter}.mp4" else: record_video_path = None - env, _ = test(train_agent, explore=False, viewer_kwargs={ + env, _ = test(train_agent, explore=True, viewer_kwargs={ "record_video_path": record_video_path, "scene_name": f"iter_{iter}"}) env.write_log(f"{train_agent.logdir}/iter_{iter}.hdf5") From 4a6be3c6338f8cac798df390374fe20468647938 Mon Sep 17 00:00:00 2001 From: Alexis Duburcq Date: Mon, 17 May 2021 10:03:59 +0200 Subject: [PATCH 16/16] Update release patch tag. --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 9f4e3f6dd..30dcdc1d5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -2,7 +2,7 @@ cmake_minimum_required(VERSION 3.10) # Set the build version -set(BUILD_VERSION 1.6.15) +set(BUILD_VERSION 1.6.16) # Set compatibility if(CMAKE_VERSION VERSION_GREATER "3.11.0")