diff --git a/CHANGELOG.md b/CHANGELOG.md index 9e2377af42..a717dfdbc0 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -11,6 +11,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] ### Added +- Add core single-precision Python bindings through the `pinocchio.float32` module - Add `DataTpl::lastChild` deprecation notice in Python binding - Fix `addFrame` to ignore frame without inertial to preserse parent body's CoM diff --git a/CMakeLists.txt b/CMakeLists.txt index dc95c6f58e..5990a33ef0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -187,6 +187,13 @@ cmake_dependent_option( BUILD_PYTHON_INTERFACE OFF ) +cmake_dependent_option( + BUILD_PYTHON_BINDINGS_WITH_FLOAT32_SUPPORT + "Build the Python interface with single-precision support" + ON + BUILD_PYTHON_INTERFACE + OFF +) cmake_dependent_option( BUILD_WITH_LIBPYTHON "Build the library with Python format support" diff --git a/bindings/python/CMakeLists.txt b/bindings/python/CMakeLists.txt index ab5cb488c3..e27583885a 100644 --- a/bindings/python/CMakeLists.txt +++ b/bindings/python/CMakeLists.txt @@ -222,6 +222,22 @@ if(BUILD_PYTHON_INTERFACE) set(PYTHON_LIB_NAME "${PYWRAP}_default") set(STUBGEN_DEPENDENCIES "${PYWRAP}_default") + if(BUILD_PYTHON_BINDINGS_WITH_FLOAT32_SUPPORT) + pinocchio_python_bindings_specific_type(float32) + list(APPEND STUBGEN_DEPENDENCIES "${PYWRAP}_float32") + + if(PINOCCHIO_BUILD_BINDING_WITH_PCH) + target_precompile_headers( + ${PYWRAP}_float32 + PRIVATE + ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/pch.hpp + ) + endif() + + # --- INSTALL SCRIPTS + install_python_files(MODULE float32 FILES __init__.py explog.py utils.py) + endif() + if(BUILD_WITH_AUTODIFF_SUPPORT) pinocchio_python_bindings_specific_type(cppad cppad) # CPPAD_DEBUG_AND_RELEASE allow to mix debug and release versions of CppAD in the same program. diff --git a/bindings/python/algorithm/expose-frames.cpp b/bindings/python/algorithm/expose-frames.cpp index 3363115b8c..b15460b296 100644 --- a/bindings/python/algorithm/expose-frames.cpp +++ b/bindings/python/algorithm/expose-frames.cpp @@ -286,7 +286,7 @@ namespace pinocchio bp::def( "computeSupportedInertiaByFrame", - &computeSupportedInertiaByFrame, + &computeSupportedInertiaByFrame, bp::args("model", "data", "frame_id", "with_subtree"), "Computes the supported inertia by the frame (given by frame_id) and returns it.\n" "The supported inertia corresponds to the sum of the inertias of all the child frames " @@ -296,7 +296,7 @@ namespace pinocchio bp::def( "computeSupportedForceByFrame", - &computeSupportedForceByFrame, + &computeSupportedForceByFrame, bp::args("model", "data", "frame_id"), "Computes the supported force of the frame (given by frame_id) and returns it.\n" "The supported force corresponds to the sum of all the forces experienced after the given " diff --git a/bindings/python/algorithm/expose-model.cpp b/bindings/python/algorithm/expose-model.cpp index 1523ce03db..49cff39bc8 100644 --- a/bindings/python/algorithm/expose-model.cpp +++ b/bindings/python/algorithm/expose-model.cpp @@ -101,15 +101,17 @@ namespace pinocchio void exposeModelAlgo() { using namespace Eigen; + typedef context::Scalar Scalar; + using context::Options; typedef std::vector GeometryModelVector; StdVectorPythonVisitor::expose("StdVec_GeometryModel"); bp::def( "appendModel", - (Model (*)( - const Model &, const Model &, const FrameIndex, - const SE3 &))&appendModel, + (context::Model (*)( + const context::Model &, const context::Model &, const FrameIndex, + const context::SE3 &))&appendModel, bp::args("modelA", "modelB", "frame_in_modelA", "aMb"), "Append a child model into a parent model, after a specific frame given by its index.\n\n" "Parameters:\n" @@ -119,7 +121,7 @@ namespace pinocchio "\taMb: pose of modelB universe joint (index 0) in frameInModelA\n"); bp::def( - "appendModel", &appendModel_proxy, + "appendModel", &appendModel_proxy, bp::args("modelA", "modelB", "geomModelA", "geomModelB", "frame_in_modelA", "aMb"), "Append a child (geometry) model into a parent (geometry) model, after a specific frame " "given by its index.\n\n" @@ -133,10 +135,10 @@ namespace pinocchio bp::def( "buildReducedModel", - (Model (*)( - const Model &, const std::vector &, - const Eigen::MatrixBase &))&pinocchio:: - buildReducedModel, + (context::Model (*)( + const context::Model &, const std::vector &, + const Eigen::MatrixBase &))&pinocchio:: + buildReducedModel, bp::args("model", "list_of_joints_to_lock", "reference_configuration"), "Build a reduce model from a given input model and a list of joint to lock.\n\n" "Parameters:\n" @@ -148,9 +150,10 @@ namespace pinocchio bp::def( "buildReducedModel", (bp::tuple (*)( - const Model &, const GeometryModel &, const std::vector &, + const context::Model &, const GeometryModel &, const std::vector &, const Eigen::MatrixBase< - VectorXd> &))&buildReducedModel, + context:: + VectorXs> &))&buildReducedModel, bp::args("model", "geom_model", "list_of_joints_to_lock", "reference_configuration"), "Build a reduced model and a reduced geometry model from a given input model," "an input geometry model and a list of joints to lock.\n\n" @@ -164,9 +167,9 @@ namespace pinocchio bp::def( "buildReducedModel", (bp::tuple (*)( - const Model &, const std::vector &, const std::vector &, - const Eigen::MatrixBase &)) - buildReducedModel, + const context::Model &, const std::vector &, + const std::vector &, const Eigen::MatrixBase &)) + buildReducedModel, bp::args( "model", "list_of_geom_models", "list_of_joints_to_lock", "reference_configuration"), "Build a reduced model and the related reduced geometry models from a given " @@ -191,7 +194,7 @@ namespace pinocchio bp::def( "transformJointIntoMimic", - transformJointIntoMimic_proxy, + transformJointIntoMimic_proxy, bp::args("input_model", "index_mimicked", "index_mimicking", "scaling", "offset"), "Transform of a joint of the model into a mimic joint. Keep the type of the joint as it " "was previously. \n\n" diff --git a/bindings/python/multibody/sample-models.cpp b/bindings/python/multibody/sample-models.cpp index c4fd81a245..f02140fda3 100644 --- a/bindings/python/multibody/sample-models.cpp +++ b/bindings/python/multibody/sample-models.cpp @@ -3,6 +3,7 @@ // #include "pinocchio/multibody/sample-models.hpp" +#include "pinocchio/bindings/python/fwd.hpp" #include @@ -12,22 +13,22 @@ namespace pinocchio { namespace bp = boost::python; - Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) + context::Model buildSampleModelHumanoidRandom(bool usingFF, bool mimic) { - Model model; + context::Model model; buildModels::humanoidRandom(model, usingFF, mimic); return model; } - Model buildSampleModelManipulator(bool mimic) + context::Model buildSampleModelManipulator(bool mimic) { - Model model; + context::Model model; buildModels::manipulator(model, mimic); return model; } #ifdef PINOCCHIO_WITH_COLLISION - GeometryModel buildSampleGeometryModelManipulator(const Model & model) + GeometryModel buildSampleGeometryModelManipulator(const context::Model & model) { GeometryModel geom; buildModels::manipulatorGeometries(model, geom); @@ -35,15 +36,15 @@ namespace pinocchio } #endif - Model buildSampleModelHumanoid(bool usingFF) + context::Model buildSampleModelHumanoid(bool usingFF) { - Model model; + context::Model model; buildModels::humanoid(model, usingFF); return model; } #ifdef PINOCCHIO_WITH_COLLISION - GeometryModel buildSampleGeometryModelHumanoid(const Model & model) + GeometryModel buildSampleGeometryModelHumanoid(const context::Model & model) { GeometryModel geom; buildModels::humanoidGeometries(model, geom); @@ -55,34 +56,35 @@ namespace pinocchio { bp::def( "buildSampleModelHumanoidRandom", - static_cast(pinocchio::python::buildSampleModelHumanoidRandom), + static_cast( + pinocchio::python::buildSampleModelHumanoidRandom), (bp::arg("using_free_flyer") = true, bp::arg("mimic") = false), "Generate a (hard-coded) model of a humanoid robot with 6-DOF limbs and random joint " "placements.\nOnly meant for unit tests."); bp::def( "buildSampleModelManipulator", - static_cast(pinocchio::python::buildSampleModelManipulator), + static_cast(pinocchio::python::buildSampleModelManipulator), (bp::arg("mimic") = false), "Generate a (hard-coded) model of a simple manipulator."); #ifdef PINOCCHIO_WITH_COLLISION bp::def( "buildSampleGeometryModelManipulator", - static_cast( + static_cast( pinocchio::python::buildSampleGeometryModelManipulator), bp::args("model"), "Generate a (hard-coded) geometry model of a simple manipulator."); #endif bp::def( "buildSampleModelHumanoid", - static_cast(pinocchio::python::buildSampleModelHumanoid), + static_cast(pinocchio::python::buildSampleModelHumanoid), (bp::arg("using_free_flyer") = true), "Generate a (hard-coded) model of a simple humanoid."); #ifdef PINOCCHIO_WITH_COLLISION bp::def( "buildSampleGeometryModelHumanoid", - static_cast( + static_cast( pinocchio::python::buildSampleGeometryModelHumanoid), bp::args("model"), "Generate a (hard-coded) geometry model of a simple humanoid."); #endif diff --git a/bindings/python/pinocchio/float32/__init__.py b/bindings/python/pinocchio/float32/__init__.py new file mode 100644 index 0000000000..1c10beb5d2 --- /dev/null +++ b/bindings/python/pinocchio/float32/__init__.py @@ -0,0 +1,47 @@ +# +# Copyright (c) 2026 Heriot-Watt University +# +"""Single-precision bindings for Pinocchio's kinematics and dynamics API. + +Models, spatial types, algorithms, and NumPy results exposed here use ``float32``. +File parsers, parallel algorithms, and reachable-workspace helpers remain available +only from the main module. Geometry and collision objects keep their existing scalar +representation when used with a float32 model. +""" + +# ruff: noqa: F401, F403, F405 +# Manually register submodules +import inspect +import sys + +from .. import pinocchio_pywrap_float32 as _pinocchio_pywrap_float32 +from ..pinocchio_pywrap_float32 import * +from ..pinocchio_pywrap_float32 import __raw_version__, __version__ +from . import utils +from .explog import exp, log + +submodules = inspect.getmembers(_pinocchio_pywrap_float32, inspect.ismodule) +for module_info in submodules: + sys.modules[__name__ + "." + module_info[0]] = module_info[1] + +if WITH_COLLISION: + import coal + from coal import ( + CachedMeshLoader, + CollisionGeometry, + CollisionResult, + Contact, + DistanceResult, + MeshLoader, + StdVec_CollisionResult, + StdVec_Contact, + StdVec_DistanceResult, + ) + + # Pickling support becauso Vec3s is registered by + # coal and pinocchio (see pinocchio/binding/python/multibody/data.hpp) + coal.StdVec_Vec3s.__safe_for_unpickling__ = True + coal.StdVec_Vec3s.__getstate_manages_dict__ = True + + # Deprecated, should be removed in next major release + hppfcl = coal diff --git a/bindings/python/pinocchio/float32/explog.py b/bindings/python/pinocchio/float32/explog.py new file mode 100644 index 0000000000..a96fd312ce --- /dev/null +++ b/bindings/python/pinocchio/float32/explog.py @@ -0,0 +1,46 @@ +# +# Copyright (c) 2015-2018 CNRS INRIA +# Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. +# Copyright (c) 2026 Heriot-Watt University +# + +import math + +import numpy as np + +from .. import pinocchio_pywrap_float32 as pin + + +def exp(x): + if isinstance(x, pin.Motion): + return pin.exp6(x) + if np.isscalar(x): + return math.exp(x) + if isinstance(x, np.ndarray): + if x.shape == (6, 1) or x.shape == (6,): + return pin.exp6(pin.Motion(x)) + if x.shape == (3, 1) or x.shape == (3,): + return pin.exp3(x) + raise ValueError("Error only 3 and 6 vectors are allowed.") + raise ValueError( + "Error exp is only defined for real, vector3, vector6 and pin.Motion objects." + ) + + +def log(x): + if isinstance(x, pin.SE3): + return pin.log6(x) + if np.isscalar(x): + return math.log(x) + if isinstance(x, np.ndarray): + if x.shape == (4, 4): + return pin.log6(x) + if x.shape == (3, 3): + return pin.log3(x) + raise ValueError("Error only 3 and 4 matrices are allowed.") + raise ValueError( + "Error log is only defined for real, matrix3, matrix4 and pin.SE3 objects." + ) + + +__all__ = ["exp", "log"] diff --git a/bindings/python/pinocchio/float32/utils.py b/bindings/python/pinocchio/float32/utils.py new file mode 100644 index 0000000000..3a1ec0fbba --- /dev/null +++ b/bindings/python/pinocchio/float32/utils.py @@ -0,0 +1,115 @@ +# +# Copyright (c) 2015-2022 CNRS INRIA +# Copyright (c) 2026 Heriot-Watt University +# + +import sys + +import numpy as np +import numpy.linalg as npl + +from .. import pinocchio_pywrap_float32 as pin + +matrixToRpy = pin.rpy.matrixToRpy +rotate = pin.rpy.rotate +rpyToMatrix = pin.rpy.rpyToMatrix + + +def npToTTuple(M): + L = M.tolist() + for i in range(len(L)): + L[i] = tuple(L[i]) + return tuple(L) + + +def npToTuple(M): + if len(M.shape) == 1: + return tuple(M.tolist()) + if M.shape[0] == 1: + return tuple(M.tolist()[0]) + if M.shape[1] == 1: + return tuple(M.T.tolist()[0]) + return npToTTuple(M) + + +def eye(n): + return np.eye(n, dtype=np.float32) + + +def zero(n): + return np.zeros(n, dtype=np.float32) + + +def rand(n): + shape = (n,) if isinstance(n, int) else (n[0], n[1]) + return np.random.rand(*shape).astype(np.float32) + + +def isapprox(a, b, epsilon=1e-6): + if "np" in a.__class__.__dict__: + a = a.np + if "np" in b.__class__.__dict__: + b = b.np + if isinstance(a, (np.ndarray, list)) and isinstance(b, (np.ndarray, list)): + a = np.squeeze(np.array(a)) + b = np.squeeze(np.array(b)) + return np.allclose(a, b, epsilon) + return abs(a - b) < epsilon + + +def mprint(M, name="ans", eps=1e-15): + """ + Matlab-style pretty matrix print. + """ + if isinstance(M, pin.SE3): + M = M.homogeneous + if len(M.shape) == 1: + M = np.expand_dims(M, axis=0) + ncol = M.shape[1] + NC = 6 + print(name, " = ") + print() + + Mm = (abs(M[np.nonzero(M)])).min() + MM = (abs(M[np.nonzero(M)])).max() + + fmt = "% 10.3e" if Mm < 1e-5 or MM > 1e6 or MM / Mm > 1e3 else "% 1.5f" + + for i in range((ncol - 1) // NC + 1): + cmin = i * 6 + cmax = (i + 1) * 6 + cmax = ncol if ncol < cmax else cmax + print(f"Columns {cmin} through {cmax - 1}") + print() + for r in range(M.shape[0]): + sys.stdout.write(" ") + for c in range(cmin, cmax): + if abs(M[r, c]) > eps: + sys.stdout.write(fmt % M[r, c] + " ") + else: + sys.stdout.write(" 0" + " " * 9) + print() + print() + + +def fromListToVectorOfString(items): + vector = pin.StdVec_StdString() + vector.extend(item for item in items) + return vector + + +__all__ = [ + "eye", + "fromListToVectorOfString", + "isapprox", + "matrixToRpy", + "mprint", + "np", + "npToTTuple", + "npToTuple", + "npl", + "rand", + "rotate", + "rpyToMatrix", + "zero", +] diff --git a/bindings/python/utils/dependencies.cpp b/bindings/python/utils/dependencies.cpp index 20e43b7b20..d22f7f7222 100644 --- a/bindings/python/utils/dependencies.cpp +++ b/bindings/python/utils/dependencies.cpp @@ -3,6 +3,7 @@ // #include +#include #include @@ -65,8 +66,11 @@ namespace pinocchio void exposeDependencies() { - bp::class_("DeprecatedBool", bp::no_init) - .def("__bool__", &DeprecatedBool::__bool__); + if (!eigenpy::register_symbolic_link_to_registered_type()) + { + bp::class_("DeprecatedBool", bp::no_init) + .def("__bool__", &DeprecatedBool::__bool__); + } bp::scope().attr("WITH_COLLISION") = WITH_COLLISION; // To conserve back compatibility diff --git a/include/pinocchio/bindings/python/context/float32.hpp b/include/pinocchio/bindings/python/context/float32.hpp new file mode 100644 index 0000000000..6a63d5fa78 --- /dev/null +++ b/include/pinocchio/bindings/python/context/float32.hpp @@ -0,0 +1,29 @@ +// +// Copyright (c) 2026 Heriot-Watt University +// + +#pragma once + +#define PINOCCHIO_PYTHON_SCALAR_TYPE float +#define PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE +#define PINOCCHIO_PYTHON_SKIP_REACHABLE_WORKSPACE + +#include "pinocchio/bindings/python/context/generic.hpp" +#include + +namespace pinocchio +{ + namespace python + { + + inline void exposeSpecificTypeFeatures() {}; + + inline boost::python::object getScalarType() + { + namespace bp = boost::python; + return bp::import("numpy").attr("float32"); + } + } // namespace python +} // namespace pinocchio + +#undef PINOCCHIO_PYTHON_SCALAR_TYPE diff --git a/include/pinocchio/src/algorithm/frames.hxx b/include/pinocchio/src/algorithm/frames.hxx index 6ebd99032d..e272b2dd0d 100644 --- a/include/pinocchio/src/algorithm/frames.hxx +++ b/include/pinocchio/src/algorithm/frames.hxx @@ -381,6 +381,8 @@ namespace pinocchio assert(model.check(data) && "data is not consistent with model."); typedef ModelTpl Model; + typedef typename Model::Frame Frame; + typedef typename Model::SE3 SE3; typedef InertiaTpl Inertia; const Frame & frame = model.frames[frame_id]; @@ -419,7 +421,7 @@ namespace pinocchio I += data.oMi[j_id].act(model.inertias[j_id]); } - const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement; + const SE3 oMf = data.oMi[joint_id] * frame.placement; return oMf.actInv(I); } @@ -430,6 +432,8 @@ namespace pinocchio const FrameIndex frame_id) { typedef ModelTpl Model; + typedef typename Model::Frame Frame; + typedef typename Model::SE3 SE3; typedef InertiaTpl Inertia; typedef MotionTpl Motion; typedef ForceTpl Force; @@ -439,7 +443,7 @@ namespace pinocchio // Compute 'in body' forces const Inertia fI = computeSupportedInertiaByFrame(model, data, frame_id, false); - const pinocchio::SE3 oMf = data.oMi[joint_id] * frame.placement; + const SE3 oMf = data.oMi[joint_id] * frame.placement; const Motion v = getFrameVelocity(model, data, frame_id, LOCAL); const Motion a = getFrameAcceleration(model, data, frame_id, LOCAL); Force f = fI.vxiv(v) + fI * (a - oMf.actInv(model.gravity)); diff --git a/include/pinocchio/src/algorithm/loop-constrained-aba.hxx b/include/pinocchio/src/algorithm/loop-constrained-aba.hxx index f74a1ad68b..1d93be1477 100644 --- a/include/pinocchio/src/algorithm/loop-constrained-aba.hxx +++ b/include/pinocchio/src/algorithm/loop-constrained-aba.hxx @@ -282,6 +282,7 @@ namespace pinocchio typedef std::pair JointPair; typedef typename Data::Matrix6 Matrix6; + typedef typename Data::Force Force; typedef boost::fusion::vector ArgsType; @@ -293,6 +294,7 @@ namespace pinocchio Data & data) { typedef typename Model::JointIndex JointIndex; + typedef typename Data::Force Force; typedef typename Data::Matrix6x Matrix6x; typedef typename SizeDepType::template ColsReturn::Type ColBlock; @@ -337,6 +339,7 @@ namespace pinocchio typedef std::pair JointPair; typedef typename Data::Matrix6 Matrix6; + typedef typename Data::Force Force; typedef boost::fusion::vector ArgsType; @@ -411,6 +414,7 @@ namespace pinocchio typedef typename Model::JointIndex JointIndex; typedef typename Data::Matrix6 Matrix6; typedef typename ConstraintModel::Matrix36 Matrix36; + typedef typename ConstraintData::Motion Motion; cdata.contact_force.setZero(); @@ -600,6 +604,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointIndex JointIndex; typedef typename ConstraintModel::Matrix36 Matrix36; + typedef typename ConstraintData::Force Force; data.u = tau; data.oa_gf[0] = -model.gravity; @@ -680,7 +685,7 @@ namespace pinocchio contact_acc_err = cdata.oMc1.actInv((data.oa[joint1_id])) - cdata.contact_acceleration_desired; - const auto mu_lambda = Force(mu * contact_acc_err.toVector()); + const Force mu_lambda = Force(mu * contact_acc_err.toVector()); cdata.contact_force += mu_lambda; if (joint1_id > 0) @@ -698,7 +703,7 @@ namespace pinocchio contact_acc_err.linear() -= cdata.c1Mc2.rotation() * cdata.oMc2.actInv(data.oa[joint2_id]).linear(); - const auto mu_lambda = Force(mu * contact_acc_err.toVector()); + const Force mu_lambda = Force(mu * contact_acc_err.toVector()); cdata.contact_force.linear() += mu_lambda.linear(); if (joint1_id > 0) diff --git a/include/pinocchio/src/algorithm/model.hxx b/include/pinocchio/src/algorithm/model.hxx index 4db3e46a5c..30659055aa 100644 --- a/include/pinocchio/src/algorithm/model.hxx +++ b/include/pinocchio/src/algorithm/model.hxx @@ -127,7 +127,9 @@ namespace pinocchio { go.parentFrame = parentFrame; } - go.placement = (pframe_placement * pfMAB) * go.placement; + typedef typename GeometryObject::SE3 GeometrySE3; + const GeometrySE3 geometry_placement(pframe_placement * pfMAB); + go.placement = geometry_placement * go.placement; geomModel.addGeometryObject(go); } } @@ -843,7 +845,7 @@ namespace pinocchio { const FrameIndex reduced_frame_id = reduced_model.getFrameId(parent_joint_name); reduced_joint_id = reduced_model.frames[reduced_frame_id].parentJoint; - relative_placement = reduced_model.frames[reduced_frame_id].placement; + relative_placement = SE3(reduced_model.frames[reduced_frame_id].placement); } GeometryObject reduced_geom(geom); @@ -890,6 +892,7 @@ namespace pinocchio typedef ModelTpl Model; typedef typename Model::JointModel JointModel; + typedef JointModelMimicTpl JointModelMimic; output_model = input_model; diff --git a/include/pinocchio/src/algorithm/solvers/admm-solver.hxx b/include/pinocchio/src/algorithm/solvers/admm-solver.hxx index 8737521a52..445340dcb2 100644 --- a/include/pinocchio/src/algorithm/solvers/admm-solver.hxx +++ b/include/pinocchio/src/algorithm/solvers/admm-solver.hxx @@ -145,7 +145,8 @@ namespace pinocchio break; } case (ADMMUpdateRule::OSQP): - admm_update_rule_container.osqp_rule = ADMMOSQPUpdateRule(settings.ratio_primal_dual, 1e-8); + admm_update_rule_container.osqp_rule = + ADMMOSQPUpdateRule(settings.ratio_primal_dual, Scalar(1e-8)); break; case (ADMMUpdateRule::LINEAR): admm_update_rule_container.linear_rule = @@ -518,7 +519,7 @@ namespace pinocchio { PINOCCHIO_TRACY_ZONE_SCOPED_N("ADMMConstraintSolverTpl::solve - lanczos"); workspace.lanczos_decomposition.compute(G); - L = ::pinocchio::computeLargestEigenvalue(workspace.lanczos_decomposition.Ts(), 1e-8); + L = ::pinocchio::computeLargestEigenvalue(workspace.lanczos_decomposition.Ts(), Scalar(1e-8)); #ifndef NDEBUG const bool enforce_symmetry = true; MatrixXs delassus = G.matrix(enforce_symmetry); diff --git a/include/pinocchio/src/multibody/sample-models.hxx b/include/pinocchio/src/multibody/sample-models.hxx index a4a521a978..c928f16893 100644 --- a/include/pinocchio/src/multibody/sample-models.hxx +++ b/include/pinocchio/src/multibody/sample-models.hxx @@ -103,6 +103,9 @@ namespace pinocchio ModelTpl::SE3::Random(), bool setRandomLimits = true) { + typedef ModelTpl Model; + typedef typename Model::Inertia Inertia; + typedef typename Model::SE3 SE3; typedef typename JointModel::ConfigVector_t CV; typedef typename JointModel::TangentVector_t TV; @@ -220,7 +223,7 @@ namespace pinocchio model.addBodyFrame(pre + "effector_body", joint_id); const int nq = mimic ? 5 : 6; - const JointModel & base_joint = model.joints[root_joint_id]; + const typename Model::JointModel & base_joint = model.joints[root_joint_id]; const int idx_q = base_joint.idx_q(); const int idx_v = base_joint.idx_v(); @@ -255,7 +258,7 @@ namespace pinocchio { typedef ModelTpl Model; typedef typename Model::FrameIndex FrameIndex; - typedef typename Model::SE3 SE3; + typedef typename GeometryObject::SE3 SE3; const Eigen::Vector4d meshColor(1., 1., 0.78, 1.0); @@ -512,7 +515,7 @@ namespace pinocchio { typedef ModelTpl Model; typedef typename Model::FrameIndex FrameIndex; - typedef typename Model::SE3 SE3; + typedef typename GeometryObject::SE3 SE3; details::addManipulatorGeometries(model, geom, "rleg_"); details::addManipulatorGeometries(model, geom, "lleg_"); diff --git a/sources.cmake b/sources.cmake index e9ff793a62..216b24be92 100644 --- a/sources.cmake +++ b/sources.cmake @@ -875,6 +875,7 @@ set(${PROJECT_NAME}_BINDINGS_PYTHON_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/context/cppad.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/context/casadi.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/context/default.hpp + ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/context/float32.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/context/mpfr.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/context/generic.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/bindings/python/fwd.hpp diff --git a/unittest/python/CMakeLists.txt b/unittest/python/CMakeLists.txt index 30b710e3b1..0fca45d0cb 100644 --- a/unittest/python/CMakeLists.txt +++ b/unittest/python/CMakeLists.txt @@ -53,6 +53,10 @@ set(${PROJECT_NAME}_PYTHON_TESTS bindings_std_map ) +if(BUILD_PYTHON_BINDINGS_WITH_FLOAT32_SUPPORT) + list(APPEND ${PROJECT_NAME}_PYTHON_TESTS bindings_float32) +endif() + function(pinocchio_add_python_cpp_module name) set(target_name "test-ext-${name}") string(REPLACE "_" "-" target_name ${target_name}) diff --git a/unittest/python/bindings_float32.py b/unittest/python/bindings_float32.py new file mode 100644 index 0000000000..b63ab598b1 --- /dev/null +++ b/unittest/python/bindings_float32.py @@ -0,0 +1,244 @@ +# +# Copyright (c) 2026 Heriot-Watt University +# + +import importlib +import subprocess +import sys +import unittest + +import numpy as np +import pinocchio as pin +import pinocchio.float32 as pin32 + + +class TestFloat32Bindings(unittest.TestCase): + def test_sample_models(self): + for factory in ( + pin32.buildSampleModelManipulator, + pin32.buildSampleModelHumanoid, + pin32.buildSampleModelHumanoidRandom, + ): + model = factory() + self.assertIsInstance(model, pin32.Model) + self.assertNotIsInstance(model, pin.Model) + self.assertEqual(pin32.neutral(model).dtype, np.float32) + + @unittest.skipUnless(pin32.WITH_COLLISION, "Needs collision support") + def test_sample_geometry_models(self): + for model_factory, geometry_factory in ( + ( + pin32.buildSampleModelManipulator, + pin32.buildSampleGeometryModelManipulator, + ), + ( + pin32.buildSampleModelHumanoid, + pin32.buildSampleGeometryModelHumanoid, + ), + ): + geometry_model = geometry_factory(model_factory()) + self.assertIsInstance(geometry_model, pin.GeometryModel) + self.assertGreater(geometry_model.ngeoms, 0) + + def test_rnea(self): + self.assertIs(pin32.ScalarType, np.float32) + self.assertIsNot(pin32.Model, pin.Model) + + model = pin.buildSampleModelHumanoidRandom() + model32 = pin32.Model(model) + data = model.createData() + data32 = model32.createData() + + q = pin.neutral(model) + v = np.linspace(-0.5, 0.5, model.nv) + a = np.linspace(0.5, -0.5, model.nv) + q32 = q.astype(np.float32) + v32 = v.astype(np.float32) + a32 = a.astype(np.float32) + + tau = pin.rnea(model, data, q, v, a) + tau32 = pin32.rnea(model32, data32, q32, v32, a32) + + self.assertEqual(tau32.dtype, np.float32) + np.testing.assert_allclose(tau32, tau, rtol=1e-5, atol=1e-5) + + def test_model_algorithms(self): + model = pin32.buildSampleModelManipulator() + q = pin32.neutral(model) + + appended = pin32.appendModel(model, pin32.Model(), 0, pin32.SE3.Identity()) + reduced = pin32.buildReducedModel(model, [1], q) + mimic = pin32.transformJointIntoMimic(model, 1, 2, 1.0, 0.0) + appended_with_geometry = pin32.appendModel( + model, + pin32.Model(), + pin.GeometryModel(), + pin.GeometryModel(), + 0, + pin32.SE3.Identity(), + ) + reduced_with_geometry = pin32.buildReducedModel( + model, pin.GeometryModel(), [1], q + ) + + self.assertIsInstance(appended, pin32.Model) + self.assertIsInstance(reduced, pin32.Model) + self.assertIsInstance(mimic, pin32.Model) + self.assertIsInstance(appended_with_geometry[0], pin32.Model) + self.assertIsInstance(appended_with_geometry[1], pin.GeometryModel) + self.assertIsInstance(reduced_with_geometry[0], pin32.Model) + self.assertIsInstance(reduced_with_geometry[1], pin.GeometryModel) + + def test_supported_frame_quantities(self): + model = pin32.buildSampleModelManipulator() + data = model.createData() + q = pin32.neutral(model) + v = np.zeros(model.nv, dtype=np.float32) + a = np.zeros(model.nv, dtype=np.float32) + frame_id = model.getFrameId("effector_body") + + pin32.forwardKinematics(model, data, q) + inertia = pin32.computeSupportedInertiaByFrame(model, data, frame_id, True) + pin32.rnea(model, data, q, v, a) + force = pin32.computeSupportedForceByFrame(model, data, frame_id) + + self.assertIsInstance(inertia, pin32.Inertia) + self.assertIsInstance(force, pin32.Force) + self.assertEqual(inertia.matrix().dtype, np.float32) + self.assertEqual(force.vector.dtype, np.float32) + + def test_python_helpers(self): + vector3 = np.ones(3, dtype=np.float32) + matrix3 = np.eye(3, dtype=np.float32) + + self.assertEqual(pin32.exp(vector3).dtype, np.float32) + self.assertEqual(pin32.log(matrix3).dtype, np.float32) + self.assertIsInstance(pin32.exp(pin32.Motion.Zero()), pin32.SE3) + self.assertIsInstance(pin32.log(pin32.SE3.Identity()), pin32.Motion) + + self.assertEqual(pin32.utils.eye(3).dtype, np.float32) + self.assertEqual(pin32.utils.zero(3).dtype, np.float32) + self.assertEqual(pin32.utils.rand(3).dtype, np.float32) + self.assertEqual(pin32.utils.rpyToMatrix(vector3).dtype, np.float32) + + def test_submodule_imports(self): + for module_name in ( + "cholesky", + "liegroups", + "linalg", + "rpy", + "serialization", + "utils", + "explog", + ): + module = importlib.import_module(f"pinocchio.float32.{module_name}") + self.assertIsNotNone(module) + + def test_import_has_no_duplicate_converter_warning(self): + process = subprocess.run( + [sys.executable, "-c", "import pinocchio.float32"], + check=True, + capture_output=True, + text=True, + ) + self.assertNotIn("converter already registered", process.stderr) + + def test_lcaba(self): + model = pin32.buildSampleModelManipulator() + data = model.createData() + constraint_model = pin32.RigidConstraintModel( + pin32.ContactType.CONTACT_3D, + model, + model.njoints - 1, + pin32.SE3.Identity(), + 0, + pin32.SE3.Identity(), + ) + constraint_models = pin32.StdVec_RigidConstraintModel() + constraint_models.append(constraint_model) + constraint_datas = pin32.StdVec_RigidConstraintData() + constraint_datas.append(constraint_model.createData()) + + pin32.computeJointMinimalOrdering(model, data, constraint_models) + ddq = pin32.lcaba( + model, + data, + pin32.neutral(model), + np.zeros(model.nv, dtype=np.float32), + np.zeros(model.nv, dtype=np.float32), + constraint_models, + constraint_datas, + pin32.ProximalSettings(1e-5, 1e-4, 2), + ) + + self.assertEqual(ddq.dtype, np.float32) + + def test_admm(self): + model = pin32.Model() + joint_id = model.addJoint( + 0, pin32.JointModelFreeFlyer(), pin32.SE3.Identity(), "free_flyer" + ) + model.appendBodyToJoint( + joint_id, + pin32.Inertia.FromBox(1e-3, 1.0, 1.0, 1.0), + pin32.SE3.Identity(), + ) + + rotation = np.eye(3, dtype=np.float32) + upper = pin32.SE3(rotation, np.array([0.5, 0.5, 0.5], dtype=np.float32)) + lower = pin32.SE3(rotation, np.array([0.5, 0.5, -0.5], dtype=np.float32)) + point_contact = pin32.PointContactConstraintModel( + model, 0, upper, joint_id, lower + ) + point_contact.set = pin32.CoulombFrictionCone(0.4) + constraint_models = pin32.StdVec_ConstraintModel() + constraint_models.append(pin32.ConstraintModel(point_contact)) + constraint_datas = pin32.StdVec_ConstraintData() + constraint_datas.append(constraint_models[0].createData()) + + data = model.createData() + q = pin32.neutral(model) + zero = np.zeros(model.nv, dtype=np.float32) + external_forces = pin32.StdVec_Force() + external_forces.extend(pin32.Force.Zero() for _ in range(model.njoints)) + pin32.aba(model, data, q, zero, zero, external_forces) + data.q_in = q + data.v_in = zero + data.tau_in = zero + pin32.crba(model, data, q, pin32.Convention.WORLD) + free_velocity = zero + np.float32(1e-3) * pin32.aba( + model, data, q, zero, zero, external_forces + ) + constraint_models[0].calc(model, data, constraint_datas[0]) + + decomposition = pin32.ConstraintCholeskyDecomposition( + model, data, constraint_models, constraint_datas + ) + decomposition.compute(model, data, constraint_models, constraint_datas, 1e-6) + delassus_matrix = decomposition.getDelassusOperatorCholeskyExpression().matrix() + jacobian = pin32.getConstraintsJacobian( + model, data, constraint_models, constraint_datas + ) + drift = jacobian @ free_velocity + + settings = pin32.ADMMSolverSettings() + settings.lanczos_size = drift.size + settings.max_iterations = 2 + result = pin32.ADMMSolverResult() + solver = pin32.ADMMConstraintSolver(drift.size) + solver.solve( + pin32.DelassusOperatorDense(delassus_matrix), + drift, + constraint_models, + constraint_datas, + settings, + result, + ) + + self.assertEqual(delassus_matrix.dtype, np.float32) + self.assertEqual(drift.dtype, np.float32) + self.assertEqual(result.retrieveConstraintImpulses().dtype, np.float32) + + +if __name__ == "__main__": + unittest.main()