Adding a spline joint to the joint collection

CHANGELOG.md

@@ -22,6 +22,8 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
     - Extended `ModelTpl::addJoint` overload to accept `min_acceleration`, `max_acceleration`, `min_jerk`, `max_jerk` parameters
 - Add `PINOCCHIO_BUILD_BINDING_WITH_PCH` CMake option to use PCH to build Python bindings (default OFF) ([#2886](https://github.com/stack-of-tasks/pinocchio/pull/2886))
 - Add PINOCCHIO_BUILD_VISUALIZERS option ([#2900](https://github.com/stack-of-tasks/pinocchio/pull/2900))
+- Add spline joint (JointModelSplineTpl) to default joint collection ([#2784](https://github.com/stack-of-tasks/pinocchio/pull/2784))
+  - [Python example here](./examples/spline-joint.py)
 
 ### Fixed
 

bindings/python/parsers/graph/expose-edges.cpp

@@ -53,6 +53,9 @@ namespace pinocchio
 
     const int JointMimic::nq;
     const int JointMimic::nv;
+
+    const int JointSpline::nq;
+    const int JointSpline::nv;
   } // namespace graph
 
   namespace python
@@ -160,6 +163,49 @@ namespace pinocchio
         .def_readonly("nq", &JointUniversal::nq, "Number of configuration variables.")
         .def_readonly("nv", &JointUniversal::nv, "Number of tangent variables.");
 
+      bp::class_<JointSpline>(
+        "JointSpline", "Represents a spline-based joint.",
+        bp::init<>(bp::args("self"), "Default constructor."))
+        .def(
+          bp::init<const std::vector<SE3> &, const Eigen::VectorXd &, std::size_t>(
+            bp::args("self", "ctrlFrame", "knots", "degree"),
+            "Constructor with a single control frame and degree."))
+        .def_readwrite("ctrlFrames", &JointSpline::ctrlFrames, "Control frames of the spline.")
+        .def_readwrite("knots", &JointSpline::knots, "Control frames of the spline.")
+        .def_readwrite("degree", &JointSpline::degree, "Degree of the spline.")
+        .def_readonly("nq", &JointSpline::nq, "Number of configuration variables.")
+        .def_readonly("nv", &JointSpline::nv, "Number of tangent variables.");
+
+      bp::class_<JointSplineBuilder>(
+        "JointSplineBuilder", "JointSpline builder helper.",
+        bp::init<>(bp::args("self"), "Default constructor."))
+        .def(
+          "addControlFrame", &JointSplineBuilder::addControlFrame, bp::return_self<>(),
+          (bp::arg("self"), bp::arg("frame")), "Add a B-spline control frame")
+        .def(
+          "withControlFrameVector", &JointSplineBuilder::withControlFrameVector,
+          bp::return_self<>(), (bp::arg("self"), bp::arg("frames")),
+          "Set B-spline control frame vector")
+        .def(
+          "withDegree", &JointSplineBuilder::withDegree, bp::return_self<>(),
+          (bp::arg("self"), bp::arg("degree")), "Set B-spline degree")
+        .def(
+          "withKnotVector",
+          +[](JointSplineBuilder & builder, const std::vector<double> & k) -> auto {
+            return builder.withKnotVector(k);
+          },
+          bp::return_self<>(), (bp::arg("self"), bp::arg("knots")), "Set B-spline knot vector")
+        .def(
+          "withOpenUniformKnots", &JointSplineBuilder::withOpenUniformKnots, bp::return_self<>(),
+          (bp::arg("self"), bp::arg("min"), bp::arg("max")),
+          "Set B-spline knot vector as open uniform")
+        .def(
+          "withUniformKnots", &JointSplineBuilder::withUniformKnots, bp::return_self<>(),
+          (bp::arg("self"), bp::arg("min"), bp::arg("max")), "Set B-spline knot vector as uniform")
+        .def(
+          "build", &JointSplineBuilder::build, (bp::arg("self")),
+          "Build a JointSpline from provided parameters");
+
       bp::class_<JointComposite>(
         "JointComposite", "Represents a composite joint.",
         bp::init<>(bp::args("self"), "Default constructor."))

examples/CMakeLists.txt

@@ -155,6 +155,8 @@ if(BUILD_PYTHON_INTERFACE)
             simulation-closed-kinematic-chains
             simulation-lcaba
             model-graph-geometry
+            spline-joint
+            spline-joint-knee
         )
         if(BUILD_WITH_URDF_SUPPORT)
             list(

examples/spline-joint-knee.py

@@ -0,0 +1,146 @@
+"""Knee spline joint driven by its physical flexion angle.
+
+This example uses the SE(3) spline joint (``pin.JointModelSpline`` /
+``pin.JointModelSplineBuilder``) to reproduce experimental knee kinematics: the
+joint constrains its child (tibia) frame to a cumulative B-spline on SE(3)
+defined by a list of control frames, a knot vector and a degree
+
+source: Lee, S. H., & Terzopoulos, D. (2008).
+Spline joints for multibody dynamics. In ACM SIGGRAPH 2008 papers (pp. 1-8).
+
+The 8 control frames below were fit offline (Gauss-Newton, degree=3, n_ctrl=8)
+to a planar OpenSim knee, where the tx/ty translations the rotation
+about Z (knee flexion) are driven by knee flexion itself.
+"""
+
+import time
+from pathlib import Path
+
+import coal
+import meshcat.geometry as mg
+import numpy as np
+import pinocchio as pin
+from pinocchio.visualize import MeshcatVisualizer
+
+# Bones: mesh folder, OpenSim (subject-specific) scale factors and display color.
+MESH_DIR = Path(__file__).resolve().parent.parent / "models" / "biomechanics"
+FEMUR_SCALE, TIBIA_SCALE = 1.17378, 1.12373
+BONE_COLOR = np.array([0.96, 0.94, 0.86, 0.7])  # bone ivory, semi-transparent.
+
+# Recenter so the femoral condyle (knee) sits at the world origin instead of the
+# hip: the fixed femur and the spline joint are both lifted by the knee offset.
+KNEE_FROM_HIP = np.array([-0.0046, -0.4644, 0.0])
+RECENTER = pin.SE3(np.eye(3), -KNEE_FROM_HIP)
+
+# Control frames from the offline 2D-knee regression, as (yaw [rad], tx, ty [m]).
+CONTROL_FRAMES = [
+    (-2.094400, -0.003842, -0.496205),
+    (-1.815147, 0.001278, -0.489246),
+    (-1.256640, 0.006243, -0.480186),
+    (-0.418880, -0.002220, -0.466444),
+    (0.418880, -0.008427, -0.463391),
+    (1.256640, -0.005740, -0.461613),
+    (1.815147, -0.006127, -0.464906),
+    (2.094400, -0.006285, -0.464416),
+]
+
+
+def mesh_object(name, parent_joint, stl, scale, placement):
+    """Build a mesh GeometryObject fixed to ``parent_joint`` at ``placement``."""
+    mesh_scale = scale * np.ones(3)
+    geom = coal.MeshLoader().load(str(stl), mesh_scale)
+    obj = pin.GeometryObject(name, parent_joint, placement, geom, str(stl), mesh_scale)
+    obj.meshColor = BONE_COLOR
+    return obj
+
+
+class PointTracer:
+    """Trace a fixed point in Meshcat, keeping the dotted trail."""
+
+    def __init__(self, viz, name, point=None, color=0xFF3030, size=0.004):
+        self._world_to_femur = RECENTER.inverse()
+        self._point = np.zeros(3) if point is None else np.asarray(point, float)
+        self._material = mg.PointsMaterial(size=size, color=color)
+        self._trail = []
+        self._node = viz.viewer[f"tibia_traces/{name}"]
+        self._node.set_transform(RECENTER.homogeneous)
+
+    def add(self, tibia_pose):
+        self._trail.append(self._world_to_femur.act(tibia_pose.act(self._point)))
+        pts = np.asarray(self._trail, dtype=np.float32).T
+        self._node.set_object(mg.Points(mg.PointsGeometry(pts), self._material))
+
+
+def main():
+
+    control_frames = [
+        pin.SE3(pin.rpy.rpyToMatrix(0.0, 0.0, yaw), np.array([tx, ty, 0.0]))
+        for (yaw, tx, ty) in CONTROL_FRAMES
+    ]
+
+    # Knee-angle domain of the fitted spline (in degree).
+    THETA_MIN_DEG, THETA_MAX_DEG = -120.0, 120.0
+
+    knee = (
+        pin.JointModelSplineBuilder()
+        .withDegree(
+            3
+        )  # degree of the splines, degree 3 means acceleration is continuous.
+        .withControlFrameVector(control_frames)  # list of SE3 transforms
+        .withOpenUniformKnots(
+            THETA_MIN_DEG, THETA_MAX_DEG
+        )  # Map knee flexion angle (degrees) to spline parameter q in [0, 1].
+        .build()
+    )
+    model = pin.Model()
+    joint_id = model.addJoint(0, knee, RECENTER, "knee")
+
+    # visual only code
+    try:
+        visual_model = pin.GeometryModel()
+        visual_model.addGeometryObject(
+            mesh_object("femur", 0, MESH_DIR / "femur_r.stl", FEMUR_SCALE, RECENTER)
+        )
+        visual_model.addGeometryObject(
+            mesh_object(
+                "tibia",
+                joint_id,
+                MESH_DIR / "tibia_r.stl",
+                TIBIA_SCALE,
+                pin.SE3.Identity(),
+            )
+        )
+
+        viz = MeshcatVisualizer(model, None, visual_model)
+        viz.initViewer(open=True)
+        viz.loadViewerModel()
+
+    except ImportError as e:
+        print("Error while initializing the viewer.")
+        print(e)
+        return
+
+    # Trace tibia-frame points and show the tibia frame as a triad, updated every step.
+    plateau = np.array([0.03100708, -0.03819558, -0.00347008])
+    tracers = [
+        PointTracer(viz, "origin", color=0xFF3030),
+        PointTracer(viz, "plateau", plateau, color=0x3060FF),
+    ]
+    tibia_frame = viz.viewer["tibia_frame"]
+    tibia_frame.set_object(mg.triad(0.08))
+
+    # Sweep the physical knee angle (+10 deg hyperextension to -120 deg flexion),
+    angles = np.linspace(10.0, -120.0, 120)
+    angles = np.concatenate([angles, angles[::-1]])
+    for _ in range(5):
+        for theta_deg in angles:
+            viz.display(np.array([theta_deg]))
+            tibia_pose = viz.data.oMi[joint_id]
+            for tracer in tracers:
+                tracer.add(tibia_pose)
+            tibia_frame.set_transform(tibia_pose.homogeneous)
+            time.sleep(0.015)
+
+
+if __name__ == "__main__":
+    main()

examples/spline-joint.py

@@ -0,0 +1,111 @@
+import time
+
+import meshcat.geometry as mg
+import numpy as np
+import pinocchio as pin
+from pinocchio.visualize import MeshcatVisualizer
+
+
+class PointTracer:
+    """Trace a fixed point in Meshcat, keeping the dotted trail."""
+
+    def __init__(self, viz):
+        self._material = mg.PointsMaterial(size=0.004, color=0xFF3030)
+        self._trail = []
+        self._node = viz.viewer["solid_pose"]
+
+    def add(self, solid_pose):
+        self._trail.append(solid_pose.translation.copy())
+        pts = np.asarray(self._trail, dtype=np.float32).T
+
+        line = mg.Line(
+            geometry=mg.PointsGeometry(pts),
+            material=mg.LineBasicMaterial(color=0xFF3030),
+        )
+        self._node.set_object(geometry=line)
+
+
+def main():
+
+    PARABOLA_CONTROL_FRAMES = [
+        (-1.000000, 1.000000),
+        (-0.888889, 0.777778),
+        (-0.666667, 0.407407),
+        (-0.333333, 0.074074),
+        (0.000000, -0.037037),
+        (0.333333, 0.074074),
+        (0.666667, 0.407407),
+        (0.888889, 0.777778),
+        (1.000000, 1.000000),
+    ]
+
+    control_frames = [
+        pin.SE3(np.eye(3), np.array([0, ty, tz]))
+        for (ty, tz) in PARABOLA_CONTROL_FRAMES
+    ]
+
+    # Create a Pinocchio model with a single free-flyer joint
+    model = pin.Model()
+    spline_joint = (
+        pin.JointModelSplineBuilder()
+        .withDegree(3)
+        .withControlFrameVector(control_frames)
+        .withOpenUniformKnots(-1, 1)
+        .build()
+    )
+    joint_id = model.addJoint(0, spline_joint, pin.SE3.Identity(), "spline-joint")
+
+    # adding some inertia to drop the solid in the parabola
+    box_size = 1.0  # edge length of each cube [m]
+    box_mass = 1.0  # mass of each cube [kg]
+    box_inertia = pin.Inertia.FromBox(box_mass, box_size, box_size, box_size)
+    model.appendBodyToJoint(joint_id, box_inertia, pin.SE3.Identity())
+
+    # visual only code
+    try:
+        visual_model = pin.GeometryModel()
+
+        viz = MeshcatVisualizer(model, None, visual_model)
+        viz.initViewer(open=True)
+        viz.loadViewerModel()
+
+    except ImportError as e:
+        print("Error while initializing the viewer.")
+        print(e)
+        return
+
+    tracers = [PointTracer(viz)]
+    solid_frame = viz.viewer["solid_frame"]
+    solid_frame.set_object(mg.triad(0.2))
+
+    dt = 0.01
+    qs, _ = sim_loop(model, dt)
+
+    for theta in qs:
+        viz.display(np.array([theta]))
+        solid_pose = viz.data.oMi[joint_id]
+        for tracer in tracers:
+            tracer.add(solid_pose)
+        solid_frame.set_transform(solid_pose.homogeneous)
+        time.sleep(dt)
+
+
+def sim_loop(model, dt=0.01, nsteps=800):
+
+    qs = [np.array([1.0])]
+    vs = [np.array([0])]
+    data = model.createData()
+    for i in range(nsteps):
+        q = qs[i]
+        v = vs[i]
+        tau = -1 * v  # a little bit of damping
+        a1 = pin.aba(model, data, q, v, tau)
+        vnext = v + dt * a1
+        qnext = pin.integrate(model, q, dt * vnext)
+        qs.append(qnext)
+        vs.append(vnext)
+    return qs, vs
+
+
+if __name__ == "__main__":
+    main()

include/pinocchio/autodiff/casadi.hpp

@@ -25,6 +25,7 @@
 #include "pinocchio/eigen-common.hpp"
 #include "pinocchio/math.hpp"
 #include "pinocchio/spatial.hpp"
+#include "pinocchio/src/multibody/joint/spline-utils.hxx"
 // IWYU pragma: end_keep
 
 namespace boost
@@ -349,4 +350,5 @@ namespace Eigen
 #include "pinocchio/src/autodiff/casadi/math/triangular-matrix.hxx"
 #include "pinocchio/src/autodiff/casadi/spatial/se3-tpl.hxx"
 #include "pinocchio/src/autodiff/casadi/utils/static-if.hxx"
+#include "pinocchio/src/autodiff/casadi/multibody/joint/spline-utils.hxx"
 // IWYU pragma: end_exports

include/pinocchio/bindings/python/context/generic.hpp

@@ -135,6 +135,10 @@ namespace pinocchio
       typedef JointModelUniversalTpl<Scalar> JointModelUniversal;
       typedef JointDataUniversalTpl<Scalar> JointDataUniversal;
 
+      typedef JointModelSplineTpl<Scalar> JointModelSpline;
+      typedef JointModelSplineBuilderTpl<Scalar> JointModelSplineBuilder;
+      typedef JointDataSplineTpl<Scalar> JointDataSpline;
+
       typedef JointModelTranslationTpl<Scalar> JointModelTranslation;
       typedef JointDataTranslationTpl<Scalar> JointDataTranslation;
 

include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp

@@ -83,5 +83,11 @@ namespace pinocchio
         .add_property("StU", &JointDataComposite::StU);
     }
 
+    template<>
+    inline bp::class_<JointDataSpline> &
+    expose_joint_data<JointDataSpline>(bp::class_<JointDataSpline> & cl)
+    {
+      return cl.add_property("N", &JointDataSpline::N);
+    }
   } // namespace python
 } // namespace pinocchio