From 792278b8323af83afd62e5924e7677500659f466 Mon Sep 17 00:00:00 2001 From: Alexis DUBURCQ Date: Sun, 10 Oct 2021 15:29:48 +0200 Subject: [PATCH] Jiminy 1.7.0 (#411) * [core] Move init of contact solver in reset instead of setOptions. * [core] Add support of torsional friction and contact normal to impulse contact model. * [core] Better default 'stabilizationFreq' for impulse contact model. * [core] 'engine_options.contacts.stabilizationFreq' only applies to contact constraints. * [core] Refactor 'JointConstraint' to be consistent with 'FixedFrameConstraint'. * [python/robot] Fix support of URDF model with a single link for automatic hardware config generation. * [python/viewer] 'update_floor' method in panda3d now expects numpy array instead of callable to fix serialization issues. * [python/viewer] Add support of contact frames display. And method to render floor based on . * [python/viewer] Add option to enable/disable display of ground profile meshes. * [python/viewer] Speedup rendering of ground profile and improve visual by providing normal to panda3d. * [python/viewer] Add support of ipykernel 6 for meshcat backend interactive display. * [python/simulator] 'replay' now display the ground profile used by the engine. * [python/simulator] Do not reset camera pose if gui already available. * [python/dynamics] Circumvent weird segfault in pinocchio bindings of 'centerOfMass' method. * [python/generator] Add random tile ground profile generator. * [gym/common] Fix automatic telemetry registration of the action. * [gym/rllib] Add temporal barrier regularizer to PPO. * [gym/envs] Set baumgarte frequency for cassie internal constraints. * [misc] Enforce scipy>=1.2.0 to fix buggy optimizer. * [misc] Update 'typing_extensions' required version to fix pylint support for Python 3.6. Co-authored-by: Alexis Duburcq --- CMakeLists.txt | 2 +- core/include/jiminy/core/Types.h | 2 +- .../core/constraints/FixedFrameConstraint.h | 16 +- .../jiminy/core/constraints/JointConstraint.h | 8 +- .../core/constraints/SphereConstraint.h | 2 +- .../jiminy/core/constraints/WheelConstraint.h | 2 +- .../jiminy/core/engine/EngineMultiRobot.h | 11 +- core/src/constraints/FixedFrameConstraint.cc | 78 ++++-- core/src/constraints/JointConstraint.cc | 7 +- core/src/constraints/SphereConstraint.cc | 2 +- core/src/constraints/WheelConstraint.cc | 2 +- core/src/engine/EngineMultiRobot.cc | 248 ++++++++++-------- core/src/robot/Model.cc | 4 +- .../gym_jiminy/common/bases/block_bases.py | 2 +- .../gym_jiminy/common/envs/env_generic.py | 13 +- .../common/gym_jiminy/common/utils/helpers.py | 41 +-- .../gym_jiminy/envs/gym_jiminy/envs/cassie.py | 4 + .../gym_jiminy/rllib/gym_jiminy/rllib/ppo.py | 50 +++- python/jiminy_py/setup.py | 12 +- python/jiminy_py/src/jiminy_py/dynamics.py | 8 +- python/jiminy_py/src/jiminy_py/generator.py | 171 ++++++++++++ python/jiminy_py/src/jiminy_py/robot.py | 41 +-- python/jiminy_py/src/jiminy_py/simulator.py | 6 + .../src/jiminy_py/viewer/meshcat/server.py | 2 +- .../src/jiminy_py/viewer/meshcat/wrapper.py | 41 +-- .../viewer/panda3d/panda3d_visualizer.py | 108 ++++---- .../jiminy_py/src/jiminy_py/viewer/viewer.py | 128 +++++++-- .../include/jiminy/python/Utilities.h | 14 +- python/jiminy_pywrap/src/Constraints.cc | 22 +- unit_py/data/box_collision_mesh.urdf | 3 + unit_py/data/sphere_primitive.urdf | 3 + 31 files changed, 711 insertions(+), 342 deletions(-) create mode 100644 python/jiminy_py/src/jiminy_py/generator.py diff --git a/CMakeLists.txt b/CMakeLists.txt index bb9781a17..63c2bb847 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.30) +set(BUILD_VERSION 1.7.0) # Set compatibility if(CMAKE_VERSION VERSION_GREATER "3.11.0") diff --git a/core/include/jiminy/core/Types.h b/core/include/jiminy/core/Types.h index 16dccdd1b..4503abc05 100644 --- a/core/include/jiminy/core/Types.h +++ b/core/include/jiminy/core/Types.h @@ -67,10 +67,10 @@ namespace jiminy using matrixN_t = Eigen::Matrix; using matrix2_t = Eigen::Matrix; using matrix3_t = Eigen::Matrix; + using matrix6N_t = Eigen::Matrix; using vectorN_t = Eigen::Matrix; using vector3_t = Eigen::Matrix; using vector6_t = Eigen::Matrix; - using rowN_t = Eigen::Matrix; using constMatrixBlock_t = Eigen::Block const; using constVectorBlock_t = Eigen::VectorBlock const; diff --git a/core/include/jiminy/core/constraints/FixedFrameConstraint.h b/core/include/jiminy/core/constraints/FixedFrameConstraint.h index 9840dfba4..2d7996f3a 100644 --- a/core/include/jiminy/core/constraints/FixedFrameConstraint.h +++ b/core/include/jiminy/core/constraints/FixedFrameConstraint.h @@ -39,8 +39,7 @@ namespace jiminy /// \param[in] frameName Name of the frame on which the constraint is to be applied. /////////////////////////////////////////////////////////////////////////////////////////////// FixedFrameConstraint(std::string const & frameName, - Eigen::Matrix const & maskFixed = Eigen::Matrix::Constant(true), - pinocchio::ReferenceFrame const & frameRef = pinocchio::LOCAL_WORLD_ALIGNED); + Eigen::Matrix const & maskFixed = Eigen::Matrix::Constant(true)); virtual ~FixedFrameConstraint(void); std::string const & getFrameName(void) const; @@ -48,10 +47,11 @@ namespace jiminy std::vector const & getDofsFixed(void) const; - pinocchio::ReferenceFrame const & getReferenceFrame(void) const; - void setReferenceTransform(pinocchio::SE3 const & transformRef); - pinocchio::SE3 & getReferenceTransform(void); + pinocchio::SE3 const & getReferenceTransform(void) const; + + void setLocalFrame(matrix3_t const & frameRot); + matrix3_t const & getLocalFrame(void) const; virtual hresult_t reset(vectorN_t const & q, vectorN_t const & v) override final; @@ -62,11 +62,11 @@ namespace jiminy private: std::string const frameName_; ///< Name of the frame on which the constraint operates. frameIndex_t frameIdx_; ///< Corresponding frame index. - pinocchio::ReferenceFrame frameRef_; ///< Reference frame. std::vector dofsFixed_; ///< Degrees of freedom to fix. pinocchio::SE3 transformRef_; ///< Reference pose of the frame to enforce. - matrixN_t frameJacobian_; ///< Stores full frame jacobian in reference frame. - vector6_t frameDrift_; ///< Stores full frame drift in reference frame. + matrix3_t rotationLocal_; ///< Rotation matrix of the local frame in which to apply masking + matrix6N_t frameJacobian_; ///< Stores full frame jacobian in reference frame. + pinocchio::Motion frameDrift_; ///< Stores full frame drift in reference frame. }; } diff --git a/core/include/jiminy/core/constraints/JointConstraint.h b/core/include/jiminy/core/constraints/JointConstraint.h index 38c6dd330..e7384d4c3 100644 --- a/core/include/jiminy/core/constraints/JointConstraint.h +++ b/core/include/jiminy/core/constraints/JointConstraint.h @@ -34,12 +34,8 @@ namespace jiminy std::string const & getJointName(void) const; jointIndex_t const & getJointIdx(void) const; - template - void setReferenceConfiguration(Eigen::MatrixBase const & configurationRef) - { - configurationRef_ = configurationRef; - } - vectorN_t & getReferenceConfiguration(void); + void setReferenceConfiguration(vectorN_t const & configurationRef); + vectorN_t const & getReferenceConfiguration(void) const; virtual hresult_t reset(vectorN_t const & q, vectorN_t const & v) override final; diff --git a/core/include/jiminy/core/constraints/SphereConstraint.h b/core/include/jiminy/core/constraints/SphereConstraint.h index 046550cf0..d9ffe9ee1 100644 --- a/core/include/jiminy/core/constraints/SphereConstraint.h +++ b/core/include/jiminy/core/constraints/SphereConstraint.h @@ -50,7 +50,7 @@ namespace jiminy frameIndex_t const & getFrameIdx(void) const; void setReferenceTransform(pinocchio::SE3 const & transformRef); - pinocchio::SE3 & getReferenceTransform(void); + pinocchio::SE3 const & getReferenceTransform(void) const; virtual hresult_t reset(vectorN_t const & /* q */, vectorN_t const & /* v */) override final; diff --git a/core/include/jiminy/core/constraints/WheelConstraint.h b/core/include/jiminy/core/constraints/WheelConstraint.h index 3dd72e8f7..00cd2b9a4 100644 --- a/core/include/jiminy/core/constraints/WheelConstraint.h +++ b/core/include/jiminy/core/constraints/WheelConstraint.h @@ -52,7 +52,7 @@ namespace jiminy frameIndex_t const & getFrameIdx(void) const; void setReferenceTransform(pinocchio::SE3 const & transformRef); - pinocchio::SE3 & getReferenceTransform(void); + pinocchio::SE3 const & getReferenceTransform(void) const; virtual hresult_t reset(vectorN_t const & /* q */, vectorN_t const & /* v */) override final; diff --git a/core/include/jiminy/core/engine/EngineMultiRobot.h b/core/include/jiminy/core/engine/EngineMultiRobot.h index 466daa5c7..405e2bf94 100644 --- a/core/include/jiminy/core/engine/EngineMultiRobot.h +++ b/core/include/jiminy/core/engine/EngineMultiRobot.h @@ -117,11 +117,12 @@ namespace jiminy config["model"] = std::string("spring_damper"); // ["spring_damper", "impulse"] config["solver"] = std::string("PGS"); // ["PGS",] config["regularization"] = 0.0; // Relative inverse damping wrt. diagonal of J.Minv.J.t. 0.0 to enforce the minimum absolute regularizer. - config["stabilizationFreq"] = 0.0; // [s-1]: 0.0 to disable + config["stabilizationFreq"] = 20.0; // [s-1]: 0.0 to disable config["stiffness"] = 1.0e6; config["damping"] = 2.0e3; config["transitionEps"] = 1.0e-3; // [m] config["friction"] = 1.0; + config["torsion"] = 0.0; config["transitionVelocity"] = 1.0e-2; // [m.s-1] return config; @@ -205,6 +206,7 @@ namespace jiminy float64_t const damping; float64_t const transitionEps; float64_t const friction; + float64_t const torsion; float64_t const transitionVelocity; contactOptions_t(configHolder_t const & options) : @@ -216,6 +218,7 @@ namespace jiminy damping(boost::get(options.at("damping"))), transitionEps(boost::get(options.at("transitionEps"))), friction(boost::get(options.at("friction"))), + torsion(boost::get(options.at("torsion"))), transitionVelocity(boost::get(options.at("transitionVelocity"))) { // Empty. @@ -515,8 +518,6 @@ namespace jiminy /// \return Contact force, at parent joint, in the local frame. void computeContactDynamicsAtBody(systemHolder_t const & system, pairIndex_t const & collisionPairIdx, - vectorN_t const & q, - vectorN_t const & v, std::shared_ptr & contactConstraint, pinocchio::Force & fextLocal) const; @@ -527,8 +528,6 @@ namespace jiminy /// \return Contact force, at parent joint, in the local frame. void computeContactDynamicsAtFrame(systemHolder_t const & system, frameIndex_t const & frameIdx, - vectorN_t const & q, - vectorN_t const & v, std::shared_ptr & collisionConstraint, pinocchio::Force & fextLocal) const; @@ -550,8 +549,6 @@ namespace jiminy vectorN_t & uInternal) const; void computeCollisionForces(systemHolder_t const & system, systemDataHolder_t & systemData, - vectorN_t const & q, - vectorN_t const & v, forceVector_t & fext) const; void computeExternalForces(systemHolder_t const & system, systemDataHolder_t & systemData, diff --git a/core/src/constraints/FixedFrameConstraint.cc b/core/src/constraints/FixedFrameConstraint.cc index 33f9cce98..d7f81e0da 100644 --- a/core/src/constraints/FixedFrameConstraint.cc +++ b/core/src/constraints/FixedFrameConstraint.cc @@ -12,14 +12,13 @@ namespace jiminy std::string const AbstractConstraintTpl::type_("FixedFrameConstraint"); FixedFrameConstraint::FixedFrameConstraint(std::string const & frameName, - Eigen::Matrix const & maskFixed, - pinocchio::ReferenceFrame const & frameRef) : + Eigen::Matrix const & maskFixed) : AbstractConstraintTpl(), frameName_(frameName), frameIdx_(0), - frameRef_(frameRef), dofsFixed_(), transformRef_(), + rotationLocal_(), frameJacobian_(), frameDrift_() { @@ -50,11 +49,6 @@ namespace jiminy return frameIdx_; } - pinocchio::ReferenceFrame const & FixedFrameConstraint::getReferenceFrame(void) const - { - return frameRef_; - } - std::vector const & FixedFrameConstraint::getDofsFixed(void) const { return dofsFixed_; @@ -65,11 +59,21 @@ namespace jiminy transformRef_ = transformRef; } - pinocchio::SE3 & FixedFrameConstraint::getReferenceTransform(void) + pinocchio::SE3 const & FixedFrameConstraint::getReferenceTransform(void) const { return transformRef_; } + void FixedFrameConstraint::setLocalFrame(matrix3_t const & frameRot) + { + rotationLocal_ = frameRot; + } + + matrix3_t const & FixedFrameConstraint::getLocalFrame(void) const + { + return rotationLocal_; + } + hresult_t FixedFrameConstraint::reset(vectorN_t const & /* q */, vectorN_t const & /* v */) { @@ -92,7 +96,7 @@ namespace jiminy if (returnCode == hresult_t::SUCCESS) { // Initialize jacobian, drift and multipliers - frameJacobian_ = matrixN_t::Zero(6, model->pncModel_.nv); + frameJacobian_ = matrix6N_t::Zero(6, model->pncModel_.nv); frameDrift_ = vector6_t::Zero(); uint64_t const dim = dofsFixed_.size(); jacobian_ = matrixN_t::Zero(dim, model->pncModel_.nv); @@ -101,6 +105,9 @@ namespace jiminy // Get the current frame position and use it as reference transformRef_ = model->pncData_.oMf[frameIdx_]; + + // Set local frame to world by default + rotationLocal_.setIdentity(); } return returnCode; @@ -118,40 +125,53 @@ namespace jiminy // Assuming the model still exists. auto model = model_.lock(); - // Get jacobian + // Define inverse rotation matrix of local frame + auto rotInvLocal = rotationLocal_.transpose(); + + // Get jacobian in local frame getFrameJacobian(model->pncModel_, model->pncData_, frameIdx_, - frameRef_, + pinocchio::LOCAL_WORLD_ALIGNED, frameJacobian_); - // Get drift + pinocchio::Frame const & frame = model->pncModel_.frames[frameIdx_]; + pinocchio::JointModel const & joint = model->pncModel_.joints[frame.parent]; + int32_t const colRef = joint.nv() + joint.idx_v() - 1; + for(Eigen::DenseIndex j=colRef; j>=0; j=model->pncData_.parents_fromRow[j]) + { + pinocchio::MotionRef J_col(frameJacobian_.col(j)); + J_col.linear() = rotInvLocal * J_col.linear(); + J_col.angular() = rotInvLocal * J_col.angular(); + } + + // Get drift in world frame frameDrift_ = getFrameAcceleration(model->pncModel_, model->pncData_, frameIdx_, - frameRef_).toVector(); - - // Add Baumgarte stabilization drift - if (frameRef_ == pinocchio::LOCAL_WORLD_ALIGNED || frameRef_ == pinocchio::WORLD) - { - auto deltaPosition = model->pncData_.oMf[frameIdx_].translation() - transformRef_.translation(); - frameDrift_.head<3>() += kp_ * deltaPosition; - auto deltaRotation = transformRef_.rotation().transpose() * model->pncData_.oMf[frameIdx_].rotation(); - vectorN_t const axis = pinocchio::log3(deltaRotation); - frameDrift_.tail<3>() += kp_ * axis; - } - vector6_t const velocity = getFrameVelocity(model->pncModel_, - model->pncData_, - frameIdx_, - frameRef_).toVector(); + pinocchio::LOCAL_WORLD_ALIGNED); + + // Add Baumgarte stabilization to drift in world frame + vector3_t const deltaPosition = model->pncData_.oMf[frameIdx_].translation() - transformRef_.translation(); + matrix3_t const deltaRotation = transformRef_.rotation().transpose() * model->pncData_.oMf[frameIdx_].rotation(); + frameDrift_.linear() += kp_ * deltaPosition; + frameDrift_.angular() += kp_ * pinocchio::log3(deltaRotation); + pinocchio::Motion const velocity = getFrameVelocity(model->pncModel_, + model->pncData_, + frameIdx_, + pinocchio::LOCAL_WORLD_ALIGNED); frameDrift_ += kd_ * velocity; + // Compute drift in local frame + frameDrift_.linear() = rotInvLocal * frameDrift_.linear(); + frameDrift_.angular() = rotInvLocal * frameDrift_.angular(); + // Extract masked jacobian and drift, only containing fixed dofs for (uint32_t i=0; i < dofsFixed_.size(); ++i) { uint32_t const & dofIndex = dofsFixed_[i]; jacobian_.row(i) = frameJacobian_.row(dofIndex); - drift_[i] = frameDrift_[dofIndex]; + drift_[i] = frameDrift_.toVector()[dofIndex]; } return hresult_t::SUCCESS; diff --git a/core/src/constraints/JointConstraint.cc b/core/src/constraints/JointConstraint.cc index 5c9169830..d9df66ef2 100644 --- a/core/src/constraints/JointConstraint.cc +++ b/core/src/constraints/JointConstraint.cc @@ -34,7 +34,12 @@ namespace jiminy return jointIdx_; } - vectorN_t & JointConstraint::getReferenceConfiguration(void) + void JointConstraint::setReferenceConfiguration(vectorN_t const & configurationRef) + { + configurationRef_ = configurationRef; + } + + vectorN_t const & JointConstraint::getReferenceConfiguration(void) const { return configurationRef_; } diff --git a/core/src/constraints/SphereConstraint.cc b/core/src/constraints/SphereConstraint.cc index 60386894a..d37cc1627 100644 --- a/core/src/constraints/SphereConstraint.cc +++ b/core/src/constraints/SphereConstraint.cc @@ -46,7 +46,7 @@ namespace jiminy transformRef_ = transformRef; } - pinocchio::SE3 & SphereConstraint::getReferenceTransform(void) + pinocchio::SE3 const & SphereConstraint::getReferenceTransform(void) const { return transformRef_; } diff --git a/core/src/constraints/WheelConstraint.cc b/core/src/constraints/WheelConstraint.cc index 564b2aded..3c06e7369 100644 --- a/core/src/constraints/WheelConstraint.cc +++ b/core/src/constraints/WheelConstraint.cc @@ -50,7 +50,7 @@ namespace jiminy transformRef_ = transformRef; } - pinocchio::SE3 & WheelConstraint::getReferenceTransform(void) + pinocchio::SE3 const & WheelConstraint::getReferenceTransform(void) const { return transformRef_; } diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc index c428b7914..bf038e7b6 100644 --- a/core/src/engine/EngineMultiRobot.cc +++ b/core/src/engine/EngineMultiRobot.cc @@ -880,6 +880,17 @@ namespace jiminy systemData.state.clear(); systemData.statePrev.clear(); } + + // Instantiate desired LCP solver + std::string const & contactSolver = engineOptions_->contacts.solver; + if (CONTACT_SOLVERS_MAP.at(contactSolver) == contactSolver_t::PGS) + { + contactSolver_ = std::make_unique( + PGS_MAX_ITERATIONS, + PGS_RANDOM_PERMUTATION_PERIOD, + engineOptions_->stepper.tolAbs, + engineOptions_->stepper.tolRel); + } } void computeExtraTerms(systemHolder_t & system) @@ -1283,17 +1294,24 @@ namespace jiminy collisionPairsIdx[i].size(), pinocchio::Force::Zero()); } - // Set Baumgarte stabilization natural frequency for every constraints - systemDataIt->constraintsHolder.foreach( - [freq = engineOptions_->contacts.stabilizationFreq]( // by-copy to avoid compilation failure for gcc<7.3 - std::shared_ptr const & constraint, - constraintsHolderType_t const & /* holderType */) - { - if (constraint) + // Set Baumgarte stabilization natural frequency for contact constraints + std::array holderTypes {{ + constraintsHolderType_t::BOUNDS_JOINTS, + constraintsHolderType_t::CONTACT_FRAMES, + constraintsHolderType_t::COLLISION_BODIES}}; + for (constraintsHolderType_t holderType : holderTypes) + { + systemDataIt->constraintsHolder.foreach(holderType, + [freq = engineOptions_->contacts.stabilizationFreq]( // by-copy to avoid compilation failure for gcc<7.3 + std::shared_ptr const & constraint, + constraintsHolderType_t const & /* holderType */) { - constraint->setBaumgarteFreq(freq); // It cannot fail at this point - } - }); + if (constraint) + { + constraint->setBaumgarteFreq(freq); // It cannot fail at this point + } + }); + } // Initialize some addition buffers used by impulse contact solver systemDataIt->jointJacobian = matrixN_t::Zero(6, systemIt->robot->pncModel_.nv); @@ -1311,7 +1329,7 @@ namespace jiminy auto & constraint = systemDataIt->constraintsHolder.contactFrames[i].second; pinocchio::Force & fextLocal = systemDataIt->contactFramesForces[i]; computeContactDynamicsAtFrame( - *systemIt, contactFramesIdx[i], q, v, constraint, fextLocal); + *systemIt, contactFramesIdx[i], constraint, fextLocal); forceMax = std::max(forceMax, fextLocal.linear().norm()); } @@ -1323,7 +1341,7 @@ namespace jiminy auto & constraint = systemDataIt->constraintsHolder.collisionBodies[i][j].second; pinocchio::Force & fextLocal = systemDataIt->collisionBodiesForces[i][j]; computeContactDynamicsAtBody( - *systemIt, collisionPairIdx, q, v, constraint, fextLocal); + *systemIt, collisionPairIdx, constraint, fextLocal); forceMax = std::max(forceMax, fextLocal.linear().norm()); } } @@ -2610,16 +2628,6 @@ namespace jiminy // Backup contact model as enum for fast check contactModel_ = contactModelIt->second; - // Instantiate desired LCP solver - if (contactSolverIt->second == contactSolver_t::PGS) - { - contactSolver_ = std::make_unique( - PGS_MAX_ITERATIONS, - PGS_RANDOM_PERMUTATION_PERIOD, - engineOptions_->stepper.tolAbs, - engineOptions_->stepper.tolRel); - } - // Set breakpoint period during the integration loop stepperUpdatePeriod_ = minUpdatePeriod; @@ -2849,8 +2857,6 @@ namespace jiminy void EngineMultiRobot::computeContactDynamicsAtBody(systemHolder_t const & system, pairIndex_t const & collisionPairIdx, - vectorN_t const & q, - vectorN_t const & v, std::shared_ptr & constraint, pinocchio::Force & fextLocal) const { @@ -2867,7 +2873,13 @@ namespace jiminy fextLocal.setZero(); - bool_t isColliding = false; + // There is no way to get access to the distance from the ground at this point, + // so it is not possible to disable the constraint only if depth > transitionEps. + if (constraint) + { + constraint->disable(); + } + for (uint32_t i = 0; i < collisionResult.numContacts(); ++i) { /* Extract the contact information. @@ -2886,7 +2898,6 @@ namespace jiminy { continue; } - isColliding = true; // Make sure the normal is always pointing upward, and the penetration depth is negative if (nGround[2] < 0.0) @@ -2916,45 +2927,29 @@ namespace jiminy else { // Some geometry shapes are not supported for now, if so the constraint is not initialized - if (constraint && !constraint->getIsEnabled()) + if (constraint) { - constraint->reset(q, v); + // In case of slippage the contact point has actually moved and must be updated constraint->enable(); auto & frameConstraint = static_cast(*constraint.get()); - vector3_t & positionRef = frameConstraint.getReferenceTransform().translation(); - positionRef.noalias() -= depth * nGround; - } - } - } + frameIndex_t const & frameIdx = frameConstraint.getFrameIdx(); + frameConstraint.setReferenceTransform({ + system.robot->pncData_.oMf[frameIdx].rotation(), + system.robot->pncData_.oMf[frameIdx].translation() - depth * nGround + }); + frameConstraint.setLocalFrame( + quaternion_t::FromTwoVectors(vector3_t::UnitZ(), nGround).toRotationMatrix() + ); - if (!isColliding) - { - if (constraint) - { - // There is no way to get access to the distance from the ground at this point, - // so it is not possible to disable the constraint only if depth > transitionEps. - constraint->disable(); + // Only one contact constraint per collision body is supported for now + break; + } } } - - /* One must update tangential reference position with the current one, - because in case of slippage the contact point has actually moved. */ - if (constraint && constraint->getIsEnabled()) - { - // auto & collisionConstraint = static_cast(*constraint.get()); - auto & collisionConstraint = static_cast(*constraint.get()); - frameIndex_t const & frameIdx = collisionConstraint.getFrameIdx(); - vector3_t const & posFrame = system.robot->pncData_.oMf[frameIdx].translation(); - vector3_t & positionRef = collisionConstraint.getReferenceTransform().translation(); - vector3_t const nGround = (vector3_t() << 0.0, 0.0, 1.0).finished(); //TODO assuming normal ground contact for now - positionRef = (posFrame + (positionRef - posFrame).dot(nGround) * nGround).eval(); - } } void EngineMultiRobot::computeContactDynamicsAtFrame(systemHolder_t const & system, frameIndex_t const & frameIdx, - vectorN_t const & q, - vectorN_t const & v, std::shared_ptr & constraint, pinocchio::Force & fextLocal) const { @@ -2999,15 +2994,10 @@ namespace jiminy } else { - // Enable fixed frame constraint and reset it if it was disable, - // then move the reference position at the surface of the ground. + // Enable fixed frame constraint if (!constraint->getIsEnabled()) { - constraint->reset(q, v); constraint->enable(); - auto & frameConstraint = static_cast(*constraint.get()); - vector3_t & positionRef = frameConstraint.getReferenceTransform().translation(); - positionRef.noalias() -= depth * nGround; } } } @@ -3026,13 +3016,19 @@ namespace jiminy } } - /* One must update tangential reference position with the current one, + /* Move the reference position at the surface of the ground. + Note that it is must done systematically as long as the constraint is enabled because in case of slippage the contact point has actually moved. */ if (constraint->getIsEnabled()) { auto & frameConstraint = static_cast(*constraint.get()); - vector3_t & positionRef = frameConstraint.getReferenceTransform().translation(); - positionRef = (posFrame + (positionRef - posFrame).dot(nGround) * nGround).eval(); + frameConstraint.setReferenceTransform({ + system.robot->pncData_.oMf[frameIdx].rotation(), + (vector3_t() << posFrame.head<2>(), zGround).finished() + }); + frameConstraint.setLocalFrame( + quaternion_t::FromTwoVectors(vector3_t::UnitZ(), nGround).toRotationMatrix() + ); } } @@ -3196,8 +3192,9 @@ namespace jiminy // Enable fixed joint constraint and reset it if it was disable if (!constraint->getIsEnabled()) { - constraint->reset(q, v); constraint->enable(); + auto & jointConstraint = static_cast(*constraint.get()); + jointConstraint.setReferenceConfiguration(joint.jointConfigSelector(q)); } } else /* if (qJoint < qJointMax - engineOptions_->contacts.transitionEps */ @@ -3395,8 +3392,6 @@ namespace jiminy void EngineMultiRobot::computeCollisionForces(systemHolder_t const & system, systemDataHolder_t & systemData, - vectorN_t const & q, - vectorN_t const & v, forceVector_t & fext) const { // Compute the forces at contact points @@ -3407,7 +3402,7 @@ namespace jiminy frameIndex_t const & frameIdx = contactFramesIdx[i]; auto & constraint = systemData.constraintsHolder.contactFrames[i].second; pinocchio::Force & fextLocal = systemData.contactFramesForces[i]; - computeContactDynamicsAtFrame(system, frameIdx, q, v, constraint, fextLocal); + computeContactDynamicsAtFrame(system, frameIdx, constraint, fextLocal); // Apply the force at the origin of the parent joint frame, in local joint frame jointIndex_t const & parentJointIdx = system.robot->pncModel_.frames[frameIdx].parent; @@ -3432,7 +3427,7 @@ namespace jiminy pairIndex_t const & collisionPairIdx = collisionPairsIdx[i][j]; auto & constraint = systemData.constraintsHolder.collisionBodies[i][j].second; pinocchio::Force & fextLocal = systemData.collisionBodiesForces[i][j]; - computeContactDynamicsAtBody(system, collisionPairIdx, q, v, constraint, fextLocal); + computeContactDynamicsAtBody(system, collisionPairIdx, constraint, fextLocal); // Apply the force at the origin of the parent joint frame, in local joint frame fext[parentJointIdx] += fextLocal; @@ -3555,7 +3550,7 @@ namespace jiminy /* Compute the collision forces and estimated time at which the contact state will changed (Take-off / Touch-down). */ - computeCollisionForces(*systemIt, *systemDataIt, *qIt, *vIt, fext); + computeCollisionForces(*systemIt, *systemDataIt, fext); // Compute the external contact forces. computeExternalForces(*systemIt, *systemDataIt, t, *qIt, *vIt, fext); @@ -3731,7 +3726,8 @@ namespace jiminy for (constraintsHolderType_t holderType : holderTypes) { systemData.constraintsHolder.foreach(holderType, - [&lo, &hi, &fIdx, &constraintIdx, &contactOptions = const_cast(engineOptions_->contacts)]( + [&lo, &hi, &fIdx, &constraintIdx, + &contactOptions = const_cast(engineOptions_->contacts)]( std::shared_ptr const & constraint, constraintsHolderType_t const & /* holderType */) { @@ -3739,16 +3735,18 @@ namespace jiminy { return; } - // auto const & frameConstraint = static_cast(*constraint.get()); - // if (frameConstraint.getIsTranslationFixed()) - // { - // Enforce friction pyramid - hi[constraintIdx] = contactOptions.friction; // Friction along x-axis - fIdx[constraintIdx] = static_cast(constraintIdx + 2); - hi[constraintIdx + 1] = contactOptions.friction; // Friction along y-axis - fIdx[constraintIdx + 1] = static_cast(constraintIdx + 2); - lo[constraintIdx + 2] = 0.0; - // } + + // Enforce tangential friction pyramid and torsional friction + hi[constraintIdx] = contactOptions.friction; // Friction along x-axis + fIdx[constraintIdx] = static_cast(constraintIdx + 2); + hi[constraintIdx + 1] = contactOptions.friction; // Friction along y-axis + fIdx[constraintIdx + 1] = static_cast(constraintIdx + 2); + hi[constraintIdx + 3] = contactOptions.torsion; // Friction around z-axis + fIdx[constraintIdx + 3] = static_cast(constraintIdx + 2); + + // Vertical force cannot be negative + lo[constraintIdx + 2] = 0.0; + constraintIdx += constraint->getDim(); }); } @@ -3783,13 +3781,27 @@ namespace jiminy hi, fIdx); - // Restore contact frame forces and bounds internal efforts + // Update lagrangian multipliers associated with the constraint constraintIdx = 0U; + systemData.constraintsHolder.foreach( + [&lambda_c = const_cast(data.lambda_c), // std::as_const is not supported by gcc<7.3 + &constraintIdx]( + std::shared_ptr const & constraint, + constraintsHolderType_t const & /* holderType */) + { + if (!constraint->getIsEnabled()) + { + return; + } + uint64_t const constraintDim = constraint->getDim(); + constraint->lambda_ = lambda_c.segment(constraintIdx, constraintDim); + constraintIdx += constraintDim; + }); + + // Restore contact frame forces and bounds internal efforts systemData.constraintsHolder.foreach( constraintsHolderType_t::BOUNDS_JOINTS, - [&constraintIdx, - &lambda_c = const_cast(data.lambda_c), // std::as_const is not supported by gcc<7.3 - &u = systemData.state.u, + [&u = systemData.state.u, &uInternal = systemData.state.uInternal, &joints = const_cast(model.joints)]( std::shared_ptr & constraint, @@ -3801,13 +3813,11 @@ namespace jiminy } vectorN_t & uJoint = constraint->lambda_; - uJoint = lambda_c.segment(constraintIdx, constraint->getDim()); auto const & jointConstraint = static_cast(*constraint.get()); auto const & jointModel = joints[jointConstraint.getJointIdx()]; jointModel.jointVelocitySelector(uInternal) += uJoint; jointModel.jointVelocitySelector(u) += uJoint; - constraintIdx += constraint->getDim(); }); constraintIt = systemData.constraintsHolder.contactFrames.begin(); @@ -3821,28 +3831,33 @@ namespace jiminy continue; } auto const & frameConstraint = static_cast(constraint); - // if (frameConstraint.getIsTranslationFixed()) - // { - // Extract part of the lagrangian multipliers associated with the constraint - vectorN_t & fextWorld = constraint.lambda_; - fextWorld = data.lambda_c.segment<3>(constraintIdx); - // Convert the force from local world aligned to local frame - frameIndex_t const & frameIdx = frameConstraint.getFrameIdx(); - pinocchio::SE3 const & transformContactInWorld = data.oMf[frameIdx]; - forceIt->linear().noalias() = transformContactInWorld.rotation().transpose() * fextWorld; + // Extract force in local reference-frame-aligned from lagrangian multipliers + pinocchio::Force fextInLocal( + frameConstraint.lambda_.head<3>(), + frameConstraint.lambda_[3] * vector3_t::UnitZ()); - // Convert the force from local world aligned to local parent joint - jointIndex_t const & jointIdx = model.frames[frameIdx].parent; - fext[jointIdx] += convertForceGlobalFrameToJoint( - model, data, frameIdx, {fextWorld, vector3_t::Zero()}); - // } - constraintIdx += constraint.getDim(); + // Compute force in local world aligned frame + matrix3_t const & rotationLocal = frameConstraint.getLocalFrame(); + pinocchio::Force const fextInWorld({ + rotationLocal * fextInLocal.linear(), + rotationLocal * fextInLocal.angular(), + }); + + // Convert the force from local world aligned frame to local frame + frameIndex_t const & frameIdx = frameConstraint.getFrameIdx(); + auto rotationWorldInContact = data.oMf[frameIdx].rotation().transpose(); + forceIt->linear().noalias() = rotationWorldInContact * fextInWorld.linear(); + forceIt->angular().noalias() = rotationWorldInContact * fextInWorld.angular(); + + // Convert the force from local world aligned to local parent joint + jointIndex_t const & jointIdx = model.frames[frameIdx].parent; + fext[jointIdx] += convertForceGlobalFrameToJoint(model, data, frameIdx, fextInWorld); } systemData.constraintsHolder.foreach( constraintsHolderType_t::COLLISION_BODIES, - [&constraintIdx, &fext, &model, &data]( + [&fext, &model, &data]( std::shared_ptr & constraint, constraintsHolderType_t const & /* holderType */) { @@ -3850,21 +3865,24 @@ namespace jiminy { return; } - - // auto const & collisionConstraint = static_cast(*constraint.get()); - auto const & collisionConstraint = static_cast(*constraint.get()); - - // Extract part of the lagrangian multipliers associated with the constraint - vectorN_t & fextWorld = constraint->lambda_; - fextWorld = data.lambda_c.segment<3>(constraintIdx); + auto const & frameConstraint = static_cast(*constraint.get()); + + // Extract force in world frame from lagrangian multipliers + pinocchio::Force fextInLocal( + frameConstraint.lambda_.head<3>(), + frameConstraint.lambda_[3] * vector3_t::UnitZ()); + + // Compute force in world frame + matrix3_t const & rotationLocal = frameConstraint.getLocalFrame(); + pinocchio::Force const fextInWorld({ + rotationLocal * fextInLocal.linear(), + rotationLocal * fextInLocal.angular(), + }); // Convert the force from local world aligned to local parent joint - frameIndex_t const & frameIdx = collisionConstraint.getFrameIdx(); + frameIndex_t const & frameIdx = frameConstraint.getFrameIdx(); jointIndex_t const & jointIdx = model.frames[frameIdx].parent; - fext[jointIdx] += convertForceGlobalFrameToJoint( - model, data, frameIdx, {fextWorld, vector3_t::Zero()}); - - constraintIdx += constraint->getDim(); + fext[jointIdx] += convertForceGlobalFrameToJoint(model, data, frameIdx, fextInWorld); }); return data.ddq; diff --git a/core/src/robot/Model.cc b/core/src/robot/Model.cc index b48e983dd..b98d06bf6 100644 --- a/core/src/robot/Model.cc +++ b/core/src/robot/Model.cc @@ -613,7 +613,7 @@ namespace jiminy // collisionConstraintsMap.emplace_back(geom.name, std::make_shared( // geom.name, sphere.radius)); collisionConstraintsMap.emplace_back(geom.name, std::make_shared( - geom.name, (Eigen::Matrix() << true, true, true, false, false, false).finished())); + geom.name, (Eigen::Matrix() << true, true, true, false, false, true).finished())); } else { @@ -760,7 +760,7 @@ namespace jiminy for (std::string const & frameName : frameNames) { frameConstraintsMap.emplace_back(frameName, std::make_shared( - frameName, (Eigen::Matrix() << true, true, true, false, false, false).finished())); + frameName, (Eigen::Matrix() << true, true, true, false, false, true).finished())); } returnCode = addConstraints(frameConstraintsMap, constraintsHolderType_t::CONTACT_FRAMES); diff --git a/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py b/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py index 96f4a6818..a4cd3dd3c 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py +++ b/python/gym_jiminy/common/gym_jiminy/common/bases/block_bases.py @@ -316,5 +316,5 @@ def get_fieldnames(self) -> FieldDictNested: :param measure: Observation of the environment. :param action: Target to achieve. - :returns: Action to perform + :returns: Action to perform. """ 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 e85f073ad..986b1a0ee 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 @@ -19,7 +19,7 @@ ContactSensor as contact, ForceSensor as force, ImuSensor as imu) -from jiminy_py.viewer.viewer import DEFAULT_CAMERA_XYZRPY_REL +from jiminy_py.viewer.viewer import DEFAULT_CAMERA_XYZRPY_REL, Viewer from jiminy_py.dynamics import compute_freeflyer_state_from_fixed_body from jiminy_py.simulator import Simulator @@ -564,9 +564,11 @@ def reset(self, # Fallback: Get generic fieldnames otherwise self.logfile_action_headers = get_fieldnames( self.action_space, "action") - register_variables(self.simulator.controller, - self.logfile_action_headers, - self._action) + if self.logfile_action_headers: + # Only register the variable to the telemetry if not empty + register_variables(self.simulator.controller, + self.logfile_action_headers, + self._action) # Sample the initial state and reset the low-level engine qpos, qvel = self._sample_state() @@ -843,7 +845,8 @@ def replay(self, enable_travelling: bool = True, **kwargs: Any) -> None: self.simulator.stop() # Set default camera pose if viewer not already available - if not self.simulator.is_viewer_available and self.robot.has_freeflyer: + if not self.simulator.is_viewer_available and \ + self.robot.has_freeflyer and not Viewer.has_gui(): # Get root frame name. # The first and second frames are respectively "universe" no matter # if the robot has a freeflyer or not, and the second one is the 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 ef8ede40c..c374003d3 100644 --- a/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py +++ b/python/gym_jiminy/common/gym_jiminy/common/utils/helpers.py @@ -1,10 +1,10 @@ """ TODO: Write documentation. """ -from typing import Union, Dict, List, ValuesView +from typing import Union, Dict, List, ValuesView, Tuple, Iterable import numpy as np import numba as nb -from gym import spaces +import gym import jiminy_py.core as jiminy @@ -29,7 +29,7 @@ def is_breakpoint(t: float, dt: float, eps: float) -> bool: return False -def get_fieldnames(space: spaces.Space, +def get_fieldnames(space: gym.spaces.Space, namespace: str = "") -> FieldDictNested: """Get generic fieldnames from `gym.spaces.Space`, so that it can be used in conjunction with `register_variables`, to register any value from gym @@ -40,25 +40,32 @@ def get_fieldnames(space: spaces.Space, Empty string to disable. Optional: Disabled by default. """ - # Fancy action space: Trying to be clever and infer meaningful - # telemetry names based on action space - if isinstance(space, (spaces.Box, spaces.Tuple, spaces.MultiBinary)): - # No information: fallback to basic numbering - return [".".join(filter(None, (namespace, str(i)))) - for i in range(len(space))] - if isinstance(space, ( - spaces.Discrete, spaces.MultiDiscrete)): - # The space is already scalar. Namespace alone as fieldname is enough. + # Scalar scape: namespace alone as fieldname is enough + if isinstance(space, gym.spaces.Discrete): return [namespace] - if isinstance(space, spaces.Dict): - assert space.spaces, "Dict space cannot be empty." + + # Vector space: basic numbering + if isinstance(space, (gym.spaces.Box, gym.spaces.MultiBinary)): + return [".".join(filter(None, (namespace, str(i)))) + for i in range(np.prod(space.shape))] + + # Fancy action spaces: trying to be clever and infer meaningful telemetry + # names based on action space. + if isinstance(space, (gym.spaces.Dict, gym.spaces.Tuple)): + assert space.spaces, "Dict and Tuple spaces cannot be empty." + if isinstance(space, gym.spaces.Tuple): + spaces: Iterable[Tuple[str, gym.spaces.Space]] = ( + (str(i), value) for i, value in enumerate(space.spaces)) + else: + spaces = dict.items(space.spaces) out: List[Union[Dict[str, FieldDictNested], str]] = [] - for field, subspace in dict.items(space.spaces): - if isinstance(subspace, spaces.Dict): + for field, subspace in spaces: + if isinstance(subspace, (gym.spaces.Dict, gym.spaces.Tuple)): out.append({field: get_fieldnames(subspace, namespace)}) else: out.append(field) return out + raise NotImplementedError( f"Gym.Space of type {type(space)} is not supported.") @@ -81,7 +88,7 @@ def register_variables(controller: jiminy.AbstractController, :param field: Nested variable names, as returned by `get_fieldnames` method. It can be a nested list or/and dict. The leaf are str corresponding to the name of each scalar data. - :param data: Data from `Gym.spaces.Space` to register. Note that the + :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.copyto` or `[:]` operator (faster). diff --git a/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py b/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py index dbc2ddd79..e1a2d0044 100644 --- a/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py +++ b/python/gym_jiminy/envs/gym_jiminy/envs/cassie.py @@ -78,6 +78,7 @@ def __init__(self, debug: bool = False, **kwargs): "right_pushrod_hip", "hip_flexion_right", M_pushrod_hip_right) pushrod_right = DistanceConstraint( "right_pushrod_tarsus", "right_pushrod_hip", 0.5012) + pushrod_right.baumgarte_freq = 20.0 self.robot.add_constraint("pushrod_right", pushrod_right) M_pushrod_tarsus_left = SE3( np.eye(3), np.array([-0.12, 0.03, 0.005])) @@ -89,13 +90,16 @@ def __init__(self, debug: bool = False, **kwargs): "left_pushrod_hip", "hip_flexion_left", M_pushrod_hip_left) pushrod_left = DistanceConstraint( "left_pushrod_tarsus", "left_pushrod_hip", 0.5012) + pushrod_left.baumgarte_freq = 20.0 self.robot.add_constraint("pushrod_left", pushrod_left) # Replace knee to shin spring by fixed joint constraint right_spring_knee_to_shin = JointConstraint("knee_to_shin_right") + right_spring_knee_to_shin.baumgarte_freq = 20.0 self.robot.add_constraint( "right_spring_knee_to_shin", right_spring_knee_to_shin) left_spring_knee_to_shin = JointConstraint("knee_to_shin_left") + left_spring_knee_to_shin.baumgarte_freq = 20.0 self.robot.add_constraint( "left_spring_knee_to_shin", left_spring_knee_to_shin) diff --git a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py index daa14dd61..5b282e949 100644 --- a/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py +++ b/python/gym_jiminy/rllib/gym_jiminy/rllib/ppo.py @@ -1,5 +1,6 @@ """ TODO: Write documentation. """ +import math from typing import Dict, List, Type, Union, Optional, Any, Tuple import gym @@ -23,6 +24,9 @@ DEFAULT_CONFIG, { "noise_scale": 1.0, + "temporal_barrier_scale": 1.0, + "temporal_barrier_threshold": math.inf, + "temporal_barrier_reg": 0.0, "symmetric_policy_reg": 0.0, "caps_temporal_reg": 0.0, "caps_spatial_reg": 0.0, @@ -53,6 +57,7 @@ def ppo_init(policy: Policy, policy.view_requirements.update(caps_view_requirements) # Initialize extra loss + policy._mean_temporal_barrier_loss = 0.0 policy._mean_symmetric_policy_loss = 0.0 policy._mean_temporal_caps_loss = 0.0 policy._mean_spatial_caps_loss = 0.0 @@ -113,7 +118,8 @@ def ppo_loss(policy: Policy, # Initialize the set of training batches to forward to the model train_batches = {"original": train_batch} - if policy.config["caps_temporal_reg"] > 0.0: + if policy.config["caps_temporal_reg"] > 0.0 or \ + policy.config["temporal_barrier_reg"] > 0.0: # Shallow copy the original training batch. # Be careful accessing fields using the original batch to properly # keep track of acessed keys, which will be used to discard useless @@ -127,7 +133,8 @@ def ppo_loss(policy: Policy, # Append the training batches to the set train_batches["prev"] = train_batch_copy - if policy._spatial_reg > 0.0 or policy.config["caps_global_reg"] > 0.0: + if policy.config["caps_spatial_reg"] > 0.0 or \ + policy.config["caps_global_reg"] > 0.0: # Shallow copy the original training batch train_batch_copy = train_batch.copy(shallow=True) @@ -229,7 +236,8 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: action_mean_true = action_dist.deterministic_sample() # Compute the mean action corresponding to the previous observation - if policy.config["caps_temporal_reg"] > 0.0: + if policy.config["caps_temporal_reg"] > 0.0 or \ + policy.config["temporal_barrier_reg"] > 0.0: action_logits_prev = logits["prev"] if issubclass(dist_class, TorchDiagGaussian): action_mean_prev, _ = torch.chunk(action_logits_prev, 2, dim=1) @@ -238,7 +246,8 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: action_mean_prev = action_dist_prev.deterministic_sample() # Compute the mean action corresponding to the noisy observation - if policy._spatial_reg > 0.0 or policy.config["caps_global_reg"] > 0.0: + if policy.config["caps_spatial_reg"] > 0.0 or \ + policy.config["caps_global_reg"] > 0.0: action_logits_noisy = logits["noisy"] if issubclass(dist_class, TorchDiagGaussian): action_mean_noisy, _ = torch.chunk(action_logits_noisy, 2, dim=1) @@ -258,16 +267,34 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: policy.action_space.mirror_mat = action_mirror_mat action_mean_mirror = action_mean_mirror @ action_mirror_mat - if policy.config["caps_temporal_reg"] > 0.0: + if policy.config["caps_temporal_reg"] > 0.0 or \ + policy.config["temporal_barrier_reg"] > 0.0: + # Compute action delta + action_delta = (action_mean_prev - action_mean_true).abs() + # Minimize the difference between the successive action mean - policy._mean_temporal_caps_loss = torch.mean( - (action_mean_prev - action_mean_true).abs()) + if policy.config["caps_temporal_reg"] > 0.0: + policy._mean_temporal_caps_loss = torch.mean(action_delta) # Add temporal smoothness loss to total loss total_loss += policy.config["caps_temporal_reg"] * \ policy._mean_temporal_caps_loss - if policy._spatial_reg > 0.0: + # Add temporal barrier loss to total loss: + # exp(scale * (err - thr)) - 1.0 if err > thr else 0.0 + if policy.config["temporal_barrier_reg"] > 0.0: + scale = policy.config["temporal_barrier_scale"] + threshold = policy.config["temporal_barrier_threshold"] + policy._mean_temporal_barrier_loss = torch.mean(torch.exp( + torch.clamp( + scale * (action_delta - threshold), min=0.0, max=5.0 + )) - 1.0) + + # Add spatial smoothness loss to total loss + total_loss += policy.config["temporal_barrier_reg"] * \ + policy._mean_temporal_barrier_loss + + if policy.config["caps_spatial_reg"] > 0.0: # Minimize the difference between the original action mean and the # one corresponding to the noisy observation. policy._mean_spatial_caps_loss = torch.mean( @@ -277,7 +304,8 @@ def value_function(self, *args: Any, **kwargs: Any) -> torch.Tensor: observation_noisy - observation_true) ** 2, dim=-1)) # Add spatial smoothness loss to total loss - total_loss += policy._spatial_reg * policy._mean_spatial_caps_loss + total_loss += policy.config["caps_spatial_reg"] * \ + policy._mean_spatial_caps_loss if policy.config["caps_global_reg"] > 0.0: # Minimize the magnitude of action mean @@ -320,9 +348,11 @@ def ppo_stats(policy: Policy, # Add spatial CAPS loss to the report if policy.config["symmetric_policy_reg"] > 0.0: stats_dict["symmetry"] = policy._mean_symmetric_policy_loss + if policy.config["temporal_barrier_reg"] > 0.0: + stats_dict["temporal_barrier"] = policy._mean_temporal_barrier_loss if policy.config["caps_temporal_reg"] > 0.0: stats_dict["temporal_smoothness"] = policy._mean_temporal_caps_loss - if policy._spatial_reg > 0.0: + if policy.config["caps_spatial_reg"] > 0.0: stats_dict["spatial_smoothness"] = policy._mean_spatial_caps_loss if policy.config["caps_global_reg"] > 0.0: stats_dict["global_smoothness"] = policy._mean_global_caps_loss diff --git a/python/jiminy_py/setup.py b/python/jiminy_py/setup.py index eb22af578..1528f72f3 100644 --- a/python/jiminy_py/setup.py +++ b/python/jiminy_py/setup.py @@ -84,15 +84,18 @@ def finalize_options(self) -> None: # >= 8.0 is required to support Python3.9. "pillow", # Add support of TypedDict to any Python 3 version. - "typing_extensions", + # 3.10.0 adds 'ParamSpec' that is required for pylint>=2.11.1. + "typing_extensions>=3.10.0", # Display elegant and versatile process bar. "tqdm", # Standard library for matrix algebra. "numpy", # Used internally for interpolation and filtering. # No wheel is distributed for PyPy on pypi, and pip is only able to - # build from source after install `libatlas-base-dev` system depdency. - "scipy", + # build from source after install `libatlas-base-dev` system + # dependency. + # 1.2.0 fixes `fmin_slsqp` optimizer returning wrong `imode` ouput. + "scipy>=1.2.0", # Standard library to generate figures. f"matplotlib{matplotlib_spec}", # Used internally to read HDF5 format log files. @@ -115,8 +118,7 @@ def finalize_options(self) -> None: # tile floor. "meshcat>=0.0.19", # Standalone mesh visualizer used as Viewer's backend. - # Panda3d>1.10.9 adds support of Nvidia EGL rendering without X11 - # server. + # 1.10.9 adds support of Nvidia EGL rendering without X11 server. # Panda3d is NOT supported by PyPy and cannot be built from source. "panda3d_viewer", # Used internally by Viewer to record video programmatically when diff --git a/python/jiminy_py/src/jiminy_py/dynamics.py b/python/jiminy_py/src/jiminy_py/dynamics.py index 967b67dbc..8e31712a6 100644 --- a/python/jiminy_py/src/jiminy_py/dynamics.py +++ b/python/jiminy_py/src/jiminy_py/dynamics.py @@ -170,12 +170,12 @@ def update_quantities(robot: jiminy.Model, if update_com: if velocity is None: - pin.centerOfMass(pnc_model, pnc_data, position) + kinematic_level = pin.KinematicLevel.POSITION elif acceleration is None: - pin.centerOfMass(pnc_model, pnc_data, position, velocity) + kinematic_level = pin.KinematicLevel.VELOCITY else: - pin.centerOfMass( - pnc_model, pnc_data, position, velocity, acceleration) + kinematic_level = pin.KinematicLevel.ACCELERATION + pin.centerOfMass(pnc_model, pnc_data, kinematic_level, False) if update_jacobian: if update_com: diff --git a/python/jiminy_py/src/jiminy_py/generator.py b/python/jiminy_py/src/jiminy_py/generator.py new file mode 100644 index 000000000..ba215d28b --- /dev/null +++ b/python/jiminy_py/src/jiminy_py/generator.py @@ -0,0 +1,171 @@ +""" TODO: Write documentation. +""" +import ctypes +from typing import Callable, Union, Tuple + +import numpy as np +import numba as nb +from numba.extending import get_cython_function_address + +from .core import HeatMapFunctor, heatMapType_t + + +murmurhash3_32_addr = get_cython_function_address( + "sklearn.utils.murmurhash", "murmurhash3_int_u32") +murmurhash3_32_functype = ctypes.CFUNCTYPE( + ctypes.c_uint32, ctypes.c_int, ctypes.c_uint) +murmurhash3_32_ptr = murmurhash3_32_functype(murmurhash3_32_addr) + + +@nb.generated_jit(nopython=True) +def murmurhash3_32(key: int, seed: np.uint32) -> Union[int, float]: + """Compute the 32 bit murmurhash3 encoding of key at seed. + + :param key: integer to hash. + :param seed: Unsigned integer seed for the hashing algorithm. + Optional: 0 by default. + + :returns: Unsigned integer corresponding to the hash of the key. + """ + if isinstance(key, nb.types.Array): + def _murmurhash3_32_impl(key, seed): + encoding = 0 + for i, val in enumerate(key): + encoding = murmurhash3_32_ptr(encoding + val, seed + i) + return encoding + elif isinstance(key, nb.types.Integer): + def _murmurhash3_32_impl(key, seed): + return murmurhash3_32_ptr(key, seed) + else: + raise RuntimeError("'key' must have type `int` for `np.ndarray`.") + + return _murmurhash3_32_impl + + +@nb.jit +def _random_height(key: int, + proba_inv: int, + seed: np.uint32) -> Union[int, float]: + """ TODO: Write documentation. + """ + encoding = murmurhash3_32(key, seed) + if encoding % proba_inv == 0: + encoding /= (2 ** 32 - 1) + else: + encoding = 0 + return encoding + + +@nb.jit +def _tile_2d_interp_1d(p_idx: np.ndarray, + p_rel: np.ndarray, + dim: int, + tile_size: np.ndarray, + tile_proba_inv: float, + tile_height_max: float, + tile_interp_threshold: np.ndarray, + seed: np.uint32) -> Tuple[float, float]: + """ TODO: Write documentation. + """ + z = tile_height_max * _random_height(p_idx, tile_proba_inv, seed) + if p_rel[dim] < tile_interp_threshold[dim]: + p_idx = p_idx.copy() + p_idx[dim] -= 1 + z_m = tile_height_max * _random_height(p_idx, tile_proba_inv, seed) + ratio = (1.0 - p_rel[dim] / tile_interp_threshold[dim]) / 2.0 + height = z + (z_m - z) * ratio + dheight = (z - z_m) / ( + 2.0 * tile_size[dim] * tile_interp_threshold[dim]) + elif 1.0 - p_rel[dim] < tile_interp_threshold[dim]: + p_idx = p_idx.copy() + p_idx[dim] += 1 + z_p = tile_height_max * _random_height(p_idx, tile_proba_inv, seed) + ratio = (1.0 + (p_rel[dim] - 1.0) / tile_interp_threshold[dim]) / 2.0 + height = z + (z_p - z) * ratio + dheight = (z_p - z) / ( + 2.0 * tile_size[dim] * tile_interp_threshold[dim]) + else: + height = z + dheight = 0.0 + + return height, dheight + + +def get_random_tile_ground(tile_size: np.ndarray, + tile_proba_inv: float, + tile_height_max: float, + tile_interp_delta: float, + seed: np.uint32) -> Callable[ + [float, float], Tuple[float, np.ndarray]]: + """ TODO: Write documentation. + """ + # Make sure the arguments are valid + assert (0.01 <= tile_interp_delta and np.all( + tile_interp_delta <= tile_size / 2.0)), ( + "'tile_interp_delta' must be in range [0.01, 'tile_size'/2.0].") + + # Compute some proxies + tile_interp_threshold = tile_interp_delta / tile_size + + @nb.jit + def _random_tile_ground_impl(x: float, + y: float, + height: np.ndarray, + normal: np.ndarray) -> None: + nonlocal tile_size, tile_proba_inv, tile_height_max, \ + tile_interp_threshold, seed + + # Compute the tile index and relative coordinate + p = np.array([x, y]) + tile_size / 2 + p_idx = (p // tile_size).astype(np.int32) + p_rel = p / tile_size - p_idx + + # Interpolate height based on nearby tiles if necessary + is_edge = np.logical_or(p_rel < tile_interp_threshold, + 1.0 - p_rel < tile_interp_threshold) + if is_edge[0] and not is_edge[1]: + height[()], dheight_x = _tile_2d_interp_1d( + p_idx, p_rel, 0, tile_size, tile_proba_inv, tile_height_max, + tile_interp_threshold, seed) + dheight_y = 0.0 + elif is_edge[1] and not is_edge[0]: + height[()], dheight_y = _tile_2d_interp_1d( + p_idx, p_rel, 1, tile_size, tile_proba_inv, tile_height_max, + tile_interp_threshold, seed) + dheight_x = 0.0 + elif is_edge[1] and is_edge[0]: + height_0, dheight_x = _tile_2d_interp_1d( + p_idx, p_rel, 0, tile_size, tile_proba_inv, tile_height_max, + tile_interp_threshold, seed) + if p_rel[1] < tile_interp_threshold[1]: + p_idx[1] -= 1 + height_m, dheight_x_m = _tile_2d_interp_1d( + p_idx, p_rel, 0, tile_size, tile_proba_inv, + tile_height_max, tile_interp_threshold, seed) + ratio = (1.0 - p_rel[1] / tile_interp_threshold[1]) / 2.0 + height[()] = height_0 + (height_m - height_0) * ratio + dheight_x = dheight_x + (dheight_x_m - dheight_x) * ratio + dheight_y = (height_0 - height_m) / ( + 2.0 * tile_size[1] * tile_interp_threshold[1]) + else: + p_idx[1] += 1 + height_p, dheight_x_p = _tile_2d_interp_1d( + p_idx, p_rel, 0, tile_size, tile_proba_inv, + tile_height_max, tile_interp_threshold, seed) + ratio = ( + 1.0 + (p_rel[1] - 1.0) / tile_interp_threshold[1]) / 2.0 + height[()] = height_0 + (height_p - height_0) * ratio + dheight_x = dheight_x + (dheight_x_p - dheight_x) * ratio + dheight_y = (height_p - height_0) / ( + 2.0 * tile_size[1] * tile_interp_threshold[1]) + else: + height[()] = tile_height_max * _random_height( + p_idx, tile_proba_inv, seed) + dheight_x, dheight_y = 0.0, 0.0 + + # Compute the resulting normal to the surface + # normal = (-d.height/d.x, -d.height/d.y, 1.0) + normal[:] = -dheight_x, -dheight_y, 1.0 + normal /= np.linalg.norm(normal) + + return HeatMapFunctor(_random_tile_ground_impl, heatMapType_t.GENERIC) diff --git a/python/jiminy_py/src/jiminy_py/robot.py b/python/jiminy_py/src/jiminy_py/robot.py index 4dd872a05..edeb66b53 100644 --- a/python/jiminy_py/src/jiminy_py/robot.py +++ b/python/jiminy_py/src/jiminy_py/robot.py @@ -148,25 +148,32 @@ def generate_hardware_description_file( Sensor=OrderedDict() ) + # Extract the root link. It is the one having no parent at all. + links = set() + for link_descr in root.findall('./link'): + links.add(link_descr.attrib["name"]) + for joint_descr in root.findall('./joint'): + links.remove(joint_descr.find('./child').get('link')) + link_root = next(iter(links)) + # Extract the list of parent and child links, excluding the one related # to fixed link not having collision geometry, because they are likely not # "real" joint. - parent_links = set() - child_links = set() + links_parent = set() + links_child = set() for joint_descr in root.findall('./joint'): parent_link = joint_descr.find('./parent').get('link') child_link = joint_descr.find('./child').get('link') if joint_descr.get('type').casefold() != 'fixed' or root.find( f"./link[@name='{child_link}']/collision") is not None: - parent_links.add(parent_link) - child_links.add(child_link) + links_parent.add(parent_link) + links_child.add(child_link) - # Compute the root link and the leaf ones - if parent_links: - root_link = next(iter(parent_links.difference(child_links))) + # Determine leaf links. If there is no parent, then use root link instead. + if links_parent: + links_leaf = sorted(list(links_child.difference(links_parent))) else: - root_link = None - leaf_links = sorted(list(child_links.difference(parent_links))) + links_leaf = [link_root] # Parse the gazebo plugins, if any. # Note that it is only useful to extract "advanced" hardware, not basic @@ -266,26 +273,26 @@ def generate_hardware_description_file( logger.warning(f"Unsupported Gazebo plugin '{plugin}'") # Add IMU sensor to the root link if no Gazebo IMU sensor has been found - if root_link and imu.type not in hardware_info['Sensor'].keys(): + if link_root and imu.type not in hardware_info['Sensor'].keys(): hardware_info['Sensor'].setdefault(imu.type, {}).update({ - root_link: OrderedDict( - body_name=root_link, + link_root: OrderedDict( + body_name=link_root, frame_pose=6*[0.0]) }) # Add force sensors and collision bodies if no Gazebo plugin is available if not gazebo_plugins_found: - for leaf_link in leaf_links: + for link_leaf in links_leaf: # Add a force sensor hardware_info['Sensor'].setdefault(force.type, {}).update({ - leaf_link: OrderedDict( - body_name=leaf_link, + link_leaf: OrderedDict( + body_name=link_leaf, frame_pose=6*[0.0]) }) # Add the related body to the collision set if possible - if root.find(f"./link[@name='{leaf_link}']/collision") is not None: - collision_bodies_names.add(leaf_link) + if root.find(f"./link[@name='{link_leaf}']/collision") is not None: + collision_bodies_names.add(link_leaf) # Specify collision bodies and ground model in global config options hardware_info['Global']['collisionBodiesNames'] = \ diff --git a/python/jiminy_py/src/jiminy_py/simulator.py b/python/jiminy_py/src/jiminy_py/simulator.py index 75b485b8c..8cc6b1831 100644 --- a/python/jiminy_py/src/jiminy_py/simulator.py +++ b/python/jiminy_py/src/jiminy_py/simulator.py @@ -601,6 +601,12 @@ def replay(self, 'record_video_path', None) is not None, **kwargs}) + # Enable the ground profile if possible + if self.viewer_backend.startswith('panda3d'): + engine_options = self.engine.get_options() + ground_profile = engine_options["world"]["groundProfile"] + self.viewer.update_floor(ground_profile, False) + # Define sequence of viewer instances viewers = [self.viewer, *[None for _ in trajectories[:-1]]] 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 88209d009..835b865ba 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/server.py @@ -138,11 +138,11 @@ def handle_zmq(self, frames: Sequence[bytes]) -> None: if not self.websocket_pool and not self.comm_pool: self.zmq_socket.send(b"") msg = umsgpack.packb({"type": "ready"}) + self.is_waiting_ready_msg = True for websocket in self.websocket_pool: websocket.write_message(msg, binary=True) for comm_id in self.comm_pool: self.forward_to_comm(comm_id, msg) - self.is_waiting_ready_msg = True else: super().handle_zmq(frames) diff --git a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py index 1289f316c..453ea85d0 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py +++ b/python/jiminy_py/src/jiminy_py/viewer/meshcat/wrapper.py @@ -7,7 +7,9 @@ import pathlib import umsgpack import threading +import tornado.gen import tornado.ioloop +from pkg_resources import parse_version as version from contextlib import redirect_stdout, redirect_stderr from typing import Optional, Sequence, Dict, Any @@ -33,15 +35,18 @@ # is tricky to do it properly, so instead every message is process # without distinction. import ipykernel - from pkg_resources import parse_version as version - if version(ipykernel.__version__) >= version("5.0"): - import tornado.gen + ipykernel_version_major = version(ipykernel.__version__).major + if ipykernel_version_major == 5: from ipykernel.kernelbase import SHELL_PRIORITY + elif ipykernel_version_major > 5: + logging.warning( + "ipykernel version 6.X.Y detected. The viewer works optimally " + "with ipykernel 5.X.Y. Revert to old version in case of issues.") else: logging.warning( "Old ipykernel version < 5.0 detected. Please do not schedule " "other cells for execution while the viewer is busy otherwise " - "it will be not executed properly.\nUpdate to a newer version " + "it will be not executed properly. Update to a newer version " "if possible to avoid such limitation.") class CommProcessor: @@ -54,11 +59,10 @@ class CommProcessor: kernel state. This method only processes comm messages to avoid such side effects. """ - def __init__(self): from IPython import get_ipython self.__kernel = get_ipython().kernel - self.__old_api = version(ipykernel.__version__) < version("5.0") + self.__old_api = version(ipykernel.__version__).major < 5 if self.__old_api: logging.warning( "Pre/post kernel handler hooks must be disable for the " @@ -104,9 +108,9 @@ def __call__(self, unsafe: bool = False) -> None: # One must go through all the messages to keep them in order for _ in range(qsize): - priority, t, dispatch, args = \ + *priority, t, dispatch, args = \ self.__kernel.msg_queue.get_nowait() - if priority <= SHELL_PRIORITY: + if not priority or priority[0] <= SHELL_PRIORITY: # New message: reading message without deserializing its # content at this point for efficiency. _, msg = self.__kernel.session.feed_identities( @@ -136,14 +140,19 @@ def __call__(self, unsafe: bool = False) -> None: continue # The message is not related to meshcat comm, so putting it - # back in the queue after lowering its priority so that it is - # send at the "end of the queue", ie just at the right place: - # after the next unchecked messages, after the other messages - # already put back in the queue, but before the next one to go - # the same way. Note that every shell messages have - # SHELL_PRIORITY by default. - self.__kernel.msg_queue.put_nowait( - (SHELL_PRIORITY + 1, t, dispatch, args)) + # back in the queue at the "end of the queue", ie just at the + # right place: after the next unchecked messages, after the + # other messages already put back in the queue, but before the + # next one to go the same way. + # Note that its priority is also lowered, so that the next time + # it can be directly forwarded without analyzing its content, + # since every shell messages have SHELL_PRIORITY by default. + # Note that ipykernel 6 removed priority feature. + if priority: + self.__kernel.msg_queue.put_nowait( + (SHELL_PRIORITY + 1, t, dispatch, args)) + else: + self.__kernel.msg_queue.put_nowait((t, dispatch, args)) self.qsize_old = self.__kernel.msg_queue.qsize() # Ensure the eventloop wakes up 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 bdcd8559b..f2a63aec2 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 @@ -4,7 +4,6 @@ import sys import math import array -import pickle import warnings import xml.etree.ElementTree as ET from datetime import datetime @@ -17,15 +16,15 @@ from matplotlib.patches import Patch from panda3d.core import ( - NodePath, Point3, Vec3, Mat4, Quat, LQuaternion, Geom, GeomEnums, GeomNode, - GeomVertexData, GeomTriangles, GeomVertexArrayFormat, GeomVertexFormat, - GeomVertexWriter, CullFaceAttrib, GraphicsWindow, PNMImage, InternalName, - OmniBoundingVolume, CompassEffect, BillboardEffect, Filename, TextNode, - Texture, TextureStage, PNMImageHeader, PGTop, Camera, PerspectiveLens, - TransparencyAttrib, OrthographicLens, ClockObject, GraphicsPipe, - WindowProperties, FrameBufferProperties, loadPrcFileData, AntialiasAttrib, - CollisionNode, CollisionRay, CollisionTraverser, CollisionHandlerQueue, - RenderModeAttrib) + NodePath, Point3, Vec3, Vec4, Mat4, Quat, LQuaternion, Material, Geom, + GeomEnums, GeomNode, GeomVertexData, GeomTriangles, GeomVertexArrayFormat, + GeomVertexFormat, GeomVertexWriter, CullFaceAttrib, GraphicsWindow, + PNMImage, InternalName, OmniBoundingVolume, CompassEffect, BillboardEffect, + Filename, TextNode, Texture, TextureStage, PNMImageHeader, PGTop, Camera, + PerspectiveLens, TransparencyAttrib, OrthographicLens, ClockObject, + GraphicsPipe, WindowProperties, FrameBufferProperties, loadPrcFileData, + AntialiasAttrib, CollisionNode, CollisionRay, CollisionTraverser, + CollisionHandlerQueue, RenderModeAttrib) from direct.showbase.ShowBase import ShowBase from direct.gui.OnscreenImage import OnscreenImage from direct.gui.OnscreenText import OnscreenText @@ -214,41 +213,34 @@ def make_cone(num_sides: int = 16) -> Geom: return geom -def make_height_map(height_map: Callable[ - [np.ndarray], Tuple[float, np.ndarray]], - grid_size: float, - grid_unit: float) -> Geom: +def make_height_map(height_map: np.ndarray) -> Geom: """Create height map. """ - # Compute grid size and number of vertices - grid_dim = int(np.ceil(grid_size / grid_unit)) + 1 - num_vertices = grid_dim ** 2 + # Compute the number of vertices + num_vertices = int(np.prod(height_map.shape[:2])) # Define vertex format - vformat = GeomVertexFormat.get_v3n3t2() + vformat = GeomVertexFormat.get_v3n3() vdata = GeomVertexData('vdata', vformat, Geom.UH_static) vdata.uncleanSetNumRows(num_vertices) vertex = GeomVertexWriter(vdata, 'vertex') normal = GeomVertexWriter(vdata, 'normal') - tcoord = GeomVertexWriter(vdata, 'texcoord') # # Add grid points - for x in np.arange(grid_dim) * grid_unit - grid_size / 2.0: - for y in np.arange(grid_dim) * grid_unit - grid_size / 2.0: - height, normal_i = height_map(np.array([x, y, 0.0])) - vertex.addData3(x, y, height) - normal.addData3(*normal_i) - tcoord.addData2(x, y) + for i in range(height_map.shape[0]): + for j in range(height_map.shape[1]): + vertex.addData3(*height_map[i, j][:3]) + normal.addData3(*height_map[i, j][3:]) # Make triangles prim = GeomTriangles(Geom.UH_static) - for j in range(grid_dim): - for i in range(grid_dim - 1): - k = j * grid_dim + i - if j < grid_dim - 1: - prim.add_vertices(k + 1, k, k + grid_dim) + for j in range(height_map.shape[1]): + for i in range(height_map.shape[0] - 1): + k = j * height_map.shape[0] + i + if j < height_map.shape[1] - 1: + prim.add_vertices(k + 1, k, k + height_map.shape[0]) if j > 0: - prim.add_vertices(k, k + 1, k + 1 - grid_dim) + prim.add_vertices(k, k + 1, k + 1 - height_map.shape[0]) # Create geometry object geom = Geom(vdata) @@ -734,9 +726,8 @@ def _make_axes(self) -> NodePath: return node def _make_floor(self, - height_map: Optional[Callable[ - [np.ndarray], Tuple[float, np.ndarray]]] = None, - grid_unit: float = 0.2) -> NodePath: + height_map: Optional[np.ndarray] = None, + show_mesh: bool = False) -> NodePath: model = GeomNode('floor') node = self.render.attach_new_node(model) @@ -748,32 +739,47 @@ def _make_floor(self, tile_path = node.attach_new_node(tile) tile_path.set_pos((xi, yi, 0.0)) if (xi + yi) % 2: - tile_path.set_color((0.95, 0.95, 1.0, 1)) + tile_path.set_color((0.95, 0.95, 1.0, 1.0)) else: - tile_path.set_color((0.13, 0.13, 0.2, 1)) + tile_path.set_color((0.13, 0.13, 0.2, 1.0)) else: - model.add_geom(make_height_map(height_map, 20.0, grid_unit)) - render_attrib = node.get_state().get_attrib_def( - RenderModeAttrib.get_class_slot()) - node.set_attrib(RenderModeAttrib.make( - RenderModeAttrib.M_filled_wireframe, - 0.5, # thickness - render_attrib.perspective, - (0.7, 0.7, 0.7, 1.0) # wireframe_color - )) + model.add_geom(make_height_map(height_map)) + material = Material() + material.set_ambient(Vec4(0.95, 0.95, 1.0, 1.0)) + material.set_diffuse(Vec4(0.95, 0.95, 1.0, 1.0)) + material.set_specular(Vec3(1.0, 1.0, 1.0)) + material.set_roughness(0.4) + node.set_material(material, 1) + # node.set_color((0.95, 0.95, 1.0, 1.0)) + if show_mesh: + render_attrib = node.get_state().get_attrib_def( + RenderModeAttrib.get_class_slot()) + node.set_attrib(RenderModeAttrib.make( + RenderModeAttrib.M_filled_wireframe, + 0.5, # thickness + render_attrib.perspective, + (0.7, 0.7, 0.7, 1.0) # wireframe_color + )) node.set_two_sided(True) return node def update_floor(self, - height_map: Optional[Callable[ - [np.ndarray], Tuple[float, np.ndarray]]] = None, - grid_unit: float = 0.2) -> NodePath: - if height_map is not None and not callable(height_map): - height_map = pickle.loads(height_map) + height_map: Optional[np.ndarray] = None, + show_mesh: bool = False) -> NodePath: + """Update the floor. + + :param height_map: Height map of the ground, as a 3D nd.array of shape + [N_X, N_Y, 6], where N_X, N_Y are the number of + vertices on x and y axes respectively, while the + last dimension corresponds to the position (x, y, z) + and normal (n_x, n_y, nz) of the vertex in space. It + renders a flat tile ground if not specified. + Optional: None by default. + """ self._floor.remove_node() - self._floor = self._make_floor(height_map, grid_unit) + self._floor = self._make_floor(height_map, show_mesh) def append_group(self, root_path: str, diff --git a/python/jiminy_py/src/jiminy_py/viewer/viewer.py b/python/jiminy_py/src/jiminy_py/viewer/viewer.py index 5fae1bd07..441310f2f 100644 --- a/python/jiminy_py/src/jiminy_py/viewer/viewer.py +++ b/python/jiminy_py/src/jiminy_py/viewer/viewer.py @@ -38,7 +38,7 @@ from pinocchio.visualize import GepettoVisualizer from .. import core as jiminy -from ..core import ContactSensor as contact +from ..core import ContactSensor as contact, HeatMapFunctor from ..state import State from ..dynamics import XYZQuatToXYZRPY from .meshcat.utilities import interactive_mode @@ -309,7 +309,8 @@ def __init__(self, scene_name: str = 'world', display_com: bool = False, display_dcm: bool = False, - display_contacts: bool = False, + display_contact_frames: bool = False, + display_contact_forces: bool = False, display_f_external: Optional[ Union[Sequence[bool], bool]] = None, **kwargs: Any): @@ -356,11 +357,14 @@ def __init__(self, Optional: Disabled by default. :param display_dcm: Whether or not to display the capture point / DCM. Optional: Disabled 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 - computing kinematic quantities. - Optional: Disabled by default. + :param display_contact_frames: + Whether or not to display the contact frames. + Optional: Disabled by default. + :param display_contact_forces: + 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. + Optional: Disabled 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 @@ -379,11 +383,15 @@ def __init__(self, # Make sure user arguments are valid if not Viewer.backend.startswith('panda3d'): - if display_com or display_dcm or display_contacts: + if display_com or display_dcm or display_contact_frames or \ + display_contact_forces: logger.warning( "Panda3d backend is required to display markers, e.g. " "CoM, DCM or Contact.") - display_com, display_dcm, display_contacts = False, False, False + display_com = False + display_dcm = False + display_contact_frames = False + display_contact_forces = False # Backup some user arguments self.robot_color = get_color_code(robot_color) @@ -394,7 +402,8 @@ def __init__(self, self._lock = lock or Viewer._lock self._display_com = display_com self._display_dcm = display_dcm - self._display_contacts = display_contacts + self._display_contact_frames = display_contact_frames + self._display_contact_forces = display_contact_forces self._display_f_external = display_f_external # Initialize marker register @@ -715,6 +724,20 @@ def get_dcm_scale() -> Tuple3FType: self.display_capture_point(self._display_dcm) + # Add contact frame markers + for frame_name, frame_idx in zip( + robot.contact_frames_names, robot.contact_frames_idx): + frame_pose = robot.pinocchio_data.oMf[frame_idx] + self.add_marker(name='_'.join(("ContactFrame", frame_name)), + shape="sphere", + color="yellow", + pose=[frame_pose.translation, None], + remove_if_exists=True, + auto_refresh=False, + radius=0.02) + + self.display_contact_frames(self._display_contact_frames) + # Add contact sensor markers def get_contact_pose( sensor: contact) -> Tuple[Tuple3FType, Tuple4FType]: @@ -740,7 +763,7 @@ def get_contact_scale(sensor: contact) -> Tuple3FType: length=0.5, anchor_bottom=True) - self.display_contact_forces(self._display_contacts) + self.display_contact_forces(self._display_contact_forces) # Add external forces def get_force_pose( @@ -1698,6 +1721,39 @@ def set_color(self, else: logger.warning("This method is only supported by Panda3d.") + @__must_be_open + @__with_lock + def update_floor(self, + height_map: Optional[HeatMapFunctor] = None, + show_mesh: bool = False, + grid_size: float = 20.0, + grid_unit: float = 0.05) -> None: + """Display a custom ground profile as a height map or the original tile + ground floor. + + .. note:: + This method is only supported by Panda3d for now. + + :param height_map: `jiminy_py.core.HeatMapFunctor` associated with the + ground profile. It renders a flat tile ground if + not specified. + Optional: None by default. + """ + if Viewer.backend.startswith('panda3d'): + # Generate discrete grid + grid_dim = int(np.ceil(grid_size / grid_unit)) + 1 + height_grid = np.empty((grid_dim, grid_dim, 6)) + height_grid[..., 0], height_grid[..., 1] = np.meshgrid( + *(2 * (np.arange(grid_dim) * grid_unit - grid_size / 2.0,)), + copy=False) + for i in range(grid_dim): + for j in range(grid_dim): + height_grid[i, j][2], height_grid[i, j][3:] = height_map( + height_grid[i, j][:3]) + self._gui.update_floor(height_grid, show_mesh) + else: + logger.warning("This method is only supported by Panda3d.") + @__must_be_open @__with_lock def capture_frame(self, @@ -1944,7 +2000,7 @@ def display_center_of_mass(self, visibility: bool) -> None: @__with_lock 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. + component of motion (DCM) as a sphere. .. note:: Calling `Viewer.display` will update it automatically, while @@ -1969,11 +2025,43 @@ def display_capture_point(self, visibility: bool) -> None: if visibility: self.refresh() + @__must_be_open + @__with_lock + def display_contact_frames(self, visibility: bool) -> None: + """Display the contact frames of the robot as spheres. + + .. note:: + The frames to display are specified by the attribute + `contact_frames_names` of the provided `robot`. Calling + `Viewer.display` will update it automatically, while + `Viewer.refresh` will not. + + .. warning:: + This method is only supported by Panda3d. + + :param visibility: Whether or not to display the contact frames. + """ + # 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("ContactFrame"): + self._gui.show_node(self._markers_group, name, visibility) + self._markers_visibility[name] = visibility + self._display_contact_frames = visibility + + # Must refresh the scene + if visibility: + self.refresh() + @__must_be_open @__with_lock def display_contact_forces(self, visibility: bool) -> None: """Display forces associated with the contact sensors attached to the - robot, as a capsule of variable length depending of Fz. + robot, as cylinders of variable length depending of Fz. .. note:: Fz can be signed. It will affect the orientation of the capsule. @@ -1998,7 +2086,7 @@ def display_contact_forces(self, visibility: bool) -> None: if name.startswith(contact.type): self._gui.show_node(self._markers_group, name, visibility) self._markers_visibility[name] = visibility - self._display_contacts = visibility + self._display_contact_forces = visibility # Must refresh the scene if visibility: @@ -2009,8 +2097,8 @@ def display_contact_forces(self, visibility: bool) -> None: 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. + """Display external forces applied on the joints the robot, as arrows + of variable length depending of magnitude of the force. .. warning:: It only display the linear component of the force, while ignoring @@ -2260,9 +2348,9 @@ def replay(self, :param wait: Whether or not to wait for rendering to finish. """ # Disable display of sensor data if no update hook is provided - disable_display_contacts = False - if update_hook is None and self._display_contacts: - disable_display_contacts = True + disable_display_contact_forces = False + if update_hook is None and self._display_contact_forces: + disable_display_contact_forces = True self.display_contact_forces(False) # Disable display of DCM if no velocity data provided @@ -2341,7 +2429,7 @@ def replay(self, Viewer.set_clock() # Restore display if necessary - if disable_display_contacts: + if disable_display_contact_forces: self.display_contact_forces(True) if disable_display_dcm: self.display_capture_point(True) diff --git a/python/jiminy_pywrap/include/jiminy/python/Utilities.h b/python/jiminy_pywrap/include/jiminy/python/Utilities.h index 2c56bec09..af171cfb4 100644 --- a/python/jiminy_pywrap/include/jiminy/python/Utilities.h +++ b/python/jiminy_pywrap/include/jiminy/python/Utilities.h @@ -167,8 +167,8 @@ namespace python return array; } - template - PyObject * getNumpyReferenceFromEigenMatrix(Eigen::Matrix & value) + template + PyObject * getNumpyReferenceFromEigenMatrix(Eigen::Matrix & value) { npy_intp dims[2] = {npy_intp(value.cols()), npy_intp(value.rows())}; PyObject * array = PyArray_SimpleNewFromData(2, dims, getPyType(*value.data()), const_cast(value.data())); @@ -177,8 +177,8 @@ namespace python return arrayT; } - template - PyObject * getNumpyReferenceFromEigenMatrix(Eigen::Ref > & value) + template + PyObject * getNumpyReferenceFromEigenMatrix(Eigen::Ref > & value) { npy_intp dims[2] = {npy_intp(value.cols()), npy_intp(value.rows())}; PyObject * array = PyArray_SimpleNewFromData(2, dims, getPyType(*value.data()), value.data()); @@ -187,11 +187,11 @@ namespace python return arrayT; } - template - PyObject * getNumpyReferenceFromEigenMatrix(Eigen::Matrix const & value) + template + PyObject * getNumpyReferenceFromEigenMatrix(Eigen::Matrix const & value) { PyObject * array = getNumpyReferenceFromEigenMatrix( - const_cast &>(value)); + const_cast &>(value)); PyArray_CLEARFLAGS(reinterpret_cast(array), NPY_ARRAY_WRITEABLE); return array; } diff --git a/python/jiminy_pywrap/src/Constraints.cc b/python/jiminy_pywrap/src/Constraints.cc index e4cd19da9..775d8fe14 100644 --- a/python/jiminy_pywrap/src/Constraints.cc +++ b/python/jiminy_pywrap/src/Constraints.cc @@ -81,8 +81,7 @@ namespace python } static std::shared_ptr fixedFrameConstraintFactory(std::string const & frameName, - bp::object const & maskFixedPy, - pinocchio::ReferenceFrame const & frameRef) + bp::object const & maskFixedPy) { Eigen::Matrix maskFixed; if (maskFixedPy.is_none()) @@ -102,7 +101,7 @@ namespace python maskFixed[i] = maskFixedListPyExtract(); } } - return std::make_shared(frameName, maskFixed, frameRef); + return std::make_shared(frameName, maskFixed); } static void setIsEnable(AbstractConstraintBase & self, @@ -118,11 +117,6 @@ namespace python } } - static void setReferenceConfiguration(JointConstraint & self, vectorN_t const & value) - { - self.setReferenceConfiguration(value); - } - public: /////////////////////////////////////////////////////////////////////////////// /// \brief Expose. @@ -156,15 +150,14 @@ namespace python bp::return_value_policy())) .add_property("reference_configuration", bp::make_function(&JointConstraint::getReferenceConfiguration, bp::return_value_policy >()), - &PyConstraintVisitor::setReferenceConfiguration); + &JointConstraint::setReferenceConfiguration); bp::class_, std::shared_ptr, boost::noncopyable>("FixedFrameConstraint", bp::no_init) .def("__init__", bp::make_constructor(&PyConstraintVisitor::fixedFrameConstraintFactory, bp::default_call_policies(), (bp::arg("frame_name"), - bp::arg("mask_fixed")=bp::object(), - bp::arg("reference_frame")=pinocchio::LOCAL_WORLD_ALIGNED))) + bp::arg("mask_fixed")=bp::object()))) .def_readonly("type", &FixedFrameConstraint::type_) .add_property("frame_name", bp::make_function(&FixedFrameConstraint::getFrameName, bp::return_value_policy())) @@ -172,11 +165,12 @@ namespace python bp::return_value_policy())) .add_property("dofs_fixed", bp::make_function(&FixedFrameConstraint::getDofsFixed, bp::return_value_policy())) - .add_property("reference_frame", bp::make_function(&FixedFrameConstraint::getReferenceFrame, - bp::return_value_policy())) .add_property("reference_transform", bp::make_function(&FixedFrameConstraint::getReferenceTransform, bp::return_internal_reference<>()), - &FixedFrameConstraint::setReferenceTransform); + &FixedFrameConstraint::setReferenceTransform) + .add_property("local_rotation", bp::make_function(&FixedFrameConstraint::getLocalFrame, + bp::return_value_policy >()), + &FixedFrameConstraint::setLocalFrame); bp::class_, std::shared_ptr, diff --git a/unit_py/data/box_collision_mesh.urdf b/unit_py/data/box_collision_mesh.urdf index 28468307e..8c64f0268 100644 --- a/unit_py/data/box_collision_mesh.urdf +++ b/unit_py/data/box_collision_mesh.urdf @@ -15,6 +15,9 @@ The force sensor is mounted with an offset rotation to verify frame computations + + + diff --git a/unit_py/data/sphere_primitive.urdf b/unit_py/data/sphere_primitive.urdf index 7496d66da..4569f5ef1 100644 --- a/unit_py/data/sphere_primitive.urdf +++ b/unit_py/data/sphere_primitive.urdf @@ -15,6 +15,9 @@ The force sensor is mounted with an offset rotation to verify frame computations + + +