[jiminy/viewer] Panda3d has default viewer backend. (#290)

* [core] Minor improvement of robot weak_ptr management for sensors/motors. * [core] Various minor improvements. * [jiminy/simulator] Fix viewer not opening graphical window after closing it once. * [jiminy/simulator] Rename 'disable_flexiblity_data' or 'enable_flexiblity_data' for clarity. * [jiminy/viewer] Fix widgets positioning and scaling on Panda3d. * [jiminy/viewer] Do not shrink legend automatically and slightly increase size on Panda3d. * [jiminy/viewer] Fallback to software rendering with panda3d if necessary. * [jiminy/viewer] Fix panda3d mesh loader backend 'assimp' requiring specific path formatting on Windows. * [jiminy/viewer] Limit Panda3d framerate to 30FPS to avoid consuming too much resources. * [jiminy/viewer] Improve exception handling if window clsoed during replay to avoid deadlock. * [jiminy/viewer] Fix panda3d offscreen rendering not always updated before capturing frame. * [jiminy/viewer] Zoom out default relative camera pose for travelling. * [gym/common/envs] Automatically update rendering after 'reset' if available. * [gym/common/envs] Fix keyword arguments forwarding for WalkerEnvs class. * [gym/examples] Rllib 'test' method now returns the environement used for testing. * [misc] Enable 'panda3d' by default instead of 'meshcat'. * [misc] Do not remove Pinocchio::interpolate from Windows build anymore. * [misc] Globally less restrictive Python requirements by using different specifications depending on the platform. * [misc] Mirror type check warning fixes. * [misc] Update atlas URDF and meshes to improve rendering. * [misc] More robust viewer unit tests comparing frame to both software and hardware rendering. Co-authored-by: Alexis Duburcq <[email protected]>
duburcqa · Mar 10, 2021 · 624de49 · 624de49
1 parent b86cf5e
commit 624de49
Show file tree

Hide file tree

Showing 101 changed files with 113,376 additions and 46,651 deletions.
diff --git 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.2)
+set(BUILD_VERSION 1.6.3)
 
 # Add definition of Jiminy version for C++ headers
 add_definitions("-DJIMINY_VERSION=\"${BUILD_VERSION}\"")

diff --git a/build_tools/docs/python_docstring_substitution.py b/build_tools/docs/python_docstring_substitution.py
@@ -3,7 +3,7 @@
 import sys
 import pathlib
 from queue import deque
-from textwrap import indent, dedent
+from textwrap import indent
 from typing import Sequence
 
 

diff --git a/build_tools/patch_deps_windows/pinocchio.patch b/build_tools/patch_deps_windows/pinocchio.patch
@@ -115,7 +115,7 @@ diff --git a/bindings/python/algorithm/expose-joints.cpp b/bindings/python/algor
 index 9d72c12a..41aa429f 100644
 --- a/bindings/python/algorithm/expose-joints.cpp
 +++ b/bindings/python/algorithm/expose-joints.cpp
-@@ -24,112 +24,11 @@ namespace pinocchio
+@@ -24,91 +24,11 @@
      {
        return randomConfiguration(model);
      }
@@ -204,17 +204,13 @@ index 9d72c12a..41aa429f 100644
 -              "\tq: the joint configuration vector (size model.nq)\n"
 -              "\tv: the joint velocity vector (size model.nv)\n"
 -              "\targument_position: either pinocchio.ArgumentPosition.ARG0 or pinocchio.ArgumentPosition.ARG1, depending on the desired Jacobian value.\n");
--
--      bp::def("interpolate",
--              &interpolate<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
--              bp::args("model","q1","q2","alpha"),
--              "Interpolate between two given joint configuration vectors q1 and q2.\n\n"
--              "Parameters:\n"
--              "\tmodel: model of the kinematic tree\n"
--              "\tq1: the initial joint configuration vector (size model.nq)\n"
--              "\tq2: the terminal joint configuration vector (size model.nq)\n"
--              "\talpha: the interpolation coefficient in [0,1]\n");
--      
+
+       bp::def("interpolate",
+               &interpolate<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
+@@ -120,16 +40,6 @@
+               "\tq2: the terminal joint configuration vector (size model.nq)\n"
+               "\talpha: the interpolation coefficient in [0,1]\n");
+
 -      bp::def("difference",
 -              &difference<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
 -              bp::args("model","q1","q2"),
@@ -228,7 +224,7 @@ index 9d72c12a..41aa429f 100644
        bp::def("squaredDistance",
                &squaredDistance<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
                bp::args("model","q1","q2"),
-@@ -148,27 +47,6 @@ namespace pinocchio
+@@ -148,27 +58,6 @@
                "\tq1: the initial joint configuration vector (size model.nq)\n"
                "\tq2: the terminal joint configuration vector (size model.nq)\n");
 

diff --git a/core/include/jiminy/core/robot/AbstractSensor.tpp b/core/include/jiminy/core/robot/AbstractSensor.tpp
@@ -77,47 +77,44 @@ namespace jiminy
             return hresult_t::ERROR_GENERIC;
         }
 
-        if (!robot_.expired())
+        // Remove associated col in the shared data buffers
+        if (sensorIdx_ < sharedHolder_->num_ - 1)
         {
-            // Remove associated col in the shared data buffers
-            if (sensorIdx_ < sharedHolder_->num_ - 1)
-            {
-                int32_t sensorShift = sharedHolder_->num_ - sensorIdx_ - 1;
-                for (matrixN_t & data : sharedHolder_->data_)
-                {
-                    data.middleCols(sensorIdx_, sensorShift) =
-                        data.middleCols(sensorIdx_ + 1, sensorShift).eval();
-                }
-                sharedHolder_->dataMeasured_.middleCols(sensorIdx_, sensorShift) =
-                    sharedHolder_->dataMeasured_.middleCols(sensorIdx_ + 1, sensorShift).eval();
-            }
+            int32_t sensorShift = sharedHolder_->num_ - sensorIdx_ - 1;
             for (matrixN_t & data : sharedHolder_->data_)
             {
-                data.conservativeResize(Eigen::NoChange, sharedHolder_->num_ - 1);
+                data.middleCols(sensorIdx_, sensorShift) =
+                    data.middleCols(sensorIdx_ + 1, sensorShift).eval();
             }
-            sharedHolder_->dataMeasured_.conservativeResize(Eigen::NoChange, sharedHolder_->num_ - 1);
+            sharedHolder_->dataMeasured_.middleCols(sensorIdx_, sensorShift) =
+                sharedHolder_->dataMeasured_.middleCols(sensorIdx_ + 1, sensorShift).eval();
+        }
+        for (matrixN_t & data : sharedHolder_->data_)
+        {
+            data.conservativeResize(Eigen::NoChange, sharedHolder_->num_ - 1);
+        }
+        sharedHolder_->dataMeasured_.conservativeResize(Eigen::NoChange, sharedHolder_->num_ - 1);
 
-            // Shift the sensor indices
-            for (int32_t i = sensorIdx_ + 1; i < sharedHolder_->num_; ++i)
-            {
-                AbstractSensorTpl<T> * sensor =
-                    static_cast<AbstractSensorTpl<T> *>(sharedHolder_->sensors_[i]);
-                --sensor->sensorIdx_;
-            }
+        // Shift the sensor indices
+        for (int32_t i = sensorIdx_ + 1; i < sharedHolder_->num_; ++i)
+        {
+            AbstractSensorTpl<T> * sensor =
+                static_cast<AbstractSensorTpl<T> *>(sharedHolder_->sensors_[i]);
+            --sensor->sensorIdx_;
+        }
 
-            // Remove the sensor from the shared memory
-            sharedHolder_->sensors_.erase(sharedHolder_->sensors_.begin() + sensorIdx_);
-            --sharedHolder_->num_;
+        // Remove the sensor from the shared memory
+        sharedHolder_->sensors_.erase(sharedHolder_->sensors_.begin() + sensorIdx_);
+        --sharedHolder_->num_;
 
-            // Update delayMax_ proxy
-            if (sharedHolder_->delayMax_ < baseSensorOptions_->delay + EPS)
+        // Update delayMax_ proxy
+        if (sharedHolder_->delayMax_ < baseSensorOptions_->delay + EPS)
+        {
+            sharedHolder_->delayMax_ = 0.0;
+            for (AbstractSensorBase * sensor : sharedHolder_->sensors_)
             {
-                sharedHolder_->delayMax_ = 0.0;
-                for (AbstractSensorBase * sensor : sharedHolder_->sensors_)
-                {
-                    sharedHolder_->delayMax_ = std::max(sharedHolder_->delayMax_,
-                                                        sensor->baseSensorOptions_->delay);
-                }
+                sharedHolder_->delayMax_ = std::max(sharedHolder_->delayMax_,
+                                                    sensor->baseSensorOptions_->delay);
             }
         }
 

diff --git a/core/src/Utilities.cc b/core/src/Utilities.cc
@@ -1335,6 +1335,7 @@ namespace jiminy
         // Update child joint previousFrame index
         modelInOut.frames[childFrameIdx].parent = newJointIdx;
         modelInOut.frames[childFrameIdx].previousFrame = newFrameIdx;
+        modelInOut.frames[childFrameIdx].placement = SE3::Identity();
 
         // Update new joint subtree to include all the joints below it
         for (uint32_t i = 0; i < modelInOut.subtrees[childJointIdx].size(); ++i)

diff --git a/core/src/engine/EngineMultiRobot.cc b/core/src/engine/EngineMultiRobot.cc
@@ -3404,8 +3404,8 @@ namespace jiminy
             auto constraintsDrift = system.robot->getConstraintsDrift();
 
             // Compute constraints bounds
-            lo = vectorN_t::Constant(constraintsDrift.size(),- INF);
-            hi = vectorN_t::Constant(constraintsDrift.size(), INF);
+            lo = vectorN_t::Constant(constraintsDrift.size(), -INF);
+            hi = vectorN_t::Constant(constraintsDrift.size(), +INF);
             fIdx = std::vector<int32_t>(constraintsDrift.size(), -1);
 
             uint32_t jointsIdx = 0U;
@@ -3492,85 +3492,83 @@ namespace jiminy
                                                  fIdx);
 
             // Restore contact frame forces and bounds internal efforts
-            if (contactModel_ == contactModel_t::IMPULSE)
-            {
-                uint32_t constraintIdx = 0U;
-                systemData.constraintsHolder.foreach(
-                    constraintsHolderType_t::BOUNDS_JOINTS,
-                    [&constraintIdx,
-                     &lambda_c = std::as_const(data.lambda_c),
-                     &u = systemData.state.u,
-                     &uInternal = systemData.state.uInternal,
-                     &joints = model.joints](
-                        std::shared_ptr<AbstractConstraintBase> const & constraint,
-                        constraintsHolderType_t const & /* holderType */)
+            uint32_t constraintIdx = 0U;
+            systemData.constraintsHolder.foreach(
+                constraintsHolderType_t::BOUNDS_JOINTS,
+                [&constraintIdx,
+                    &lambda_c = std::as_const(data.lambda_c),
+                    &u = systemData.state.u,
+                    &uInternal = systemData.state.uInternal,
+                    &joints = model.joints](
+                    std::shared_ptr<AbstractConstraintBase> const & constraint,
+                    constraintsHolderType_t const & /* holderType */)
+                {
+                    if (!constraint->getIsEnabled())
                     {
-                        if (!constraint->getIsEnabled())
-                        {
-                            return;
-                        }
-                        auto const & jointConstraint = static_cast<JointConstraint const &>(*constraint.get());
-                        auto const & jointModel = joints[jointConstraint.getJointIdx()];
-                        auto uJoint = lambda_c.segment(constraintIdx, constraint->getDim());  // auto to avoid memory allocation
-                        jointModel.jointVelocitySelector(uInternal) += uJoint;
-                        jointModel.jointVelocitySelector(u) += uJoint;
-                        constraintIdx += constraint->getDim();
-                    });
+                        return;
+                    }
+                    auto const & jointConstraint = static_cast<JointConstraint const &>(*constraint.get());
+                    auto const & jointModel = joints[jointConstraint.getJointIdx()];
+                    auto uJoint = lambda_c.segment(constraintIdx, constraint->getDim());  // auto to avoid memory allocation
+                    jointModel.jointVelocitySelector(uInternal) += uJoint;
+                    jointModel.jointVelocitySelector(u) += uJoint;
+                    constraintIdx += constraint->getDim();
+                });
 
-                constraintIt = systemData.constraintsHolder.contactFrames.begin();
-                auto forceIt = system.robot->contactForces_.begin();
-                for ( ; constraintIt != systemData.constraintsHolder.contactFrames.end() ;
-                    ++constraintIt, ++forceIt)
+            constraintIt = systemData.constraintsHolder.contactFrames.begin();
+            auto forceIt = system.robot->contactForces_.begin();
+            for ( ; constraintIt != systemData.constraintsHolder.contactFrames.end() ;
+                ++constraintIt, ++forceIt)
+            {
+                auto const & constraint = *constraintIt->second.get();
+                if (!constraint.getIsEnabled())
                 {
-                    auto const & constraint = *constraintIt->second.get();
-                    if (!constraint.getIsEnabled())
+                    continue;
+                }
+                auto const & frameConstraint = static_cast<FixedFrameConstraint const &>(constraint);
+                // if (frameConstraint.getIsTranslationFixed())
+                // {
+                    // Extract force from lagrangian multipliers
+                    auto fextWorld = data.lambda_c.segment<3>(constraintIdx);  // auto to avoid memory allocation
+
+                    // Convert the force from local world aligned to local frame
+                    int32_t const & frameIdx = frameConstraint.getFrameIdx();
+                    pinocchio::SE3 const & transformContactInWorld = data.oMf[frameIdx];
+                    forceIt->linear() = transformContactInWorld.rotation().transpose() * fextWorld;
+
+                    // Convert the force from local world aligned to local parent joint
+                    int32_t const & jointIdx = model.frames[frameIdx].parent;
+                    fext[jointIdx] += convertForceGlobalFrameToJoint(
+                        model, data, frameIdx, {fextWorld, vector3_t::Zero()});
+                // }
+                constraintIdx += constraint.getDim();
+            }
+
+            systemData.constraintsHolder.foreach(
+                constraintsHolderType_t::COLLISION_BODIES,
+                [&constraintIdx, &fext, &model, &data](
+                    std::shared_ptr<AbstractConstraintBase> const & constraint,
+                    constraintsHolderType_t const & /* holderType */)
+                {
+                    if (!constraint || !constraint->getIsEnabled())
                     {
-                        continue;
+                        return;
                     }
-                    auto const & frameConstraint = static_cast<FixedFrameConstraint const &>(constraint);
-                    // if (frameConstraint.getIsTranslationFixed())
-                    // {
-                        // Extract force from lagrangian multipliers
-                        auto fextWorld = data.lambda_c.segment<3>(constraintIdx);  // auto to avoid memory allocation
-
-                        // Convert the force from local world aligned to local frame
-                        int32_t const & frameIdx = frameConstraint.getFrameIdx();
-                        pinocchio::SE3 const & transformContactInWorld = data.oMf[frameIdx];
-                        forceIt->linear() = transformContactInWorld.rotation().transpose() * fextWorld;
-
-                        // Convert the force from local world aligned to local parent joint
-                        int32_t const & jointIdx = model.frames[frameIdx].parent;
-                        fext[jointIdx] += convertForceGlobalFrameToJoint(
-                            model, data, frameIdx, {fextWorld, vector3_t::Zero()});
-                    // }
-                    constraintIdx += constraint.getDim();
-                }
 
-                systemData.constraintsHolder.foreach(
-                    constraintsHolderType_t::COLLISION_BODIES,
-                    [&constraintIdx, &fext, &model, &data](
-                        std::shared_ptr<AbstractConstraintBase> const & constraint,
-                        constraintsHolderType_t const & /* holderType */)
-                    {
-                        if (!constraint || !constraint->getIsEnabled())
-                        {
-                            return;
-                        }
-                        // auto const & collisionConstraint = static_cast<SphereConstraint const &>(*constraint.get());
-                        auto const & collisionConstraint = static_cast<FixedFrameConstraint const &>(*constraint.get());
+                    // auto const & collisionConstraint = static_cast<SphereConstraint const &>(*constraint.get());
+                    auto const & collisionConstraint = static_cast<FixedFrameConstraint const &>(*constraint.get());
 
-                        // Extract force from lagrangian multipliers
-                        auto fextWorld = data.lambda_c.segment<3>(constraintIdx);  // auto to avoid memory allocation
+                    // Extract force from lagrangian multipliers
+                    auto fextWorld = data.lambda_c.segment<3>(constraintIdx);  // auto to avoid memory allocation
 
-                        // Convert the force from local world aligned to local parent joint
-                        int32_t const & frameIdx = collisionConstraint.getFrameIdx();
-                        int32_t const & jointIdx = model.frames[frameIdx].parent;
-                        fext[jointIdx] += convertForceGlobalFrameToJoint(
-                            model, data, frameIdx, {fextWorld, vector3_t::Zero()});
+                    // Convert the force from local world aligned to local parent joint
+                    int32_t const & frameIdx = collisionConstraint.getFrameIdx();
+                    int32_t const & jointIdx = model.frames[frameIdx].parent;
+                    fext[jointIdx] += convertForceGlobalFrameToJoint(
+                        model, data, frameIdx, {fextWorld, vector3_t::Zero()});
 
-                        constraintIdx += constraint->getDim();
-                    });
-            }
+                    constraintIdx += constraint->getDim();
+                });
 
             return data.ddq;
         }

diff --git a/core/src/robot/AbstractMotor.cc b/core/src/robot/AbstractMotor.cc
@@ -83,28 +83,25 @@ namespace jiminy
             return hresult_t::ERROR_GENERIC;
         }
 
-        if (auto robot = robot_.lock())
+        // Remove associated col in the global data buffer
+        if (motorIdx_ < sharedHolder_->num_ - 1)
         {
-            // Remove associated col in the global data buffer
-            if (motorIdx_ < sharedHolder_->num_ - 1)
-            {
-                int32_t motorShift = sharedHolder_->num_ - motorIdx_ - 1;
-                sharedHolder_->data_.segment(motorIdx_, motorShift) =
-                    sharedHolder_->data_.segment(motorIdx_ + 1, motorShift).eval();  // eval to avoid aliasing
-            }
-            sharedHolder_->data_.conservativeResize(sharedHolder_->num_ - 1);
-
-            // Shift the motor ids
-            for (int32_t i = motorIdx_ + 1; i < sharedHolder_->num_; ++i)
-            {
-                --sharedHolder_->motors_[i]->motorIdx_;
-            }
+            int32_t motorShift = sharedHolder_->num_ - motorIdx_ - 1;
+            sharedHolder_->data_.segment(motorIdx_, motorShift) =
+                sharedHolder_->data_.segment(motorIdx_ + 1, motorShift).eval();  // eval to avoid aliasing
+        }
+        sharedHolder_->data_.conservativeResize(sharedHolder_->num_ - 1);
 
-            // Remove the motor to the shared memory
-            sharedHolder_->motors_.erase(sharedHolder_->motors_.begin() + motorIdx_);
-            --sharedHolder_->num_;
+        // Shift the motor ids
+        for (int32_t i = motorIdx_ + 1; i < sharedHolder_->num_; ++i)
+        {
+            --sharedHolder_->motors_[i]->motorIdx_;
         }
 
+        // Remove the motor to the shared memory
+        sharedHolder_->motors_.erase(sharedHolder_->motors_.begin() + motorIdx_);
+        --sharedHolder_->num_;
+
         // Clear the references to the robot and shared data
         robot_.reset();
         sharedHolder_ = nullptr;