diff --git a/CHANGELOG.md b/CHANGELOG.md index 644d46e26d..93ef1cfc55 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -18,6 +18,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Add names to joints that are inside a composite joint ([#2786](https://github.com/stack-of-tasks/pinocchio/pull/2786)) +- Add a new spline joint to default joint collection ([#2784](https://github.com/stack-of-tasks/pinocchio/pull/2784)) ## [3.8.0] - 2025-09-17 diff --git a/bindings/python/parsers/graph/expose-edges.cpp b/bindings/python/parsers/graph/expose-edges.cpp index c5a260a82f..db4ebc2873 100644 --- a/bindings/python/parsers/graph/expose-edges.cpp +++ b/bindings/python/parsers/graph/expose-edges.cpp @@ -50,6 +50,9 @@ namespace pinocchio const int JointMimic::nq; const int JointMimic::nv; + + const int JointSpline::nq; + const int JointSpline::nv; } // namespace graph namespace python @@ -141,6 +144,21 @@ namespace pinocchio .def_readonly("nq", &JointUniversal::nq, "Number of configuration variables.") .def_readonly("nv", &JointUniversal::nv, "Number of tangent variables."); + bp::class_( + "JointSpline", "Represents a spline-based joint.", + bp::init<>(bp::args("self"), "Default constructor.")) + .def(bp::init(bp::args("self", "degree"), "Constructor with degree.")) + .def(bp::init( + bp::args("self", "ctrlFrame", "degree"), + "Constructor with a single control frame and degree.")) + .def_readwrite("ctrlFrames", &JointSpline::ctrlFrames, "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.") + .def( + "addCtrlFrame", &JointSpline::addCtrlFrame, bp::args("self", "ctrlFrame"), + "Add a control frame to the spline."); + bp::class_( "JointComposite", "Represents a composite joint.", bp::init<>(bp::args("self"), "Default constructor.")) diff --git a/examples/spline-joint.py b/examples/spline-joint.py new file mode 100644 index 0000000000..f10fb98604 --- /dev/null +++ b/examples/spline-joint.py @@ -0,0 +1,94 @@ +import time + +import hppfcl +import numpy as np +import pinocchio as pin +from pinocchio.visualize import MeshcatVisualizer + + +def generate_random_se3_trajectory(num_steps, radius, num_revolutions, height): + """ + Generates a random trajectory in SE(3) with rotating orientation. + + Args: + num_keyframes: The number of random keyframes to generate. + num_steps_per_segment: The number of intermediate steps between each keyframe. + + Returns: + A list of pinocchio.SE3 objects representing the trajectory. + """ + trajectory = [] + for i in range(num_steps): + # 1. Parameter to track progress along the helix (from 0.0 to 1.0) + alpha = float(i) / num_steps + + # 2. Define the translational part (the helical path) + # The angle determines the position on the XY plane + angle = alpha * num_revolutions * 2 * np.pi + + # Calculate the x, y, z coordinates for the helix + translation = np.array( + [radius * np.cos(angle), radius * np.sin(angle), alpha * height] + ) + + # 3. Define the rotational part (a new random orientation at each step) + # pin.SE3.Random().rotation generates a random 3x3 rotation matrix + random_rotation = pin.SE3.Random().rotation + + # 4. Combine the translation and random rotation into a single SE(3) pose + pose = pin.SE3(random_rotation, translation) + trajectory.append(pose) + + return trajectory + + +# --- Visualization Setup --- + +# Generate the random trajectory +# Parameters for the helical trajectory +num_steps = 30 +radius = 1.0 +num_revolutions = 3 +height = 2 + +trajectory = generate_random_se3_trajectory(num_steps, radius, num_revolutions, height) + +# Create a Pinocchio model with a single free-flyer joint +model = pin.Model() +joint_id = model.addJoint( + 0, pin.JointModelSpline(trajectory, 3), pin.SE3.Identity(), "free_flyer" +) + +# Attach a simple visual geometry (a box) to the joint +visual_model = pin.GeometryModel() +box_shape = hppfcl.Box(0.1, 0.2, 0.3) +# The placement of the geometry with respect to the joint frame +geom_placement = pin.SE3.Identity() +geom_obj = pin.GeometryObject("box", joint_id, geom_placement, box_shape) +# Assign a color to the geometry +geom_obj.meshColor = np.array([1.0, 0.5, 0.5, 1.0]) # RGBA +visual_model.addGeometryObject(geom_obj) + +# --- Main Execution --- + +# Initialize the MeshCat visualizer. +try: + viz = MeshcatVisualizer(model, visual_model, visual_model) + viz.initViewer(open=True) + viz.loadViewerModel() +except ImportError as e: + print("Error while initializing the viewer.") + print(e) + + +time.sleep(0.1) + +q = pin.neutral(model) + +q_vector = np.arange(0, 1, 0.05) +for q in q_vector: + # Display the new configuration. + viz.display(np.array([q])) + + # Delay for visualization + time.sleep(0.05) diff --git a/include/pinocchio/algorithm/splines.hpp b/include/pinocchio/algorithm/splines.hpp new file mode 100644 index 0000000000..4c66426f9c --- /dev/null +++ b/include/pinocchio/algorithm/splines.hpp @@ -0,0 +1,49 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_algorithm_splines_hpp__ +#define __pinocchio_algorithm_splines_hpp__ + +namespace pinocchio +{ + struct SpanIndexes + { + size_t start_idx; + size_t end_idx; + }; + + template + struct FindSpan + { + template + static SpanIndexes run( + const Eigen::MatrixBase & q, + const int degree, + const int nbCtrlFrames, + const Eigen::MatrixBase & knots) + { + // Edge case: if q is at or beyond the end of the spline parameterization + if (q[0] >= 1.0) + return { + static_cast(nbCtrlFrames - (degree + 1)), static_cast(nbCtrlFrames)}; + + int low = degree; + int high = nbCtrlFrames; + int mid; + + while (low < high) + { + mid = low + (high - low) / 2; + if (q[0] < knots[mid]) + high = mid; + else + low = mid + 1; + } + + return {static_cast(low - (degree + 1)), static_cast(low)}; + } + }; +} // namespace pinocchio + +#endif // __pinocchio_algorithm_splines_hpp__ diff --git a/include/pinocchio/autodiff/casadi.hpp b/include/pinocchio/autodiff/casadi.hpp index 0340d0121e..fb4e74daff 100644 --- a/include/pinocchio/autodiff/casadi.hpp +++ b/include/pinocchio/autodiff/casadi.hpp @@ -443,5 +443,6 @@ namespace Eigen #include "pinocchio/autodiff/casadi/math/matrix.hpp" #include "pinocchio/autodiff/casadi/math/quaternion.hpp" #include "pinocchio/autodiff/casadi/math/triangular-matrix.hpp" +#include "pinocchio/autodiff/casadi/algorithm/splines.hpp" #endif // #ifndef __pinocchio_autodiff_casadi_hpp__ diff --git a/include/pinocchio/autodiff/casadi/algorithm/splines.hpp b/include/pinocchio/autodiff/casadi/algorithm/splines.hpp new file mode 100644 index 0000000000..e82813c45b --- /dev/null +++ b/include/pinocchio/autodiff/casadi/algorithm/splines.hpp @@ -0,0 +1,33 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_autodiff_casadi_splines_hpp__ +#define __pinocchio_autodiff_casadi_splines_hpp__ + +#include "pinocchio/algorithm/splines.hpp" + +namespace pinocchio +{ + // Fwd Declare + struct SpanIndexes; + + template + struct FindSpan; + + template + struct FindSpan<::casadi::SX, Options> + { + template + static SpanIndexes run( + const Eigen::MatrixBase & /*q*/, + const int /*degree*/, + const int nbCtrlFrames, + const Eigen::MatrixBase & /*knots*/) + { + return {0, static_cast(nbCtrlFrames)}; + } + }; +} // namespace pinocchio + +#endif // __pinocchio_autodiff_casadi_splines_hpp__ diff --git a/include/pinocchio/bindings/python/context/generic.hpp b/include/pinocchio/bindings/python/context/generic.hpp index cf2020aa8e..4f4face8f8 100644 --- a/include/pinocchio/bindings/python/context/generic.hpp +++ b/include/pinocchio/bindings/python/context/generic.hpp @@ -132,6 +132,9 @@ namespace pinocchio typedef JointModelUniversalTpl JointModelUniversal; typedef JointDataUniversalTpl JointDataUniversal; + typedef JointModelSplineTpl JointModelSpline; + typedef JointDataSplineTpl JointDataSpline; + typedef JointModelTranslationTpl JointModelTranslation; typedef JointDataTranslationTpl JointDataTranslation; diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp index ca78e24d7a..431857628e 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-datas.hpp @@ -72,7 +72,6 @@ namespace pinocchio .add_property("pjMi", &JointDataComposite::pjMi) .add_property("StU", &JointDataComposite::StU); } - } // namespace python } // namespace pinocchio diff --git a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp index 0afd1017a7..440b674fe1 100644 --- a/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp +++ b/include/pinocchio/bindings/python/multibody/joint/joints-models.hpp @@ -269,6 +269,38 @@ namespace pinocchio "Second rotation axis of the JointModelUniversal."); } + // Specialization for JointModelSpline + template<> + bp::class_ & + expose_joint_model(bp::class_ & cl) + { + return cl + .def(bp::init<>( + bp::args("self"), + "Init an empty joint Spline. Default degree of spline basis function is 3.")) + .def(bp::init( + bp::args("self", "degree"), + "Init an empty joint Spline, with the degree of the future basis functions")) + .def(bp::init &, int>( + bp::args("self", "controlFrames", "degree"), + "Init an empty joint Spline, with a list of controlFrames and the degree of the future " + "basis functions")) + .def( + "setControlFrames", &context::JointModelSpline::setControlFrames, bp::arg("frames"), + "Add a vector of frames to the joint. It cannot be changed afterward and no frames can " + "be added afterwards either") + .def( + "makeKnots", &context::JointModelSpline::makeKnots, + "generate the knots vector for the spline. Call after all the control frame have been " + "added") + .def( + "computeRelativeMotion", &context::JointModelSpline::computeRelativeMotions, + "compute the relative motion between the control frames. Call after all the control " + "frames have been added") + .def_readwrite( + "degree", &context::JointModelSpline::degree, "Degree of the spline basis functions"); + } + // specialization for JointModelComposite struct JointModelCompositeAddJointVisitor diff --git a/include/pinocchio/math/multiprecision.hpp b/include/pinocchio/math/multiprecision.hpp index 19b3f30603..2b571fe84d 100644 --- a/include/pinocchio/math/multiprecision.hpp +++ b/include/pinocchio/math/multiprecision.hpp @@ -7,6 +7,7 @@ #include "pinocchio/math/fwd.hpp" +#include #include #include #include diff --git a/include/pinocchio/multibody/joint/fwd.hpp b/include/pinocchio/multibody/joint/fwd.hpp index 774111bb1e..33effc37e1 100644 --- a/include/pinocchio/multibody/joint/fwd.hpp +++ b/include/pinocchio/multibody/joint/fwd.hpp @@ -130,6 +130,14 @@ namespace pinocchio struct JointDataTranslationTpl; typedef JointDataTranslationTpl JointDataTranslation; + template + struct JointModelSplineTpl; + typedef JointModelSplineTpl JointModelSpline; + + template + struct JointDataSplineTpl; + typedef JointDataSplineTpl JointDataSpline; + template struct JointCollectionDefaultTpl; typedef JointCollectionDefaultTpl JointCollectionDefault; diff --git a/include/pinocchio/multibody/joint/joint-collection.hpp b/include/pinocchio/multibody/joint/joint-collection.hpp index 540d8fb3fb..b0e8c4623f 100644 --- a/include/pinocchio/multibody/joint/joint-collection.hpp +++ b/include/pinocchio/multibody/joint/joint-collection.hpp @@ -82,6 +82,9 @@ namespace pinocchio // Joint Universal typedef JointModelUniversalTpl JointModelUniversal; + // Joint Spline + typedef JointModelSplineTpl JointModelSpline; + typedef boost::variant< // JointModelVoid, JointModelRX, @@ -106,6 +109,7 @@ namespace pinocchio JointModelHz, JointModelHelicalUnaligned, JointModelUniversal, + JointModelSpline, boost::recursive_wrapper, boost::recursive_wrapper> JointModelVariant; @@ -169,6 +173,9 @@ namespace pinocchio // Joint Universal typedef JointDataUniversalTpl JointDataUniversal; + // Joint Spline + typedef JointDataSplineTpl JointDataSpline; + typedef boost::variant< // JointDataVoid JointDataRX, @@ -193,6 +200,7 @@ namespace pinocchio JointDataHz, JointDataHelicalUnaligned, JointDataUniversal, + JointDataSpline, boost::recursive_wrapper, boost::recursive_wrapper> JointDataVariant; diff --git a/include/pinocchio/multibody/joint/joint-spline.hpp b/include/pinocchio/multibody/joint/joint-spline.hpp new file mode 100644 index 0000000000..9148c07467 --- /dev/null +++ b/include/pinocchio/multibody/joint/joint-spline.hpp @@ -0,0 +1,540 @@ +// +// Copyright (c) 2025 INRIA +// + +#ifndef __pinocchio_multibody_joint_spline_hpp__ +#define __pinocchio_multibody_joint_spline_hpp__ + +#include "pinocchio/eigen-macros.hpp" +#include "pinocchio/spatial/explog.hpp" +#include "pinocchio/multibody/joint/joint-base.hpp" +#include "pinocchio/multibody/joint-motion-subspace.hpp" +#include "pinocchio/algorithm/splines.hpp" + +namespace pinocchio +{ + template + struct JointSplineTpl; + + template + struct traits> + { + enum + { + NQ = 1, + NV = 1, + NVExtended = 1 + }; + typedef _Scalar Scalar; + enum + { + Options = _Options + }; + typedef JointDataSplineTpl JointDataDerived; + typedef JointModelSplineTpl JointModelDerived; + // typedef JointMotionSubspace1d Constraint_t; + typedef JointMotionSubspaceTpl<1, Scalar, Options, 1> Constraint_t; + typedef SE3Tpl Transformation_t; + typedef MotionTpl Motion_t; + typedef MotionTpl Bias_t; + + // [ABA] + typedef Eigen::Matrix U_t; + typedef Eigen::Matrix D_t; + typedef Eigen::Matrix UD_t; + + typedef Eigen::Matrix ConfigVector_t; + typedef Eigen::Matrix TangentVector_t; + + typedef boost::mpl::false_ is_mimicable_t; + + PINOCCHIO_JOINT_DATA_BASE_ACCESSOR_DEFAULT_RETURN_TYPE + }; + + template + struct traits> + { + typedef JointSplineTpl<_Scalar, _Options> JointDerived; + typedef _Scalar Scalar; + }; + + template + struct traits> + { + typedef JointSplineTpl<_Scalar, _Options> JointDerived; + typedef _Scalar Scalar; + }; + + template + struct JointDataSplineTpl : public JointDataBase> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef JointSplineTpl<_Scalar, _Options> JointDerived; + typedef Eigen::Vector<_Scalar, Eigen::Dynamic> Vector; + + PINOCCHIO_JOINT_DATA_TYPEDEF_TEMPLATE(JointDerived); + PINOCCHIO_JOINT_DATA_BASE_DEFAULT_ACCESSOR + + ConfigVector_t joint_q; + TangentVector_t joint_v; + + Constraint_t S; + Transformation_t M; + Motion_t v; + Bias_t c; + + // [ABA] specific data + U_t U; + D_t Dinv; + UD_t UDinv; + D_t StU; + + // Bspline values + Vector N; + Vector N_der; + Vector N_der2; + + JointDataSplineTpl() + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) + , v(Motion_t::Zero()) + , c(Bias_t::Zero()) + , U(U_t::Zero()) + , Dinv(D_t::Zero()) + , UDinv(UD_t::Identity()) + , StU(D_t::Zero()) + , N(Vector::Zero(1)) + , N_der(Vector::Zero(1)) + , N_der2(Vector::Zero(1)) + { + } + + JointDataSplineTpl(const int nbCtrlFrames) + : joint_q(ConfigVector_t::Zero()) + , joint_v(TangentVector_t::Zero()) + , M(Transformation_t::Identity()) + , v(Motion_t::Zero()) + , c(Bias_t::Zero()) + , U(U_t::Zero()) + , Dinv(D_t::Zero()) + , UDinv(UD_t::Identity()) + , StU(D_t::Zero()) + , N(Vector::Zero(nbCtrlFrames)) + , N_der(Vector::Zero(nbCtrlFrames)) + , N_der2(Vector::Zero(nbCtrlFrames)) + { + } + + static std::string classname() + { + return std::string("JointDataSpline"); + } + std::string shortname() const + { + return classname(); + } + + }; // struct JointDataSplinerTpl + + /// @brief Spline joint in \f$SE(3)\f$. + /// + /// A spline joint constrains the movement of the child frame to follow the spline defined by the + /// controlFrames and the degree of the spline. Implementation of the joint is based on the paper + /// from Lee et al. Spline Joints for Multibody Dynamics + /// (https://web.cs.ucla.edu/~dt/papers/siggraph08/siggraph08.pdf) + /// + PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSplineTpl); + template + struct JointModelSplineTpl : public JointModelBase> + { + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + typedef JointSplineTpl<_Scalar, _Options> JointDerived; + typedef Eigen::Vector<_Scalar, Eigen::Dynamic> Vector; + PINOCCHIO_JOINT_TYPEDEF_TEMPLATE(JointDerived); + + typedef JointModelBase Base; + using Base::id; + using Base::idx_q; + using Base::idx_v; + using Base::idx_vExtended; + using Base::setIndexes; + + JointModelSplineTpl() + : degree(3) + , nbCtrlFrames(0) + { + } + + JointModelSplineTpl(const int degree) + : degree(degree) + , nbCtrlFrames(0) + { + } + + JointModelSplineTpl( + const PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) & controlFrames, const int degree = 3) + : degree(degree) + , ctrlFrames(controlFrames) + , nbCtrlFrames(controlFrames.size()) + { + buildJoint(); + } + + JointModelSplineTpl(const std::vector & controlFrames, const int degree = 3) + : degree(degree) + { + setControlFrames(controlFrames); + } + + void setControlFrames(const std::vector & controlFrames) + { + nbCtrlFrames = controlFrames.size(); + for (size_t i = 0; i < nbCtrlFrames; i++) + ctrlFrames.push_back(controlFrames[i]); + + buildJoint(); + } + + JointDataDerived createData() const + { + return JointDataDerived(nbCtrlFrames); + } + + const std::vector hasConfigurationLimit() const + { + return {true}; + } + + const std::vector hasConfigurationLimitInTangent() const + { + return {true}; + } + + using Base::isEqual; + bool isEqual(const JointModelSplineTpl & other) const + { + return Base::isEqual(other) && other.degree == degree && other.nbCtrlFrames == nbCtrlFrames + && other.ctrlFrames == ctrlFrames && other.knots == knots + && other.relativeMotions == relativeMotions; + } + + template + void calc(JointDataDerived & data, const Eigen::MatrixBase & qs) const + { + // TODO : Fix it with casadi, or not include include when compiled with casadi + // assert( + // qs[0] >= 0.0 && qs[0] <= 1.0 && "Spline joint configuration (q) must be between 0 + // and 1."); + data.joint_q = qs.template segment(idx_q()); + + SpanIndexes indexes = FindSpan::run(qs, degree, nbCtrlFrames, knots); + + // Basis functions and their derivatives + data.N.setZero(); + data.N_der.setZero(); + for (size_t i = indexes.start_idx; i < indexes.end_idx; i++) + { + data.N[i] = bsplineBasis(i, degree, data.joint_q[0]); + data.N_der[i] = bsplineBasisDerivative(i, degree, data.joint_q[0]); + } + // Compute joint transform M + data.M = ctrlFrames[indexes.start_idx]; + // joint subspace S + data.S.matrix().setZero(); + for (size_t i = indexes.start_idx + 1; i < indexes.end_idx; i++) + { + const Scalar phi_i = data.N.segment(i, indexes.end_idx - i).sum(); + const Scalar phi_dot_i = data.N_der.segment(i, indexes.end_idx - i).sum(); + + const Transformation_t transformation_temp(exp6(relativeMotions[i - 1] * phi_i)); + + data.M = data.M * transformation_temp; + data.S.matrix() = + transformation_temp.actInv(data.S) + relativeMotions[i - 1].toVector() * phi_dot_i; + } + } + + template + void calc( + JointDataDerived & data, + const Eigen::MatrixBase & qs, + const Eigen::MatrixBase & vs) const + { + // assert( + // qs[0] >= 0.0 && qs[0] <= 1.0 && "Spline joint configuration (q) must be between 0 + // and 1."); + + data.joint_q = qs.template segment(idx_q()); + data.joint_v = vs.template segment(idx_v()); + + SpanIndexes indexes = FindSpan::run(qs, degree, nbCtrlFrames, knots); + + // Basis functions and their derivatives + data.N.setZero(); + data.N_der.setZero(); + data.N_der2.setZero(); + for (size_t i = indexes.start_idx; i < indexes.end_idx; i++) + { + data.N[i] = bsplineBasis(i, degree, data.joint_q[0]); + data.N_der[i] = bsplineBasisDerivative(i, degree, data.joint_q[0]); + data.N_der2[i] = bsplineBasisDerivative2(i, degree, data.joint_q[0]); + } + + data.S.matrix().setZero(); + data.c.setZero(); + data.M = ctrlFrames[indexes.start_idx]; + for (size_t i = indexes.start_idx + 1; i < indexes.end_idx; i++) + { + const Scalar phi_i = data.N.segment(i, indexes.end_idx - i).sum(); + const Scalar phi_dot_i = data.N_der.segment(i, indexes.end_idx - i).sum(); + const Scalar phi_ddot_i = data.N_der2.segment(i, indexes.end_idx - i).sum(); + + const Transformation_t transformation_temp(exp6(relativeMotions[i - 1] * phi_i)); + + data.M = data.M * transformation_temp; + // Compute dS/dq recursively + data.c = relativeMotions[i - 1] * phi_ddot_i + + transformation_temp.actInv( + data.c + Motion_t(data.S.matrix()).cross(relativeMotions[i - 1]) * phi_dot_i); + data.S.matrix() = + transformation_temp.actInv(data.S) + relativeMotions[i - 1].toVector() * phi_dot_i; + } + // C = Sdot * qdot = (dS/dq * qdot) * dot + data.c = data.c * data.joint_v[0] * data.joint_v[0]; + data.v = data.S * data.joint_v; + } + + template + void + calc(JointDataDerived & data, const Blank not_used, const Eigen::MatrixBase & vs) + const + { + data.joint_v = vs.template segment(idx_v()); + + // Basis functions and their derivatives + data.N.setZero(); + data.N_der.setZero(); + data.N_der2.setZero(); + for (int i = 0; i < nbCtrlFrames; ++i) + { + data.N[i] = bsplineBasis(i, degree, data.joint_q[0]); + data.N_der[i] = bsplineBasisDerivative(i, degree, data.joint_q[0]); + data.N_der2[i] = bsplineBasisDerivative2(i, degree, data.joint_q[0]); + } + + data.S.matrix().setZero(); + data.c.setZero(); + for (int i = 0; i < nbCtrlFrames - 1; ++i) + { + const Scalar phi_i = data.N.tail(nbCtrlFrames - (i + 1)).sum(); + const Scalar phi_dot_i = data.N_der.tail(nbCtrlFrames - (i + 1)).sum(); + const Scalar phi_ddot_i = data.N_der2.tail(nbCtrlFrames - (i + 1)).sum(); + + const Transformation_t transformation_temp(exp6(relativeMotions[i] * phi_i)); + + data.c = relativeMotions[i] * phi_ddot_i + + transformation_temp.actInv( + data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i); + data.S.matrix() = + transformation_temp.actInv(data.S) + relativeMotions[i].toVector() * phi_dot_i; + } + + data.c = data.c * data.joint_v[0]; + data.v = data.S * data.joint_v; + } + + template + void calc_aba( + JointDataDerived & data, + const Eigen::MatrixBase & armature, + const Eigen::MatrixBase & I, + const bool update_I) const + { + data.U.noalias() = I * data.S.matrix(); + data.StU.noalias() = data.S.transpose() * data.U; + data.StU.diagonal() += armature; + internal::PerformStYSInversion::run(data.StU, data.Dinv); + + data.UDinv.noalias() = data.U * data.Dinv; + + if (update_I) + PINOCCHIO_EIGEN_CONST_CAST(Matrix6Like, I).noalias() -= data.UDinv * data.U.transpose(); + } + + static std::string classname() + { + return std::string("JointModelSpline"); + } + std::string shortname() const + { + return classname(); + } + + template + JointModelSplineTpl cast() const + { + typedef JointModelSplineTpl ReturnType; + ReturnType res; + res.degree = degree; + res.nbCtrlFrames = nbCtrlFrames; + for (size_t k = 0; k < ctrlFrames.size(); k++) + res.ctrlFrames.push_back(ctrlFrames[k].template cast()); + + res.makeKnots(); + res.computeRelativeMotions(); + res.setIndexes(id(), idx_q(), idx_v(), idx_vExtended()); + return res; + } + + void makeKnots() + { + if (nbCtrlFrames <= degree) + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, + "JointSpline - Number of control frames must be greater than degree of the spline.") + const int n_knots = nbCtrlFrames + degree + 1; + knots.resize(n_knots); + knots.head(degree + 1).setZero(); + const Scalar denominator = static_cast(nbCtrlFrames - degree + 1); + + for (size_t i = degree + 1; i < nbCtrlFrames; i++) + knots[i] = static_cast(i - degree) / denominator; + + knots.tail(degree + 1).setOnes(); + } + + void computeRelativeMotions() + { + for (size_t i = 0; i < nbCtrlFrames - 1; i++) + relativeMotions.push_back(log6(ctrlFrames[i].inverse() * ctrlFrames[i + 1])); + } + + Scalar bsplineBasis(int i, int k, const Scalar x) const + { + using internal::if_then_else; + if (k == 0) + { + // clang-format off + // if(knots[i] <= x && x <= knots[i]) + // return 1; + // else + // return 0; + // clang-format on + return if_then_else( + internal::LE, knots[i], x, + if_then_else(internal::LE, x, knots[i + 1], Scalar(1), Scalar(0)), Scalar(0)); + } + + // Calculate the left term + const Scalar den1 = knots[i + k] - knots[i]; + const Scalar left = if_then_else( + internal::GT, den1, Eigen::NumTraits::dummy_precision(), + (x - knots[i]) / den1 * bsplineBasis(i, k - 1, x), Scalar(0)); + + // Calculate the right term + const Scalar den2 = knots[i + k + 1] - knots[i + 1]; + const Scalar right = if_then_else( + internal::GT, den2, Eigen::NumTraits::dummy_precision(), + (knots[i + k + 1] - x) / den2 * bsplineBasis(i + 1, k - 1, x), Scalar(0)); + + return left + right; + } + Scalar bsplineBasisDerivative(int i, int k, const Scalar x) const + { + using internal::if_then_else; + + if (k == 0) + { + return Scalar(0); + } + const Scalar k_scalar = static_cast(k); + + // Calculate the first term of the derivative + const Scalar den1 = knots[i + k] - knots[i]; + const Scalar term1 = if_then_else( + internal::GT, den1, Eigen::NumTraits::dummy_precision(), + (k_scalar / den1) * bsplineBasis(i, k - 1, x), Scalar(0)); + + // Calculate the second term of the derivative + const Scalar den2 = knots[i + k + 1] - knots[i + 1]; + const Scalar term2 = if_then_else( + internal::GT, den2, Eigen::NumTraits::dummy_precision(), + (k_scalar / den2) * bsplineBasis(i + 1, k - 1, x), Scalar(0)); + + return term1 - term2; + } + + Scalar bsplineBasisDerivative2(int i, int k, const Scalar x) const + { + using internal::if_then_else; + + if (k < 2) + { + return Scalar(0); + } + + const Scalar k_scalar = static_cast(k); + + // Calculate the first term + const Scalar den1 = knots[i + k] - knots[i]; + const Scalar term1 = if_then_else( + internal::GT, den1, Eigen::NumTraits::dummy_precision(), + (k_scalar / den1) * bsplineBasisDerivative(i, k - 1, x), Scalar(0)); + + // Calculate the second term + const Scalar den2 = knots[i + k + 1] - knots[i + 1]; + const Scalar term2 = if_then_else( + internal::GT, den2, Eigen::NumTraits::dummy_precision(), + (k_scalar / den2) * bsplineBasisDerivative(i + 1, k - 1, x), Scalar(0)); + + return term1 - term2; + } + + // attributes + int degree; + int nbCtrlFrames; + Vector knots; + PINOCCHIO_ALIGNED_STD_VECTOR(Transformation_t) ctrlFrames; + PINOCCHIO_ALIGNED_STD_VECTOR(Motion_t) relativeMotions; + + private: + void buildJoint() + { + makeKnots(); + computeRelativeMotions(); + } + }; // struct JointModelSplineTpl + +} // namespace pinocchio + +#include + +namespace boost +{ + template + struct has_nothrow_constructor<::pinocchio::JointModelSplineTpl> + : public integral_constant + { + }; + + template + struct has_nothrow_copy<::pinocchio::JointModelSplineTpl> + : public integral_constant + { + }; + + template + struct has_nothrow_constructor<::pinocchio::JointDataSplineTpl> + : public integral_constant + { + }; + + template + struct has_nothrow_copy<::pinocchio::JointDataSplineTpl> + : public integral_constant + { + }; +} // namespace boost + +#endif // ifndef __pinocchio_multibody_joint_spline_hpp__ diff --git a/include/pinocchio/multibody/joint/joints.hpp b/include/pinocchio/multibody/joint/joints.hpp index 546bd4e498..27f1afcca3 100644 --- a/include/pinocchio/multibody/joint/joints.hpp +++ b/include/pinocchio/multibody/joint/joints.hpp @@ -20,5 +20,6 @@ #include "pinocchio/multibody/joint/joint-helical.hpp" #include "pinocchio/multibody/joint/joint-helical-unaligned.hpp" #include "pinocchio/multibody/joint/joint-universal.hpp" +#include "pinocchio/multibody/joint/joint-spline.hpp" #endif // ifndef __pinocchio_multibody_joint_joints_hpp__ diff --git a/include/pinocchio/parsers/graph/graph-visitor.hpp b/include/pinocchio/parsers/graph/graph-visitor.hpp index 8c7520522d..daafc2ef48 100644 --- a/include/pinocchio/parsers/graph/graph-visitor.hpp +++ b/include/pinocchio/parsers/graph/graph-visitor.hpp @@ -129,6 +129,13 @@ namespace pinocchio std::invalid_argument, "Graph - Joint Mimic cannot have a q_ref. Please use the joint offset directly."); } + + SE3 operator()(const JointSpline &) const + { + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, + "Graph - Joint Spline cannot have a q_ref. Please use the joint offset directly."); + } }; /// Return qref transformation. diff --git a/include/pinocchio/parsers/graph/joints.hpp b/include/pinocchio/parsers/graph/joints.hpp index d6dc9d2f54..9af6a8854e 100644 --- a/include/pinocchio/parsers/graph/joints.hpp +++ b/include/pinocchio/parsers/graph/joints.hpp @@ -229,6 +229,43 @@ namespace pinocchio } }; + struct JointSpline + { + std::vector ctrlFrames; + int degree = 3; + + static constexpr int nq = 1; + static constexpr int nv = 1; + + JointSpline() = default; + JointSpline(const int degree) + : degree(degree) + { + } + + JointSpline(const SE3 & ctrlFrame, const int degree = 3) + : degree(degree) + { + ctrlFrames.push_back(ctrlFrame); + } + + JointSpline(const std::vector & ctrlFrames, const int degree = 3) + : degree(degree) + , ctrlFrames(ctrlFrames) + { + } + + void addCtrlFrame(const SE3 & ctrlFrame) + { + ctrlFrames.push_back(ctrlFrame); + } + + bool operator==(const JointSpline & other) const + { + return degree == other.degree && ctrlFrames == other.ctrlFrames; + } + }; + // Forward declare struct JointComposite; struct JointMimic; @@ -245,6 +282,7 @@ namespace pinocchio JointPlanar, JointHelical, JointUniversal, + JointSpline, boost::recursive_wrapper, boost::recursive_wrapper> JointVariant; diff --git a/include/pinocchio/serialization/joints-data.hpp b/include/pinocchio/serialization/joints-data.hpp index 157d3ba7d7..5507f7eda4 100644 --- a/include/pinocchio/serialization/joints-data.hpp +++ b/include/pinocchio/serialization/joints-data.hpp @@ -269,6 +269,20 @@ namespace boost fix::serialize(ar, static_cast &>(joint), version); } + template + void serialize( + Archive & ar, + pinocchio::JointDataSplineTpl & joint, + const unsigned int version) + { + ar & make_nvp("N", joint.N); + ar & make_nvp("N_der", joint.N_der); + ar & make_nvp("N_der2", joint.N_der2); + + typedef pinocchio::JointDataSplineTpl JointType; + fix::serialize(ar, static_cast &>(joint), version); + } + } // namespace serialization } // namespace boost diff --git a/include/pinocchio/serialization/joints-model.hpp b/include/pinocchio/serialization/joints-model.hpp index cc6392a657..2669857342 100644 --- a/include/pinocchio/serialization/joints-model.hpp +++ b/include/pinocchio/serialization/joints-model.hpp @@ -312,6 +312,21 @@ namespace boost fix::serialize(ar, *static_cast *>(&joint), version); } + template + void serialize( + Archive & ar, + pinocchio::JointModelSplineTpl & joint, + const unsigned int version) + { + typedef pinocchio::JointModelSplineTpl JointType; + ar & make_nvp("ctrlFrames", joint.ctrlFrames); + ar & make_nvp("degree", joint.degree); + ar & make_nvp("nbCtrlFrames", joint.nbCtrlFrames); + ar & make_nvp("relativeMotions", joint.relativeMotions); + ar & make_nvp("knots", joint.knots); + + fix::serialize(ar, *static_cast *>(&joint), version); + } } // namespace serialization } // namespace boost diff --git a/include/pinocchio/spatial/act-on-set.hxx b/include/pinocchio/spatial/act-on-set.hxx index a00f037dba..c1e0f111da 100644 --- a/include/pinocchio/spatial/act-on-set.hxx +++ b/include/pinocchio/spatial/act-on-set.hxx @@ -427,7 +427,7 @@ namespace pinocchio EIGEN_STATIC_ASSERT_VECTOR_ONLY(Mat); EIGEN_STATIC_ASSERT_VECTOR_ONLY(MatRet); - typedef MotionRef MotionRefOnMat; + typedef MotionRef MotionRefOnMat; typedef MotionRef MotionRefOnMatRet; MotionRefOnMat min(iV.derived()); diff --git a/include/pinocchio/spatial/log.hxx b/include/pinocchio/spatial/log.hxx index 4ce80c10fd..61eab1614e 100644 --- a/include/pinocchio/spatial/log.hxx +++ b/include/pinocchio/spatial/log.hxx @@ -23,9 +23,12 @@ namespace pinocchio static const long i1 = (i0 + 1) % 3; static const long i2 = (i0 + 2) % 3; Vector3 & axis = _axis.const_cast_derived(); - + // boost::multiprecision return an expression that can't + // be a math::sqrt input. + // To overcome this issue we force the expression to be evaluated in a Scalar. + Scalar sqrt_input = val + eps + eps * eps; const Scalar s = - math::sqrt(val + eps + eps * eps) + math::sqrt(sqrt_input) * if_then_else( GE, R.coeff(i2, i1), R.coeff(i1, i2), Scalar(1.), Scalar(-1.)); // Ensure value in sqrt is non negative and that s is non zero diff --git a/sources.cmake b/sources.cmake index 2aec1df8cc..636c6a8c16 100644 --- a/sources.cmake +++ b/sources.cmake @@ -97,6 +97,7 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/rnea.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/rnea-second-order-derivatives.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/rnea-second-order-derivatives.hxx + ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/splines.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/utils/force.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/algorithm/utils/motion.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi-algo.hpp @@ -104,6 +105,7 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi/math/matrix.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi/math/quaternion.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi/math/triangular-matrix.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi/algorithm/splines.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi/spatial/se3-tpl.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/casadi/utils/static-if.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/autodiff/cppad/algorithm/aba.hpp @@ -194,6 +196,7 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-spherical-ZYX.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-translation.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-universal.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint/joint-spline.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint-motion-subspace-base.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint-motion-subspace-generic.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/joint-motion-subspace.hpp diff --git a/src/parsers/graph/model-graph-algo.cpp b/src/parsers/graph/model-graph-algo.cpp index e0f2ad7d1b..cb6245da3c 100644 --- a/src/parsers/graph/model-graph-algo.cpp +++ b/src/parsers/graph/model-graph-algo.cpp @@ -117,6 +117,9 @@ namespace pinocchio // Joint Universal typedef typename JointCollectionDefault::JointModelUniversal JointModelUniversal; + // JointSpline + typedef typename JointCollectionDefault::JointModelSpline JointModelSpline; + typedef JointModel ReturnType; ReturnType operator()(const JointFixed & /*joint*/) const @@ -229,6 +232,13 @@ namespace pinocchio { return boost::apply_visitor(*this, joint.secondary_joint); } + ReturnType operator()(const JointSpline & joint) const + { + JointModelSpline jmodel(joint.degree); + jmodel.setControlFrames(joint.ctrlFrames); + + return jmodel; + } ReturnType operator()(const JointComposite & joint) const { JointModelComposite jmodel; @@ -338,6 +348,28 @@ namespace pinocchio model.addBodyFrame(target_vertex.name, j_id, body_pose); } + void operator()(const JointSpline & joint, const BodyFrame & b_f) + { + if (!edge.forward) + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Graph - JointSpline cannot be reversed."); + + if (boost::get(&source_vertex.frame) == nullptr) + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Graph - Invalid joint between a body and a non body frame."); + + const SE3 & joint_pose = edge.source_to_joint; + const SE3 & body_pose = edge.joint_to_target; + + const Frame previous_body = model.frames[model.getFrameId(source_vertex.name, BODY)]; + JointIndex j_id = model.addJoint( + previous_body.parentJoint, cjm(joint), previous_body.placement * joint_pose, edge.name); + + model.addJointFrame(j_id); + model.appendBodyToJoint(j_id, b_f.inertia); // check this + model.addBodyFrame(target_vertex.name, j_id, body_pose); + } + void operator()(const JointFixed & joint, const BodyFrame & b_f) { // Need to check what's vertex the edge is coming from. If it's a body, then we add diff --git a/src/parsers/graph/model-graph.cpp b/src/parsers/graph/model-graph.cpp index 02fbfdbb6f..faa7c8cf22 100644 --- a/src/parsers/graph/model-graph.cpp +++ b/src/parsers/graph/model-graph.cpp @@ -85,6 +85,10 @@ namespace pinocchio { return {joint, SE3::Identity()}; } + ReturnType operator()(const JointSpline & joint) const + { + return {joint, SE3::Identity()}; + } ReturnType operator()(const JointComposite & joint) const { JointComposite jReturn; diff --git a/unittest/CMakeLists.txt b/unittest/CMakeLists.txt index c470a509ae..99daa1813b 100644 --- a/unittest/CMakeLists.txt +++ b/unittest/CMakeLists.txt @@ -284,6 +284,7 @@ add_pinocchio_unit_test(joint-composite) add_pinocchio_unit_test(joint-mimic) add_pinocchio_unit_test(joint-helical) add_pinocchio_unit_test(joint-universal) +add_pinocchio_unit_test(joint-spline) # Main corpus add_pinocchio_unit_test(model COLLISION_OPTIONAL) diff --git a/unittest/all-joints.cpp b/unittest/all-joints.cpp index c1d0802e48..ecd512a67e 100644 --- a/unittest/all-joints.cpp +++ b/unittest/all-joints.cpp @@ -167,6 +167,23 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelSplineTpl JointModel; + + static JointModel run() + { + PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.))); + JointModel jmodel(ctrlFrames, 1); + + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + BOOST_AUTO_TEST_SUITE(joint_model_base_test) template diff --git a/unittest/casadi/joints.cpp b/unittest/casadi/joints.cpp index 86b33c6a61..6c1062e75c 100644 --- a/unittest/casadi/joints.cpp +++ b/unittest/casadi/joints.cpp @@ -285,6 +285,25 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelSplineTpl JointModel; + typedef pinocchio::SE3Tpl SE3; + typedef Eigen::Matrix Matrix3s; + typedef Eigen::Matrix Vector3s; + + static JointModel run() + { + PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Matrix3s::Identity(), Vector3s(Scalar(0.), Scalar(0.), Scalar(1.)))); + JointModel jmodel(ctrlFrames, 1); + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + struct TestADOnJoints { template @@ -359,6 +378,24 @@ struct TestADOnJoints test(jmodel); } + template + void operator()(const pinocchio::JointModelSplineTpl &) const + { + typedef pinocchio::JointModelSplineTpl JointModel; + typedef pinocchio::SE3Tpl SE3; + typedef Eigen::Matrix Matrix3s; + typedef Eigen::Matrix Vector3s; + + PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Matrix3s::Identity(), Vector3s(Scalar(0.), Scalar(0.), Scalar(1.)))); + JointModel jmodel(ctrlFrames, 1); + + jmodel.setIndexes(0, 0, 0); + + test(jmodel); + } + template class JointCollection> void operator()(const pinocchio::JointModelTpl &) const { @@ -390,6 +427,145 @@ struct TestADOnJoints { /* do nothing */ } + template + static void test(const pinocchio::JointModelSplineTpl & jmodel) + { + std::cout << "--" << std::endl; + std::cout << "jmodel: " << jmodel.shortname() << std::endl; + + typedef casadi::SX AD_double; + + typedef pinocchio::SE3Tpl SE3AD; + typedef pinocchio::MotionTpl MotionAD; + typedef pinocchio::SE3Tpl SE3; + typedef pinocchio::MotionTpl Motion; + typedef pinocchio::JointMotionSubspaceTpl JointMotionSubspaceXd; + + typedef Eigen::Matrix VectorXAD; + typedef Eigen::Matrix Vector6AD; + + typedef pinocchio::JointModelSplineTpl JointModel; + typedef typename pinocchio::CastType::type JointModelAD; + typedef typename JointModelAD::JointDataDerived JointDataAD; + + typedef typename JointModelAD::ConfigVector_t ConfigVectorAD; + + typedef typename JointModel::JointDataDerived JointData; + typedef typename JointModel::ConfigVector_t ConfigVector; + typedef typename JointModel::TangentVector_t TangentVector; + + JointData jdata(jmodel.createData()); + pinocchio::JointDataBase & jdata_base = jdata; + + JointModelAD jmodel_ad = jmodel.template cast(); + JointDataAD jdata_ad(jmodel_ad.createData()); + pinocchio::JointDataBase & jdata_ad_base = jdata_ad; + + ConfigVector q(jmodel.nq()); + + ConfigVector lb(ConfigVector::Constant(jmodel.nq(), 0.)); + ConfigVector ub(ConfigVector::Constant(jmodel.nq(), 1.)); + + typedef pinocchio::RandomConfigurationStep< + pinocchio::LieGroupMap, ConfigVector, ConfigVector, ConfigVector> + RandomConfigAlgo; + RandomConfigAlgo::run(jmodel.derived(), typename RandomConfigAlgo::ArgsType(q, lb, ub)); + + casadi::SX cs_q = casadi::SX::sym("q", jmodel.nq()); + ConfigVectorAD q_ad(jmodel.nq()); + for (Eigen::DenseIndex k = 0; k < jmodel.nq(); ++k) + { + q_ad[k] = cs_q(k); + } + + // Zero order + jmodel_ad.calc(jdata_ad, q_ad); + jmodel.calc(jdata, q); + + SE3 M1(jdata.M); + SE3AD M2(jdata_ad_base.M()); + + casadi::SX cs_trans(3, 1); + for (Eigen::DenseIndex k = 0; k < 3; ++k) + { + cs_trans(k) = M2.translation()[k]; + } + casadi::SX cs_rot(3, 3); + for (Eigen::DenseIndex i = 0; i < 3; ++i) + { + for (Eigen::DenseIndex j = 0; j < 3; ++j) + { + cs_rot(i, j) = M2.rotation()(i, j); + } + } + + casadi::Function eval_placement( + "eval_placement", casadi::SXVector{cs_q}, casadi::SXVector{cs_trans, cs_rot}); + std::cout << "Joint Placement = " << eval_placement << std::endl; + + std::vector q_vec((size_t)jmodel.nq()); + Eigen::Map(q_vec.data(), jmodel.nq(), 1) = q; + casadi::DMVector res = eval_placement(casadi::DMVector{q_vec}); + std::cout << "M(q)=" << res << std::endl; + + BOOST_CHECK(M1.translation().isApprox(Eigen::Map(res[0]->data()))); + BOOST_CHECK(M1.rotation().isApprox(Eigen::Map(res[1]->data()))); + + // First order + casadi::SX cs_v = casadi::SX::sym("v", jmodel.nv()); + TangentVector v(TangentVector::Random(jmodel.nv())); + VectorXAD v_ad(jmodel_ad.nv()); + + std::vector v_vec((size_t)jmodel.nv()); + Eigen::Map(v_vec.data(), jmodel.nv(), 1) = v; + + for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) + { + v_ad[k] = cs_v(k); + } + + jmodel.calc(jdata, q, v); + Motion m(jdata_base.v()); + JointMotionSubspaceXd Sref(jdata_base.S().matrix()); + + jmodel_ad.calc(jdata_ad, q_ad, v_ad); + Vector6AD Y; + MotionAD m_ad(jdata_ad_base.v()); + + casadi::SX cs_vel(6, 1); + for (Eigen::DenseIndex k = 0; k < 6; ++k) + { + cs_vel(k) = m_ad.toVector()[k]; + } + casadi::Function eval_velocity( + "eval_velocity", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{cs_vel}); + std::cout << "Joint Velocity = " << eval_velocity << std::endl; + + casadi::DMVector res_vel = eval_velocity(casadi::DMVector{q_vec, v_vec}); + std::cout << "v(q,v)=" << res_vel << std::endl; + + BOOST_CHECK(m.linear().isApprox(Eigen::Map(res_vel[0]->data()))); + BOOST_CHECK(m.angular().isApprox(Eigen::Map(res_vel[0]->data() + 3))); + + casadi::SX dvel_dv = jacobian(cs_vel, cs_v); + casadi::Function eval_S("eval_S", casadi::SXVector{cs_q, cs_v}, casadi::SXVector{dvel_dv}); + std::cout << "S = " << eval_S << std::endl; + + casadi::DMVector res_S = eval_S(casadi::DMVector{q_vec, v_vec}); + std::cout << "res_S:" << res_S << std::endl; + JointMotionSubspaceXd::DenseBase Sref_mat = Sref.matrix(); + + for (Eigen::DenseIndex i = 0; i < 6; ++i) + { + for (Eigen::DenseIndex j = 0; i < Sref.nv(); ++i) + BOOST_CHECK( + std::fabs(Sref_mat(i, j) - (double)res_S[0](i, j)) + <= Eigen::NumTraits::dummy_precision()); + } + + std::cout << "--" << std::endl << std::endl; + } + template static void test(const pinocchio::JointModelBase & jmodel) { diff --git a/unittest/cppad/joints.cpp b/unittest/cppad/joints.cpp index 28f53678db..b6a4b759db 100644 --- a/unittest/cppad/joints.cpp +++ b/unittest/cppad/joints.cpp @@ -189,6 +189,23 @@ struct TestADOnJoints test(jmodel); } + template + void operator()(const pinocchio::JointModelSplineTpl &) const + { + typedef pinocchio::JointModelSplineTpl JointModel; + typedef pinocchio::SE3Tpl SE3; + typedef Eigen::Matrix Matrix3s; + typedef Eigen::Matrix Vector3s; + + PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Matrix3s::Identity(), Vector3s(Scalar(0.), Scalar(0.), Scalar(1.)))); + JointModel jmodel(ctrlFrames, 1); + jmodel.setIndexes(0, 0, 0); + + test(jmodel); + } + // TODO: implement it template class JointCollection> void operator()( @@ -210,6 +227,102 @@ struct TestADOnJoints test(jmodel); } + template + static void test(const pinocchio::JointModelSplineTpl & jmodel) + { + using CppAD::AD; + using CppAD::NearEqual; + + typedef AD AD_scalar; + + typedef Eigen::Matrix VectorXAD; + typedef Eigen::Matrix MatrixX; + + typedef pinocchio::SE3Tpl SE3AD; + typedef pinocchio::MotionTpl MotionAD; + typedef pinocchio::SE3Tpl SE3; + typedef pinocchio::MotionTpl Motion; + typedef pinocchio::JointMotionSubspaceTpl JointMotionSubspaceXd; + + typedef Eigen::Matrix VectorXAD; + typedef Eigen::Matrix Vector6AD; + + typedef pinocchio::JointModelSplineTpl JointModel; + typedef typename pinocchio::CastType::type JointModelAD; + typedef typename JointModelAD::JointDataDerived JointDataAD; + + typedef typename JointModelAD::ConfigVector_t ConfigVectorAD; + + typedef typename JointModel::JointDataDerived JointData; + typedef typename JointModel::ConfigVector_t ConfigVector; + typedef typename JointModel::TangentVector_t TangentVector; + + JointData jdata(jmodel.createData()); + pinocchio::JointDataBase & jdata_base = jdata; + + JointModelAD jmodel_ad = jmodel.template cast(); + JointDataAD jdata_ad(jmodel_ad.createData()); + pinocchio::JointDataBase & jdata_ad_base = jdata_ad; + + ConfigVector q(jmodel.nq()); + ConfigVector lb(ConfigVector::Constant(jmodel.nq(), 0)); + ConfigVector ub(ConfigVector::Constant(jmodel.nq(), 1.)); + + typedef pinocchio::RandomConfigurationStep< + pinocchio::LieGroupMap, ConfigVector, ConfigVector, ConfigVector> + RandomConfigAlgo; + RandomConfigAlgo::run(jmodel.derived(), typename RandomConfigAlgo::ArgsType(q, lb, ub)); + + ConfigVectorAD q_ad(q.template cast()); + + // Zero order + jmodel_ad.calc(jdata_ad, q_ad); + jmodel.calc(jdata, q); + + SE3 M1(jdata_base.M()); + SE3AD M2(jdata_ad_base.M()); + BOOST_CHECK(M1.isApprox(M2.template cast())); + + // First order + TangentVector v(TangentVector::Random(jmodel.nv())); + VectorXAD X(jmodel_ad.nv()); + + for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) + { + X[k] = v[k]; + } + CppAD::Independent(X); + jmodel_ad.calc(jdata_ad, q_ad, X); + jmodel.calc(jdata, q, v); + VectorXAD Y(6); + MotionAD m_ad(jdata_ad_base.v()); + Motion m(jdata_base.v()); + JointMotionSubspaceXd Sref(jdata_base.S().matrix()); + + for (Eigen::DenseIndex k = 0; k < 3; ++k) + { + Y[k + Motion::LINEAR] = m_ad.linear()[k]; + Y[k + Motion::ANGULAR] = m_ad.angular()[k]; + } + + CppAD::ADFun vjoint(X, Y); + + CPPAD_TESTVECTOR(Scalar) x((size_t)jmodel_ad.nv()); + for (Eigen::DenseIndex k = 0; k < jmodel.nv(); ++k) + { + x[(size_t)k] = v[k]; + } + + CPPAD_TESTVECTOR(Scalar) jac = vjoint.Jacobian(x); + MatrixX S(6, jac.size() / 6); + S = Eigen::Map( + jac.data(), S.rows(), S.cols()); + + BOOST_CHECK(m.isApprox(m_ad.template cast())); + + BOOST_CHECK(Sref.matrix().isApprox(S)); + } + template static void test(const pinocchio::JointModelBase & jmodel) { diff --git a/unittest/cppadcg/basic.cpp b/unittest/cppadcg/basic.cpp index 8d0b367f74..d12d34423b 100644 --- a/unittest/cppadcg/basic.cpp +++ b/unittest/cppadcg/basic.cpp @@ -101,15 +101,17 @@ BOOST_AUTO_TEST_CASE(test_eigen_support) BOOST_AUTO_TEST_CASE(test_cast) { - typedef CppAD::cg::CG CGScalar; - typedef CppAD::AD ADScalar; - typedef CppAD::AD ADFloat; - typedef pinocchio::ModelTpl CGModel; + using namespace CppAD; + using namespace CppAD::cg; + + typedef CG CGD; + typedef AD ADCG; + typedef pinocchio::ModelTpl ADCGModel; pinocchio::SE3 M(pinocchio::SE3::Random()); - typedef pinocchio::SE3Tpl CGSE3; + typedef pinocchio::SE3Tpl CGSE3; - CGSE3 cg_M = M.cast(); + CGSE3 cg_M = M.cast(); BOOST_CHECK(cg_M.cast().isApprox(M)); pinocchio::SE3::Vector3 axis(1., 1., 1.); @@ -117,34 +119,15 @@ BOOST_AUTO_TEST_CASE(test_cast) BOOST_CHECK(axis.isUnitary()); pinocchio::JointModelPrismaticUnaligned jmodel_prismatic(axis); - typedef pinocchio::JointModelPrismaticUnalignedTpl CGJointModelPrismaticUnaligned; - - CGScalar cg_value; - cg_value = -1.; - ADScalar ad_value; - ad_value = -1.; - ADFloat ad_float; - ad_float = -1.; - abs(ad_value); - abs(ad_float); - abs(cg_value); + typedef pinocchio::JointModelPrismaticUnalignedTpl ADCGJointModelPrismaticUnaligned; - CPPAD_TESTVECTOR(ADScalar) ad_x(3); - CGJointModelPrismaticUnaligned cg_jmodel_prismatic(axis.cast()); + CPPAD_TESTVECTOR(ADCG) ad_x(3); + ADCGJointModelPrismaticUnaligned cg_jmodel_prismatic(axis.cast()); pinocchio::Model model; pinocchio::buildModels::humanoidRandom(model); - CGModel cg_model = model.cast(); - - { - CppAD::AD ad_value(-1.); - abs(ad_value); // works perfectly - - CppAD::cg::CG cg_value(-1.); - abs(cg_value); // does not compile because abs(const CppAD::cg::CG&) is defined - // in namespace CppAD and not CppAD::cg - } + ADCGModel cg_model = model.cast(); } BOOST_AUTO_TEST_CASE(test_dynamic_link) diff --git a/unittest/finite-differences.cpp b/unittest/finite-differences.cpp index 9b3958ef41..a4e121f34c 100644 --- a/unittest/finite-differences.cpp +++ b/unittest/finite-differences.cpp @@ -212,6 +212,22 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelSplineTpl JointModel; + + static JointModel run() + { + PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames; + ctrlFrames.push_back(pinocchio::SE3::Identity()); + ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.))); + JointModel jmodel(ctrlFrames, 1); + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + struct FiniteDiffJoint { void operator()(JointModelComposite & /*jmodel*/) const @@ -222,6 +238,46 @@ struct FiniteDiffJoint { } + void operator()(JointModelSpline & /*jmodel*/) const + { + typedef typename JointModelSpline::ConfigVector_t CV; + typedef typename JointModelSpline::TangentVector_t TV; + typedef typename LieGroup::type LieGroupType; + + JointModelSpline jmodel = init::run(); + std::cout << "name: " << jmodel.classname() << std::endl; + JointDataSpline jdata = jmodel.createData(); + + CV q = LieGroupType().randomConfiguration(CV::Zero(), CV::Ones()); + jmodel.calc(jdata, q); + SE3 M_ref(jdata.M); + + CV q_int(q); + const Eigen::DenseIndex nv = jdata.S.nv(); + TV v(nv); + v.setZero(); + double eps = 1e-8; + + Eigen::Matrix S(6, nv), S_ref(jdata.S.matrix()); + + for (int k = 0; k < nv; ++k) + { + v[k] = eps; + q_int = LieGroupType().integrate(q, v); + jmodel.calc(jdata.derived(), q_int); + SE3 M_int = jdata.M; + + S.col(k) = log6(M_ref.inverse() * M_int).toVector(); + S.col(k) /= eps; + + v[k] = 0.; + } + + BOOST_CHECK(S.isApprox(S_ref, eps * 1e1)); + std::cout << "S_ref:\n" << S_ref << std::endl; + std::cout << "S:\n" << S << std::endl; + } + template void operator()(JointModelBase & /*jmodel*/) const { diff --git a/unittest/joint-generic.cpp b/unittest/joint-generic.cpp index 1f993b7bb7..f81a83e1b6 100644 --- a/unittest/joint-generic.cpp +++ b/unittest/joint-generic.cpp @@ -75,7 +75,122 @@ void test_joint_methods( jda.S().matrix().isApprox(jdata.S().matrix()), std::string(error_prefix + " - JointMotionSubspaceXd ")); BOOST_CHECK_MESSAGE( - (jda.M()).isApprox((jdata.M())), + (jda.M()).isApprox((jdata.M()), 1e-6), + std::string(error_prefix + " - Joint transforms ")); // == or isApprox ? + BOOST_CHECK_MESSAGE( + (jda.v()).isApprox((pinocchio::Motion(jdata.v()))), + std::string(error_prefix + " - Joint motions ")); + BOOST_CHECK_MESSAGE((jda.c()) == (jdata.c()), std::string(error_prefix + " - Joint bias ")); + + BOOST_CHECK_MESSAGE( + (jda.U()).isApprox(jdata.U()), + std::string(error_prefix + " - Joint U inertia matrix decomposition ")); + BOOST_CHECK_MESSAGE( + (jda.Dinv()).isApprox(jdata.Dinv()), + std::string(error_prefix + " - Joint DInv inertia matrix decomposition ")); + BOOST_CHECK_MESSAGE( + (jda.UDinv()).isApprox(jdata.UDinv()), + std::string(error_prefix + " - Joint UDInv inertia matrix decomposition ")); + + // Test vxS + typedef typename JointModel::Constraint_t Constraint_t; + typedef typename Constraint_t::DenseBase ConstraintDense; + + Motion v(Motion::Random()); + ConstraintDense vxS(v.cross(jdata.S())); + ConstraintDense vxS_ref = v.toActionMatrix() * jdata.S().matrix(); + + BOOST_CHECK_MESSAGE(vxS.isApprox(vxS_ref), std::string(error_prefix + "- Joint vxS operation ")); + + // Test Y*S + const Inertia Isparse(Inertia::Random()); + const Inertia::Matrix6 Idense(Isparse.matrix()); + + const ConstraintDense IsparseS = Isparse * jdata.S(); + const ConstraintDense IdenseS = Idense * jdata.S(); + + BOOST_CHECK_MESSAGE( + IdenseS.isApprox(IsparseS), std::string(error_prefix + "- Joint YS operation ")); + + // Test calc + { + JointData jdata1(jdata.derived()); + + jmodel.calc(jdata1.derived(), q1, v1); + jmodel.calc(jdata1.derived(), Blank(), v2); + + JointData jdata_ref(jdata.derived()); + jmodel.calc(jdata_ref.derived(), q1, v2); + + BOOST_CHECK_MESSAGE( + pinocchio::JointData(jdata1).v() == pinocchio::JointData(jdata_ref).v(), + std::string(error_prefix + "- joint.calc(jdata,*,v) ")); + } +} + +template +void test_joint_methods( + JointModelBase> & jmodel, + JointDataBase::JointDataDerived> & jdata) +{ + typedef typename LieGroup>::type LieGroupType; + typedef typename JointModelSplineTpl::JointDataDerived JointData; + + std::cout << "Testing Joint over " << jmodel.shortname() << std::endl; + + Eigen::VectorXd q1, q2; + Eigen::VectorXd armature = + Eigen::VectorXd::Random(jdata.S().nv()) + Eigen::VectorXd::Ones(jdata.S().nv()); + + q1 = LieGroupType().randomConfiguration( + Eigen::VectorXd::Zero(jmodel.nq()), Eigen::VectorXd::Ones(jmodel.nq())); + q2 = LieGroupType().randomConfiguration( + Eigen::VectorXd::Zero(jmodel.nq()), Eigen::VectorXd::Ones(jmodel.nq())); + + Eigen::VectorXd v1(Eigen::VectorXd::Random(jdata.S().nv())), + v2(Eigen::VectorXd::Random(jdata.S().nv())); + + Inertia::Matrix6 Ia(pinocchio::Inertia::Random().matrix()), + Ia2(pinocchio::Inertia::Random().matrix()); + bool update_I = false; + + jmodel.calc(jdata.derived(), q1, v1); + jmodel.calc_aba(jdata.derived(), armature, Ia, update_I); + + pinocchio::JointModel jma(jmodel); + BOOST_CHECK(jmodel == jma); + BOOST_CHECK(jma == jmodel); + BOOST_CHECK(jma.hasSameIndexes(jmodel)); + + pinocchio::JointData jda(jdata.derived()); + BOOST_CHECK(jda == jdata); + BOOST_CHECK(jdata == jda); + + jma.calc(jda, q1, v1); + jma.calc_aba(jda, armature, Ia, update_I); + pinocchio::JointData jda_other(jdata); + + jma.calc(jda_other, q2, v2); + jma.calc_aba(jda_other, armature, Ia2, update_I); + + BOOST_CHECK(jda_other != jda); + BOOST_CHECK(jda != jda_other); + BOOST_CHECK(jda_other != jdata); + BOOST_CHECK(jdata != jda_other); + + const std::string error_prefix("JointModel on " + jma.shortname()); + BOOST_CHECK_MESSAGE(jmodel.nq() == jma.nq(), std::string(error_prefix + " - nq ")); + BOOST_CHECK_MESSAGE(jmodel.nv() == jma.nv(), std::string(error_prefix + " - nv ")); + + BOOST_CHECK_MESSAGE(jmodel.idx_q() == jma.idx_q(), std::string(error_prefix + " - Idx_q ")); + BOOST_CHECK_MESSAGE(jmodel.idx_v() == jma.idx_v(), std::string(error_prefix + " - Idx_v ")); + BOOST_CHECK_MESSAGE(jmodel.id() == jma.id(), std::string(error_prefix + " - JointId ")); + + BOOST_CHECK_MESSAGE( + jda.S().matrix().isApprox(jdata.S().matrix()), + std::string(error_prefix + " - JointMotionSubspaceXd ")); + BOOST_CHECK_MESSAGE( + (jda.M()).isApprox((jdata.M()), 1e-6), std::string(error_prefix + " - Joint transforms ")); // == or isApprox ? BOOST_CHECK_MESSAGE( (jda.v()).isApprox((pinocchio::Motion(jdata.v()))), @@ -279,6 +394,22 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelSplineTpl JointModel; + + static JointModel run() + { + PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.))); + JointModel jmodel(ctrlFrames, 1); + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + struct TestJoint { diff --git a/unittest/joint-motion-subspace.cpp b/unittest/joint-motion-subspace.cpp index 2940a930bc..4cf8ed16df 100644 --- a/unittest/joint-motion-subspace.cpp +++ b/unittest/joint-motion-subspace.cpp @@ -258,6 +258,11 @@ void test_constraint_operations(const JointModelBase & jmodel) } } +template +void test_constraint_operations(const JointModelSplineTpl & /*jmodel*/) +{ +} // Disable test for JointSpline, bc using generic subspace + template class JointCollection> void test_constraint_operations( const JointModelMimicTpl & /*jmodel*/) @@ -418,6 +423,23 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelSplineTpl JointModel; + + static JointModel run() + { + PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames; + ctrlFrames.push_back(pinocchio::SE3::Identity()); + ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.))); + JointModel jmodel(ctrlFrames, 1); + + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + struct TestJointConstraint { diff --git a/unittest/joint-spline.cpp b/unittest/joint-spline.cpp new file mode 100644 index 0000000000..d6ac3e2865 --- /dev/null +++ b/unittest/joint-spline.cpp @@ -0,0 +1,422 @@ +// +// Copyright (c) 2025 INRIA +// + +#include "pinocchio/math/fwd.hpp" +#include "pinocchio/multibody/joint/joints.hpp" +#include "pinocchio/algorithm/splines.hpp" +#include "pinocchio/algorithm/rnea.hpp" +#include "pinocchio/algorithm/aba.hpp" +#include "pinocchio/algorithm/crba.hpp" +#include "pinocchio/algorithm/jacobian.hpp" +#include "pinocchio/algorithm/compute-all-terms.hpp" + +#include +#include + +using namespace pinocchio; + +template +void addJointAndBody( + Model & model, + const JointModelBase & jmodel, + const Model::JointIndex parent_id, + const SE3 & joint_placement, + const std::string & joint_name, + const Inertia & Y) +{ + Model::JointIndex idx; + + idx = model.addJoint(parent_id, jmodel, joint_placement, joint_name); + model.appendBodyToJoint(idx, Y); +} + +/// @brief Get a predefined spline joint trajectory. +/// This was taken from the model opensim gait10dof18mus.osim, and correspond the knee joint. +void getTrajectory(std::vector & ctrlFrames) +{ + Eigen::Matrix3d rotation; + Eigen::Vector3d translation; + rotation << -0.500004, 0, -0.866023, 0, 1, 0, 0.866023, 0, -0.500004; + translation << -0.0032, -0.4226, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << -0.173649, 0, -0.984808, 0, 1, 0, 0.984808, 0, -0.173649; + translation << 0.00179, -0.416947, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.173652, 0, -0.984807, 0, 1, 0, 0.984807, 0, 0.173652; + translation << 0.00411, -0.411057, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.342021, 0, -0.939692, 0, 1, 0, 0.939692, 0, 0.342021; + translation << 0.00438827, -0.4082, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.499998, 0, -0.866027, 0, 1, 0, 0.866027, 0, 0.499998; + translation << 0.0041, -0.405495, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.766044, 0, -0.642788, 0, 1, 0, 0.642788, 0, 0.766044; + translation << 0.00212, -0.400825, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.866025, 0, -0.5, 0, 1, 0, 0.5, 0, 0.866025; + translation << 0.000757726, -0.399, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.939693, 0, -0.34202, 0, 1, 0, 0.34202, 0, 0.939693; + translation << -0.001, -0.3976, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.984808, 0, -0.173648, 0, 1, 0, 0.173648, 0, 0.984808; + translation << -0.0031, -0.3966, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.987363, 0, 0.158478, 0, 1, 0, -0.158478, 0, 0.987363; + translation << -0.00513719, -0.395264, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.980591, 0, 0.196066, 0, 1, 0, -0.196066, 0, 0.980591; + translation << -0.005227, -0.395149, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.94362, 0, 0.33103, 0, 1, 0, -0.33103, 0, 0.94362; + translation << -0.005435, -0.394792, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.882249, 0, 0.470783, 0, 1, 0, -0.470783, 0, 0.882249; + translation << -0.005574, -0.394507, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << 0.0493163, 0, 0.998783, 0, 1, 0, -0.998783, 0, 0.0493163; + translation << -0.005435, -0.394812, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); + + rotation << -0.500004, 0, 0.866023, 0, 1, 0, -0.866023, 0, -0.500004; + translation << -0.00525, -0.396, 0; + ctrlFrames.push_back(pinocchio::SE3(rotation, translation)); +} + +BOOST_AUTO_TEST_SUITE(JointSpline) + +/// @brief Test on the knot vector generation +BOOST_AUTO_TEST_CASE(makeKnots) +{ + int degree = 3; + + std::vector ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3::Random()); + ctrlFrames.push_back(SE3::Random()); + + BOOST_CHECK_THROW(JointModelSpline(ctrlFrames, degree), std::invalid_argument); + + ctrlFrames.push_back(SE3::Random()); + + JointModelSpline jmodel(ctrlFrames, degree); + + // Check size + BOOST_CHECK(jmodel.knots.size() == (degree + ctrlFrames.size() + 1)); + + // Check Values + Eigen::VectorXd knots_expected(degree + ctrlFrames.size() + 1); + knots_expected << 0., 0., 0., 0., 1., 1., 1., 1.; + BOOST_CHECK(jmodel.knots == knots_expected); +} + +/// @brief Test to make sure the relative motions are correct +BOOST_AUTO_TEST_CASE(relativeMotions) +{ + int degree = 3; + + std::vector ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3::Random()); + ctrlFrames.push_back(SE3::Random()); + ctrlFrames.push_back(SE3::Random()); + + JointModelSpline jmodel(ctrlFrames, degree); + + // Check size + BOOST_CHECK(jmodel.relativeMotions.size() == (ctrlFrames.size() - 1)); + + // check values + for (size_t i = 0; i < ctrlFrames.size() - 1; i++) + BOOST_CHECK(jmodel.relativeMotions[i].isApprox( + pinocchio::log6(ctrlFrames[i].inverse() * ctrlFrames[i + 1]))); +} + +/// @brief Test on the basisSpline function and its first derivative +BOOST_AUTO_TEST_CASE(basisSplineFunctions) +{ + int degree = 3; + + std::vector ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3::Random()); + ctrlFrames.push_back(SE3::Random()); + ctrlFrames.push_back(SE3::Random()); + + JointModelSpline jmodel(ctrlFrames, degree); + jmodel.setIndexes(0, 0, 0); + JointDataSpline jdata = jmodel.createData(); + + Eigen::VectorXd q(1); + q << 0; + jmodel.calc(jdata, q); + + Eigen::VectorXd bSpline_expected(4); + bSpline_expected << 1, 0, 0, 0; + BOOST_CHECK(bSpline_expected.isApprox(jdata.N)); + + q << 1; + jmodel.calc(jdata, q); + bSpline_expected << 0, 0, 0, 1; + BOOST_CHECK(bSpline_expected.isApprox(jdata.N)); + + // Check the integral + q << 0.3; + jmodel.calc(jdata, q); + BOOST_CHECK_CLOSE(jdata.N.sum(), 1, 1e-8); + + // Check first derivative + for (size_t i = 0; i < jmodel.nbCtrlFrames; i++) + { + double den1 = (jmodel.knots[i + degree] - jmodel.knots[i]); + double left = 0; + if (den1 > Eigen::NumTraits::dummy_precision()) + left = degree / den1 * jmodel.bsplineBasis(i, degree - 1, q[0]); + + double den2 = (jmodel.knots[i + degree + 1] - jmodel.knots[i + 1]); + double right = 0; + if (den2 > Eigen::NumTraits::dummy_precision()) + right = degree / den2 * jmodel.bsplineBasis(i + 1, degree - 1, q[0]); + + BOOST_CHECK_CLOSE(left - right, jdata.N_der[i], 1e-5); + } +} + +/// @brief Test the spanning function +BOOST_AUTO_TEST_CASE(findSpan) +{ + int degree = 2; + + std::vector ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + for (int k = 0; k < 10; k++) + ctrlFrames.push_back(SE3::Random()); + + JointModelSpline jmodel(ctrlFrames, degree); + + Eigen::VectorXd q(1); + q << 0.5; + SpanIndexes indexes = + pinocchio::FindSpan::run(q, degree, ctrlFrames.size(), jmodel.knots); + + BOOST_CHECK(indexes.start_idx == 5); + BOOST_CHECK(indexes.end_idx == 8); + + q[0] = 1; + indexes = pinocchio::FindSpan::run(q, degree, ctrlFrames.size(), jmodel.knots); + + BOOST_CHECK(indexes.start_idx == ctrlFrames.size() - (degree + 1)); + BOOST_CHECK(indexes.end_idx == ctrlFrames.size()); + + q[0] = 0; + indexes = pinocchio::FindSpan::run(q, degree, ctrlFrames.size(), jmodel.knots); + BOOST_CHECK(indexes.start_idx == 0); + BOOST_CHECK(indexes.end_idx == degree + 1); +} + +/// @brief Comparing a simple spline joint with a PZ +/// Make sure pose and joint subspace are the same +BOOST_AUTO_TEST_CASE(vsPrismaticZ) +{ + using namespace pinocchio; + typedef SE3::Vector3 Vector3; + typedef SE3::Matrix3 Matrix3; + + // Spline Joint + std::vector ctrlFrames; + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.))); + + JointModelSpline jmodel(ctrlFrames, 1); + JointDataSpline jdata = jmodel.createData(); + jmodel.setIndexes(0, 0, 0); + + // Prismatic joint + JointModelPZ jmodelPz; + JointDataPZ jdataPz = jmodelPz.createData(); + jmodelPz.setIndexes(0, 0, 0); + + Eigen::VectorXd q(Eigen::VectorXd::Zero(1)); + + // ------- + q << 0.2; + + jmodel.calc(jdata, q); + jmodelPz.calc(jdataPz, q); + + BOOST_CHECK(jdata.M.isApprox(jdataPz.M, 1e-12)); + BOOST_CHECK(jdata.S.matrix().isApprox(jdataPz.S.matrix(), 1e-12)); + + // ------- + Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1)); + q << 0.3; + q_dot << 0.4; + + jmodel.calc(jdata, q, q_dot); + jmodelPz.calc(jdataPz, q, q_dot); + + BOOST_CHECK(jdata.M.isApprox(jdataPz.M, 1e-12)); + BOOST_CHECK(jdata.S.matrix().isApprox(jdataPz.S.matrix(), 1e-12)); + BOOST_CHECK(jdata.v.isApprox(jdataPz.v, 1e-12)); + BOOST_CHECK(jdata.c.isApprox(jdataPz.c, 1e-12)); +} + +/// @brief Comparing a simple spline joint with a RX +/// Make sure pose and joint subspace are the same +BOOST_AUTO_TEST_CASE(vsRevoluteX) +{ + using namespace pinocchio; + typedef SE3::Vector3 Vector3; + typedef SE3::Matrix3 Matrix3; + + // Spline Joint + Eigen::Matrix3d rotation; + std::vector ctrlFrames; + Eigen::AngleAxisd Rx(1, Eigen::Vector3d::UnitX()); + + ctrlFrames.push_back(SE3::Identity()); + ctrlFrames.push_back(SE3(Rx.toRotationMatrix(), Eigen::Vector3d(0., 0., 0.))); + + JointModelSpline jmodel(ctrlFrames, 1); + JointDataSpline jdata = jmodel.createData(); + jmodel.setIndexes(0, 0, 0); + + // Prismatic joint + JointModelRX jmodelRx; + JointDataRX jdataRx = jmodelRx.createData(); + jmodelRx.setIndexes(0, 0, 0); + + Eigen::VectorXd q(Eigen::VectorXd::Zero(1)); + + // ------- + q << 0.2; + + jmodel.calc(jdata, q); + jmodelRx.calc(jdataRx, q); + + BOOST_CHECK(jdata.M.isApprox(jdataRx.M, 1e-12)); + BOOST_CHECK(jdata.S.matrix().isApprox(jdataRx.S.matrix(), 1e-12)); + + // ------- + Eigen::VectorXd q_dot(Eigen::VectorXd::Zero(1)); + q << 0.3; + q_dot << 0.4; + + jmodel.calc(jdata, q, q_dot); + jmodelRx.calc(jdataRx, q, q_dot); + + BOOST_CHECK(jdata.M.isApprox(jdataRx.M, 1e-12)); + BOOST_CHECK(jdata.S.matrix().isApprox(jdataRx.S.matrix(), 1e-12)); + BOOST_CHECK(jdata.v.isApprox(jdataRx.v, 1e-12)); + BOOST_CHECK(jdata.c.isApprox(jdataRx.c, 1e-12)); +} + +/// @brief Test out rnea vs aba +BOOST_AUTO_TEST_CASE(abaVSrnea) +{ + typedef SE3::Vector3 Vector3; + typedef SE3::Matrix3 Matrix3; + + Model modelSpline; + + Inertia inertia(1., Vector3(0., 0., 0.0), Matrix3::Identity()); + + std::vector ctrlFrames; + getTrajectory(ctrlFrames); + addJointAndBody( + modelSpline, JointModelSpline(ctrlFrames), 0, SE3::Identity(), "kneeSpline", inertia); + Data dataSplineRnea(modelSpline); + Data dataSplineAba(modelSpline); + + Eigen::VectorXd q(modelSpline.nq); + pinocchio::randomConfiguration( + modelSpline, Eigen::VectorXd::Zero(1), Eigen::VectorXd::Ones(1), q); + Eigen::VectorXd vq(Eigen::VectorXd::Random(modelSpline.nv)); + Eigen::VectorXd aq(Eigen::VectorXd::Random(modelSpline.nv)); + Eigen::VectorXd tauRnea(1); + Eigen::VectorXd aAba(1); + + tauRnea = pinocchio::rnea(modelSpline, dataSplineRnea, q, vq, aq); + aAba = pinocchio::aba(modelSpline, dataSplineAba, q, vq, tauRnea); + + BOOST_CHECK(aq.isApprox(aAba)); +} + +/// @brief Test S and bias c computation via finite differences +BOOST_AUTO_TEST_CASE(vsFiniteDifference) +{ + using namespace pinocchio; + + typedef typename JointModelSpline::ConfigVector_t CV; + typedef typename JointModelSpline::TangentVector_t TV; + typedef typename LieGroup::type LieGroupType; + + std::vector ctrlFrames; + getTrajectory(ctrlFrames); + + JointModelSpline jmodel(ctrlFrames); + JointDataSpline jdata = jmodel.createData(); + + jmodel.setIndexes(0, 0, 0); + + double eps = 1e-8; + CV q_ref(1); + q_ref[0] = 0.6; + CV q(q_ref); + + const Eigen::DenseIndex nv = jdata.S.nv(); + TV q_dot(nv); + TV q_dot_ref(nv); + q_dot.setZero(); + + q_dot[0] = eps; + q_dot_ref[0] = 0.3; + + q = LieGroupType().integrate(q_ref, q_dot); + // Check S + { + jmodel.calc(jdata, q_ref); + SE3 M_ref(jdata.M); + Eigen::Matrix S(6, JointModelSpline::NV), + S_ref(jdata.S.matrix()); + + jmodel.calc(jdata, q); + SE3 M_ = jdata.M; + + S.col(0) = log6(M_ref.inverse() * M_).toVector(); + S.col(0) /= eps; + + BOOST_CHECK(S.isApprox(S_ref, 1e-6)); + } + // Check bias + { + jmodel.calc(jdata, q_ref, q_dot_ref); + const Motion & c_ref = jdata.c; + Eigen::Matrix S_ref(jdata.S.matrix()); + + jmodel.calc(jdata, q); + Eigen::Matrix S_(jdata.S.matrix()); + + Motion dSdq_fd((S_ - S_ref) / eps); + Motion c_fd = dSdq_fd * q_dot_ref[0] * q_dot_ref[0]; + + BOOST_CHECK(c_ref.isApprox(c_fd, 1e-6)); + } +} + +BOOST_AUTO_TEST_SUITE_END() diff --git a/unittest/model-graph.cpp b/unittest/model-graph.cpp index ddf67bedc5..0ce799f4ab 100644 --- a/unittest/model-graph.cpp +++ b/unittest/model-graph.cpp @@ -406,6 +406,48 @@ BOOST_AUTO_TEST_CASE(test_mimic_joint) BOOST_CHECK(d.oMf[m.getFrameId("body3", pinocchio::BODY)].isApprox(bodyPose)); } +/// @brief test spline joint, and that we cannot reverse it +BOOST_AUTO_TEST_CASE(test_spline) +{ + using namespace pinocchio::graph; + + ModelGraph g; + //////////////////////////////////////// Bodies + g.addBody("body1", pinocchio::Inertia::Identity()); + g.addFrame("body2", BodyFrame(pinocchio::Inertia::Identity())); + + /////////////////////////////////////// Joints + pinocchio::SE3 pose_body1_joint1(Eigen::Matrix3d::Identity(), Eigen::Vector3d(2., 0., 0.)); + pinocchio::SE3 pose_body2_joint1(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 4., 0.)); + + pinocchio::SE3 finalPose(pinocchio::SE3::Random()); + std::vector ctrlFrames; + ctrlFrames.push_back(pinocchio::SE3::Identity()); + ctrlFrames.push_back(finalPose); + + g.edgeBuilder() + .withName("body1_to_body2") + .withSourceVertex("body1") + .withSourcePose(pose_body1_joint1) + .withTargetVertex("body2") + .withTargetPose(pose_body2_joint1) + .withJointType(JointSpline(ctrlFrames, 1)) + .build(); + + pinocchio::SE3 pose_body1_universe = pinocchio::SE3::Identity(); + pinocchio::Model m = buildModel(g, "body1", pose_body1_universe); + pinocchio::Data data(m); + + Eigen::VectorXd q = Eigen::VectorXd::Ones(m.nq); + pinocchio::framesForwardKinematics(m, data, q); + + BOOST_CHECK(data.oMf[m.getFrameId("body2", pinocchio::BODY)].isApprox( + pose_body1_joint1 * finalPose * pose_body2_joint1)); + + ///////////////// Reverse Model + BOOST_CHECK_THROW(buildModel(g, "body2", pinocchio::SE3::Identity()), std::invalid_argument); +} + /// @brief test out reverse joint for revolute BOOST_AUTO_TEST_CASE(test_reverse_revolute) { diff --git a/unittest/serialization.cpp b/unittest/serialization.cpp index fc810abb67..8ac5726345 100644 --- a/unittest/serialization.cpp +++ b/unittest/serialization.cpp @@ -412,6 +412,22 @@ struct init> } }; +template +struct init> +{ + typedef pinocchio::JointModelSplineTpl JointModel; + + static JointModel run() + { + PINOCCHIO_ALIGNED_STD_VECTOR(pinocchio::SE3) ctrlFrames; + ctrlFrames.push_back(pinocchio::SE3::Identity()); + ctrlFrames.push_back(pinocchio::SE3(Eigen::Matrix3d::Identity(), Eigen::Vector3d(0., 0., 1.))); + JointModel jmodel(ctrlFrames, 1); + jmodel.setIndexes(0, 0, 0); + return jmodel; + } +}; + struct TestJointModel { template @@ -465,6 +481,36 @@ struct TestJointTransform test(S); } + template + void operator()(const pinocchio::JointModelSplineTpl &) + { + typedef typename pinocchio::JointModelSplineTpl JointModel; + + typedef typename JointModel::JointDerived JointDerived; + typedef typename pinocchio::traits::Transformation_t Transform; + typedef typename pinocchio::traits::Constraint_t Constraint; + typedef typename pinocchio::JointDataSplineTpl JointData; + + JointModel jmodel = init::run(); + + JointData jdata = jmodel.createData(); + + typedef typename pinocchio::LieGroup::type LieGroupType; + LieGroupType lg; + + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), 0)); + Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + + Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); + + jmodel.calc(jdata, q_random); + Transform & m = jdata.M; + test(m); + + Constraint & S = jdata.S; + test(S); + } + template class JointCollectionTpl> void operator()(const pinocchio::JointModelCompositeTpl &) { @@ -523,6 +569,38 @@ struct TestJointMotion test(b); } + template + void operator()(const pinocchio::JointModelSplineTpl &) + { + typedef typename pinocchio::JointModelSplineTpl JointModel; + + typedef typename JointModel::JointDerived JointDerived; + typedef typename pinocchio::traits::Motion_t Motion; + typedef typename pinocchio::traits::Bias_t Bias; + typedef typename pinocchio::JointDataSplineTpl JointData; + + JointModel jmodel = init::run(); + + JointData jdata = jmodel.createData(); + + typedef typename pinocchio::LieGroup::type LieGroupType; + LieGroupType lg; + + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), 0)); + Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + + Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); + Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + + jmodel.calc(jdata, q_random, v_random); + Motion & m = jdata.v; + + test(m); + + Bias & b = jdata.c; + test(b); + } + template class JointCollectionTpl> void operator()(const pinocchio::JointModelCompositeTpl &) { @@ -612,6 +690,8 @@ struct TestJointData JointModel jmodel = init::run(); JointData jdata = jmodel.createData(); + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), 0)); + Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); Eigen::VectorXd q_random = Eigen::VectorXd::Random(jmodel.jmodel().nq()); Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.jmodel().nv()); jmodel.calc(jdata, q_random, v_random); @@ -619,6 +699,31 @@ struct TestJointData test(jdata); } + template + void operator()(const pinocchio::JointModelSplineTpl &) + { + typedef pinocchio::JointModelSplineTpl JointModel; + typedef typename JointModel::JointDerived JointDerived; + typedef typename pinocchio::traits::JointDataDerived JointData; + + JointModel jmodel = init::run(); + JointData jdata = jmodel.createData(); + + typedef typename pinocchio::LieGroup::type LieGroupType; + LieGroupType lg; + + Eigen::VectorXd lb(Eigen::VectorXd::Constant(jmodel.nq(), 0)); + Eigen::VectorXd ub(Eigen::VectorXd::Constant(jmodel.nq(), 1.)); + + Eigen::VectorXd q_random = lg.randomConfiguration(lb, ub); + Eigen::VectorXd v_random = Eigen::VectorXd::Random(jmodel.nv()); + + jmodel.calc(jdata, q_random, v_random); + pinocchio::Inertia::Matrix6 I(pinocchio::Inertia::Matrix6::Identity()); + jmodel.calc_aba(jdata, Eigen::VectorXd::Zero(jmodel.nv()), I, false); + test(jdata); + } + template static void test(JointData & joint_data) {