Various fixes

core/include/jiminy/core/constraints/FixedFrameConstraint.h

@@ -50,7 +50,7 @@ namespace jiminy
         void setReferenceTransform(pinocchio::SE3 const & transformRef);
         pinocchio::SE3 const & getReferenceTransform(void) const;
 
-        void setLocalFrame(matrix3_t const & frameRot);
+        void setNormal(vector3_t const & normal);
         matrix3_t const & getLocalFrame(void) const;
 
         virtual hresult_t reset(vectorN_t const & q,
@@ -64,6 +64,7 @@ namespace jiminy
         frameIndex_t frameIdx_;               ///< Corresponding frame index.
         std::vector<uint32_t> dofsFixed_;     ///< Degrees of freedom to fix.
         pinocchio::SE3 transformRef_;         ///< Reference pose of the frame to enforce.
+        vector3_t normal_;                    ///< Normal direction locally at the interface.
         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.

core/src/constraints/FixedFrameConstraint.cc

@@ -18,13 +18,14 @@ namespace jiminy
     frameIdx_(0),
     dofsFixed_(),
     transformRef_(),
-    rotationLocal_(),
+    normal_(),
+    rotationLocal_(matrix3_t::Identity()),
     frameJacobian_(),
     frameDrift_()
     {
         dofsFixed_.resize(static_cast<std::size_t>(maskFixed.cast<int32_t>().array().sum()));
         uint32_t dofIndex = 0;
-        for (uint32_t i=0; i < 6; ++i)
+        for (uint32_t i : std::vector<uint32_t>{{3, 4, 5, 0, 1, 2}})
         {
             if (maskFixed[i])
             {
@@ -64,9 +65,12 @@ namespace jiminy
         return transformRef_;
     }
 
-    void FixedFrameConstraint::setLocalFrame(matrix3_t const & frameRot)
+    void FixedFrameConstraint::setNormal(vector3_t const & normal)
     {
-        rotationLocal_ = frameRot;
+        normal_ = normal;
+        rotationLocal_.col(2) = normal_;
+        rotationLocal_.col(1) = normal_.cross(vector3_t::UnitX()).normalized();
+        rotationLocal_.col(0) = rotationLocal_.col(1).cross(rotationLocal_.col(2));
     }
 
     matrix3_t const & FixedFrameConstraint::getLocalFrame(void) const

core/src/engine/EngineMultiRobot.cc

@@ -1899,6 +1899,8 @@ namespace jiminy
                 {
                     dtNextGlobal = tEnd - t;
                 }
+
+                // Update next dt
                 tNext += dtNextGlobal;
 
                 // Compute the next step using adaptive step method
@@ -1920,18 +1922,29 @@ namespace jiminy
                         hasDynamicsChanged = false;
                     }
 
-                    /* Adjust stepsize to end up exactly at the next breakpoint,
-                       prevent steps larger than dtMax, trying to reach multiples of
-                       STEPPER_MIN_TIMESTEP whenever possible. The idea here is to
-                       reach only multiples of 1us, making logging easier, given that,
-                       in robotics, 1us can be consider an 'infinitesimal' time. This
-                       arbitrary threshold many not be suited for simulating different,
-                       faster dynamics, that require sub-microsecond precision. */
+                    // Adjust stepsize to end up exactly at the next breakpoint
                     dt = min(dt, tNext - t);
-                    if (tNext - (t + dt) < STEPPER_MIN_TIMESTEP)
+                    if (dtLargest > SIMULATION_MIN_TIMESTEP)
                     {
-                        dt = tNext - t;
+                        if (tNext - (t + dt) < SIMULATION_MIN_TIMESTEP)
+                        {
+                            dt = tNext - t;
+                        }
+                    }
+                    else
+                    {
+                        if (tNext - (t + dt) < STEPPER_MIN_TIMESTEP)
+                        {
+                            dt = tNext - t;
+                        }
                     }
+
+                    /* Trying to reach multiples of STEPPER_MIN_TIMESTEP whenever
+                       possible. The idea here is to reach only multiples of 1us,
+                       making logging easier, given that, 1us can be consider an
+                       'infinitesimal' time in robotics. This arbitrary threshold
+                       many not be suited for simulating different, faster
+                       dynamics, that require sub-microsecond precision. */
                     if (dt > SIMULATION_MIN_TIMESTEP)
                     {
                         float64_t const dtResidual = std::fmod(dt, SIMULATION_MIN_TIMESTEP);
@@ -2946,9 +2959,7 @@ namespace jiminy
                         system.robot->pncData_.oMf[frameIdx].rotation(),
                         system.robot->pncData_.oMf[frameIdx].translation() - depth * nGround
                     });
-                    frameConstraint.setLocalFrame(
-                        quaternion_t::FromTwoVectors(vector3_t::UnitZ(), nGround).toRotationMatrix()
-                    );
+                    frameConstraint.setNormal(nGround);
 
                     // Only one contact constraint per collision body is supported for now
                     break;
@@ -3035,9 +3046,7 @@ namespace jiminy
                 system.robot->pncData_.oMf[frameIdx].rotation(),
                 (vector3_t() << posFrame.head<2>(), zGround).finished()
             });
-            frameConstraint.setLocalFrame(
-                quaternion_t::FromTwoVectors(vector3_t::UnitZ(), nGround).toRotationMatrix()
-            );
+            frameConstraint.setNormal(nGround);
         }
     }
 
@@ -3745,17 +3754,17 @@ namespace jiminy
                             return;
                         }
 
-                        // Friction cone in tangential plane
-                        hi[constraintIdx] = contactOptions.friction;
-                        fIndices[constraintIdx] = {static_cast<int32_t>(constraintIdx + 2),
-                                                   static_cast<int32_t>(constraintIdx + 1)};
-
                         // Torsional friction around normal axis
-                        hi[constraintIdx + 3] = contactOptions.torsion;
-                        fIndices[constraintIdx + 3] = {static_cast<int32_t>(constraintIdx + 2)};
+                        hi[constraintIdx] = contactOptions.torsion;
+                        fIndices[constraintIdx] = {static_cast<int32_t>(constraintIdx + 3)};
+
+                        // Friction cone in tangential plane
+                        hi[constraintIdx + 1] = contactOptions.friction;
+                        fIndices[constraintIdx + 1] = {static_cast<int32_t>(constraintIdx + 3),
+                                                       static_cast<int32_t>(constraintIdx + 2)};
 
                         // Non-penetration normal force
-                        lo[constraintIdx + 2] = 0.0;
+                        lo[constraintIdx + 3] = 0.0;
 
                         constraintIdx += constraint->getDim();
                     });
@@ -3844,8 +3853,8 @@ namespace jiminy
 
                 // Extract force in local reference-frame-aligned from lagrangian multipliers
                 pinocchio::Force fextInLocal(
-                    frameConstraint.lambda_.head<3>(),
-                    frameConstraint.lambda_[3] * vector3_t::UnitZ());
+                    frameConstraint.lambda_.tail<3>(),
+                    frameConstraint.lambda_[0] * vector3_t::UnitZ());
 
                 // Compute force in local world aligned frame
                 matrix3_t const & rotationLocal = frameConstraint.getLocalFrame();
@@ -3879,8 +3888,8 @@ namespace jiminy
 
                     // Extract force in world frame from lagrangian multipliers
                     pinocchio::Force fextInLocal(
-                        frameConstraint.lambda_.head<3>(),
-                        frameConstraint.lambda_[3] * vector3_t::UnitZ());
+                        frameConstraint.lambda_.tail<3>(),
+                        frameConstraint.lambda_[0] * vector3_t::UnitZ());
 
                     // Compute force in world frame
                     matrix3_t const & rotationLocal = frameConstraint.getLocalFrame();

core/src/solver/LCPSolvers.cc

@@ -50,15 +50,18 @@ namespace jiminy
         // Update every coefficients sequentially
         for (uint32_t const & i : indices_)
         {
-            // Extract single coefficient
+            // Extract a single coefficient
             float64_t & e = x[i];
 
-            // Update a single coefficient
-            e += (b[i] - A.col(i).dot(x)) / A(i, i);
-
-            // Project the coefficient between lower and upper bounds
+            // Update the coefficient if relevant
             std::vector<int32_t> const & fIdx = fIndices[i];
             std::size_t const fSize = fIdx.size();
+            if ((fSize == 0 && (hi[i] - lo[i] > EPS)) || (hi[i] > EPS))
+            {
+                e += (b[i] - A.col(i).dot(x)) / A(i, i);
+            }
+
+            // Project the coefficient between lower and upper bounds
             if (fSize == 0)
             {
                 e = clamp(e, lo[i], hi[i]);
@@ -131,35 +134,15 @@ namespace jiminy
            Note that it may converge faster to enforce constraints in reverse order,
            since usually constraints bounds dependending on others have lower indices
            by design, aka. coulomb friction law.
+           TODO: Avoid resetting it completely when the size changes.
            TODO: take into account the actual value of 'fIndices' to order the indices. */
         size_t const nIndices = b.size();
-        size_t const nIndicesOrig = indices_.size();
-        if (nIndicesOrig < nIndices)
+        if (indices_.size() != nIndices)
         {
             indices_.resize(nIndices);
-            std::generate(indices_.begin() + nIndicesOrig, indices_.end(),
+            std::generate(indices_.begin(), indices_.end(),
                           [n = static_cast<uint32_t>(nIndices - 1)]() mutable { return n--; });
         }
-        else if (nIndicesOrig > nIndices)
-        {
-            size_t shiftIdx = nIndices;
-            for (size_t i = 0; i < nIndices; ++i)
-            {
-                if (static_cast<size_t>(indices_[i]) >= nIndices)
-                {
-                    for (size_t j = shiftIdx; j < nIndicesOrig; ++j)
-                    {
-                        ++shiftIdx;
-                        if (static_cast<size_t>(indices_[j]) < nIndices)
-                        {
-                            indices_[i] = indices_[j];
-                            break;
-                        }
-                    }
-                }
-            }
-            indices_.resize(nIndices);
-        }
 
         // Normalizing
         // for (Eigen::Index i = 0; i < b.size(); ++i)

core/src/stepper/AbstractRungeKuttaStepper.cc

@@ -79,10 +79,7 @@ namespace jiminy
                                                  state_t   const & /* solution */,
                                                  float64_t       & dt)
     {
-        /* Fixed-step by default, which never fails. By default INF is retuned
-           in case of fixed time step, so that the engine will always try to
-           perform the latest timestep possible, or stop to the next breakpoint
-           otherwise. */
+        // Fixed-step by default, which never fails
         dt = INF;
         return true;
     }

core/src/stepper/EulerExplicitStepper.cc

@@ -19,6 +19,11 @@ namespace jiminy
         stateDerivative = f(t, state);
         state.sumInPlace(stateDerivative, dt);
 
+        /* By default INF is retuned in case of fixed time step, so that the
+           engine will always try to perform the latest timestep possible,
+           or stop to the next breakpoint otherwise. */
+        dt = INF;
+
         // Scheme never considers failure.
         return true;
     }

python/CMakeLists.txt

@@ -97,11 +97,11 @@ deployPythonPackage("${LIBRARY_NAME}_py")
 ################ gym_jiminy ################
 
 buildPythonWheel("python/gym_${LIBRARY_NAME}/common"
-                 "python/gym_${LIBRARY_NAME}/envs"
                  "python/gym_${LIBRARY_NAME}/toolbox"
+                 "python/gym_${LIBRARY_NAME}/envs"
                  "python/gym_${LIBRARY_NAME}/rllib"
                  "${CMAKE_BINARY_DIR}/pypi/dist/gym_${LIBRARY_NAME}")
 deployPythonPackageDevelop("python/gym_${LIBRARY_NAME}/common"
-                           "python/gym_${LIBRARY_NAME}/envs"
                            "python/gym_${LIBRARY_NAME}/toolbox"
+                           "python/gym_${LIBRARY_NAME}/envs"
                            "python/gym_${LIBRARY_NAME}/rllib")

python/gym_jiminy/envs/gym_jiminy/envs/atlas.py

@@ -12,6 +12,7 @@
     F_PROFILE_WAVELENGTH, F_PROFILE_PERIOD)
 from gym_jiminy.common.controllers import PDController
 from gym_jiminy.common.pipeline import build_pipeline
+from gym_jiminy.toolbox.math import ConvexHull
 
 from pinocchio import neutral
 
@@ -37,9 +38,9 @@
     5000.0, 5000.0, 8000.0, 4000.0, 8000.0, 5000.0])
 PID_REDUCED_KD = np.array([
     # Left leg: [HpX, HpZ, HpY, KnY, AkY, AkX]
-    0.02, 0.01, 0.015, 0.01, 0.02, 0.01,
+    0.02, 0.01, 0.015, 0.01, 0.0175, 0.01,
     # Right leg: [HpX, HpZ, HpY, KnY, AkY, AkX]
-    0.02, 0.01, 0.015, 0.01, 0.02, 0.01])
+    0.02, 0.01, 0.015, 0.01, 0.0175, 0.01])
 
 PID_FULL_KP = np.array([
     # Neck: [Y]
@@ -79,6 +80,23 @@
 }
 
 
+def _cleanup_contact_points(env: "AtlasJiminyEnv") -> None:
+    contact_frames_idx = env.robot.contact_frames_idx
+    contact_frames_names = env.robot.contact_frames_names
+    num_contacts = int(len(env.robot.contact_frames_idx) // 2)
+    for contact_slice in (slice(num_contacts), slice(num_contacts, None)):
+        contact_positions = np.stack([
+            env.robot.pinocchio_data.oMf[frame_idx].translation
+            for frame_idx in contact_frames_idx[contact_slice]], axis=0)
+        contact_bottom_idx = np.argsort(
+            contact_positions[:, 2])[:int(num_contacts//2)]
+        convex_hull = ConvexHull(contact_positions[contact_bottom_idx, :2])
+        env.robot.remove_contact_points([
+            contact_frames_names[contact_slice][i]
+            for i in set(range(num_contacts)).difference(
+                contact_bottom_idx[convex_hull._vertex_indices])])
+
+
 class AtlasJiminyEnv(WalkerJiminyEnv):
     def __init__(self, debug: bool = False, **kwargs: Any) -> None:
         # Get the urdf and mesh paths
@@ -98,9 +116,7 @@ def __init__(self, debug: bool = False, **kwargs: Any) -> None:
             debug=debug), **kwargs})
 
         # Remove unrelevant contact points
-        self.robot.remove_contact_points([
-            name for name in self.robot.contact_frames_names
-            if int(name.split("_")[-1]) in (1, 3, 5, 7)])
+        _cleanup_contact_points(self)
 
     def _neutral(self):
         def joint_position_idx(joint_name):
@@ -200,9 +216,7 @@ def joint_position_idx(joint_name):
             step_dt=STEP_DT, debug=debug), **kwargs})
 
         # Remove unrelevant contact points
-        self.robot.remove_contact_points([
-            name for name in self.robot.contact_frames_names
-            if int(name.split("_")[-1]) in (1, 3, 5, 7)])
+        _cleanup_contact_points(self)
 
 
 AtlasPDControlJiminyEnv = build_pipeline(**{

python/gym_jiminy/toolbox/setup.py

@@ -29,7 +29,7 @@
     keywords="reinforcement-learning robotics gym jiminy",
     packages=find_namespace_packages(),
     install_requires=[
-        f"gym_jiminy=={version}"
+        f"gym_jiminy_toolbox=={version}"
     ],
     zip_safe=False
 )

python/gym_jiminy/unit_py/test_pipeline_control.py

@@ -42,7 +42,7 @@ def _test_pid_standing(self):
             :, self.env.controller.motor_to_encoder]
 
         # Run the simulation
-        while self.env.stepper_state.t < 12.0:
+        while self.env.stepper_state.t < 14.0:
             self.env.step(action_init)
 
         # Get the final posture of the robot as an RGB array

python/jiminy_py/src/jiminy_py/viewer/panda3d/panda3d_visualizer.py

@@ -878,6 +878,23 @@ def highlight_node(self, root_path: str, name: str, enable: bool) -> None:
                     return
                 node.clear_render_mode()
 
+    def append_frame(self,
+                     root_path: str,
+                     name: str,
+                     frame: Optional[FrameType] = None) -> None:
+        """Append a cartesian frame primitive node to the group.
+        """
+        model = GeomNode('axes')
+        model.add_geom(geometry.make_axes())
+        node = NodePath(model)
+        node.set_light_off()
+        node.set_render_mode_wireframe()
+        node.set_render_mode_thickness(4)
+        node.set_antialias(AntialiasAttrib.MLine)
+        node.hide(self.LightMask)
+        node.set_shader_off()
+        self.append_node(root_path, name, node, frame)
+
     def append_cone(self,
                     root_path: str,
                     name: str,
@@ -1416,6 +1433,9 @@ def __dir__(self) -> List[str]:
     def set_material(self, *args: Any, **kwargs: Any) -> None:
         self._app.set_material(*args, **kwargs)
 
+    def append_frame(self, *args: Any, **kwargs: Any) -> None:
+        self._app.append_frame(*args, **kwargs)
+
     def append_cylinder(self, *args: Any, **kwargs: Any) -> None:
         self._app.append_cylinder(*args, **kwargs)