Merge pull request #2528 from jcarpent/topic/cmake

Add missing Python example for testing
stack-of-tasks · Mar 10, 2025 · 2f33ea1 · 2f33ea1
2 parents d97e3df + 74beafa
commit 2f33ea1
Show file tree

Hide file tree

Showing 7 changed files with 35 additions and 21 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -41,6 +41,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 - Rewrite `JointModelMimic` and rename it `JointModelMimicTpl`, since `JointModelMimic` wasn't working, we don't consider it a breaking change ([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441))
 - Stop using context::Scalar for GeometryObject([#2441](https://github.com/stack-of-tasks/pinocchio/pull/2441))
 
+### Fixed
+- Add missing Python examples ([#2528](https://github.com/stack-of-tasks/pinocchio/pull/2528))
+
 ## [3.4.0] - 2025-02-12
 
 ### Added

diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
@@ -88,7 +88,7 @@ if(BUILD_PYTHON_INTERFACE)
 
   if(BUILD_WITH_COLLISION_SUPPORT)
     list(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES sample-model-viewer display-shapes
-         simulation-pendulum)
+         simulation-pendulum simulation-closed-kinematic-chains)
     if(BUILD_WITH_URDF_SUPPORT)
       list(
         APPEND
@@ -98,10 +98,9 @@ if(BUILD_PYTHON_INTERFACE)
         collisions
         collision-with-point-clouds
         append-urdf-model-with-another-model
-        simulation-contact-dynamics)
-      if(PYTHON_VERSION_MAJOR EQUAL 3)
-        list(APPEND ${PROJECT_NAME}_PYTHON_EXAMPLES static-contact-dynamics)
-      endif()
+        simulation-contact-dynamics
+        # talos-simulation
+        static-contact-dynamics)
     endif()
   endif()
 

diff --git a/examples/simulation-closed-kinematic-chains.py b/examples/simulation-closed-kinematic-chains.py
@@ -1,3 +1,4 @@
+import sys
 import time
 
 import hppfcl as fcl
@@ -76,14 +77,9 @@
 geom_obj3 = pin.GeometryObject("link_B2", joint3_id, placement_shape_B, shape_link_B)
 geom_obj3.meshColor = RED_COLOR
 collision_model.addGeometryObject(geom_obj3)
-
 visual_model = collision_model
-viz = MeshcatVisualizer(model, collision_model, visual_model)
-viz.initViewer(open=True)
-viz.loadViewerModel()
 
 q0 = pin.neutral(model)
-viz.display(q0)
 
 data = model.createData()
 pin.forwardKinematics(model, data, q0)
@@ -146,22 +142,30 @@
     y -= alpha * (-dy + y)
 
 q_sol = (q[:] + np.pi) % np.pi - np.pi
-viz.display(q_sol)
 
 # Perform the simulation
 q = q_sol.copy()
 v = np.zeros(model.nv)
 tau = np.zeros(model.nv)
 dt = 5e-3
 
-T_sim = 100000
+T_sim = 10
 t = 0
 mu_sim = 1e-10
 constraint_model.corrector.Kp[:] = 10
 constraint_model.corrector.Kd[:] = 2.0 * np.sqrt(constraint_model.corrector.Kp)
 pin.initConstraintDynamics(model, data, [constraint_model])
 prox_settings = pin.ProximalSettings(1e-8, mu_sim, 10)
 
+try:
+    viz = MeshcatVisualizer(model, collision_model, visual_model)
+    viz.initViewer(open=True)
+except ImportError as error:
+    print(error)
+    sys.exit(0)
+viz.loadViewerModel()
+viz.display(q_sol)
+
 while t <= T_sim:
     a = pin.constraintDynamics(
         model, data, q, v, tau, [constraint_model], [constraint_data], prox_settings

diff --git a/include/pinocchio/algorithm/jacobian.hxx b/include/pinocchio/algorithm/jacobian.hxx
@@ -219,7 +219,7 @@ namespace pinocchio
              jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended])
         {
           MotionIn v_in(Jin.col(jExtended));
-          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out = v_in;
         }
@@ -228,7 +228,7 @@ namespace pinocchio
              jExtended = data.mimic_parents_fromRow[(size_t)jExtended])
         {
           MotionIn v_in(Jin.col(jExtended));
-          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out += v_in;
         }
@@ -239,7 +239,7 @@ namespace pinocchio
              jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended])
         {
           MotionIn v_in(Jin.col(jExtended));
-          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out = v_in;
           v_out.linear().noalias() -= placement.translation().cross(v_in.angular());
@@ -249,7 +249,7 @@ namespace pinocchio
              jExtended = data.mimic_parents_fromRow[(size_t)jExtended])
         {
           MotionIn v_in(Jin.col(jExtended));
-          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out += v_in;
           v_out.linear().noalias() -= placement.translation().cross(v_in.angular());
@@ -261,7 +261,7 @@ namespace pinocchio
              jExtended = data.non_mimic_parents_fromRow[(size_t)jExtended])
         {
           MotionIn v_in(Jin.col(jExtended));
-          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out = placement.actInv(v_in);
         }
@@ -270,7 +270,7 @@ namespace pinocchio
              jExtended = data.mimic_parents_fromRow[(size_t)jExtended])
         {
           MotionIn v_in(Jin.col(jExtended));
-          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(Jout_.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out += placement.actInv(v_in);
         }
@@ -808,7 +808,7 @@ namespace pinocchio
           typedef typename Matrix6xLike::ColXpr ColXprOut;
           typedef MotionRef<ColXprOut> MotionOut;
           MotionIn v_in(data.J.col(jExtended));
-          MotionOut v_out(dJ.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(dJ.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out -= v_joint.cross(oMjoint.actInv(v_in));
         }
@@ -827,7 +827,7 @@ namespace pinocchio
           typedef typename Matrix6xLike::ColXpr ColXprOut;
           typedef MotionRef<ColXprOut> MotionOut;
           MotionIn v_in(data.J.col(jExtended));
-          MotionOut v_out(dJ.col(data.idx_vExtended_to_idx_v_fromRow[jExtended]));
+          MotionOut v_out(dJ.col(data.idx_vExtended_to_idx_v_fromRow[size_t(jExtended)]));
 
           v_out.linear() -=
             Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation()))

diff --git a/include/pinocchio/multibody/data.hxx b/include/pinocchio/multibody/data.hxx
@@ -251,7 +251,6 @@ namespace pinocchio
     {
       const Index & parent = model.parents[joint];
       const int idx_vj = model.joints[joint].idx_v();
-      const int nvj = model.joints[joint].nv();
       const int nvExtended_j = model.joints[joint].nvExtended();
       const int idx_vExtended_j = model.joints[joint].idx_vExtended();
 

diff --git a/include/pinocchio/multibody/joint/joint-mimic.hpp b/include/pinocchio/multibody/joint/joint-mimic.hpp
@@ -397,6 +397,11 @@ namespace pinocchio
       joint_v_transformed.resize(nv, 1);
     }
 
+    JointDataMimicTpl(const JointDataMimicTpl & other)
+    {
+      *this = other;
+    }
+
     JointDataMimicTpl & operator=(const JointDataMimicTpl & other)
     {
       m_jdata_mimicking = other.m_jdata_mimicking;

diff --git a/unittest/python/CMakeLists.txt b/unittest/python/CMakeLists.txt
@@ -56,6 +56,10 @@ if(urdfdom_FOUND)
                                    bindings_geometry_model_urdf bindings_contact_inverse_dynamics)
 endif(urdfdom_FOUND)
 
+if(TARGET build_tests)
+  add_dependencies(build_tests ${PROJECT_NAME}-python)
+endif()
+
 foreach(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
   set(TEST_NAME "${PROJECT_NAME}-test-py-${TEST}")
   add_python_unit_test(${TEST_NAME} "unittest/python/${TEST}.py" "bindings/python")