├── .clang-format ├── .cmake-format.yaml ├── .coveragerc ├── .git-blame-ignore-revs ├── .gitattributes ├── .github ├── ISSUE_TEMPLATE │ ├── bug_report.md │ └── feature_request.md ├── dependabot.yml └── workflows │ ├── check-changelog.yml │ ├── check_label.yml │ ├── cmake │ └── linux-debug-toolchain.cmake │ ├── linux.yml │ ├── macos-linux-windows-pixi.yml │ ├── nix.yml │ ├── ros_ci.yml │ ├── scripts │ ├── check_label.js │ └── get_compilation_flags.js │ ├── update-flake-lock.yml │ └── update_pixi_lockfile.yml ├── .gitignore ├── .gitlab-ci.yml ├── .gitmodules ├── .pre-commit-config.yaml ├── CHANGELOG.md ├── CITATION.bib ├── CITATION.cff ├── CMakeLists.txt ├── LICENSE ├── README.md ├── benchmark ├── CMakeLists.txt ├── README.md ├── model-fixture.hpp ├── plot.py ├── timings-cg.cpp ├── timings-cholesky.cpp ├── timings-constrained-dynamics-derivatives.cpp ├── timings-contact-dynamics.cpp ├── timings-cppad-jit.cpp ├── timings-delassus-operations.cpp ├── timings-derivatives.cpp ├── timings-eigen.cpp ├── timings-geometry.cpp ├── timings-impulse-dynamics-derivatives.cpp ├── timings-impulse-dynamics.cpp ├── timings-jacobian.cpp ├── timings-parallel.cpp └── timings.cpp ├── bindings ├── CMakeLists.txt └── python │ ├── CMakeLists.txt │ ├── algorithm │ ├── admm-solver.cpp │ ├── constraints │ │ └── expose-cones.cpp │ ├── expose-aba-derivatives.cpp │ ├── expose-aba.cpp │ ├── expose-algorithms.cpp │ ├── expose-cat.cpp │ ├── expose-centroidal-derivatives.cpp │ ├── expose-centroidal.cpp │ ├── expose-cholesky.cpp │ ├── expose-com.cpp │ ├── expose-constrained-dynamics-derivatives.cpp │ ├── expose-constrained-dynamics.cpp │ ├── expose-contact-dynamics.cpp │ ├── expose-contact-inverse-dynamics.cpp │ ├── expose-contact-jacobian.cpp │ ├── expose-contact-solvers.cpp │ ├── expose-crba.cpp │ ├── expose-delassus.cpp │ ├── expose-energy.cpp │ ├── expose-frames-derivatives.cpp │ ├── expose-frames.cpp │ ├── expose-geometry.cpp │ ├── expose-impulse-dynamics-derivatives.cpp │ ├── expose-impulse-dynamics.cpp │ ├── expose-jacobian.cpp │ ├── expose-joints.cpp │ ├── expose-kinematic-regressor.cpp │ ├── expose-kinematics-derivatives.cpp │ ├── expose-kinematics.cpp │ ├── expose-model.cpp │ ├── expose-regressor.cpp │ ├── expose-rnea-derivatives.cpp │ ├── expose-rnea.cpp │ ├── parallel │ │ ├── aba.cpp │ │ ├── expose-parallel.cpp │ │ └── rnea.cpp │ └── pgs-solver.cpp │ ├── collision │ ├── expose-broadphase-callbacks.cpp │ ├── expose-broadphase.cpp │ ├── expose-collision.cpp │ ├── expose-fcl.cpp │ ├── parallel │ │ ├── broadphase.cpp │ │ ├── expose-parallel.cpp │ │ └── geometry.cpp │ └── pool │ │ └── expose-pool.cpp │ ├── extra │ ├── expose-extras.cpp │ ├── expose-reachable-workspace.cpp │ └── mpfr │ │ └── boost_number.cpp │ ├── math │ ├── expose-eigen-types.cpp │ ├── expose-lanczos-decomposition.cpp │ ├── expose-linalg.cpp │ ├── expose-rpy.cpp │ └── expose-tridiagonal-matrix.cpp │ ├── module.cpp │ ├── multibody │ ├── expose-data.cpp │ ├── expose-frame.cpp │ ├── expose-geometry.cpp │ ├── expose-liegroups.cpp │ ├── expose-model.cpp │ ├── joint │ │ └── expose-joints.cpp │ ├── pool │ │ └── expose-pool.cpp │ └── sample-models.cpp │ ├── parsers │ ├── expose-parsers.cpp │ ├── mjcf │ │ ├── geometry.cpp │ │ └── model.cpp │ ├── sdf │ │ ├── geometry.cpp │ │ └── model.cpp │ ├── srdf.cpp │ └── urdf │ │ ├── console-bridge.cpp │ │ ├── geometry.cpp │ │ └── model.cpp │ ├── pinocchio │ ├── __init__.py │ ├── casadi │ │ └── __init__.py │ ├── cppad │ │ └── __init__.py │ ├── cppadcg │ │ └── __init__.py │ ├── deprecated.py │ ├── deprecation.py │ ├── derivative │ │ ├── dcrba.py │ │ ├── lambdas.py │ │ └── xm.py │ ├── explog.py │ ├── mpfr │ │ └── __init__.py │ ├── robot_wrapper.py │ ├── romeo_wrapper.py │ ├── shortcuts.py │ ├── utils.py │ ├── visualize │ │ ├── __init__.py │ │ ├── base_visualizer.py │ │ ├── gepetto_visualizer.py │ │ ├── meshcat_visualizer.py │ │ ├── panda3d_visualizer.py │ │ ├── rviz_visualizer.py │ │ └── visualizers.py │ └── windows_dll_manager.py │ ├── pinocchiopy.pc.cmake │ ├── serialization │ └── serialization.cpp │ ├── spatial │ ├── expose-SE3.cpp │ ├── expose-explog.cpp │ ├── expose-force.cpp │ ├── expose-inertia.cpp │ ├── expose-motion.cpp │ ├── expose-skew.cpp │ └── expose-symmetric3.cpp │ └── utils │ ├── conversions.cpp │ ├── dependencies.cpp │ ├── path.cpp │ └── version.cpp ├── colcon.pkg ├── development ├── build.md ├── contributing.md ├── release.md └── scripts │ ├── misc │ └── common_symbols.py │ └── pixi │ ├── activation.bat │ ├── activation.sh │ ├── activation_clang.sh │ └── activation_clang_cl.bat ├── doc ├── Doxyfile.extra.in ├── Overview.md ├── _porting.md ├── a-features │ ├── a-spatial.md │ ├── b-model-data.md │ ├── c-joints.md │ ├── d-model.md │ ├── e-lie.md │ ├── f-kinematic.md │ ├── g-dynamic.md │ ├── h-frames.md │ ├── j-analytical-derivatives.md │ ├── k-automatic-differentiation.md │ ├── l-python.md │ └── m-tests.md ├── b-examples │ ├── a-model.md │ ├── ab-geometry-models.md │ ├── b-display.md │ ├── c-collisions.md │ ├── d-inverse-kinematics.md │ ├── display │ │ ├── a-gepetto-viewer.md │ │ ├── b-meshcat-viewer.md │ │ └── c-robot-wrapper-viewer.md │ ├── e-reduced-model.md │ ├── f-update-model.md │ └── intro.md ├── bib │ ├── carpentier-rss18.bib │ ├── carpentier-sii19.bib │ └── pinocchioweb.bib ├── c-maths │ ├── a-rigid-bodies.md │ ├── b-joints.md │ ├── intro.md │ └── se3.md ├── customdoxygen.css ├── d-practical-exercises │ ├── 1-directgeom.md │ ├── 2-invgeom.md │ ├── 3-invkine.md │ ├── 4-dyn.md │ ├── 5-planner.md │ ├── 6-wpg.md │ ├── 7-learn.md │ ├── intro.md │ └── src │ │ ├── continuous.py │ │ ├── display.py │ │ ├── dpendulum.py │ │ ├── factor.py │ │ ├── foot_steps.py │ │ ├── graph.py │ │ ├── mobilerobot.py │ │ ├── ocp.py │ │ ├── pendulum.py │ │ ├── prm_display.py │ │ ├── qnet.py │ │ ├── qtable.py │ │ ├── robot_hand.py │ │ └── ur5x4.py ├── header.html ├── images │ ├── pinocchio-logo-large.pdf │ ├── pinocchio-logo-large.png │ └── pinocchio-performances.png ├── outline.txt ├── pictures │ ├── SE2MotivatingExample.svg │ ├── SE3Example1.jpg │ ├── cube-rotation_picture.jpg │ ├── cube_rotation.gif │ └── joints_gif │ │ ├── cylindrical_laas.gif │ │ ├── planar_laas.gif │ │ ├── prismatic_laas.gif │ │ ├── revolute_laas.gif │ │ └── spherical_laas.gif ├── pinocchio.ico ├── pinocchio.png ├── pinocchio_cheat_sheet.pdf ├── pinocchio_cheat_sheet.tex └── treeview.dox ├── examples ├── CMakeLists.txt ├── README.md ├── anymal-simulation.py ├── append-urdf-model-with-another-model.py ├── build-reduced-model.cpp ├── build-reduced-model.py ├── capsule-approximation.py ├── casadi │ ├── CMakeLists.txt │ ├── cartpole.py │ ├── casadi-aba.cpp │ ├── casadi-crba.cpp │ ├── casadi-rnea.cpp │ ├── quadrotor-ocp.py │ └── simulation-pendulum-variational.ipynb ├── cassie-simulation.py ├── codegen │ ├── CMakeLists.txt │ ├── codegen-crba.cpp │ ├── codegen-rnea.cpp │ └── codegen-rnea.py ├── collision-with-point-clouds.py ├── collisions.cpp ├── collisions.py ├── contact-cholesky.py ├── cppad │ ├── CMakeLists.txt │ ├── autodiff-rnea.cpp │ └── autodiff-rnea.py ├── display-shapes-meshcat.py ├── display-shapes.py ├── floating-base-velocity-viewer.py ├── forward-dynamics-derivatives.cpp ├── forward-dynamics-derivatives.py ├── geometry-models.cpp ├── geometry-models.py ├── gepetto-viewer.py ├── interpolation-SE3.cpp ├── inverse-dynamics-derivatives.cpp ├── inverse-dynamics-derivatives.py ├── inverse-dynamics.cpp ├── inverse-dynamics.py ├── inverse-kinematics-3d.cpp ├── inverse-kinematics-3d.py ├── inverse-kinematics.cpp ├── inverse-kinematics.py ├── kinematics-derivatives.cpp ├── kinematics-derivatives.py ├── meshcat-viewer-dae.py ├── meshcat-viewer-octree.py ├── meshcat-viewer-solo.py ├── meshcat-viewer.py ├── mimic_dynamics.py ├── multiprecision.cpp ├── overview-SE3.cpp ├── overview-lie.cpp ├── overview-simple.cpp ├── overview-simple.py ├── overview-urdf.cpp ├── overview-urdf.py ├── panda3d-viewer-play.py ├── panda3d-viewer.py ├── parallel.sdf ├── reachable-workspace-with-collisions.py ├── reachable-workspace.py ├── robot-wrapper-viewer.py ├── run-algo-in-parallel.py ├── rviz-viewer.py ├── sample-model-viewer.py ├── simulation-closed-kinematic-chains.py ├── simulation-contact-dynamics.py ├── simulation-inverted-pendulum.py ├── simulation-pendulum.py ├── static-contact-dynamics.py ├── talos-simulation.py └── update-model-after-urdf.py ├── flake.lock ├── flake.nix ├── include └── pinocchio │ ├── algorithm │ ├── aba-derivatives.hpp │ ├── aba-derivatives.hxx │ ├── aba-derivatives.txx │ ├── aba.hpp │ ├── aba.hxx │ ├── aba.txx │ ├── admm-solver.hpp │ ├── admm-solver.hxx │ ├── center-of-mass-derivatives.hpp │ ├── center-of-mass-derivatives.hxx │ ├── center-of-mass-derivatives.txx │ ├── center-of-mass.hpp │ ├── center-of-mass.hxx │ ├── center-of-mass.txx │ ├── centroidal-derivatives.hpp │ ├── centroidal-derivatives.hxx │ ├── centroidal-derivatives.txx │ ├── centroidal.hpp │ ├── centroidal.hxx │ ├── centroidal.txx │ ├── check-base.hpp │ ├── check-data.hpp │ ├── check-data.hxx │ ├── check-model.hpp │ ├── check-model.hxx │ ├── check.hpp │ ├── cholesky.hpp │ ├── cholesky.hxx │ ├── cholesky.txx │ ├── compute-all-terms.hpp │ ├── compute-all-terms.hxx │ ├── compute-all-terms.txx │ ├── constrained-dynamics-derivatives.hpp │ ├── constrained-dynamics-derivatives.hxx │ ├── constrained-dynamics-derivatives.txx │ ├── constrained-dynamics.hpp │ ├── constrained-dynamics.hxx │ ├── constrained-dynamics.txx │ ├── constrained-problem-data.hpp │ ├── constraints │ │ ├── constraint-data-base.hpp │ │ ├── constraint-data-generic.hpp │ │ ├── constraint-model-base.hpp │ │ ├── constraint-model-generic.hpp │ │ ├── constraints.hpp │ │ ├── coulomb-friction-cone.hpp │ │ ├── fwd.hpp │ │ └── visitors │ │ │ └── constraint-model-visitor.hpp │ ├── contact-cholesky.hpp │ ├── contact-cholesky.hxx │ ├── contact-cholesky.txx │ ├── contact-dynamics.hpp │ ├── contact-dynamics.hxx │ ├── contact-dynamics.txx │ ├── contact-info.hpp │ ├── contact-inverse-dynamics.hpp │ ├── contact-jacobian.hpp │ ├── contact-jacobian.hxx │ ├── contact-jacobian.txx │ ├── contact-solver-base.hpp │ ├── contact-solver-utils.hpp │ ├── contact.hpp │ ├── contact.hxx │ ├── copy.hpp │ ├── crba.hpp │ ├── crba.hxx │ ├── crba.txx │ ├── default-check.hpp │ ├── delassus-operartor-ref.hpp │ ├── delassus-operator-base.hpp │ ├── delassus-operator-dense.hpp │ ├── delassus-operator-rigid-body.hpp │ ├── delassus-operator-sparse.hpp │ ├── delassus.hpp │ ├── delassus.hxx │ ├── energy.hpp │ ├── energy.hxx │ ├── energy.txx │ ├── frames-derivatives.hpp │ ├── frames-derivatives.hxx │ ├── frames-derivatives.txx │ ├── frames.hpp │ ├── frames.hxx │ ├── frames.txx │ ├── fwd.hpp │ ├── geometry.hpp │ ├── geometry.hxx │ ├── geometry.txx │ ├── impulse-dynamics-derivatives.hpp │ ├── impulse-dynamics-derivatives.hxx │ ├── impulse-dynamics.hpp │ ├── impulse-dynamics.hxx │ ├── impulse-dynamics.txx │ ├── jacobian.hpp │ ├── jacobian.hxx │ ├── jacobian.txx │ ├── joint-configuration.hpp │ ├── joint-configuration.hxx │ ├── joint-configuration.txx │ ├── kinematics-derivatives.hpp │ ├── kinematics-derivatives.hxx │ ├── kinematics-derivatives.txx │ ├── kinematics.hpp │ ├── kinematics.hxx │ ├── kinematics.txx │ ├── model.hpp │ ├── model.hxx │ ├── model.txx │ ├── parallel │ │ ├── aba.hpp │ │ ├── geometry.hpp │ │ ├── omp.hpp │ │ └── rnea.hpp │ ├── pgs-solver.hpp │ ├── pgs-solver.hxx │ ├── proximal.hpp │ ├── proximal.txx │ ├── pv.hpp │ ├── pv.hxx │ ├── regressor.hpp │ ├── regressor.hxx │ ├── regressor.txx │ ├── rnea-derivatives.hpp │ ├── rnea-derivatives.hxx │ ├── rnea-derivatives.txx │ ├── rnea-second-order-derivatives.hpp │ ├── rnea-second-order-derivatives.hxx │ ├── rnea.hpp │ ├── rnea.hxx │ ├── rnea.txx │ └── utils │ │ ├── force.hpp │ │ └── motion.hpp │ ├── autodiff │ ├── casadi-algo.hpp │ ├── casadi.hpp │ ├── casadi │ │ ├── math │ │ │ ├── matrix.hpp │ │ │ ├── quaternion.hpp │ │ │ └── triangular-matrix.hpp │ │ ├── spatial │ │ │ └── se3-tpl.hpp │ │ └── utils │ │ │ └── static-if.hpp │ ├── cppad.hpp │ └── cppad │ │ ├── algorithm │ │ └── aba.hpp │ │ ├── math │ │ ├── eigen_plugin.hpp │ │ └── quaternion.hpp │ │ ├── spatial │ │ ├── log.hxx │ │ └── se3-tpl.hpp │ │ └── utils │ │ └── static-if.hpp │ ├── bindings │ └── python │ │ ├── algorithm │ │ ├── algorithms.hpp │ │ ├── constraints │ │ │ └── coulomb-friction-cone.hpp │ │ ├── contact-cholesky.hpp │ │ ├── contact-info.hpp │ │ ├── contact-solver-base.hpp │ │ ├── delassus-operator.hpp │ │ └── proximal.hpp │ │ ├── collision │ │ ├── broadphase-manager-base.hpp │ │ ├── broadphase-manager.hpp │ │ ├── collision.hpp │ │ ├── fcl │ │ │ └── transform.hpp │ │ ├── geometry-functors.hpp │ │ ├── pool │ │ │ ├── broadphase-manager.hpp │ │ │ └── geometry.hpp │ │ └── tree-broadphase-manager.hpp │ │ ├── context.hpp │ │ ├── context │ │ ├── casadi.hpp │ │ ├── cppad.hpp │ │ ├── cppadcg.hpp │ │ ├── default.hpp │ │ ├── generic.hpp │ │ └── mpfr.hpp │ │ ├── extra │ │ └── extras.hpp │ │ ├── fwd.hpp │ │ ├── math │ │ ├── lanczos-decomposition.hpp │ │ ├── multiprecision │ │ │ └── boost │ │ │ │ └── number.hpp │ │ └── tridiagonal-matrix.hpp │ │ ├── multibody │ │ ├── data.hpp │ │ ├── frame.hpp │ │ ├── geometry-data.hpp │ │ ├── geometry-model.hpp │ │ ├── geometry-object.hpp │ │ ├── joint │ │ │ ├── joint-data.hpp │ │ │ ├── joint-derived.hpp │ │ │ ├── joint-model.hpp │ │ │ ├── joint.hpp │ │ │ ├── joints-datas.hpp │ │ │ ├── joints-models.hpp │ │ │ └── joints-variant.hpp │ │ ├── liegroups.hpp │ │ ├── model.hpp │ │ └── pool │ │ │ └── model.hpp │ │ ├── parsers │ │ ├── mjcf.hpp │ │ ├── python.hpp │ │ ├── sdf.hpp │ │ ├── srdf.hpp │ │ └── urdf.hpp │ │ ├── pch.hpp │ │ ├── pybind11-all.hpp │ │ ├── pybind11.hpp │ │ ├── serialization │ │ ├── serializable.hpp │ │ └── serialization.hpp │ │ ├── spatial │ │ ├── classic-acceleration.hpp │ │ ├── explog.hpp │ │ ├── force.hpp │ │ ├── inertia.hpp │ │ ├── motion.hpp │ │ ├── se3.hpp │ │ └── symmetric3.hpp │ │ ├── utils │ │ ├── address.hpp │ │ ├── cast.hpp │ │ ├── comparable.hpp │ │ ├── constant.hpp │ │ ├── conversions.hpp │ │ ├── copyable.hpp │ │ ├── dependencies.hpp │ │ ├── deprecation.hpp │ │ ├── eigen.hpp │ │ ├── list.hpp │ │ ├── macros.hpp │ │ ├── model-checker.hpp │ │ ├── namespace.hpp │ │ ├── path.hpp │ │ ├── pickle-map.hpp │ │ ├── pickle-vector.hpp │ │ ├── pickle.hpp │ │ ├── printable.hpp │ │ ├── registration.hpp │ │ ├── std-aligned-vector.hpp │ │ ├── std-map.hpp │ │ ├── std-vector.hpp │ │ └── version.hpp │ │ └── visualizers │ │ └── visualizer-visitor.hpp │ ├── codegen │ ├── code-generator-algo.hpp │ ├── code-generator-base.hpp │ └── cppadcg.hpp │ ├── collision │ ├── broadphase-callbacks.hpp │ ├── broadphase-callbacks.hxx │ ├── broadphase-manager-base.hpp │ ├── broadphase-manager.hpp │ ├── broadphase-manager.hxx │ ├── broadphase.hpp │ ├── broadphase.hxx │ ├── collision.hpp │ ├── collision.hxx │ ├── collision.txx │ ├── distance.hpp │ ├── distance.hxx │ ├── distance.txx │ ├── fcl-pinocchio-conversions.hpp │ ├── parallel │ │ ├── broadphase.hpp │ │ └── geometry.hpp │ ├── pool │ │ ├── broadphase-manager.hpp │ │ └── fwd.hpp │ ├── tree-broadphase-manager.hpp │ └── tree-broadphase-manager.hxx │ ├── container │ ├── aligned-vector.hpp │ └── boost-container-limits.hpp │ ├── context.hpp │ ├── context │ ├── casadi.hpp │ ├── cppad.hpp │ ├── cppadcg.hpp │ ├── default.hpp │ └── generic.hpp │ ├── core │ ├── binary-op.hpp │ └── unary-op.hpp │ ├── deprecated-macros.hpp │ ├── deprecated-namespaces.hpp │ ├── deprecation.hpp │ ├── eigen-macros.hpp │ ├── extra │ ├── reachable-workspace.hpp │ └── reachable-workspace.hxx │ ├── fwd.hpp │ ├── macros.hpp │ ├── math │ ├── casadi.hpp │ ├── comparison-operators.hpp │ ├── cppad.hpp │ ├── cppadcg.hpp │ ├── eigenvalues.hpp │ ├── fwd.hpp │ ├── gram-schmidt-orthonormalisation.hpp │ ├── lanczos-decomposition.hpp │ ├── matrix-block.hpp │ ├── matrix.hpp │ ├── multiprecision-mpfr.hpp │ ├── multiprecision.hpp │ ├── quaternion.hpp │ ├── rotation.hpp │ ├── rpy.hpp │ ├── rpy.hxx │ ├── sign.hpp │ ├── sincos.hpp │ ├── taylor-expansion.hpp │ ├── tensor.hpp │ ├── triangular-matrix.hpp │ └── tridiagonal-matrix.hpp │ ├── multibody │ ├── data.hpp │ ├── data.hxx │ ├── data.txx │ ├── fcl.hpp │ ├── force-set.hpp │ ├── frame.hpp │ ├── fwd.hpp │ ├── geometry-object-filter.hpp │ ├── geometry-object.hpp │ ├── geometry-object.hxx │ ├── geometry.hpp │ ├── geometry.hxx │ ├── instance-filter.hpp │ ├── joint-motion-subspace-base.hpp │ ├── joint-motion-subspace-generic.hpp │ ├── joint-motion-subspace.hpp │ ├── joint │ │ ├── fwd.hpp │ │ ├── joint-base.hpp │ │ ├── joint-basic-visitors.hpp │ │ ├── joint-basic-visitors.hxx │ │ ├── joint-collection.hpp │ │ ├── joint-common-operations.hpp │ │ ├── joint-composite.hpp │ │ ├── joint-composite.hxx │ │ ├── joint-data-base.hpp │ │ ├── joint-free-flyer.hpp │ │ ├── joint-generic.hpp │ │ ├── joint-helical-unaligned.hpp │ │ ├── joint-helical.hpp │ │ ├── joint-mimic.hpp │ │ ├── joint-model-base.hpp │ │ ├── joint-planar.hpp │ │ ├── joint-prismatic-unaligned.hpp │ │ ├── joint-prismatic.hpp │ │ ├── joint-revolute-unaligned.hpp │ │ ├── joint-revolute-unbounded-unaligned.hpp │ │ ├── joint-revolute-unbounded.hpp │ │ ├── joint-revolute.hpp │ │ ├── joint-spherical-ZYX.hpp │ │ ├── joint-spherical.hpp │ │ ├── joint-translation.hpp │ │ ├── joint-universal.hpp │ │ └── joints.hpp │ ├── liegroup │ │ ├── cartesian-product-variant.hpp │ │ ├── cartesian-product-variant.hxx │ │ ├── cartesian-product.hpp │ │ ├── fwd.hpp │ │ ├── liegroup-algo.hpp │ │ ├── liegroup-algo.hxx │ │ ├── liegroup-base.hpp │ │ ├── liegroup-base.hxx │ │ ├── liegroup-collection.hpp │ │ ├── liegroup-generic.hpp │ │ ├── liegroup-variant-visitors.hpp │ │ ├── liegroup-variant-visitors.hxx │ │ ├── liegroup.hpp │ │ ├── special-euclidean.hpp │ │ ├── special-orthogonal.hpp │ │ └── vector-space.hpp │ ├── model-item.hpp │ ├── model.hpp │ ├── model.hxx │ ├── model.txx │ ├── pool │ │ ├── fwd.hpp │ │ ├── geometry.hpp │ │ └── model.hpp │ ├── sample-models.hpp │ ├── sample-models.hxx │ ├── sample-models.txx │ ├── visitor.hpp │ └── visitor │ │ ├── fusion.hpp │ │ ├── joint-binary-visitor.hpp │ │ └── joint-unary-visitor.hpp │ ├── parsers │ ├── meshloader-fwd.hpp │ ├── mjcf.hpp │ ├── mjcf │ │ ├── geometry.hxx │ │ ├── mjcf-graph.hpp │ │ └── model.hxx │ ├── python.hpp │ ├── sample-models.hpp │ ├── sdf.hpp │ ├── sdf │ │ ├── geometry.hxx │ │ └── model.hxx │ ├── srdf.hpp │ ├── srdf.hxx │ ├── urdf.hpp │ ├── urdf │ │ ├── geometry.hxx │ │ ├── model.hxx │ │ ├── types.hpp │ │ └── utils.hpp │ └── utils.hpp │ ├── serialization │ ├── aligned-vector.hpp │ ├── archive.hpp │ ├── csv.hpp │ ├── data.hpp │ ├── eigen.hpp │ ├── fcl.hpp │ ├── force.hpp │ ├── frame.hpp │ ├── fwd.hpp │ ├── geometry.hpp │ ├── inertia.hpp │ ├── joints-data.hpp │ ├── joints-model.hpp │ ├── joints-motion-subspace.hpp │ ├── joints-motion.hpp │ ├── joints-transform.hpp │ ├── joints.hpp │ ├── model.hpp │ ├── motion.hpp │ ├── se3.hpp │ ├── serializable.hpp │ ├── spatial.hpp │ ├── static-buffer.hpp │ ├── symmetric3.hpp │ └── vector.hpp │ ├── spatial │ ├── act-on-set.hpp │ ├── act-on-set.hxx │ ├── cartesian-axis.hpp │ ├── classic-acceleration.hpp │ ├── explog-quaternion.hpp │ ├── explog.hpp │ ├── fcl-pinocchio-conversions.hpp │ ├── force-base.hpp │ ├── force-dense.hpp │ ├── force-ref.hpp │ ├── force-tpl.hpp │ ├── force.hpp │ ├── fwd.hpp │ ├── inertia.hpp │ ├── log.hpp │ ├── log.hxx │ ├── motion-base.hpp │ ├── motion-dense.hpp │ ├── motion-ref.hpp │ ├── motion-tpl.hpp │ ├── motion-zero.hpp │ ├── motion.hpp │ ├── se3-base.hpp │ ├── se3-tpl.hpp │ ├── se3.hpp │ ├── skew.hpp │ ├── spatial-axis.hpp │ └── symmetric3.hpp │ ├── unsupported.hpp │ ├── utils │ ├── axis-label.hpp │ ├── cast.hpp │ ├── check.hpp │ ├── eigen-fix.hpp │ ├── file-explorer.hpp │ ├── file-io.hpp │ ├── helpers.hpp │ ├── openmp.hpp │ ├── shared-ptr.hpp │ ├── static-if.hpp │ ├── string-generator.hpp │ ├── string.hpp │ ├── timer.hpp │ ├── timer2.hpp │ └── version.hpp │ └── visualizers │ └── base-visualizer.hpp ├── models ├── baxter_simple.urdf ├── humanoid.xml ├── simple_humanoid.sdf ├── simple_humanoid.srdf ├── simple_humanoid.urdf ├── simple_humanoid.xml ├── simple_humanoid_description │ └── box.stl ├── simple_humanoid_rel_mesh.urdf └── simple_model.py ├── package.xml ├── pixi.lock ├── pixi.toml ├── pyproject.toml ├── sources.cmake ├── src ├── CMakeLists.txt ├── algorithm │ ├── aba-derivatives.cpp │ ├── aba.cpp │ ├── center-of-mass-derivatives.cpp │ ├── center-of-mass.cpp │ ├── centroidal-derivatives.cpp │ ├── centroidal.cpp │ ├── cholesky.cpp │ ├── compute-all-terms.cpp │ ├── constrained-dynamics-derivatives.cpp │ ├── constrained-dynamics.cpp │ ├── contact-cholesky.cpp │ ├── contact-dynamics.cpp │ ├── contact-jacobian.cpp │ ├── crba.cpp │ ├── energy.cpp │ ├── frames-derivatives.cpp │ ├── frames.cpp │ ├── geometry.cpp │ ├── impulse-dynamics.cpp │ ├── jacobian.cpp │ ├── joint-configuration.cpp │ ├── kinematics-derivatives.cpp │ ├── kinematics.cpp │ ├── model.cpp │ ├── proximal.cpp │ ├── regressor.cpp │ ├── rnea-derivatives.cpp │ └── rnea.cpp ├── collision │ ├── collision.cpp │ └── distance.cpp ├── extra │ └── reachable-workspace.cpp ├── multibody │ ├── data.cpp │ ├── model.cpp │ └── sample-models.cpp ├── parsers │ ├── mjcf │ │ ├── mjcf-graph-geom.cpp │ │ └── mjcf-graph.cpp │ ├── python │ │ └── model.cpp │ ├── sdf │ │ ├── geometry.cpp │ │ └── model.cpp │ └── urdf │ │ ├── geometry.cpp │ │ ├── model.cpp │ │ └── utils.cpp ├── utils │ └── file-explorer.cpp └── visualizers │ └── base-visualizer.cpp ├── unittest ├── CMakeLists.txt ├── aba-derivatives.cpp ├── aba.cpp ├── algo-check.cpp ├── algorithm │ ├── CMakeLists.txt │ └── utils │ │ ├── CMakeLists.txt │ │ ├── force.cpp │ │ └── motion.cpp ├── all-joints.cpp ├── broadphase.cpp ├── cartesian-product-liegroups.cpp ├── casadi │ ├── CMakeLists.txt │ ├── aba-derivatives.cpp │ ├── aba.cpp │ ├── algorithms.cpp │ ├── basic.cpp │ ├── casadi-utils.hpp │ ├── constraint-dynamics-derivatives.cpp │ ├── constraint-dynamics.cpp │ ├── explog.cpp │ ├── integrate-derivatives.cpp │ ├── joints.cpp │ ├── rnea-derivatives.cpp │ └── spatial.cpp ├── center-of-mass-derivatives.cpp ├── centroidal-derivatives.cpp ├── centroidal.cpp ├── cholesky.cpp ├── classic-acceleration.cpp ├── closed-loop-dynamics.cpp ├── com.cpp ├── compute-all-terms.cpp ├── constrained-dynamics-derivatives.cpp ├── constrained-dynamics.cpp ├── constraint-variants.cpp ├── contact-cholesky.cpp ├── contact-dynamics-derivatives.cpp ├── contact-dynamics.cpp ├── contact-inverse-dynamics.cpp ├── contact-models.cpp ├── copy.cpp ├── coulomb-friction-cone.cpp ├── cppad │ ├── CMakeLists.txt │ ├── algorithms.cpp │ ├── basic.cpp │ ├── derivatives.cpp │ ├── joint-configurations.cpp │ ├── joints.cpp │ └── spatial.cpp ├── cppadcg │ ├── CMakeLists.txt │ ├── algorithms.cpp │ ├── basic.cpp │ ├── contact-dynamics.cpp │ └── joint-configurations.cpp ├── crba.cpp ├── csv.cpp ├── data.cpp ├── delassus.cpp ├── eigen-basic-op.cpp ├── eigen-tensor.cpp ├── eigenvalues.cpp ├── energy.cpp ├── explog.cpp ├── finite-differences.cpp ├── frames-derivatives.cpp ├── frames.cpp ├── fusion.cpp ├── geometry-algorithms.cpp ├── geometry-model.cpp ├── geometry-object.cpp ├── gram-schmidt-orthonormalisation.cpp ├── impulse-dynamics-derivatives.cpp ├── impulse-dynamics.cpp ├── joint-composite.cpp ├── joint-configurations.cpp ├── joint-free-flyer.cpp ├── joint-generic.cpp ├── joint-helical.cpp ├── joint-jacobian.cpp ├── joint-mimic.cpp ├── joint-motion-subspace.cpp ├── joint-planar.cpp ├── joint-prismatic.cpp ├── joint-revolute.cpp ├── joint-spherical.cpp ├── joint-translation.cpp ├── joint-universal.cpp ├── kinematics-derivatives.cpp ├── kinematics.cpp ├── lanczos-decomposition.cpp ├── liegroups.cpp ├── macros.cpp ├── mjcf.cpp ├── model.cpp ├── models │ ├── 3DOF_planar.urdf │ ├── closed_chain.xml │ ├── link_and_joint_identical_name.urdf │ ├── test_composite.xml │ ├── test_mjcf.urdf │ └── test_mjcf.xml ├── multiprecision-mpfr.cpp ├── multiprecision.cpp ├── packaging │ ├── cmake │ │ └── CMakeLists.txt │ ├── external │ │ └── CMakeLists.txt │ ├── load_urdf.cpp │ ├── pinocchio_header │ │ ├── CMakeLists.txt │ │ └── run_fk.cpp │ ├── pkgconfig │ │ └── CMakeLists.txt │ └── run_rnea.cpp ├── parallel-aba.cpp ├── parallel-geometry.cpp ├── parallel-rnea.cpp ├── pv-solver.cpp ├── python │ ├── CMakeLists.txt │ ├── bindings.py │ ├── bindings_SE3.py │ ├── bindings_Symmetric3.py │ ├── bindings_aba.py │ ├── bindings_build_geom_from_urdf_memorycheck.py │ ├── bindings_build_geom_from_urdf_memorycheck.supp │ ├── bindings_centroidal_dynamics_derivatives.py │ ├── bindings_com.py │ ├── bindings_com_velocity_derivatives.py │ ├── bindings_contact_inverse_dynamics.py │ ├── bindings_data.py │ ├── bindings_dynamics.py │ ├── bindings_fcl_transform.py │ ├── bindings_force.py │ ├── bindings_forward_dynamics_derivatives.py │ ├── bindings_frame.py │ ├── bindings_frame_derivatives.py │ ├── bindings_geometry_model.py │ ├── bindings_geometry_model_urdf.py │ ├── bindings_geometry_object.py │ ├── bindings_inertia.py │ ├── bindings_inverse_dynamics_derivatives.py │ ├── bindings_joint_algorithms.py │ ├── bindings_joint_composite.py │ ├── bindings_joints.py │ ├── bindings_kinematic_regressor.py │ ├── bindings_kinematics.py │ ├── bindings_kinematics_derivatives.py │ ├── bindings_liegroups.py │ ├── bindings_model.py │ ├── bindings_motion.py │ ├── bindings_regressor.py │ ├── bindings_rnea.py │ ├── bindings_sample_models.py │ ├── bindings_spatial.py │ ├── bindings_std_map.py │ ├── bindings_std_vector.py │ ├── bindings_urdf.py │ ├── bindings_visualizer.cpp │ ├── bindings_visualizer.py │ ├── casadi │ │ ├── CMakeLists.txt │ │ ├── bindings_explog.py │ │ └── bindings_main_algo.py │ ├── explog.py │ ├── pybind11 │ │ ├── CMakeLists.txt │ │ ├── cpp2pybind11.cpp │ │ └── test-cpp2pybind11.py │ ├── robot_wrapper.py │ ├── rpy.py │ ├── serialization.py │ ├── test_case.py │ ├── utils.py │ └── version.py ├── python_parser.cpp ├── quaternion.cpp ├── reachable-workspace.cpp ├── regressor.cpp ├── rnea-derivatives.cpp ├── rnea-second-order-derivatives.cpp ├── rnea.cpp ├── rotation.cpp ├── rpy.cpp ├── sample-models.cpp ├── sdf.cpp ├── serialization.cpp ├── sincos.cpp ├── spatial.cpp ├── srdf.cpp ├── symmetric.cpp ├── tree-broadphase.cpp ├── tridiagonal-matrix.cpp ├── urdf.cpp ├── utils │ ├── macros.hpp │ └── model-generator.hpp ├── value.cpp ├── vector.cpp ├── version.cpp ├── visitor.cpp └── visualizer.cpp └── utils ├── CMakeLists.txt └── pinocchio_read_model.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | Language: Cpp 2 | BasedOnStyle: Microsoft 3 | 4 | AlwaysBreakTemplateDeclarations: Yes 5 | BinPackParameters: false 6 | 7 | BreakBeforeBinaryOperators: NonAssignment 8 | BreakConstructorInitializers: BeforeComma 9 | BreakInheritanceList: BeforeComma 10 | 11 | ConstructorInitializerIndentWidth: 0 12 | ContinuationIndentWidth: 2 13 | 14 | IndentWidth: 2 15 | 16 | NamespaceIndentation: All 17 | 18 | PackConstructorInitializers: Never 19 | 20 | PenaltyReturnTypeOnItsOwnLine: 10 21 | PointerAlignment: Middle 22 | 23 | SpaceAfterTemplateKeyword: false 24 | 25 | ColumnLimit: 100 26 | 27 | SortIncludes: false 28 | 29 | IndentPPDirectives: BeforeHash 30 | 31 | AlignAfterOpenBracket: AlwaysBreak 32 | -------------------------------------------------------------------------------- /.coveragerc: -------------------------------------------------------------------------------- 1 | [run] 2 | source = src,python 3 | -------------------------------------------------------------------------------- /.git-blame-ignore-revs: -------------------------------------------------------------------------------- 1 | # pre-commit run -a (format-all, 2024-05-16) 2 | 4cc842428c8cf0636bd8b062b73788abbcf50304 3 | 4 | # pre-commit run -a (2024-08-12) 5 | 3e4163b025ae0d876bc10db83cb889491efa0228 6 | 7 | # ruff check --fix (2024-08-12) 8 | 45576e056b2a1000a6fa16caabd015ce622093d1 9 | 10 | # pre-commit run -a (2024-09-04) 11 | d7c57794672df89734a80c40c0bd3166c7a984c6 12 | -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # GitHub syntax highlighting 2 | pixi.lock linguist-language=YAML linguist-generated=true 3 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/bug_report.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: 🐞 Bug report 3 | about: Something in Pinocchio is not working the way it should 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | ## Bug description 11 | 12 | A clear and concise description of what the bug is. 13 | 14 | ## Expected behavior 15 | 16 | A clear and concise description of what you expected to happen. 17 | 18 | ## Reproduction steps 19 | 20 | Steps to reproduce the behavior: 21 | 1. Load to '...' 22 | 2. Click on '....' 23 | 3. Scroll down to '....' 24 | 4. See error 25 | 26 | ### Docker image 27 | 28 | If this is an installation issue, a Docker image to reproduce it 29 | 30 | ```dockerfile 31 | ... 32 | ``` 33 | 34 | ### Code 35 | 36 | If this is a runtime issue, provide some minimal code to reproduce it: 37 | 38 | ``` 39 | ... 40 | ``` 41 | 42 | ## Additional context 43 | 44 | Add any other context about the problem here. 45 | 46 | ### Screenshots 47 | 48 | If applicable, add screenshots to help explain your problem. 49 | 50 | ### System 51 | 52 | - OS: [e.g. iOS] 53 | - Pinocchio version: [e.g. 0.1.0] 54 | -------------------------------------------------------------------------------- /.github/ISSUE_TEMPLATE/feature_request.md: -------------------------------------------------------------------------------- 1 | --- 2 | name: 💡 Feature request 3 | about: Suggest an idea to improve Pinocchio 4 | title: '' 5 | labels: '' 6 | assignees: '' 7 | 8 | --- 9 | 10 | ⚠️ **Please open a discussion in the [Ideas](https://github.com/stack-of-tasks/pinocchio/discussions/categories/ideas) section rather than an issue.** 11 | 12 | Make sure your proposal checks out the following questions: 13 | 14 | - [ ] Is your feature request related to a problem? Please describe, *e.g.* "I'm always frustrated when..." 15 | - [ ] Describe concisely the solution you'd like. 16 | - [ ] Describe alternatives you've considered. 17 | - [ ] Do you have screenshots or sketches to illustrate your proposal? 18 | -------------------------------------------------------------------------------- /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: github-actions 4 | directory: / 5 | target-branch: devel 6 | schedule: 7 | interval: weekly 8 | -------------------------------------------------------------------------------- /.github/workflows/check-changelog.yml: -------------------------------------------------------------------------------- 1 | name: CI - Check-changelog 2 | on: 3 | pull_request: 4 | types: [assigned, opened, synchronize, reopened, labeled, unlabeled] 5 | branches: 6 | - devel 7 | jobs: 8 | check-changelog: 9 | name: Check changelog action 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: tarides/changelog-check-action@v3 13 | with: 14 | changelog: CHANGELOG.md 15 | -------------------------------------------------------------------------------- /.github/workflows/check_label.yml: -------------------------------------------------------------------------------- 1 | name: Verify Pull Request Labels 2 | 3 | on: 4 | pull_request_target: 5 | types: [opened, reopened, labeled, unlabeled] 6 | 7 | jobs: 8 | verify_pr_labels: 9 | runs-on: ubuntu-latest 10 | 11 | steps: 12 | - name: Checkout 13 | uses: actions/checkout@v4 14 | 15 | - name: Verify Pull Request Labels 16 | uses: actions/github-script@v7 17 | with: 18 | github-token: ${{ secrets.GITHUB_TOKEN }} 19 | script: | 20 | const script = require('./.github/workflows/scripts/check_label.js'); 21 | await script({github, context, core}) 22 | -------------------------------------------------------------------------------- /.github/workflows/cmake/linux-debug-toolchain.cmake: -------------------------------------------------------------------------------- 1 | # Don't generate debug symbols. 2 | # This is useless for the CI build (we don't retrieve any binaries) and can stop the build 3 | # by taking all the disk space. 4 | set(CMAKE_CXX_FLAGS_DEBUG 5 | "" 6 | CACHE STRING "") 7 | -------------------------------------------------------------------------------- /.github/workflows/nix.yml: -------------------------------------------------------------------------------- 1 | name: "CI - Nix" 2 | 3 | on: 4 | push: 5 | branches: 6 | - master 7 | - devel 8 | pull_request: 9 | branches: 10 | - master 11 | - devel 12 | 13 | jobs: 14 | nix: 15 | runs-on: "${{ matrix.os }}-latest" 16 | strategy: 17 | matrix: 18 | os: [ubuntu, macos] 19 | steps: 20 | - uses: actions/checkout@v4 21 | - uses: cachix/install-nix-action@v31 22 | - uses: cachix/cachix-action@v16 23 | with: 24 | name: gepetto 25 | authToken: '${{ secrets.CACHIX_AUTH_TOKEN }}' 26 | - run: nix build -L 27 | 28 | check: 29 | if: always() 30 | name: check-macos-linux-nix 31 | 32 | needs: 33 | - nix 34 | 35 | runs-on: Ubuntu-latest 36 | 37 | steps: 38 | - name: Decide whether the needed jobs succeeded or failed 39 | uses: re-actors/alls-green@release/v1 40 | with: 41 | jobs: ${{ toJSON(needs) }} 42 | -------------------------------------------------------------------------------- /.github/workflows/update-flake-lock.yml: -------------------------------------------------------------------------------- 1 | name: update-flake-lock 2 | 3 | on: 4 | workflow_dispatch: 5 | schedule: 6 | - cron: '0 1 5 * *' 7 | 8 | jobs: 9 | lockfile: 10 | runs-on: ubuntu-latest 11 | steps: 12 | - name: Checkout repository 13 | uses: actions/checkout@v4 14 | - name: Install Nix 15 | uses: DeterminateSystems/nix-installer-action@main 16 | - name: Update flake.lock 17 | uses: DeterminateSystems/update-flake-lock@main 18 | with: 19 | token: ${{ secrets.GH_TOKEN_FOR_UPDATES }} 20 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build*/ 2 | Xcode* 3 | *~ 4 | *.pyc 5 | **/.ipynb_checkpoints 6 | coverage* 7 | .travis 8 | .vscode* 9 | *.orig 10 | 11 | # pixi environments 12 | .pixi 13 | *.egg-info 14 | 15 | # benchmark 16 | plot.html 17 | -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | include: http://rainboard.laas.fr/project/pinocchio/.gitlab-ci.yml 2 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "cmake"] 2 | path = cmake 3 | url = https://github.com/jrl-umi3218/jrl-cmakemodules 4 | [submodule "models/example-robot-data"] 5 | path = models/example-robot-data 6 | url = https://github.com/Gepetto/example-robot-data.git 7 | -------------------------------------------------------------------------------- /CITATION.bib: -------------------------------------------------------------------------------- 1 | @inproceedings{carpentier:hal-01866228, 2 | TITLE = {{The Pinocchio C++ library -- A fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives}}, 3 | AUTHOR = {Carpentier, Justin and Saurel, Guilhem and Buondonno, Gabriele and Mirabel, Joseph and Lamiraux, Florent and Stasse, Olivier and Mansard, Nicolas}, 4 | URL = {https://hal.laas.fr/hal-01866228}, 5 | BOOKTITLE = {{SII 2019 - International Symposium on System Integrations}}, 6 | ADDRESS = {Paris, France}, 7 | HAL_LOCAL_REFERENCE = {Rapport LAAS n{\textdegree} 18288}, 8 | YEAR = {2019}, 9 | MONTH = Jan, 10 | PDF = {https://hal.laas.fr/hal-01866228v2/file/19-sii-pinocchio.pdf}, 11 | HAL_ID = {hal-01866228}, 12 | HAL_VERSION = {v2}, 13 | } 14 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2014-2023, CNRS 4 | Copyright (c) 2018-2025, INRIA 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | 10 | 1. Redistributions of source code must retain the above copyright notice, this 11 | list of conditions and the following disclaimer. 12 | 13 | 2. Redistributions in binary form must reproduce the above copyright notice, 14 | this list of conditions and the following disclaimer in the documentation 15 | and/or other materials provided with the distribution. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 21 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | -------------------------------------------------------------------------------- /bindings/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2016-2019 CNRS INRIA 3 | # 4 | 5 | add_subdirectory(python) 6 | if(BUILD_PYTHON_INTERFACE) 7 | set(PYWRAP 8 | ${PYWRAP} 9 | PARENT_SCOPE) 10 | endif(BUILD_PYTHON_INTERFACE) 11 | -------------------------------------------------------------------------------- /bindings/python/algorithm/expose-contact-solvers.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022-2024 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | namespace python 10 | { 11 | 12 | // Forward declaration 13 | void exposePGSContactSolver(); 14 | void exposeADMMContactSolver(); 15 | 16 | void exposeContactSolvers() 17 | { 18 | exposePGSContactSolver(); 19 | exposeADMMContactSolver(); 20 | } 21 | 22 | } // namespace python 23 | } // namespace pinocchio 24 | -------------------------------------------------------------------------------- /bindings/python/algorithm/expose-crba.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS 3 | // 4 | 5 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" 6 | #include "pinocchio/bindings/python/utils/eigen.hpp" 7 | #include "pinocchio/bindings/python/utils/namespace.hpp" 8 | #include "pinocchio/algorithm/crba.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | static context::MatrixXs crba_proxy( 15 | const context::Model & model, 16 | context::Data & data, 17 | const context::VectorXs & q, 18 | const Convention convention) 19 | { 20 | data.M.fill(context::Scalar(0)); 21 | crba(model, data, q, convention); 22 | make_symmetric(data.M); 23 | return data.M; 24 | } 25 | 26 | void exposeCRBA() 27 | { 28 | bp::def( 29 | "crba", crba_proxy, 30 | (bp::arg("model"), bp::arg("data"), bp::arg("q"), 31 | bp::arg("convention") = pinocchio::Convention::LOCAL), 32 | "Computes CRBA, store the result in Data and return it.\n" 33 | "Parameters:\n" 34 | "\tmodel: model of the kinematic tree\n" 35 | "\tdata: data related to the model\n" 36 | "\tq: the joint configuration vector (size model.nq)\n" 37 | "\t convention: Convention to use"); 38 | } 39 | 40 | } // namespace python 41 | } // namespace pinocchio 42 | -------------------------------------------------------------------------------- /bindings/python/algorithm/expose-geometry.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2022 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/algorithm/algorithms.hpp" 6 | #include "pinocchio/algorithm/geometry.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | 13 | void exposeGeometryAlgo() 14 | { 15 | using namespace Eigen; 16 | 17 | bp::def( 18 | "updateGeometryPlacements", 19 | &updateGeometryPlacements, 20 | bp::args("model", "data", "geometry_model", "geometry_data", "q"), 21 | "Update the placement of the collision objects according to the current configuration.\n" 22 | "The algorithm also updates the current placement of the joint in Data."); 23 | 24 | bp::def( 25 | "updateGeometryPlacements", &updateGeometryPlacements, 26 | bp::args("model", "data", "geometry_model", "geometry_data"), 27 | "Update the placement of the collision objects according to the current joint " 28 | "placement stored in data."); 29 | } 30 | } // namespace python 31 | } // namespace pinocchio 32 | -------------------------------------------------------------------------------- /bindings/python/algorithm/parallel/expose-parallel.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 INRIA 3 | // 4 | 5 | #include 6 | 7 | #include "pinocchio/bindings/python/fwd.hpp" 8 | 9 | namespace pinocchio 10 | { 11 | namespace python 12 | { 13 | 14 | void exposeParallelRNEA(); 15 | void exposeParallelABA(); 16 | 17 | void exposeParallelAlgorithms() 18 | { 19 | namespace bp = boost::python; 20 | 21 | exposeParallelRNEA(); 22 | exposeParallelABA(); 23 | 24 | bp::def( 25 | "omp_get_max_threads", &omp_get_max_threads, 26 | "Returns an upper bound on the number of threads that could be used."); 27 | } 28 | 29 | } // namespace python 30 | } // namespace pinocchio 31 | -------------------------------------------------------------------------------- /bindings/python/collision/parallel/expose-parallel.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include 6 | 7 | #include "pinocchio/bindings/python/fwd.hpp" 8 | 9 | namespace pinocchio 10 | { 11 | namespace python 12 | { 13 | 14 | void exposeParallelGeometry(); 15 | void exposeParallelBroadPhase(); 16 | 17 | void exposeParallelCollision() 18 | { 19 | namespace bp = boost::python; 20 | 21 | exposeParallelGeometry(); 22 | exposeParallelBroadPhase(); 23 | } 24 | 25 | } // namespace python 26 | } // namespace pinocchio 27 | -------------------------------------------------------------------------------- /bindings/python/collision/pool/expose-pool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/multibody/pool/model.hpp" 7 | 8 | #include "pinocchio/bindings/python/collision/pool/geometry.hpp" 9 | #include "pinocchio/bindings/python/collision/pool/broadphase-manager.hpp" 10 | #include "pinocchio/collision/tree-broadphase-manager.hpp" 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | namespace pinocchio 21 | { 22 | namespace python 23 | { 24 | 25 | void exposePoolCollision() 26 | { 27 | GeometryPoolPythonVisitor::expose(); 28 | BroadPhaseManagerPoolPythonVisitor< 29 | BroadPhaseManagerPool>::expose(); 30 | BroadPhaseManagerPoolPythonVisitor< 31 | TreeBroadPhaseManagerPool>::expose(); 32 | } 33 | 34 | } // namespace python 35 | } // namespace pinocchio 36 | -------------------------------------------------------------------------------- /bindings/python/extra/expose-extras.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/extra/extras.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | namespace python 10 | { 11 | 12 | void exposeExtras() 13 | { 14 | #if defined(PINOCCHIO_WITH_EXTRA_SUPPORT) 15 | exposeReachableWorkspace(); 16 | #endif // defined(PINOCCHIO_WITH_EXTRA_SUPPORT) 17 | } 18 | 19 | } // namespace python 20 | } // namespace pinocchio 21 | -------------------------------------------------------------------------------- /bindings/python/extra/mpfr/boost_number.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/context/mpfr.hpp" 6 | 7 | #include 8 | #include "pinocchio/bindings/python/math/multiprecision/boost/number.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | void exposeSpecificTypeFeatures() 15 | { 16 | BoostNumberPythonVisitor::expose("mpfr"); 17 | }; 18 | 19 | boost::python::object getScalarType() 20 | { 21 | namespace bp = boost::python; 22 | PyTypeObject * pytype = const_cast( 23 | bp::converter::registered_pytype_direct::get_pytype()); 24 | return bp::object(bp::handle<>(bp::borrowed(reinterpret_cast(pytype)))); 25 | } 26 | } // namespace python 27 | } // namespace pinocchio 28 | -------------------------------------------------------------------------------- /bindings/python/math/expose-lanczos-decomposition.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/math/lanczos-decomposition.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | namespace bp = boost::python; 13 | 14 | void exposeLanczosDecomposition() 15 | { 16 | #ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED 17 | typedef LanczosDecompositionTpl LanczosDecomposition; 18 | LanczosDecompositionPythonVisitor::expose(); 19 | #endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED 20 | } 21 | 22 | } // namespace python 23 | } // namespace pinocchio 24 | -------------------------------------------------------------------------------- /bindings/python/math/expose-tridiagonal-matrix.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/math/tridiagonal-matrix.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | namespace bp = boost::python; 13 | 14 | void exposeTridiagonalMatrix() 15 | { 16 | typedef TridiagonalSymmetricMatrixTpl 17 | TridiagonalSymmetricMatrix; 18 | TridiagonalSymmetricMatrixPythonVisitor::expose(); 19 | } 20 | 21 | } // namespace python 22 | } // namespace pinocchio 23 | -------------------------------------------------------------------------------- /bindings/python/multibody/expose-data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/multibody/data.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | 13 | void exposeData() 14 | { 15 | DataPythonVisitor::expose(); 16 | } 17 | 18 | } // namespace python 19 | } // namespace pinocchio 20 | -------------------------------------------------------------------------------- /bindings/python/multibody/expose-frame.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/serialization/aligned-vector.hpp" 6 | #include "pinocchio/serialization/frame.hpp" 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | #include "pinocchio/bindings/python/multibody/frame.hpp" 10 | #include "pinocchio/bindings/python/serialization/serialization.hpp" 11 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 12 | 13 | namespace pinocchio 14 | { 15 | namespace python 16 | { 17 | 18 | void exposeFrame() 19 | { 20 | FramePythonVisitor::expose(); 21 | StdAlignedVectorPythonVisitor::expose("StdVec_Frame"); 22 | #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 23 | serialize::vector_type>(); 24 | #endif 25 | } 26 | 27 | } // namespace python 28 | } // namespace pinocchio 29 | -------------------------------------------------------------------------------- /bindings/python/multibody/expose-geometry.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/multibody/geometry-object.hpp" 7 | #include "pinocchio/bindings/python/multibody/geometry-model.hpp" 8 | #include "pinocchio/bindings/python/multibody/geometry-data.hpp" 9 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | namespace python 14 | { 15 | 16 | void exposeGeometry() 17 | { 18 | GeometryObjectPythonVisitor::expose(); 19 | StdAlignedVectorPythonVisitor::expose("StdVec_GeometryObject"); 20 | 21 | CollisionPairPythonVisitor::expose(); 22 | GeometryModelPythonVisitor::expose(); 23 | GeometryDataPythonVisitor::expose(); 24 | } 25 | 26 | } // namespace python 27 | } // namespace pinocchio 28 | -------------------------------------------------------------------------------- /bindings/python/multibody/expose-model.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/multibody/model.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | 13 | void exposeModel() 14 | { 15 | ModelPythonVisitor::expose(); 16 | } 17 | 18 | } // namespace python 19 | } // namespace pinocchio 20 | -------------------------------------------------------------------------------- /bindings/python/multibody/pool/expose-pool.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021-2022 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/bindings/python/multibody/pool/model.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | 13 | void exposePool() 14 | { 15 | ModelPoolPythonVisitor::expose(); 16 | } 17 | 18 | } // namespace python 19 | } // namespace pinocchio 20 | -------------------------------------------------------------------------------- /bindings/python/parsers/expose-parsers.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/parsers/urdf.hpp" 6 | #include "pinocchio/bindings/python/parsers/sdf.hpp" 7 | #include "pinocchio/bindings/python/parsers/srdf.hpp" 8 | #include "pinocchio/bindings/python/parsers/mjcf.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | void exposeParsers() 16 | { 17 | exposeSDFParser(); 18 | exposeURDFParser(); 19 | exposeSRDFParser(); 20 | exposeMJCFParser(); 21 | } 22 | 23 | } // namespace python 24 | } // namespace pinocchio 25 | -------------------------------------------------------------------------------- /bindings/python/parsers/urdf/console-bridge.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #include 6 | #include "pinocchio/bindings/python/parsers/urdf.hpp" 7 | #include "pinocchio/bindings/python/utils/registration.hpp" 8 | 9 | #ifdef PINOCCHIO_WITH_URDFDOM 10 | #include 11 | #endif 12 | 13 | namespace pinocchio 14 | { 15 | namespace python 16 | { 17 | void exposeConsoleBridge() 18 | { 19 | namespace bp = boost::python; 20 | 21 | #ifdef PINOCCHIO_WITH_URDFDOM 22 | 23 | // fix CONSOLE_BRIDGE warning level 24 | ::console_bridge::setLogLevel(::console_bridge::CONSOLE_BRIDGE_LOG_ERROR); 25 | 26 | typedef ::console_bridge::LogLevel LogLevel; 27 | if (!register_symbolic_link_to_registered_type()) 28 | { 29 | bp::enum_("LogLevel") 30 | .value("CONSOLE_BRIDGE_LOG_DEBUG", ::console_bridge::CONSOLE_BRIDGE_LOG_DEBUG) 31 | .value("CONSOLE_BRIDGE_LOG_INFO", ::console_bridge::CONSOLE_BRIDGE_LOG_INFO) 32 | .value("CONSOLE_BRIDGE_LOG_WARN", ::console_bridge::CONSOLE_BRIDGE_LOG_WARN) 33 | .value("CONSOLE_BRIDGE_LOG_ERROR", ::console_bridge::CONSOLE_BRIDGE_LOG_ERROR) 34 | .value("CONSOLE_BRIDGE_LOG_NONE", ::console_bridge::CONSOLE_BRIDGE_LOG_NONE); 35 | } 36 | 37 | #endif 38 | } 39 | } // namespace python 40 | } // namespace pinocchio 41 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/casadi/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020 INRIA 3 | # 4 | # ruff: noqa: F401, F403, F405 5 | 6 | # Manually register submodules 7 | import sys 8 | 9 | from .. import utils 10 | from ..explog import exp, log 11 | from ..pinocchio_pywrap_casadi import * 12 | from ..pinocchio_pywrap_casadi import __raw_version__, __version__ 13 | 14 | sys.modules["pinocchio.casadi.rpy"] = rpy 15 | sys.modules["pinocchio.casadi.cholesky"] = cholesky 16 | 17 | if WITH_HPP_FCL: 18 | try: 19 | import hppfcl 20 | from hppfcl import ( 21 | CachedMeshLoader, 22 | CollisionGeometry, 23 | CollisionResult, 24 | Contact, 25 | DistanceResult, 26 | MeshLoader, 27 | StdVec_CollisionResult, 28 | StdVec_Contact, 29 | StdVec_DistanceResult, 30 | ) 31 | 32 | WITH_HPP_FCL_BINDINGS = True 33 | except ImportError: 34 | WITH_HPP_FCL_BINDINGS = False 35 | else: 36 | WITH_HPP_FCL_BINDINGS = False 37 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/cppad/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020-2021 INRIA 3 | # 4 | # ruff: noqa: F401, F403, F405 5 | # Manually register submodules 6 | import sys 7 | 8 | from .. import utils 9 | from ..explog import exp, log 10 | from ..pinocchio_pywrap_cppad import * 11 | from ..pinocchio_pywrap_cppad import __raw_version__, __version__ 12 | 13 | sys.modules["pinocchio.cppad.rpy"] = rpy 14 | sys.modules["pinocchio.cppad.cholesky"] = cholesky 15 | 16 | if WITH_HPP_FCL: 17 | try: 18 | import hppfcl 19 | from hppfcl import ( 20 | CachedMeshLoader, 21 | CollisionGeometry, 22 | CollisionResult, 23 | Contact, 24 | DistanceResult, 25 | MeshLoader, 26 | StdVec_CollisionResult, 27 | StdVec_Contact, 28 | StdVec_DistanceResult, 29 | ) 30 | 31 | WITH_HPP_FCL_BINDINGS = True 32 | except ImportError: 33 | WITH_HPP_FCL_BINDINGS = False 34 | else: 35 | WITH_HPP_FCL_BINDINGS = False 36 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/cppadcg/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2022 INRIA 3 | # 4 | # ruff: noqa: F401, F403, F405 5 | # Manually register submodules 6 | import sys 7 | 8 | from .. import utils 9 | from ..explog import exp, log 10 | from ..pinocchio_pywrap_cppadcg import * 11 | from ..pinocchio_pywrap_cppadcg import __raw_version__, __version__ 12 | 13 | sys.modules["pinocchio.cppadcg.rpy"] = rpy 14 | sys.modules["pinocchio.cppadcg.cholesky"] = cholesky 15 | 16 | if WITH_HPP_FCL: 17 | try: 18 | import hppfcl 19 | from hppfcl import ( 20 | CachedMeshLoader, 21 | CollisionGeometry, 22 | CollisionResult, 23 | Contact, 24 | DistanceResult, 25 | MeshLoader, 26 | StdVec_CollisionResult, 27 | StdVec_Contact, 28 | StdVec_DistanceResult, 29 | ) 30 | 31 | WITH_HPP_FCL_BINDINGS = True 32 | except ImportError: 33 | WITH_HPP_FCL_BINDINGS = False 34 | else: 35 | WITH_HPP_FCL_BINDINGS = False 36 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/deprecated.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2018-2023 CNRS INRIA 3 | # 4 | 5 | ## In this file, are reported some deprecated functions 6 | # that are still maintained until the next important future releases 7 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/deprecation.py: -------------------------------------------------------------------------------- 1 | import functools 2 | import warnings 3 | 4 | 5 | class DeprecatedWarning(UserWarning): 6 | pass 7 | 8 | 9 | def deprecated(instructions): 10 | """Flags a method as deprecated. 11 | Args: 12 | instructions: A human-friendly string of instructions, such 13 | as: 'Please migrate to add_proxy() ASAP.' 14 | """ 15 | 16 | def decorator(func): 17 | """This is a decorator which can be used to mark functions 18 | as deprecated. It will result in a warning being emitted 19 | when the function is used.""" 20 | 21 | @functools.wraps(func) 22 | def wrapper(*args, **kwargs): 23 | message = f"Call to deprecated function {func.__name__}. {instructions}" 24 | 25 | warnings.warn(message, category=DeprecatedWarning, stacklevel=2) 26 | 27 | return func(*args, **kwargs) 28 | 29 | instructions_doc = "Deprecated: " + instructions 30 | if wrapper.__doc__ is None: 31 | wrapper.__doc__ = instructions_doc 32 | else: 33 | wrapper.__doc__ = wrapper.__doc__.rstrip() + "\n\n" + instructions_doc 34 | return wrapper 35 | 36 | return decorator 37 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/derivative/xm.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2016 CNRS 3 | # 4 | 5 | import numpy as np 6 | import pinocchio as pin 7 | from dcrba import DCRBA, DRNEA, Coriolis 8 | from numpy.linalg import norm 9 | from pinocchio.robot_wrapper import RobotWrapper 10 | from pinocchio.utils import rand 11 | 12 | np.random.seed(0) 13 | 14 | robot = RobotWrapper( 15 | "/home/nmansard/src/pinocchio/pinocchio/models/romeo/urdf/romeo.urdf", 16 | [ 17 | "/home/nmansard/src/pinocchio/pinocchio/models/romeo/", 18 | ], 19 | pin.JointModelFreeFlyer(), 20 | ) 21 | q = rand(robot.model.nq) 22 | q[3:7] /= norm(q[3:7]) 23 | vq = rand(robot.model.nv) 24 | aq = rand(robot.model.nv) 25 | 26 | # d/dq M(q) 27 | dcrba = DCRBA(robot) 28 | dcrba.pre(q) 29 | Mp = dcrba() 30 | 31 | # d/dvq RNEA(q,vq) = C(q,vq) 32 | coriolis = Coriolis(robot) 33 | C = coriolis(q, vq) 34 | 35 | # d/dq RNEA(q,vq,aq) 36 | drnea = DRNEA(robot) 37 | aq = rand(robot.model.nv) 38 | R = drnea(q, vq, aq) 39 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/explog.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2015-2018 CNRS INRIA 3 | # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. 4 | # 5 | 6 | import math 7 | 8 | import numpy as np 9 | 10 | from . import pinocchio_pywrap_default as pin 11 | 12 | 13 | def exp(x): 14 | if isinstance(x, pin.Motion): 15 | return pin.exp6(x) 16 | if np.isscalar(x): 17 | return math.exp(x) 18 | if isinstance(x, np.ndarray): 19 | if x.shape == (6, 1) or x.shape == (6,): 20 | return pin.exp6(pin.Motion(x)) 21 | if x.shape == (3, 1) or x.shape == (3,): 22 | return pin.exp3(x) 23 | raise ValueError("Error only 3 and 6 vectors are allowed.") 24 | raise ValueError( 25 | "Error exp is only defined for real, vector3, vector6 and pin.Motion objects." 26 | ) 27 | 28 | 29 | def log(x): 30 | if isinstance(x, pin.SE3): 31 | return pin.log6(x) 32 | if np.isscalar(x): 33 | return math.log(x) 34 | if isinstance(x, np.ndarray): 35 | if x.shape == (4, 4): 36 | return pin.log6(x) 37 | if x.shape == (3, 3): 38 | return pin.log3(x) 39 | raise ValueError("Error only 3 and 4 matrices are allowed.") 40 | raise ValueError( 41 | "Error log is only defined for real, matrix3, matrix4 and pin.SE3 objects." 42 | ) 43 | 44 | 45 | __all__ = ["exp", "log"] 46 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/mpfr/__init__.py: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2021 INRIA 3 | # 4 | # ruff: noqa: F401, F403, F405 5 | 6 | # Manually register submodules 7 | import sys 8 | 9 | from .. import utils 10 | from ..explog import exp, log 11 | from ..pinocchio_pywrap_mpfr import * 12 | from ..pinocchio_pywrap_mpfr import __raw_version__, __version__ 13 | 14 | sys.modules["pinocchio.mpfr.rpy"] = rpy 15 | sys.modules["pinocchio.mpfr.cholesky"] = cholesky 16 | 17 | if WITH_HPP_FCL: 18 | try: 19 | import hppfcl 20 | from hppfcl import ( 21 | CachedMeshLoader, 22 | CollisionGeometry, 23 | CollisionResult, 24 | Contact, 25 | DistanceResult, 26 | MeshLoader, 27 | StdVec_CollisionResult, 28 | StdVec_Contact, 29 | StdVec_DistanceResult, 30 | ) 31 | 32 | WITH_HPP_FCL_BINDINGS = True 33 | except ImportError: 34 | WITH_HPP_FCL_BINDINGS = False 35 | else: 36 | WITH_HPP_FCL_BINDINGS = False 37 | -------------------------------------------------------------------------------- /bindings/python/pinocchio/visualize/__init__.py: -------------------------------------------------------------------------------- 1 | # ruff: noqa: F401 2 | from .base_visualizer import BaseVisualizer 3 | from .gepetto_visualizer import GepettoVisualizer 4 | from .meshcat_visualizer import MeshcatVisualizer 5 | from .panda3d_visualizer import Panda3dVisualizer 6 | from .rviz_visualizer import RVizVisualizer 7 | from .visualizers import Visualizer 8 | -------------------------------------------------------------------------------- /bindings/python/pinocchiopy.pc.cmake: -------------------------------------------------------------------------------- 1 | prefix=${_PKG_CONFIG_PREFIX} 2 | exec_prefix=${_PKG_CONFIG_EXEC_PREFIX} 3 | libdir=${_PKG_CONFIG_PYWRAP_LIBDIR} 4 | bindir=${_PKG_CONFIG_PYWRAP_BINDIR} 5 | pkglibdir=${_PKG_CONFIG_PKGLIBDIR} 6 | includedir=${_PKG_CONFIG_INCLUDEDIR} 7 | datarootdir=${_PKG_CONFIG_DATAROOTDIR} 8 | pkgdatarootdir=${_PKG_CONFIG_PKGDATAROOTDIR} 9 | docdir=${_PKG_CONFIG_DOCDIR} 10 | doxygendocdir=${_PKG_CONFIG_DOXYGENDOCDIR} 11 | 12 | Name: pinocchiopy 13 | Description: Python bindings of Pinocchio 14 | URL: ${_PKG_CONFIG_URL} 15 | Version: ${_PKG_CONFIG_VERSION} 16 | Requires: ${_PKG_CONFIG_PYWRAP_REQUIRES} 17 | Conflicts: ${_PKG_CONFIG_PYWRAP_CONFLICTS} 18 | Libs: ${_PKG_CONFIG_PYWRAP_LIBS} 19 | Cflags: ${_PKG_CONFIG_PYWRAP_CFLAGS} 20 | -------------------------------------------------------------------------------- /bindings/python/spatial/expose-SE3.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/serialization/aligned-vector.hpp" 6 | #include "pinocchio/serialization/se3.hpp" 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | #include "pinocchio/bindings/python/spatial/se3.hpp" 10 | #include "pinocchio/bindings/python/serialization/serialization.hpp" 11 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 12 | 13 | namespace pinocchio 14 | { 15 | namespace python 16 | { 17 | 18 | void exposeSE3() 19 | { 20 | SE3PythonVisitor::expose(); 21 | StdAlignedVectorPythonVisitor::expose("StdVec_SE3"); 22 | #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 23 | serialize::vector_type>(); 24 | #endif 25 | } 26 | 27 | } // namespace python 28 | } // namespace pinocchio 29 | -------------------------------------------------------------------------------- /bindings/python/spatial/expose-force.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/serialization/aligned-vector.hpp" 6 | #include "pinocchio/serialization/force.hpp" 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | #include "pinocchio/bindings/python/spatial/force.hpp" 10 | #include "pinocchio/bindings/python/serialization/serialization.hpp" 11 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 12 | 13 | namespace pinocchio 14 | { 15 | namespace python 16 | { 17 | 18 | void exposeForce() 19 | { 20 | ForcePythonVisitor::expose(); 21 | StdAlignedVectorPythonVisitor::expose("StdVec_Force"); 22 | #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 23 | serialize::vector_type>(); 24 | #endif 25 | } 26 | 27 | } // namespace python 28 | } // namespace pinocchio 29 | -------------------------------------------------------------------------------- /bindings/python/spatial/expose-inertia.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/serialization/aligned-vector.hpp" 6 | #include "pinocchio/serialization/inertia.hpp" 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | #include "pinocchio/bindings/python/spatial/inertia.hpp" 10 | #include "pinocchio/bindings/python/serialization/serialization.hpp" 11 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 12 | 13 | namespace pinocchio 14 | { 15 | namespace python 16 | { 17 | 18 | void exposeInertia() 19 | { 20 | InertiaPythonVisitor::expose(); 21 | PseudoInertiaPythonVisitor::expose(); 22 | LogCholeskyParametersPythonVisitor::expose(); 23 | StdAlignedVectorPythonVisitor::expose("StdVec_Inertia"); 24 | #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 25 | serialize::vector_type>(); 26 | #endif 27 | } 28 | 29 | } // namespace python 30 | } // namespace pinocchio 31 | -------------------------------------------------------------------------------- /bindings/python/spatial/expose-motion.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/serialization/aligned-vector.hpp" 6 | #include "pinocchio/serialization/motion.hpp" 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | #include "pinocchio/bindings/python/spatial/motion.hpp" 10 | #include "pinocchio/bindings/python/spatial/classic-acceleration.hpp" 11 | #include "pinocchio/bindings/python/serialization/serialization.hpp" 12 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 13 | 14 | namespace pinocchio 15 | { 16 | namespace python 17 | { 18 | 19 | void exposeMotion() 20 | { 21 | exposeClassicAcceleration(); 22 | MotionPythonVisitor::expose(); 23 | StdAlignedVectorPythonVisitor::expose("StdVec_Motion"); 24 | #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 25 | serialize::vector_type>(); 26 | #endif 27 | } 28 | 29 | } // namespace python 30 | } // namespace pinocchio 31 | -------------------------------------------------------------------------------- /bindings/python/spatial/expose-symmetric3.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/serialization/aligned-vector.hpp" 6 | #include "pinocchio/serialization/symmetric3.hpp" 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | #include "pinocchio/bindings/python/spatial/symmetric3.hpp" 10 | #include "pinocchio/bindings/python/serialization/serialization.hpp" 11 | #include "pinocchio/bindings/python/utils/std-aligned-vector.hpp" 12 | 13 | namespace pinocchio 14 | { 15 | namespace python 16 | { 17 | 18 | void exposeSymmetric3() 19 | { 20 | Symmetric3PythonVisitor::expose(); 21 | StdAlignedVectorPythonVisitor::expose("StdVec_Symmetric3"); 22 | #ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 23 | serialize::vector_type>(); 24 | #endif // ifndef PINOCCHIO_PYTHON_NO_SERIALIZATION 25 | } 26 | 27 | } // namespace python 28 | } // namespace pinocchio 29 | -------------------------------------------------------------------------------- /bindings/python/utils/dependencies.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020-2021 INRIA 3 | // 4 | 5 | #include 6 | 7 | namespace pinocchio 8 | { 9 | namespace python 10 | { 11 | 12 | namespace bp = boost::python; 13 | 14 | void exposeDependencies() 15 | { 16 | bp::scope().attr("WITH_HPP_FCL") = 17 | #ifdef PINOCCHIO_WITH_HPP_FCL 18 | true; 19 | #else 20 | false; 21 | #endif 22 | 23 | bp::scope().attr("WITH_URDFDOM") = 24 | #ifdef PINOCCHIO_WITH_URDFDOM 25 | true; 26 | #else 27 | false; 28 | #endif 29 | 30 | bp::scope().attr("WITH_CPPAD") = 31 | #ifdef PINOCCHIO_WITH_CPPAD 32 | true; 33 | #else 34 | false; 35 | #endif 36 | 37 | bp::scope().attr("WITH_OPENMP") = 38 | #ifdef PINOCCHIO_PYTHON_INTERFACE_WITH_OPENMP 39 | true; 40 | #else 41 | false; 42 | #endif 43 | 44 | bp::scope().attr("WITH_SDFORMAT") = 45 | #ifdef PINOCCHIO_WITH_SDFORMAT 46 | true; 47 | #else 48 | false; 49 | #endif 50 | } 51 | 52 | } // namespace python 53 | } // namespace pinocchio 54 | -------------------------------------------------------------------------------- /bindings/python/utils/version.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018-2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/utils/constant.hpp" 6 | #include "pinocchio/bindings/python/utils/version.hpp" 7 | #include "pinocchio/utils/version.hpp" 8 | 9 | #include 10 | 11 | namespace pinocchio 12 | { 13 | namespace python 14 | { 15 | 16 | namespace bp = boost::python; 17 | 18 | void exposeVersion() 19 | { 20 | // Define release numbers of the current Pinocchio version. 21 | bp::def_constant("PINOCCHIO_MAJOR_VERSION", PINOCCHIO_MAJOR_VERSION); 22 | bp::def_constant("PINOCCHIO_MINOR_VERSION", PINOCCHIO_MINOR_VERSION); 23 | bp::def_constant("PINOCCHIO_PATCH_VERSION", PINOCCHIO_PATCH_VERSION); 24 | 25 | bp::def( 26 | "printVersion", printVersion, (bp::arg("delimiter") = "."), 27 | "Returns the current version of Pinocchio as a string.\n" 28 | "The user may specify the delimiter between the different semantic numbers."); 29 | 30 | bp::def( 31 | "checkVersionAtLeast", &checkVersionAtLeast, bp::args("major", "minor", "patch"), 32 | "Checks if the current version of Pinocchio is at least" 33 | " the version provided by the input arguments."); 34 | } 35 | 36 | } // namespace python 37 | } // namespace pinocchio 38 | -------------------------------------------------------------------------------- /colcon.pkg: -------------------------------------------------------------------------------- 1 | { 2 | "hooks": [ 3 | "share/pinocchio/hook/ament_prefix_path.dsv", 4 | "share/pinocchio/hook/python_path.dsv" 5 | ] 6 | } 7 | -------------------------------------------------------------------------------- /development/build.md: -------------------------------------------------------------------------------- 1 | # Build and install from source with Pixi 2 | 3 | To build **Pinocchio** from source the easiest way is to use [Pixi](https://pixi.sh/latest/#installation). 4 | 5 | [Pixi](https://pixi.sh/latest/) is a cross-platform package management tool for developers that 6 | will install all required dependencies in `.pixi` directory. 7 | It's used by our CI agent so you have the guarantee to get the right dependencies. 8 | 9 | Run the following command to install dependencies, configure, build and test the project: 10 | 11 | ```bash 12 | pixi run test 13 | ``` 14 | 15 | The project will be built in the `build` directory. 16 | You can run `pixi shell` and build the project with `cmake` and `ninja` manually. 17 | -------------------------------------------------------------------------------- /development/release.md: -------------------------------------------------------------------------------- 1 | # Release with Pixi 2 | 3 | To create a release with Pixi run the following commands on the **devel** branch: 4 | 5 | ```bash 6 | PINOCCHIO_VERSION=X.Y.Z pixi run release_new_version 7 | git push origin 8 | git push origin vX.Y.Z 9 | ``` 10 | 11 | Where `X.Y.Z` is the new version. 12 | Be careful to follow the [Semantic Versioning](https://semver.org/spec/v2.0.0.html) rules. 13 | 14 | You will find the following assets: 15 | - `./build_new_version/pinocchio-X.Y.Z.tar.gz` 16 | - `./build_new_version/pinocchio-X.Y.Z.tar.gz.sig` 17 | 18 | Then, create a new release on [GitHub](https://github.com/stack-of-tasks/pinocchio/releases/new) with: 19 | 20 | * Tag: vX.Y.Z 21 | * Title: pinocchio X.Y.Z 22 | * Body: 23 | ``` 24 | ## What's Changed 25 | 26 | CHANGELOG CONTENT 27 | 28 | **Full Changelog**: https://github.com/stack-of-tasks/pinocchio/compare/vXX.YY.ZZ...vX.Y.Z 29 | ``` 30 | 31 | Where `XX.YY.ZZ` is the last release version. 32 | 33 | Then upload `pinocchio-X.Y.Z.tar.gz` and `pinocchio-X.Y.Z.tar.gz.sig` and publish the release. 34 | -------------------------------------------------------------------------------- /development/scripts/pixi/activation_clang.sh: -------------------------------------------------------------------------------- 1 | #! /bin/bash 2 | # Clang activation script 3 | 4 | export CC=clang 5 | export CXX=clang++ 6 | 7 | # activation.sh set this variable to gcc, we must override it here 8 | export CCACHE_COMPILERTYPE=clang 9 | -------------------------------------------------------------------------------- /development/scripts/pixi/activation_clang_cl.bat: -------------------------------------------------------------------------------- 1 | :: Setup clang-cl compiler 2 | set CC=clang-cl 3 | set CXX=clang-cl 4 | -------------------------------------------------------------------------------- /doc/a-features/a-spatial.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_a-spatial Spatial algebra 2 | 3 | Spatial algebra is a mathematical notation commonly employed in rigid body 4 | dynamics to represent and manipulate physical quantities such as velocities, 5 | accelerations and forces. Pinocchio is based on this mathematical notation. 6 | Dedicated classes are provided to represent coordinate transformations in the 7 | 3D Euclidean space (named SE3), spatial motion vectors (Motion), spatial force 8 | vectors (Force), and spatial inertias (Inertia). Along with the available 9 | methods, this endows Pinocchio with an efficient software library for spatial 10 | algebra calculations. 11 | -------------------------------------------------------------------------------- /doc/a-features/d-model.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_d-model Geometric and collision models 2 | 3 | Aside from the kinematic model, Pinocchio defines a geometric model, i.e. the 4 | volumes attached to the kinematic tree. This model can be used for displaying 5 | the robot and computing quantities associated to collisions. Like the kinematic 6 | model, the fixed quantities (placement and shape of the volumes) are stored in 7 | a *GeometricModel* object, while buffers and quantities used by associated 8 | algorithms are defined in an object. The volumes are represented using the FCL 9 | library. Bodies of the robot are attached to each joint, while obstacles of the 10 | environment are defined in the world frame. Collision and distance algorithms 11 | for the kinematic trees are implemented, based on FCL methods. 12 | -------------------------------------------------------------------------------- /doc/a-features/f-kinematic.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_f-kinematic Kinematics algorithms 2 | 3 | ## Forward kinematics 4 | 5 | Pinocchio implements direct kinematic computations up to the second 6 | order. When a robot configuration is given, a forward pass is performed 7 | to compute the spatial placements of each joint and to store them as 8 | coordinate transformations. If the velocity is given, it also computes 9 | the spatial velocities of each joint (expressed in local frame), and 10 | similarly for accelerations. 11 | 12 | ## Kinematic Jacobian 13 | 14 | The spatial Jacobian of each joint can be easily computed with a single 15 | forward pass, either expressed locally or in the world frame. 16 | -------------------------------------------------------------------------------- /doc/a-features/h-frames.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_h-frames Operational frames 2 | 3 | TODO: as frames are not necessary to understand the previous sections, it could 4 | be a good idea to treat them 5 | separately. Introduce them here together with the related algorithms. 6 | -------------------------------------------------------------------------------- /doc/a-features/j-analytical-derivatives.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_j-analytical-derivatives Analytical derivatives 2 | 3 | Beside proposing standard forward and inverse dynamics algorithms, Pinocchio 4 | also provides efficient implementations of their analytical derivatives. These 5 | derivatives are for instance of primary importance in the context of whole-body 6 | trajectory optimization or more largely, for numerical optimal control. To the 7 | best of our knowledge, Pinocchio is the first rigid body framework which 8 | implements this feature natively. 9 | -------------------------------------------------------------------------------- /doc/a-features/k-automatic-differentiation.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_k-automatic-differentiation Automatic differentiation and source code generation 2 | 3 | In addition to analytical derivatives, Pinocchio supports automatic 4 | differentiation. This is made possible through the full *scalar* 5 | templatization of the whole C++ code and the use of any external library 6 | that does automatic differentiation: ADOL-C, CasADi, CppAD and others. It is 7 | important to keep in mind that these automatic derivatives are often 8 | much slower than the analytical ones. 9 | 10 | Another unique but central feature of Pinocchio is its ability to 11 | generate code both at compile time and at runtime. This is achieved by 12 | using another external toolbox called CppADCodeGen built on top of 13 | CppAD. From any function using Pinocchio, CppADCodeGen is 14 | able to generate on the fly its code in various languages: C, Latex, 15 | etc. and to make some simplifications of the math expressions. Thanks to 16 | this procedure, a code tailored for a specific robot model can be 17 | generated and used externally to Pinocchio. 18 | -------------------------------------------------------------------------------- /doc/a-features/l-python.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_l-python Python bindings 2 | 3 | TODO: ... 4 | -------------------------------------------------------------------------------- /doc/a-features/m-tests.md: -------------------------------------------------------------------------------- 1 | \page md_doc_a-features_m-tests Unit tests 2 | 3 | TODO: ... 4 | -------------------------------------------------------------------------------- /doc/b-examples/a-model.md: -------------------------------------------------------------------------------- 1 | # Loading the model 2 | 3 | ## Python 4 | \include overview-urdf.py 5 | 6 | ## C++ 7 | \include overview-urdf.cpp 8 | -------------------------------------------------------------------------------- /doc/b-examples/ab-geometry-models.md: -------------------------------------------------------------------------------- 1 | # Geometry models 2 | 3 | ## Python 4 | \include geometry-models.py 5 | 6 | ## C++ 7 | \include geometry-models.cpp 8 | -------------------------------------------------------------------------------- /doc/b-examples/b-display.md: -------------------------------------------------------------------------------- 1 | # Load and display a model 2 | 3 | Pinocchio does not feature a built-in viewer and you will have to rely on external libraries. 4 | However, Pinocchio offers Python support for a few external tools in order to ease the display of a given model. 5 | Below you will find a list of the different options. 6 | -------------------------------------------------------------------------------- /doc/b-examples/c-collisions.md: -------------------------------------------------------------------------------- 1 | # Collision detection and distances 2 | 3 | ## Python 4 | \include collisions.py 5 | 6 | ## C++ 7 | \include collisions.cpp 8 | -------------------------------------------------------------------------------- /doc/b-examples/display/a-gepetto-viewer.md: -------------------------------------------------------------------------------- 1 | # Display a model using GepettoViewer 2 | 3 | \include gepetto-viewer.py 4 | -------------------------------------------------------------------------------- /doc/b-examples/display/b-meshcat-viewer.md: -------------------------------------------------------------------------------- 1 | # Display a model using Meshcat 2 | 3 | \include meshcat-viewer.py 4 | -------------------------------------------------------------------------------- /doc/b-examples/display/c-robot-wrapper-viewer.md: -------------------------------------------------------------------------------- 1 | # Display a model using RobotWrapper 2 | 3 | `RobotWrapper` is a wrapper class written in Python holding Pinocchio models and data. 4 | It also includes wrapper functions for a few common methods. 5 | `RobotWrapper` can interoperate with different visualizers. 6 | Below, you find an example on how to load a model with `RobotWrapper` and how to use it with a visualizer of your choice. 7 | 8 | \include robot-wrapper-viewer.py 9 | -------------------------------------------------------------------------------- /doc/b-examples/e-reduced-model.md: -------------------------------------------------------------------------------- 1 | # Build reduced model 2 | 3 | This example shows how to build a reduced robot model from an existing URDF model by fixing the desired joints at a specified position. 4 | 5 | ## Python 6 | \include build-reduced-model.py 7 | 8 | ## C++ 9 | \include build-reduced-model.cpp 10 | -------------------------------------------------------------------------------- /doc/b-examples/f-update-model.md: -------------------------------------------------------------------------------- 1 | # Update model after loading its URDF 2 | 3 | This example shows how to update a robot model after loading it from a URDF description. 4 | 5 | ## Python 6 | 7 | \include update-model-after-urdf.py 8 | -------------------------------------------------------------------------------- /doc/b-examples/intro.md: -------------------------------------------------------------------------------- 1 | # Examples 2 | -------------------------------------------------------------------------------- /doc/bib/carpentier-rss18.bib: -------------------------------------------------------------------------------- 1 | @inproceedings{carpentier-rss18, 2 | title={Analytical derivatives of rigid body dynamics algorithms}, 3 | author={Justin Carpentier and Nicolas Mansard}, 4 | booktitle={Robotics: Science and Systems (RSS)}, 5 | year={2018} 6 | } 7 | -------------------------------------------------------------------------------- /doc/bib/carpentier-sii19.bib: -------------------------------------------------------------------------------- 1 | @inproceedings{carpentier-sii19, 2 | title={The {P}inocchio {C}++ library -- {A} fast and flexible implementation of rigid body dynamics algorithms and their analytical derivatives}, 3 | author={J. Carpentier and G. Saurel and G. Buondonno and J. Mirabel and F. Lamiraux and O. Stasse and N. Mansard}, 4 | booktitle={International Symposium on System Integration (SII)} 5 | year={2019} 6 | } 7 | -------------------------------------------------------------------------------- /doc/bib/pinocchioweb.bib: -------------------------------------------------------------------------------- 1 | @misc{pinocchioweb, 2 | author = {Justin Carpentier and Joseph Mirabel and Nicolas Mansard and the Pinocchio Development Team}, 3 | title = {Pinocchio: fast forward and inverse dynamics for poly-articulated systems}, 4 | howpublished = {https://stack-of-tasks.github.io/pinocchio}, 5 | year = {2015--2018} 6 | } 7 | -------------------------------------------------------------------------------- /doc/c-maths/intro.md: -------------------------------------------------------------------------------- 1 | # Mathematical formulation 2 | 3 | This section describes the mathematical formulation used by Pinocchio, which is the base for Featherstone's algorithms. 4 | -------------------------------------------------------------------------------- /doc/customdoxygen.css: -------------------------------------------------------------------------------- 1 | /* Customizing Doxygen output */ 2 | 3 | /* Needed to allow line breaks in tables*/ 4 | .memberdecls { 5 | table-layout: fixed; 6 | width: 100%; 7 | } 8 | 9 | /* Needed to break long template names*/ 10 | .memTemplItemLeft { 11 | white-space: normal !important; 12 | word-wrap: break-word; 13 | } 14 | 15 | /* Needed to break long template names*/ 16 | .memItemLeft { 17 | white-space: normal !important; 18 | word-wrap: break-word; 19 | } 20 | -------------------------------------------------------------------------------- /doc/d-practical-exercises/intro.md: -------------------------------------------------------------------------------- 1 | # Practical Exercises 2 | 3 | This section contains complete exercices to understand Pinocchio. 4 | -------------------------------------------------------------------------------- /doc/images/pinocchio-logo-large.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/images/pinocchio-logo-large.pdf -------------------------------------------------------------------------------- /doc/images/pinocchio-logo-large.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/images/pinocchio-logo-large.png -------------------------------------------------------------------------------- /doc/images/pinocchio-performances.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/images/pinocchio-performances.png -------------------------------------------------------------------------------- /doc/pictures/SE3Example1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/SE3Example1.jpg -------------------------------------------------------------------------------- /doc/pictures/cube-rotation_picture.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/cube-rotation_picture.jpg -------------------------------------------------------------------------------- /doc/pictures/cube_rotation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/cube_rotation.gif -------------------------------------------------------------------------------- /doc/pictures/joints_gif/cylindrical_laas.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/joints_gif/cylindrical_laas.gif -------------------------------------------------------------------------------- /doc/pictures/joints_gif/planar_laas.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/joints_gif/planar_laas.gif -------------------------------------------------------------------------------- /doc/pictures/joints_gif/prismatic_laas.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/joints_gif/prismatic_laas.gif -------------------------------------------------------------------------------- /doc/pictures/joints_gif/revolute_laas.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/joints_gif/revolute_laas.gif -------------------------------------------------------------------------------- /doc/pictures/joints_gif/spherical_laas.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pictures/joints_gif/spherical_laas.gif -------------------------------------------------------------------------------- /doc/pinocchio.ico: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pinocchio.ico -------------------------------------------------------------------------------- /doc/pinocchio.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pinocchio.png -------------------------------------------------------------------------------- /doc/pinocchio_cheat_sheet.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/doc/pinocchio_cheat_sheet.pdf -------------------------------------------------------------------------------- /examples/casadi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2025 INRIA 3 | # 4 | 5 | function(ADD_PINOCCHIO_CPP_CASADI_EXAMPLE EXAMPLE) 6 | add_pinocchio_cpp_example(${EXAMPLE} CASADI) 7 | endfunction() 8 | 9 | if(BUILD_WITH_CASADI_SUPPORT) 10 | 11 | add_pinocchio_cpp_casadi_example(casadi-aba) 12 | add_pinocchio_cpp_casadi_example(casadi-crba) 13 | add_pinocchio_cpp_casadi_example(casadi-rnea) 14 | 15 | if(BUILD_PYTHON_INTERFACE) 16 | 17 | set(${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES quadrotor-ocp) 18 | if(BUILD_WITH_COLLISION_SUPPORT) 19 | list(APPEND ${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES cartpole) 20 | endif() 21 | 22 | foreach(EXAMPLE ${${PROJECT_NAME}_CASADI_PYTHON_EXAMPLES}) 23 | set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-casadi-${EXAMPLE}") 24 | add_python_unit_test(${EXAMPLE_NAME} "examples/casadi/${EXAMPLE}.py" "bindings/python") 25 | add_windows_dll_path_to_test(${EXAMPLE_NAME}) 26 | endforeach() 27 | endif() 28 | endif() 29 | -------------------------------------------------------------------------------- /examples/codegen/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020-2023 INRIA 3 | # 4 | 5 | if(cppadcg_FOUND 6 | AND BUILD_WITH_CODEGEN_SUPPORT 7 | AND BUILD_WITH_URDF_SUPPORT) 8 | function(ADD_PINOCCHIO_CPP_CODEGEN_EXAMPLE EXAMPLE) 9 | add_pinocchio_cpp_example( 10 | ${EXAMPLE} 11 | CPPADCG 12 | PARSERS) 13 | target_link_libraries(${PROJECT_NAME}-example-cpp-${EXAMPLE} PUBLIC ${CMAKE_DL_LIBS}) 14 | target_compile_definitions(${PROJECT_NAME}-example-cpp-${EXAMPLE} 15 | PUBLIC PINOCCHIO_CXX_COMPILER=\"${CMAKE_CXX_COMPILER}\") 16 | endfunction() 17 | 18 | add_pinocchio_cpp_codegen_example(codegen-crba) 19 | add_pinocchio_cpp_codegen_example(codegen-rnea) 20 | 21 | if(BUILD_PYTHON_INTERFACE) 22 | set(${PROJECT_NAME}_CODEGEN_PYTHON_EXAMPLES codegen-rnea) 23 | 24 | foreach(EXAMPLE ${${PROJECT_NAME}_CODEGEN_PYTHON_EXAMPLES}) 25 | set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}") 26 | add_python_unit_test(${EXAMPLE_NAME} "examples/codegen/${EXAMPLE}.py" "bindings/python") 27 | add_windows_dll_path_to_test(${EXAMPLE_NAME}) 28 | endforeach() 29 | endif() 30 | 31 | endif() 32 | -------------------------------------------------------------------------------- /examples/codegen/codegen-rnea.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | import pinocchio.cppadcg as cgpin 4 | from pycppad import ( 5 | ADCG, 6 | CG, 7 | ADCGFun, 8 | CodeHandler, 9 | Independent, 10 | LangCDefaultVariableNameGenerator, 11 | LanguageC, 12 | ) 13 | 14 | pinmodel = pin.buildSampleModelHumanoidRandom() 15 | model = cgpin.Model(pinmodel) 16 | data = model.createData() 17 | 18 | nq = model.nq 19 | nv = model.nv 20 | 21 | x = np.array([ADCG(CG(0.0))] * (nq + nv + nv)) 22 | x[:nq] = cgpin.neutral(model) 23 | Independent(x) 24 | 25 | y = cgpin.rnea(model, data, x[:nq], x[nq : nq + nv], x[nq + nv :]) 26 | 27 | fun = ADCGFun(x, y) 28 | 29 | # /*************************************************************************** 30 | # * Generate the C source code 31 | # **************************************************************************/ 32 | 33 | # /** 34 | # * start the special steps for source code generation for a Jacobian 35 | # */ 36 | handler = CodeHandler(50) 37 | 38 | indVars = np.array([CG(1.0)] * (nq + nv + nv)) 39 | handler.makeVariables(indVars) 40 | 41 | jac = fun.Jacobian(indVars) 42 | 43 | langC = LanguageC("double", 3) 44 | nameGen = LangCDefaultVariableNameGenerator("y", "x", "v", "array", "sarray") 45 | code = handler.generateCode(langC, jac, nameGen, "source") 46 | print(code) 47 | -------------------------------------------------------------------------------- /examples/contact-cholesky.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | 3 | import pinocchio as pin 4 | 5 | pinocchio_model_dir = Path(__file__).parent.parent / "models" 6 | urdf_filename = ( 7 | pinocchio_model_dir 8 | / "example-robot-data/robots/anymal_b_simple_description/robots/anymal.urdf" 9 | ) 10 | model = pin.buildModelFromUrdf(urdf_filename) 11 | data = model.createData() 12 | 13 | q0 = pin.neutral(model) 14 | 15 | # Add feet frames 16 | feet_names = ["LH_FOOT", "RH_FOOT", "LF_FOOT", "RF_FOOT"] 17 | feet_frame_ids = [] 18 | for foot_name in feet_names: 19 | frame_id = model.getFrameId(foot_name) 20 | feet_frame_ids.append(frame_id) 21 | 22 | contact_models = [] 23 | for fid in feet_frame_ids: 24 | frame = model.frames[fid] 25 | cmodel = pin.RigidConstraintModel( 26 | pin.ContactType.CONTACT_3D, 27 | frame.parent, 28 | frame.placement, 29 | pin.LOCAL_WORLD_ALIGNED, 30 | ) 31 | contact_models.append(cmodel) 32 | 33 | contact_data = [cmodel.createData() for cmodel in contact_models] 34 | 35 | pin.initConstraintDynamics(model, data, contact_models) 36 | pin.crba(model, data, q0) 37 | 38 | data.contact_chol.compute(model, data, contact_models, contact_data) 39 | 40 | delassus_matrix = data.contact_chol.getInverseOperationalSpaceInertiaMatrix() 41 | delassus_matrix_inv = data.contact_chol.getOperationalSpaceInertiaMatrix() 42 | -------------------------------------------------------------------------------- /examples/cppad/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2022 INRIA 3 | # 4 | 5 | if(BUILD_WITH_AUTODIFF_SUPPORT) 6 | function(ADD_PINOCCHIO_CPP_AUTODIFF_EXAMPLE EXAMPLE) 7 | include_directories(SYSTEM ${cppadcg_INCLUDE_DIR}) 8 | add_pinocchio_cpp_example(${EXAMPLE}) 9 | target_link_libraries(${PROJECT_NAME}-example-cpp-${EXAMPLE} PUBLIC ${CMAKE_DL_LIBS} 10 | ${cppad_LIBRARY}) 11 | endfunction() 12 | 13 | add_pinocchio_cpp_autodiff_example(autodiff-rnea) 14 | 15 | if(BUILD_PYTHON_INTERFACE) 16 | set(${PROJECT_NAME}_AUTODIFF_PYTHON_EXAMPLES autodiff-rnea) 17 | 18 | foreach(EXAMPLE ${${PROJECT_NAME}_AUTODIFF_PYTHON_EXAMPLES}) 19 | set(EXAMPLE_NAME "${PROJECT_NAME}-example-py-${EXAMPLE}") 20 | add_python_unit_test(${EXAMPLE_NAME} "examples/cppad/${EXAMPLE}.py" "bindings/python") 21 | add_windows_dll_path_to_test(${EXAMPLE_NAME}) 22 | endforeach() 23 | endif() 24 | 25 | endif() 26 | -------------------------------------------------------------------------------- /examples/cppad/autodiff-rnea.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | import pinocchio.cppad as ADpin 4 | from pinocchio.utils import isapprox 5 | from pycppad import AD, ADFun, Independent 6 | 7 | pinmodel = pin.buildSampleModelHumanoidRandom() 8 | model = ADpin.Model(pinmodel) 9 | data = model.createData() 10 | 11 | nq = model.nq 12 | nv = model.nv 13 | 14 | q = ADpin.neutral(model) 15 | v = np.array([AD(1.0)] * nv) 16 | a = np.zeros(nv, dtype=AD) 17 | 18 | # declare independent variables and starting recording 19 | Independent(v) 20 | 21 | y = ADpin.rnea(model, data, q, v, a) 22 | 23 | # create f: v -> y and stop tape recording 24 | f = ADFun(v, y) 25 | 26 | # first-order derivates wrt v 27 | dv = np.ones(nv) 28 | ADdtau_dv = f.Jacobian(dv).reshape(nv, nv) 29 | (dtau_dq, dtau_dv, dtau_da) = pin.computeRNEADerivatives( 30 | pinmodel, pinmodel.createData(), pin.neutral(pinmodel), np.ones(nv), np.zeros(nv) 31 | ) 32 | isapprox(ADdtau_dv, dtau_dv, 1e-12) 33 | -------------------------------------------------------------------------------- /examples/display-shapes-meshcat.py: -------------------------------------------------------------------------------- 1 | import sys 2 | 3 | import numpy as np 4 | import pinocchio as pin 5 | 6 | try: 7 | import hppfcl 8 | except ImportError: 9 | print("This example requires hppfcl") 10 | sys.exit(0) 11 | from pinocchio.visualize import MeshcatVisualizer 12 | 13 | model = pin.Model() 14 | 15 | geom_model = pin.GeometryModel() 16 | geometries = [ 17 | hppfcl.Capsule(0.1, 0.8), 18 | hppfcl.Sphere(0.5), 19 | hppfcl.Box(1, 1, 1), 20 | hppfcl.Cylinder(0.1, 1.0), 21 | hppfcl.Cone(0.5, 1.0), 22 | ] 23 | for i, geom in enumerate(geometries): 24 | placement = pin.SE3(np.eye(3), np.array([i, 0, 0])) 25 | geom_obj = pin.GeometryObject(f"obj{i}", 0, 0, geom, placement) 26 | color = np.random.uniform(0, 1, 4) 27 | color[3] = 1 28 | geom_obj.meshColor = color 29 | geom_model.addGeometryObject(geom_obj) 30 | 31 | viz = MeshcatVisualizer(model, geom_model, geom_model) 32 | 33 | # Initialize the viewer. 34 | try: 35 | viz.initViewer(open=True) 36 | except ImportError as error: 37 | print(error) 38 | sys.exit(0) 39 | 40 | try: 41 | viz.loadViewerModel("shapes") 42 | except AttributeError as error: 43 | print(error) 44 | sys.exit(0) 45 | 46 | viz.display(np.zeros(0)) 47 | input("press enter to continue") 48 | -------------------------------------------------------------------------------- /examples/forward-dynamics-derivatives.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | 4 | ## 5 | ## In this short script, we show how to compute the derivatives of the 6 | ## forward dynamics, using the algorithms explained in: 7 | ## 8 | ## Analytical Derivatives of Rigid Body Dynamics Algorithms, 9 | ## Justin Carpentier and Nicolas Mansard, 10 | ## Robotics: Science and Systems, 2018 11 | ## 12 | 13 | # Create model and data 14 | 15 | model = pin.buildSampleModelHumanoidRandom() 16 | data = model.createData() 17 | 18 | # Set bounds (by default they are undefinded for a the Simple Humanoid model) 19 | 20 | model.lowerPositionLimit = -np.ones((model.nq, 1)) 21 | model.upperPositionLimit = np.ones((model.nq, 1)) 22 | 23 | q = pin.randomConfiguration(model) # joint configuration 24 | v = np.random.rand(model.nv, 1) # joint velocity 25 | tau = np.random.rand(model.nv, 1) # joint acceleration 26 | 27 | # Evaluate the derivatives 28 | 29 | pin.computeABADerivatives(model, data, q, v, tau) 30 | 31 | # Retrieve the derivatives in data 32 | 33 | ddq_dq = data.ddq_dq # Derivatives of the FD w.r.t. the joint config vector 34 | ddq_dv = data.ddq_dv # Derivatives of the FD w.r.t. the joint velocity vector 35 | ddq_dtau = data.Minv # Derivatives of the FD w.r.t. the joint acceleration vector 36 | -------------------------------------------------------------------------------- /examples/interpolation-SE3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "pinocchio/multibody/liegroup/liegroup.hpp" 3 | 4 | using namespace pinocchio; 5 | 6 | int main() 7 | { 8 | typedef double Scalar; 9 | 10 | typedef SpecialEuclideanOperationTpl<3, Scalar> SE3Operation; 11 | SE3Operation aSE3; 12 | SE3Operation::ConfigVector_t pose_s, pose_g; 13 | SE3Operation::TangentVector_t delta_u; 14 | 15 | // Starting configuration 16 | pose_s(0) = 1.0; 17 | pose_s(1) = 1.0; 18 | pose_s(2) = 1; 19 | pose_s(3) = -0.13795; 20 | pose_s(4) = 0.13795; 21 | pose_s(5) = 0.69352; 22 | pose_s(6) = 0.69352; 23 | aSE3.normalize(pose_s); 24 | 25 | // Goal configuration 26 | pose_g(0) = 4; 27 | pose_g(1) = 3; 28 | pose_g(2) = 3; 29 | pose_g(3) = -0.46194; 30 | pose_g(4) = 0.331414; 31 | pose_g(5) = 0.800103; 32 | pose_g(6) = 0.191342; 33 | aSE3.normalize(pose_g); 34 | 35 | SE3Operation::ConfigVector_t pole_u; 36 | aSE3.interpolate(pose_s, pose_g, 0.5, pole_u); 37 | std::cout << "Interpolated configuration: " << pole_u.transpose() << std::endl; 38 | 39 | return 0; 40 | } 41 | -------------------------------------------------------------------------------- /examples/inverse-dynamics-derivatives.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | 4 | ## 5 | ## In this short script, we show how to compute the derivatives of the 6 | ## inverse dynamics (RNEA), using the algorithms proposed in: 7 | ## 8 | ## Analytical Derivatives of Rigid Body Dynamics Algorithms, 9 | ## Justin Carpentier and Nicolas Mansard, 10 | ## Robotics: Science and Systems, 2018 11 | ## 12 | 13 | # Create model and data 14 | 15 | model = pin.buildSampleModelHumanoidRandom() 16 | data = model.createData() 17 | 18 | # Set bounds (by default they are undefinded for a the Simple Humanoid model) 19 | 20 | model.lowerPositionLimit = -np.ones((model.nq, 1)) 21 | model.upperPositionLimit = np.ones((model.nq, 1)) 22 | 23 | q = pin.randomConfiguration(model) # joint configuration 24 | v = np.random.rand(model.nv, 1) # joint velocity 25 | a = np.random.rand(model.nv, 1) # joint acceleration 26 | 27 | # Evaluate the derivatives 28 | 29 | pin.computeRNEADerivatives(model, data, q, v, a) 30 | 31 | # Retrieve the derivatives in data 32 | 33 | dtau_dq = data.dtau_dq # Derivatives of the ID w.r.t. the joint config vector 34 | dtau_dv = data.dtau_dv # Derivatives of the ID w.r.t. the joint velocity vector 35 | dtau_da = data.M # Derivatives of the ID w.r.t. the joint acceleration vector 36 | -------------------------------------------------------------------------------- /examples/inverse-dynamics.py: -------------------------------------------------------------------------------- 1 | # Copyright 2023 Inria 2 | # SPDX-License-Identifier: BSD-2-Clause 3 | 4 | 5 | """ 6 | In this short script, we show how to compute inverse dynamics (RNEA), i.e. the 7 | vector of joint torques corresponding to a given motion. 8 | """ 9 | 10 | from pathlib import Path 11 | 12 | import numpy as np 13 | import pinocchio as pin 14 | 15 | # Load the model from a URDF file 16 | # Change to your own URDF file here, or give a path as command-line argument 17 | pinocchio_model_dir = Path(__file__).parent.parent / "models/" 18 | model_path = pinocchio_model_dir / "example-robot-data/robots" 19 | mesh_dir = pinocchio_model_dir 20 | urdf_filename = "ur5_robot.urdf" 21 | urdf_model_path = model_path / "ur_description/urdf/" / urdf_filename 22 | model, _, _ = pin.buildModelsFromUrdf(urdf_model_path, package_dirs=mesh_dir) 23 | 24 | # Build a data frame associated with the model 25 | data = model.createData() 26 | 27 | # Sample a random joint configuration, joint velocities and accelerations 28 | q = pin.randomConfiguration(model) # in rad for the UR5 29 | v = np.random.rand(model.nv, 1) # in rad/s for the UR5 30 | a = np.random.rand(model.nv, 1) # in rad/s² for the UR5 31 | 32 | # Computes the inverse dynamics (RNEA) for all the joints of the robot 33 | tau = pin.rnea(model, data, q, v, a) 34 | 35 | # Print out to the vector of joint torques (in N.m) 36 | print("Joint torques: " + str(tau)) 37 | -------------------------------------------------------------------------------- /examples/inverse-kinematics-3d.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio 3 | from numpy.linalg import norm, solve 4 | 5 | model = pinocchio.buildSampleModelManipulator() 6 | data = model.createData() 7 | 8 | JOINT_ID = 6 9 | oMdes = pinocchio.SE3(np.eye(3), np.array([1.0, 0.0, 1.0])) 10 | 11 | q = pinocchio.neutral(model) 12 | eps = 1e-4 13 | IT_MAX = 1000 14 | DT = 1e-1 15 | damp = 1e-12 16 | 17 | it = 0 18 | while True: 19 | pinocchio.forwardKinematics(model, data, q) 20 | iMd = data.oMi[JOINT_ID].actInv(oMdes) 21 | err = iMd.translation 22 | if norm(err) < eps: 23 | success = True 24 | break 25 | if it >= IT_MAX: 26 | success = False 27 | break 28 | J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) # in joint frame 29 | J = -J[:3, :] # linear part of the Jacobian 30 | v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(3), err)) 31 | q = pinocchio.integrate(model, q, v * DT) 32 | if not it % 10: 33 | print(f"{it}: error = {err.T}") 34 | it += 1 35 | 36 | if success: 37 | print("Convergence achieved!") 38 | else: 39 | print( 40 | "\nWarning: the iterative algorithm has not reached convergence to " 41 | "the desired precision" 42 | ) 43 | 44 | print(f"\nresult: {q.flatten().tolist()}") 45 | print(f"\nfinal error: {err.T}") 46 | -------------------------------------------------------------------------------- /examples/inverse-kinematics.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio 3 | from numpy.linalg import norm, solve 4 | 5 | model = pinocchio.buildSampleModelManipulator() 6 | data = model.createData() 7 | 8 | JOINT_ID = 6 9 | oMdes = pinocchio.SE3(np.eye(3), np.array([1.0, 0.0, 1.0])) 10 | 11 | q = pinocchio.neutral(model) 12 | eps = 1e-4 13 | IT_MAX = 1000 14 | DT = 1e-1 15 | damp = 1e-12 16 | 17 | i = 0 18 | while True: 19 | pinocchio.forwardKinematics(model, data, q) 20 | iMd = data.oMi[JOINT_ID].actInv(oMdes) 21 | err = pinocchio.log(iMd).vector # in joint frame 22 | if norm(err) < eps: 23 | success = True 24 | break 25 | if i >= IT_MAX: 26 | success = False 27 | break 28 | J = pinocchio.computeJointJacobian(model, data, q, JOINT_ID) # in joint frame 29 | J = -np.dot(pinocchio.Jlog6(iMd.inverse()), J) 30 | v = -J.T.dot(solve(J.dot(J.T) + damp * np.eye(6), err)) 31 | q = pinocchio.integrate(model, q, v * DT) 32 | if not i % 10: 33 | print(f"{i}: error = {err.T}") 34 | i += 1 35 | 36 | if success: 37 | print("Convergence achieved!") 38 | else: 39 | print( 40 | "\n" 41 | "Warning: the iterative algorithm has not reached convergence " 42 | "to the desired precision" 43 | ) 44 | 45 | print(f"\nresult: {q.flatten().tolist()}") 46 | print(f"\nfinal error: {err.T}") 47 | -------------------------------------------------------------------------------- /examples/overview-SE3.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include "pinocchio/multibody/liegroup/liegroup.hpp" 3 | 4 | using namespace pinocchio; 5 | int main() 6 | { 7 | typedef double Scalar; 8 | typedef SpecialEuclideanOperationTpl<3, Scalar> SE3Operation; 9 | 10 | SE3Operation aSE3; 11 | SE3Operation::ConfigVector_t pose_s, pose_g; 12 | SE3Operation::TangentVector_t delta_u; 13 | 14 | pose_s(0) = 1.0; 15 | pose_s(1) = 1.0; 16 | pose_s(2) = 1; 17 | pose_s(3) = -0.13795; 18 | pose_s(4) = 0.13795; 19 | pose_s(5) = 0.69352; 20 | pose_s(6) = 0.69352; 21 | pose_g(0) = 4; 22 | pose_g(1) = 3; 23 | pose_g(2) = 3; 24 | pose_g(3) = -0.46194; 25 | pose_g(4) = 0.331414; 26 | pose_g(5) = 0.800103; 27 | pose_g(6) = 0.191342; 28 | 29 | // First normalize the inputs 30 | aSE3.normalize(pose_s); 31 | std::cout << "pose_s: " << pose_s.transpose() << std::endl; 32 | aSE3.normalize(pose_g); 33 | std::cout << "pose_g: " << pose_g.transpose() << std::endl; 34 | 35 | aSE3.difference(pose_s, pose_g, delta_u); 36 | std::cout << "delta_u: " << delta_u.transpose() << std::endl; 37 | 38 | SE3Operation::ConfigVector_t pose_check; 39 | 40 | aSE3.integrate(pose_s, delta_u, pose_check); 41 | std::cout << "pose_check: " << pose_check.transpose() << std::endl; 42 | 43 | return 0; 44 | } 45 | -------------------------------------------------------------------------------- /examples/overview-simple.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "pinocchio/multibody/sample-models.hpp" 4 | #include "pinocchio/algorithm/joint-configuration.hpp" 5 | #include "pinocchio/algorithm/rnea.hpp" 6 | 7 | int main() 8 | { 9 | pinocchio::Model model; 10 | pinocchio::buildModels::manipulator(model); 11 | pinocchio::Data data(model); 12 | 13 | Eigen::VectorXd q = pinocchio::neutral(model); 14 | Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); 15 | Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); 16 | 17 | const Eigen::VectorXd & tau = pinocchio::rnea(model, data, q, v, a); 18 | std::cout << "tau = " << tau.transpose() << std::endl; 19 | } 20 | -------------------------------------------------------------------------------- /examples/overview-simple.py: -------------------------------------------------------------------------------- 1 | import pinocchio 2 | 3 | model = pinocchio.buildSampleModelManipulator() 4 | data = model.createData() 5 | 6 | q = pinocchio.neutral(model) 7 | v = pinocchio.utils.zero(model.nv) 8 | a = pinocchio.utils.zero(model.nv) 9 | 10 | tau = pinocchio.rnea(model, data, q, v, a) 11 | print("tau = ", tau.T) 12 | -------------------------------------------------------------------------------- /examples/overview-urdf.py: -------------------------------------------------------------------------------- 1 | from pathlib import Path 2 | from sys import argv 3 | 4 | import pinocchio 5 | 6 | # This path refers to Pinocchio source code but you can define your own directory here. 7 | pinocchio_model_dir = Path(__file__).parent.parent / "models" 8 | 9 | # You should change here to set up your own URDF file or just pass it as an argument of 10 | # this example. 11 | urdf_filename = ( 12 | pinocchio_model_dir / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" 13 | if len(argv) < 2 14 | else argv[1] 15 | ) 16 | 17 | # Load the urdf model 18 | model = pinocchio.buildModelFromUrdf(urdf_filename) 19 | print("model name: " + model.name) 20 | 21 | # Create data required by the algorithms 22 | data = model.createData() 23 | 24 | # Sample a random configuration 25 | q = pinocchio.randomConfiguration(model) 26 | print(f"q: {q.T}") 27 | 28 | # Perform the forward kinematics over the kinematic tree 29 | pinocchio.forwardKinematics(model, data, q) 30 | 31 | # Print out the placement of each joint of the kinematic tree 32 | for name, oMi in zip(model.names, data.oMi): 33 | print("{:<24} : {: .2f} {: .2f} {: .2f}".format(name, *oMi.translation.T.flat)) 34 | -------------------------------------------------------------------------------- /examples/run-algo-in-parallel.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pinocchio as pin 3 | 4 | model = pin.buildSampleModelHumanoid() 5 | model.lowerPositionLimit[:7] = -np.ones(7) 6 | model.upperPositionLimit[:7] = +np.ones(7) 7 | 8 | pool = pin.ModelPool(model) 9 | 10 | num_threads = pin.omp_get_max_threads() 11 | batch_size = 128 12 | q = np.empty((model.nq, batch_size)) 13 | for k in range(batch_size): 14 | q[:, k] = pin.randomConfiguration(model) 15 | 16 | v = np.zeros((model.nv, batch_size)) 17 | a = np.zeros((model.nv, batch_size)) 18 | tau = np.zeros((model.nv, batch_size)) 19 | 20 | print(f"num_threads: {num_threads}") 21 | print(f"batch_size: {batch_size}") 22 | 23 | # Call RNEA 24 | res_rnea = np.empty((model.nv, batch_size)) 25 | pin.rneaInParallel(num_threads, pool, q, v, a, res_rnea) # Without allocation 26 | res_rnea2 = pin.rneaInParallel(num_threads, pool, q, v, a) # With allocation 27 | 28 | # Call ABA 29 | res_aba = np.empty((model.nv, batch_size)) 30 | pin.abaInParallel(num_threads, pool, q, v, tau, res_aba) # Without allocation 31 | res_aba2 = pin.abaInParallel(num_threads, pool, q, v, tau) # With allocation 32 | -------------------------------------------------------------------------------- /examples/rviz-viewer.py: -------------------------------------------------------------------------------- 1 | # NOTE: this example needs RViz to be installed 2 | # usage: start ROS master (roscore) and then run this test 3 | 4 | 5 | from pathlib import Path 6 | 7 | import pinocchio as pin 8 | from pinocchio.visualize import RVizVisualizer 9 | 10 | # Load the URDF model. 11 | # Conversion with str seems to be necessary when executing this file with ipython 12 | pinocchio_model_dir = Path(__file__).parent.parent / "models" 13 | 14 | model_path = pinocchio_model_dir / "example-robot-data/robots" 15 | mesh_dir = pinocchio_model_dir 16 | urdf_filename = "talos_reduced.urdf" 17 | urdf_model_path = model_path / "talos_data/robots" / urdf_filename 18 | 19 | model, collision_model, visual_model = pin.buildModelsFromUrdf( 20 | urdf_model_path, mesh_dir, pin.JointModelFreeFlyer() 21 | ) 22 | viz = RVizVisualizer(model, collision_model, visual_model) 23 | 24 | # Initialize the viewer. 25 | viz.initViewer() 26 | viz.loadViewerModel("pinocchio") 27 | 28 | # Display a robot configuration. 29 | q0 = pin.neutral(model) 30 | viz.display(q0) 31 | 32 | # Display another robot. 33 | viz2 = RVizVisualizer(model, collision_model, visual_model) 34 | viz2.initViewer(viz.viewer) 35 | viz2.loadViewerModel(rootNodeName="pinocchio2") 36 | q = q0.copy() 37 | q[1] = 1.0 38 | viz2.display(q) 39 | 40 | input("Press enter to exit...") 41 | 42 | viz.clean() 43 | -------------------------------------------------------------------------------- /examples/sample-model-viewer.py: -------------------------------------------------------------------------------- 1 | from sys import argv 2 | 3 | import pinocchio as pin 4 | from numpy import pi 5 | from pinocchio.visualize import GepettoVisualizer, MeshcatVisualizer, RVizVisualizer 6 | 7 | # GepettoVisualizer: -g 8 | # MeshcatVisualizer: -m 9 | VISUALIZER = None 10 | if len(argv) > 1: 11 | opt = argv[1] 12 | if opt == "-g": 13 | VISUALIZER = GepettoVisualizer 14 | elif opt == "-m": 15 | VISUALIZER = MeshcatVisualizer 16 | elif opt == "-r": 17 | VISUALIZER = RVizVisualizer 18 | else: 19 | raise ValueError("Unrecognized option: " + opt) 20 | 21 | model = pin.buildSampleModelHumanoid() 22 | visual_model = pin.buildSampleGeometryModelHumanoid(model) 23 | collision_model = visual_model.copy() 24 | 25 | q0 = pin.neutral(model) 26 | 27 | if VISUALIZER: 28 | viz = VISUALIZER(model, collision_model, visual_model) 29 | viz.initViewer() 30 | viz.loadViewerModel() 31 | viz.display(q0) 32 | 33 | input("Enter to check a new configuration") 34 | 35 | q = q0.copy() 36 | q[8] = pi / 2 37 | q[14] = pi / 2 38 | q[23] = -pi / 2 39 | q[29] = pi / 2 40 | 41 | viz.display(q) 42 | 43 | input("Press enter to exit...") 44 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/center-of-mass-derivatives.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_center_of_mass_derivatives_txx__ 6 | #define __pinocchio_algorithm_center_of_mass_derivatives_txx__ 7 | 8 | namespace pinocchio 9 | { 10 | 11 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 12 | getCenterOfMassVelocityDerivatives< 13 | context::Scalar, 14 | context::Options, 15 | JointCollectionDefaultTpl, 16 | context::Matrix3x>( 17 | const context::Model &, context::Data &, const Eigen::MatrixBase &); 18 | 19 | } // namespace pinocchio 20 | 21 | #endif // ifndef __pinocchio_algorithm_center_of_mass_derivatives_txx__ 22 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/check-base.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2025 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_check_base_hpp__ 6 | #define __pinocchio_algorithm_check_base_hpp__ 7 | 8 | #include "pinocchio/multibody/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | /// CRTP class describing the API of the checkers 14 | template 15 | struct AlgorithmCheckerBase 16 | { 17 | AlgorithmCheckerDerived & derived() 18 | { 19 | return *static_cast(this); 20 | } 21 | 22 | const AlgorithmCheckerDerived & derived() const 23 | { 24 | return *static_cast(this); 25 | } 26 | 27 | template class JointCollectionTpl> 28 | bool checkModel(const ModelTpl & model) const 29 | { 30 | return derived().checkModel_impl(model); 31 | } 32 | }; 33 | 34 | } // namespace pinocchio 35 | 36 | #endif // __pinocchio_algorithm_check_base_hpp__ 37 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/check-data.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2025 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_check_data_hpp__ 6 | #define __pinocchio_algorithm_check_data_hpp__ 7 | 8 | #include "pinocchio/multibody/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | /// Check the validity of data wrt to model, in particular if model has been modified. 14 | /// 15 | /// \param[in] model reference model 16 | /// \param[in] data corresponding data 17 | /// 18 | /// \returns True if data is valid wrt model. 19 | template class JointCollectionTpl> 20 | bool checkData( 21 | const ModelTpl & model, 22 | const DataTpl & data); 23 | 24 | } // namespace pinocchio 25 | 26 | /* --- Details -------------------------------------------------------------------- */ 27 | #include "pinocchio/algorithm/check-data.hxx" 28 | 29 | #endif // __pinocchio_algorithm_check_data_hpp__ 30 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/check.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_check_hpp__ 6 | #define __pinocchio_algorithm_check_hpp__ 7 | 8 | // This header is provided for backward compatibility 9 | #include "pinocchio/algorithm/check-model.hpp" 10 | #include "pinocchio/algorithm/check-data.hpp" 11 | 12 | #endif // __pinocchio_algorithm_check_hpp__ 13 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/compute-all-terms.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_compute_all_terms_txx__ 6 | #define __pinocchio_algorithm_compute_all_terms_txx__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace impl 11 | { 12 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void computeAllTerms< 13 | context::Scalar, 14 | context::Options, 15 | JointCollectionDefaultTpl, 16 | Eigen::Ref, 17 | Eigen::Ref>( 18 | const context::Model &, 19 | context::Data &, 20 | const Eigen::MatrixBase> &, 21 | const Eigen::MatrixBase> &); 22 | } // namespace impl 23 | } // namespace pinocchio 24 | 25 | #endif // ifndef __pinocchio_algorithm_compute_all_terms_txx__ 26 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/constrained-problem-data.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/include/pinocchio/algorithm/constrained-problem-data.hpp -------------------------------------------------------------------------------- /include/pinocchio/algorithm/constraints/constraint-data-base.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2023 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_constraint_data_base_hpp__ 6 | #define __pinocchio_algorithm_constraint_data_base_hpp__ 7 | 8 | #include "pinocchio/algorithm/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | template 14 | struct ConstraintDataBase : NumericalBase 15 | { 16 | typedef typename traits::Scalar Scalar; 17 | typedef typename traits::ConstraintModel ConstraintModel; 18 | 19 | Derived & derived() 20 | { 21 | return static_cast(*this); 22 | } 23 | const Derived & derived() const 24 | { 25 | return static_cast(*this); 26 | } 27 | }; 28 | 29 | } // namespace pinocchio 30 | 31 | #endif // ifndef __pinocchio_algorithm_constraint_data_base_hpp__ 32 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/constraints/constraints.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2023 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_constraints_constraints_hpp__ 6 | #define __pinocchio_algorithm_constraints_constraints_hpp__ 7 | 8 | #include "pinocchio/algorithm/constraints/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | } 13 | 14 | #endif // ifndef __pinocchio_algorithm_constraints_constraints_hpp__ 15 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/contact-dynamics.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_contact_dynamics_txx__ 6 | #define __pinocchio_algorithm_contact_dynamics_txx__ 7 | 8 | #ifndef PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS 9 | 10 | namespace pinocchio 11 | { 12 | 13 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 14 | computeKKTContactDynamicMatrixInverse< 15 | context::Scalar, 16 | context::Options, 17 | JointCollectionDefaultTpl, 18 | context::VectorXs, 19 | context::MatrixXs, 20 | context::MatrixXs>( 21 | const context::Model &, 22 | context::Data &, 23 | const Eigen::MatrixBase &, 24 | const Eigen::MatrixBase &, 25 | const Eigen::MatrixBase &, 26 | const context::Scalar &); 27 | 28 | } // namespace pinocchio 29 | 30 | #endif // PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS 31 | 32 | #endif // ifndef __pinocchio_algorithm_contact_dynamics_txx__ 33 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/contact-jacobian.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_contact_jacobian_txx__ 6 | #define __pinocchio_algorithm_contact_jacobian_txx__ 7 | 8 | #ifndef PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN 9 | 10 | namespace pinocchio 11 | { 12 | // extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void getConstraintJacobian 13 | // 14 | // (const context::Model &, const context::Data &, const context::RigidConstraintModel &, 15 | // context::RigidConstraintData &, const Eigen::MatrixBase &); 16 | 17 | // extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void getConstraintsJacobian 18 | // 21 | // (const context::Model &, context::Data &, const context::RigidConstraintModelVector &, 22 | // context::RigidConstraintDataVector &, const Eigen::MatrixBase &); 23 | 24 | } // namespace pinocchio 25 | 26 | #endif // PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN 27 | 28 | #endif // ifndef __pinocchio_algorithm_contact_jacobian_txx__ 29 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/contact.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_contact_hpp__ 6 | #define __pinocchio_algorithm_contact_hpp__ 7 | 8 | #include "pinocchio/algorithm/contact-info.hpp" 9 | #include "pinocchio/algorithm/contact-cholesky.hpp" 10 | #include "pinocchio/algorithm/contact-dynamics.hpp" 11 | #include "pinocchio/algorithm/contact-dynamics-derivatives.hpp" 12 | #include "pinocchio/algorithm/contact-jacobian.hpp" 13 | 14 | namespace pinocchio 15 | { 16 | 17 | } // namespace pinocchio 18 | 19 | #include "pinocchio/algorithm/contact.hxx" 20 | 21 | #endif // ifndef __pinocchio_algorithm_contact_hpp__ 22 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/contact.hxx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/include/pinocchio/algorithm/contact.hxx -------------------------------------------------------------------------------- /include/pinocchio/algorithm/crba.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_crba_txx__ 6 | #define __pinocchio_algorithm_crba_txx__ 7 | 8 | namespace pinocchio 9 | { 10 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::MatrixXs & 11 | crba< 12 | context::Scalar, 13 | context::Options, 14 | JointCollectionDefaultTpl, 15 | Eigen::Ref>( 16 | const context::Model &, 17 | context::Data &, 18 | const Eigen::MatrixBase> &, 19 | const Convention convention); 20 | } // namespace pinocchio 21 | 22 | #endif // ifndef __pinocchio_algorithm_crba_txx__ 23 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/default-check.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_default_check_hpp__ 6 | #define __pinocchio_default_check_hpp__ 7 | 8 | #include 9 | #include 10 | #include 11 | 12 | namespace pinocchio 13 | { 14 | /// Default checker-list, used as the default argument in Model::check(). 15 | inline AlgorithmCheckerList makeDefaultCheckerList() 16 | { 17 | return makeAlgoCheckerList(ParentChecker(), CRBAChecker(), ABAChecker()); 18 | } 19 | 20 | #define DEFAULT_CHECKERS makeDefaultCheckerList() 21 | 22 | template class JointCollectionTpl> 23 | inline bool ModelTpl::check() const 24 | { 25 | return this->check(DEFAULT_CHECKERS); 26 | } 27 | 28 | } // namespace pinocchio 29 | 30 | #endif // ifndef __pinocchio_default_check_hpp__ 31 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/delassus-operartor-ref.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/include/pinocchio/algorithm/delassus-operartor-ref.hpp -------------------------------------------------------------------------------- /include/pinocchio/algorithm/delassus-operator-rigid-body.hpp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/include/pinocchio/algorithm/delassus-operator-rigid-body.hpp -------------------------------------------------------------------------------- /include/pinocchio/algorithm/geometry.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_geometry_txx__ 6 | #define __pinocchio_algorithm_geometry_txx__ 7 | 8 | #ifndef PINOCCHIO_SKIP_ALGORTIHM_GEOMETRY 9 | 10 | namespace pinocchio 11 | { 12 | 13 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void updateGeometryPlacements< 14 | context::Scalar, 15 | context::Options, 16 | JointCollectionDefaultTpl, 17 | context::VectorXs>( 18 | const context::Model &, 19 | context::Data &, 20 | const GeometryModel &, 21 | GeometryData &, 22 | const Eigen::MatrixBase &); 23 | 24 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 25 | updateGeometryPlacements( 26 | const context::Model &, const context::Data &, const GeometryModel &, GeometryData &); 27 | 28 | } // namespace pinocchio 29 | 30 | #endif // PINOCCHIO_SKIP_ALGORITHM_GEOMETRY 31 | 32 | #endif // ifndef __pinocchio_algorithm_geometry_txx__ 33 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/impulse-dynamics.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_impulse_dynamics_txx__ 6 | #define __pinocchio_algorithm_impulse_dynamics_txx__ 7 | 8 | #ifndef PINOCCHIO_SKIP_ALGORITHM_IMPULSE_DYNAMICS 9 | 10 | namespace pinocchio 11 | { 12 | 13 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI const context::VectorXs & 14 | impulseDynamics< 15 | context::Scalar, 16 | context::Options, 17 | JointCollectionDefaultTpl, 18 | context::VectorXs, 19 | context::VectorXs, 20 | typename context::RigidConstraintModelVector::allocator_type, 21 | typename context::RigidConstraintDataVector::allocator_type>( 22 | const context::Model &, 23 | context::Data &, 24 | const Eigen::MatrixBase &, 25 | const Eigen::MatrixBase &, 26 | const context::RigidConstraintModelVector &, 27 | context::RigidConstraintDataVector &, 28 | const context::Scalar, 29 | const ProximalSettingsTpl &); 30 | 31 | } // namespace pinocchio 32 | 33 | #endif // PINOCCHIO_SKIP_ALGORITHM_IMPULSE_DYNAMICS 34 | 35 | #endif // ifndef __pinocchio_algorithm_impulse_dynamics_txx__ 36 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/parallel/geometry.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021-2024 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_parallel_geometry_hpp__ 6 | #define __pinocchio_algorithm_parallel_geometry_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/algorithm/parallel/geometry.hpp, pinocchio/collision/parallel/geometry.hpp) 12 | // clang-format on 13 | 14 | #include "pinocchio/collision/parallel/geometry.hpp" 15 | 16 | #endif // ifndef __pinocchio_algorithm_parallel_geometry_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/parallel/omp.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_parallel_omp_hpp__ 6 | #define __pinocchio_algorithm_parallel_omp_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | inline void set_default_omp_options(const size_t num_threads = (size_t)omp_get_max_threads()) 13 | { 14 | omp_set_num_threads((int)num_threads); 15 | omp_set_dynamic(0); 16 | } 17 | } // namespace pinocchio 18 | 19 | #endif // ifndef __pinocchio_algorithm_parallel_omp_hpp__ 20 | -------------------------------------------------------------------------------- /include/pinocchio/algorithm/proximal.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_algorithm_proximal_txx__ 6 | #define __pinocchio_algorithm_proximal_txx__ 7 | 8 | namespace pinocchio 9 | { 10 | extern template struct PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI 11 | ProximalSettingsTpl; 12 | 13 | } // namespace pinocchio 14 | 15 | #endif // ifndef __pinocchio_algorithm_proximal_txx__ 16 | -------------------------------------------------------------------------------- /include/pinocchio/autodiff/casadi/math/matrix.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019-2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_autodiff_casadi_math_matrix_hpp__ 6 | #define __pinocchio_autodiff_casadi_math_matrix_hpp__ 7 | 8 | #include "pinocchio/math/matrix.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace internal 13 | { 14 | template 15 | struct CallCorrectMatrixInverseAccordingToScalar<::casadi::Matrix> 16 | { 17 | typedef ::casadi::Matrix SX; 18 | template 19 | static void 20 | run(const Eigen::MatrixBase & mat, const Eigen::MatrixBase & dest) 21 | { 22 | SX cs_mat(mat.rows(), mat.cols()); 23 | casadi::copy(mat.derived(), cs_mat); 24 | 25 | SX cs_mat_inv = SX::inv(cs_mat); 26 | 27 | MatrixOut & dest_ = PINOCCHIO_EIGEN_CONST_CAST(MatrixOut, dest); 28 | casadi::copy(cs_mat_inv, dest_); 29 | } 30 | }; 31 | } // namespace internal 32 | } // namespace pinocchio 33 | 34 | #endif // ifndef __pinocchio_autodiff_casadi_math_matrix_hpp__ 35 | -------------------------------------------------------------------------------- /include/pinocchio/autodiff/casadi/spatial/se3-tpl.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_autodiff_casadi_spatial_se3_tpl_hpp__ 6 | #define __pinocchio_autodiff_casadi_spatial_se3_tpl_hpp__ 7 | 8 | #include "pinocchio/spatial/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace internal 13 | { 14 | template 15 | struct cast_call_normalize_method< 16 | SE3Tpl<::casadi::Matrix, Options>, 17 | NewScalar, 18 | ::casadi::Matrix> 19 | { 20 | template 21 | static void run(T &) 22 | { 23 | // do nothing 24 | } 25 | }; 26 | 27 | template 28 | struct cast_call_normalize_method, ::casadi::Matrix, Scalar> 29 | { 30 | template 31 | static void run(T &) 32 | { 33 | // do nothing 34 | } 35 | }; 36 | 37 | } // namespace internal 38 | 39 | } // namespace pinocchio 40 | 41 | #endif // ifndef __pinocchio_autodiff_casadi_spatial_se3_tpl_hpp__ 42 | -------------------------------------------------------------------------------- /include/pinocchio/autodiff/cppad/algorithm/aba.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019-2020 INRIA CNRS 3 | // 4 | 5 | #ifndef __pinocchio_autodiff_cppad_algorithm_aba_hpp__ 6 | #define __pinocchio_autodiff_cppad_algorithm_aba_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace internal 11 | { 12 | 13 | // Fwd def 14 | template 15 | struct SE3actOn; 16 | 17 | /// \brief Partial specialization for CppAD::AGtypes 18 | template 19 | struct SE3actOn> 20 | { 21 | typedef CppAD::AD<_Scalar> Scalar; 22 | 23 | template 24 | static typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) 25 | run(const SE3Tpl & M, const Eigen::MatrixBase & I) 26 | { 27 | typedef SE3Tpl SE3; 28 | 29 | typedef typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix6Type) ReturnType; 30 | 31 | typename SE3::ActionMatrixType dual_action_matrix(M.toDualActionMatrix()); 32 | typename SE3::ActionMatrixType action_matrix(M.toActionMatrixInverse()); 33 | ReturnType intermediate_result = dual_action_matrix * I; 34 | ReturnType res = intermediate_result * action_matrix; 35 | return res; 36 | } 37 | }; 38 | } // namespace internal 39 | } // namespace pinocchio 40 | 41 | #endif // __pinocchio_autodiff_cppad_algorithm_aba_hpp__ 42 | -------------------------------------------------------------------------------- /include/pinocchio/autodiff/cppad/math/eigen_plugin.hpp: -------------------------------------------------------------------------------- 1 | #if !EIGEN_VERSION_AT_LEAST(3, 3, 3) 2 | typedef Scalar value_type; 3 | #endif 4 | -------------------------------------------------------------------------------- /include/pinocchio/autodiff/cppad/spatial/se3-tpl.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_autodiff_cppad_spatial_se3_tpl_hpp__ 6 | #define __pinocchio_autodiff_cppad_spatial_se3_tpl_hpp__ 7 | 8 | #include "pinocchio/spatial/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace internal 13 | { 14 | template 15 | struct cast_call_normalize_method< 16 | SE3Tpl, Options>, 17 | NewScalar, 18 | CppAD::AD> 19 | { 20 | template 21 | static void run(T &) 22 | { 23 | // do nothing 24 | } 25 | }; 26 | 27 | template 28 | struct cast_call_normalize_method, CppAD::AD, Scalar> 29 | { 30 | template 31 | static void run(T &) 32 | { 33 | // do nothing 34 | } 35 | }; 36 | 37 | } // namespace internal 38 | 39 | } // namespace pinocchio 40 | 41 | #endif // ifndef __pinocchio_autodiff_cppad_spatial_se3_tpl_hpp__ 42 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/collision/collision.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | // 5 | #ifndef __pinocchio_python_collision_hpp__ 6 | #define __pinocchio_python_collision_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | 13 | void exposeBroadphaseCallbacks(); 14 | void exposeBroadphase(); 15 | 16 | } // namespace python 17 | } // namespace pinocchio 18 | 19 | #endif // ifndef __pinocchio_python_collision_hpp__ 20 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/context.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_context_hpp__ 6 | #define __pinocchio_python_context_hpp__ 7 | 8 | #define PINOCCHIO_PYTHON_SCALAR_TYPE_DEFAULT double 9 | 10 | #define PINOCCHIO_PYTHON_CONTEXT_FILE_DEFAULT "pinocchio/bindings/python/context/default.hpp" 11 | 12 | #ifndef PINOCCHIO_PYTHON_CONTEXT_FILE 13 | #define PINOCCHIO_PYTHON_CONTEXT_FILE PINOCCHIO_PYTHON_CONTEXT_FILE_DEFAULT 14 | #endif 15 | 16 | #include PINOCCHIO_PYTHON_CONTEXT_FILE 17 | 18 | #endif // #ifndef __pinocchio_python_context_hpp__ 19 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/context/cppad.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_context_cppad_hpp__ 6 | #define __pinocchio_python_context_cppad_hpp__ 7 | 8 | #include "pinocchio/autodiff/cppad.hpp" 9 | 10 | #define PINOCCHIO_PYTHON_SCALAR_TYPE ::CppAD::AD 11 | #include "pinocchio/bindings/python/context/generic.hpp" 12 | #undef PINOCCHIO_PYTHON_SCALAR_TYPE 13 | 14 | #define PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS 15 | #define PINOCCHIO_PYTHON_NO_SERIALIZATION 16 | #define PINOCCHIO_PYTHON_SKIP_REACHABLE_WORKSPACE 17 | 18 | #include 19 | #include 20 | 21 | namespace pinocchio 22 | { 23 | namespace python 24 | { 25 | inline void exposeSpecificTypeFeatures() 26 | { 27 | boost::python::import("pycppad"); 28 | }; 29 | 30 | inline boost::python::object getScalarType() 31 | { 32 | return eigenpy::getInstanceClass(); 33 | } 34 | 35 | } // namespace python 36 | } // namespace pinocchio 37 | 38 | #endif // #ifndef __pinocchio_python_context_cppad_hpp__ 39 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/context/cppadcg.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_context_cppadcg_hpp__ 6 | #define __pinocchio_python_context_cppadcg_hpp__ 7 | 8 | #include "pinocchio/codegen/cppadcg.hpp" 9 | 10 | #define PINOCCHIO_PYTHON_SCALAR_TYPE ::CppAD::AD> 11 | #include "pinocchio/bindings/python/context/generic.hpp" 12 | #undef PINOCCHIO_PYTHON_SCALAR_TYPE 13 | 14 | #define PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS 15 | #define PINOCCHIO_PYTHON_NO_SERIALIZATION 16 | #define PINOCCHIO_PYTHON_SKIP_REACHABLE_WORKSPACE 17 | 18 | #include 19 | #include 20 | 21 | namespace pinocchio 22 | { 23 | namespace python 24 | { 25 | inline void exposeSpecificTypeFeatures() 26 | { 27 | boost::python::import("pycppad"); 28 | }; 29 | 30 | inline boost::python::object getScalarType() 31 | { 32 | return eigenpy::getInstanceClass(); 33 | } 34 | 35 | } // namespace python 36 | } // namespace pinocchio 37 | 38 | #endif // #ifndef __pinocchio_python_context_cppadcg_hpp__ 39 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/context/default.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020-2023 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_context_default_hpp__ 6 | #define __pinocchio_python_context_default_hpp__ 7 | 8 | #define PINOCCHIO_PYTHON_SCALAR_TYPE PINOCCHIO_PYTHON_SCALAR_TYPE_DEFAULT 9 | #define PINOCCHIO_PYTHON_INTERFACE_MAIN_MODULE 10 | #define PINOCCHIO_PYTHON_PLAIN_SCALAR_TYPE 11 | 12 | #include "pinocchio/bindings/python/context/generic.hpp" 13 | #include 14 | 15 | namespace pinocchio 16 | { 17 | namespace python 18 | { 19 | 20 | inline void exposeSpecificTypeFeatures() {}; 21 | 22 | inline boost::python::object getScalarType() 23 | { 24 | namespace bp = boost::python; 25 | return bp::object(bp::handle<>(bp::borrowed(reinterpret_cast(&PyFloat_Type)))); 26 | } 27 | } // namespace python 28 | } // namespace pinocchio 29 | 30 | #undef PINOCCHIO_PYTHON_SCALAR_TYPE 31 | #endif // #ifndef __pinocchio_python_context_default_hpp__ 32 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/context/mpfr.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021-2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_context_mpfr_hpp__ 6 | #define __pinocchio_python_context_mpfr_hpp__ 7 | 8 | #include "pinocchio/math/multiprecision-mpfr.hpp" 9 | 10 | #define PINOCCHIO_PYTHON_SCALAR_TYPE \ 11 | ::boost::multiprecision::number< \ 12 | ::boost::multiprecision::mpfr_float_backend<0>, ::boost::multiprecision::et_off> 13 | #include "pinocchio/bindings/python/context/generic.hpp" 14 | 15 | #define PINOCCHIO_PYTHON_SKIP_COMPARISON_OPERATIONS 16 | #define PINOCCHIO_PYTHON_NO_SERIALIZATION 17 | #define PINOCCHIO_PYTHON_SKIP_REACHABLE_WORKSPACE 18 | 19 | #endif // #ifndef __pinocchio_python_context_mpfr_hpp__ 20 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/extra/extras.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_extra_extras_hpp__ 6 | #define __pinocchio_python_extra_extras_hpp__ 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | namespace bp = boost::python; 15 | 16 | #if defined(PINOCCHIO_WITH_EXTRA_SUPPORT) 17 | void exposeReachableWorkspace(); 18 | #endif // defined(PINOCCHIO_WITH_EXTRA_SUPPORT) 19 | 20 | void exposeExtras(); 21 | 22 | } // namespace python 23 | } // namespace pinocchio 24 | 25 | #endif // ifndef __pinocchio_python_extra_extras_hpp__ 26 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/parsers/mjcf.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_parsers_mjcf_hpp__ 6 | #define __pinocchio_python_parsers_mjcf_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | void exposeMJCFModel(); 13 | void exposeMJCFGeom(); 14 | 15 | inline void exposeMJCFParser() 16 | { 17 | exposeMJCFModel(); 18 | exposeMJCFGeom(); 19 | } 20 | } // namespace python 21 | } // namespace pinocchio 22 | 23 | #endif // ifndef __pinocchio_python_parsers_mjcf_hpp__ 24 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/parsers/python.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2020 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_parser_python_hpp__ 6 | #define __pinocchio_python_parser_python_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/bindings/python/parsers/python.hpp, pinocchio/parsers/python.hpp) 12 | // clang-format on 13 | 14 | #include "pinocchio/parsers/python.hpp" 15 | 16 | #endif // ifndef __pinocchio_python_parser_python_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/parsers/sdf.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_python_parsers_sdf_hpp__ 6 | #define __pinocchio_python_parsers_sdf_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | void exposeSDFModel(); 13 | void exposeSDFGeometry(); 14 | 15 | inline void exposeSDFParser() 16 | { 17 | exposeSDFModel(); 18 | exposeSDFGeometry(); 19 | } 20 | } // namespace python 21 | } // namespace pinocchio 22 | 23 | #endif // ifndef __pinocchio_python_parsers_sdf_hpp__ 24 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/parsers/srdf.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_parsers_srdf_hpp__ 6 | #define __pinocchio_python_parsers_srdf_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | void exposeSRDFParser(); 13 | } 14 | } // namespace pinocchio 15 | 16 | #endif // ifndef __pinocchio_python_parsers_srdf_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/parsers/urdf.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_parsers_urdf_hpp__ 6 | #define __pinocchio_python_parsers_urdf_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | void exposeConsoleBridge(); 13 | void exposeURDFModel(); 14 | void exposeURDFGeometry(); 15 | 16 | inline void exposeURDFParser() 17 | { 18 | exposeConsoleBridge(); 19 | exposeURDFModel(); 20 | exposeURDFGeometry(); 21 | } 22 | 23 | } // namespace python 24 | } // namespace pinocchio 25 | 26 | #endif // ifndef __pinocchio_python_parsers_urdf_hpp__ 27 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/pch.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2025 INRIA 3 | // 4 | #include "pinocchio/container/boost-container-limits.hpp" 5 | #include 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/spatial/classic-acceleration.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019-2020 INRIA 3 | // 4 | 5 | #include "pinocchio/bindings/python/fwd.hpp" 6 | #include "pinocchio/spatial/classic-acceleration.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | namespace python 11 | { 12 | namespace bp = boost::python; 13 | 14 | void exposeClassicAcceleration() 15 | { 16 | bp::def( 17 | "classicAcceleration", &classicAcceleration, 18 | bp::args("spatial_velocity", "spatial_acceleration"), 19 | "Computes the classic acceleration from a given spatial velocity and spatial " 20 | "acceleration."); 21 | 22 | bp::def( 23 | "classicAcceleration", 24 | &classicAcceleration, 25 | bp::args("spatial_velocity", "spatial_acceleration", "placement"), 26 | "Computes the classic acceleration of a frame B, given the spatial velocity and spatial " 27 | "acceleration of a frame A,\n" 28 | "and the relative placement A^M_B."); 29 | } 30 | 31 | } // namespace python 32 | } // namespace pinocchio 33 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/address.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_address_hpp__ 6 | #define __pinocchio_python_utils_address_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | namespace bp = boost::python; 16 | 17 | /// 18 | /// \brief Add the Python method to extract the address of the underlying C++ object. 19 | /// 20 | template 21 | struct AddressVisitor : public bp::def_visitor> 22 | { 23 | 24 | template 25 | void visit(PyClass & cl) const 26 | { 27 | cl.def( 28 | "__address__", 29 | +[](const C & self) -> size_t { 30 | return reinterpret_cast(static_cast(&self)); 31 | }, 32 | bp::arg("self"), "Returns the address of the underlying C++ object."); 33 | } 34 | }; 35 | 36 | } // namespace python 37 | } // namespace pinocchio 38 | 39 | #endif // ifndef __pinocchio_python_utils_address_hpp__ 40 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/comparable.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_comparable_hpp__ 6 | #define __pinocchio_python_utils_comparable_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | namespace bp = boost::python; 16 | 17 | /// 18 | /// \brief Add the Python method == and != to allow a comparison of this. 19 | /// 20 | template 21 | struct ComparableVisitor 22 | : public bp::def_visitor> 23 | { 24 | template 25 | void visit(PyClass & cl) const 26 | { 27 | cl.def(bp::self == bp::self).def(bp::self != bp::self); 28 | } 29 | }; 30 | 31 | template 32 | struct ComparableVisitor : public bp::def_visitor> 33 | { 34 | template 35 | void visit(PyClass &) const 36 | { 37 | } 38 | }; 39 | } // namespace python 40 | } // namespace pinocchio 41 | 42 | #endif // ifndef __pinocchio_python_utils_comparable_hpp__ 43 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/constant.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_constant_hpp__ 6 | #define __pinocchio_python_utils_constant_hpp__ 7 | 8 | #include 9 | 10 | namespace boost 11 | { 12 | namespace python 13 | { 14 | 15 | /// 16 | /// \brief Define a constant given its value and a name within the current Boost Python scope. 17 | /// 18 | /// \tparam T Type of the constant. 19 | /// 20 | /// \param[in] name Name of the constant. 21 | /// \param[in] value Value of the constant. 22 | /// 23 | template 24 | void def_constant(const char * name, const T & value) 25 | { 26 | namespace bp = boost::python; 27 | bp::scope().attr(name) = value; 28 | } 29 | 30 | } // namespace python 31 | } // namespace boost 32 | 33 | #endif // ifndef __pinocchio_python_utils_constant_hpp__ 34 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/conversions.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 CNRS 3 | // 4 | 5 | namespace pinocchio 6 | { 7 | namespace python 8 | { 9 | 10 | void exposeConversions(); 11 | 12 | } // namespace python 13 | } // namespace pinocchio 14 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/copyable.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2023 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_copyable_hpp__ 6 | #define __pinocchio_python_utils_copyable_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | namespace bp = boost::python; 16 | 17 | /// 18 | /// \brief Add the Python method copy to allow a copy of this by calling the copy constructor. 19 | /// 20 | template 21 | struct CopyableVisitor : public bp::def_visitor> 22 | { 23 | template 24 | void visit(PyClass & cl) const 25 | { 26 | cl.def("copy", ©, bp::arg("self"), "Returns a copy of *this."); 27 | cl.def("__copy__", ©, bp::arg("self"), "Returns a copy of *this."); 28 | cl.def( 29 | "__deepcopy__", &deepcopy, bp::args("self", "memo"), "Returns a deep copy of *this."); 30 | } 31 | 32 | private: 33 | static C copy(const C & self) 34 | { 35 | return C(self); 36 | } 37 | static C deepcopy(const C & self, bp::dict) 38 | { 39 | return C(self); 40 | } 41 | }; 42 | } // namespace python 43 | } // namespace pinocchio 44 | 45 | #endif // ifndef __pinocchio_python_utils_copyable_hpp__ 46 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/dependencies.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_dependencies_hpp__ 6 | #define __pinocchio_python_utils_dependencies_hpp__ 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | void exposeDependencies(); 16 | 17 | } // namespace python 18 | } // namespace pinocchio 19 | 20 | #endif // ifndef __pinocchio_python_utils_dependencies_hpp__ 21 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/path.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_path_hpp__ 6 | #define __pinocchio_python_utils_path_hpp__ 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | namespace bp = boost::python; 16 | 17 | /// 18 | /// \brief python pathlib.Path | str -> C++ std::string 19 | /// 20 | std::string path(const bp::object & path); 21 | 22 | /// 23 | /// \brief python typing.List[pathlib.Path] | typing.List[str] -> C++ std::vector 24 | /// 25 | std::vector pathList(const bp::object & path_list); 26 | } // namespace python 27 | } // namespace pinocchio 28 | 29 | #endif // ifndef __pinocchio_python_utils_path_hpp__ 30 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/printable.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_printable_hpp__ 6 | #define __pinocchio_python_utils_printable_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | namespace bp = boost::python; 16 | 17 | /// 18 | /// \brief Set the Python method __str__ and __repr__ to use the overloading operator<<. 19 | /// 20 | template 21 | struct PrintableVisitor : public bp::def_visitor> 22 | { 23 | template 24 | void visit(PyClass & cl) const 25 | { 26 | cl.def(bp::self_ns::str(bp::self_ns::self)).def(bp::self_ns::repr(bp::self_ns::self)); 27 | } 28 | }; 29 | } // namespace python 30 | } // namespace pinocchio 31 | 32 | #endif // ifndef __pinocchio_python_utils_printable_hpp__ 33 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/registration.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019-2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_registration_hpp__ 6 | #define __pinocchio_python_utils_registration_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | template 16 | inline bool register_symbolic_link_to_registered_type() 17 | { 18 | namespace bp = boost::python; 19 | if (eigenpy::check_registration()) 20 | { 21 | const bp::type_info info = bp::type_id(); 22 | const bp::converter::registration * reg = bp::converter::registry::query(info); 23 | bp::handle<> class_obj(bp::borrowed(reg->get_class_object())); 24 | bp::scope().attr(reg->get_class_object()->tp_name) = bp::object(class_obj); 25 | return true; 26 | } 27 | 28 | return false; 29 | } 30 | 31 | } // namespace python 32 | } // namespace pinocchio 33 | 34 | #endif // ifndef __pinocchio_python_utils_registration_hpp__ 35 | -------------------------------------------------------------------------------- /include/pinocchio/bindings/python/utils/version.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_python_utils_version_hpp__ 6 | #define __pinocchio_python_utils_version_hpp__ 7 | 8 | #include "pinocchio/bindings/python/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | namespace python 13 | { 14 | 15 | void exposeVersion(); 16 | 17 | } // namespace python 18 | } // namespace pinocchio 19 | 20 | #endif // ifndef __pinocchio_python_utils_version_hpp__ 21 | -------------------------------------------------------------------------------- /include/pinocchio/collision/broadphase-callbacks.hxx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/include/pinocchio/collision/broadphase-callbacks.hxx -------------------------------------------------------------------------------- /include/pinocchio/collision/broadphase.hxx: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/stack-of-tasks/pinocchio/ce2be464b61b3253b8079055d10358ede4d4dbf9/include/pinocchio/collision/broadphase.hxx -------------------------------------------------------------------------------- /include/pinocchio/collision/collision.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_collision_collision_txx__ 6 | #define __pinocchio_collision_collision_txx__ 7 | 8 | namespace pinocchio 9 | { 10 | 11 | extern template PINOCCHIO_COLLISION_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI bool 12 | computeCollisions< 13 | context::Scalar, 14 | context::Options, 15 | JointCollectionDefaultTpl, 16 | context::VectorXs>( 17 | const context::Model &, 18 | context::Data &, 19 | const GeometryModel &, 20 | GeometryData &, 21 | const Eigen::MatrixBase &, 22 | const bool stopAtFirstCollision); 23 | 24 | extern template PINOCCHIO_COLLISION_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 25 | computeBodyRadius( 26 | const context::Model &, const GeometryModel &, GeometryData &); 27 | 28 | } // namespace pinocchio 29 | 30 | #endif // ifndef __pinocchio_collision_collision_txx__ 31 | -------------------------------------------------------------------------------- /include/pinocchio/collision/distance.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_collision_distance_txx__ 6 | #define __pinocchio_collision_distance_txx__ 7 | 8 | namespace pinocchio 9 | { 10 | 11 | extern template PINOCCHIO_COLLISION_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI std::size_t 12 | computeDistances( 13 | const context::Model &, 14 | context::Data &, 15 | const GeometryModel &, 16 | GeometryData &, 17 | const Eigen::MatrixBase &); 18 | 19 | } // namespace pinocchio 20 | 21 | #endif // ifndef __pinocchio_collision_distance_txx__ 22 | -------------------------------------------------------------------------------- /include/pinocchio/collision/fcl-pinocchio-conversions.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2024 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_collision_fcl_convertion_hpp__ 6 | #define __pinocchio_collision_fcl_convertion_hpp__ 7 | 8 | #include 9 | #include "pinocchio/spatial/se3.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | template 14 | inline hpp::fcl::Transform3f toFclTransform3f(const SE3Tpl & m) 15 | { 16 | SE3Tpl m_ = m.template cast(); 17 | return hpp::fcl::Transform3f(m_.rotation(), m_.translation()); 18 | } 19 | 20 | inline SE3 toPinocchioSE3(const hpp::fcl::Transform3f & tf) 21 | { 22 | typedef SE3::Scalar Scalar; 23 | return SE3(tf.getRotation().cast(), tf.getTranslation().cast()); 24 | } 25 | 26 | } // namespace pinocchio 27 | 28 | #endif // ifndef __pinocchio_collision_fcl_convertion_hpp__ 29 | -------------------------------------------------------------------------------- /include/pinocchio/container/aligned-vector.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2024 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_container_aligned_vector_hpp__ 6 | #define __pinocchio_container_aligned_vector_hpp__ 7 | 8 | #include 9 | #include 10 | 11 | #define PINOCCHIO_ALIGNED_STD_VECTOR(Type) ::pinocchio::container::aligned_vector 12 | 13 | #define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T) ::pinocchio::container::aligned_vector 14 | 15 | namespace pinocchio 16 | { 17 | namespace container 18 | { 19 | 20 | template 21 | using aligned_vector = std::vector>; 22 | 23 | } // namespace container 24 | 25 | } // namespace pinocchio 26 | 27 | #endif // ifndef __pinocchio_container_aligned_vector_hpp__ 28 | -------------------------------------------------------------------------------- /include/pinocchio/context.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021-2025 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_context_hpp__ 6 | #define __pinocchio_context_hpp__ 7 | 8 | #define PINOCCHIO_SCALAR_TYPE_DEFAULT double 9 | #define PINOCCHIO_OPTIONS_DEFAULT 0 10 | 11 | #define PINOCCHIO_CONTEXT_FILE_DEFAULT "pinocchio/context/default.hpp" 12 | 13 | #ifndef PINOCCHIO_CONTEXT_FILE 14 | #define PINOCCHIO_CONTEXT_FILE PINOCCHIO_CONTEXT_FILE_DEFAULT 15 | #endif 16 | 17 | #include PINOCCHIO_CONTEXT_FILE 18 | 19 | #endif // #ifndef __pinocchio_context_hpp__ 20 | -------------------------------------------------------------------------------- /include/pinocchio/context/default.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_context_default_hpp__ 6 | #define __pinocchio_context_default_hpp__ 7 | 8 | #define PINOCCHIO_SCALAR_TYPE PINOCCHIO_SCALAR_TYPE_DEFAULT 9 | 10 | #include "pinocchio/context/generic.hpp" 11 | 12 | #undef PINOCCHIO_SCALAR_TYPE 13 | #endif // #ifndef __pinocchio_context_default_hpp__ 14 | -------------------------------------------------------------------------------- /include/pinocchio/core/binary-op.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_core_binary_op_hpp__ 6 | #define __pinocchio_core_binary_op_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | /// 11 | ///  \brief Forward declaration of the multiplication operation return type. 12 | /// Should be overloaded, otherwise it will procude a compilation error. 13 | /// 14 | template 15 | struct MultiplicationOp; 16 | 17 | namespace impl 18 | { 19 | template 20 | struct LhsMultiplicationOp; 21 | } 22 | } // namespace pinocchio 23 | 24 | #endif // ifndef __pinocchio_core_binary_op_hpp__ 25 | -------------------------------------------------------------------------------- /include/pinocchio/core/unary-op.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_core_unary_op_hpp__ 6 | #define __pinocchio_core_unary_op_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | 11 | } 12 | 13 | #endif // ifndef __pinocchio_core_unary_op_hpp__ 14 | -------------------------------------------------------------------------------- /include/pinocchio/deprecated-macros.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_deprecated_macros_hpp__ 6 | #define __pinocchio_deprecated_macros_hpp__ 7 | 8 | #ifdef PINOCCHIO_WITH_HPP_FCL 9 | #ifdef PINOCCHIO_ENABLE_COMPATIBILITY_WITH_VERSION_1 // for backward compatibility 10 | #define WITH_HPP_FCL 11 | #endif 12 | #endif 13 | 14 | #ifdef PINOCCHIO_WITH_URDFDOM 15 | #ifdef PINOCCHIO_ENABLE_COMPATIBILITY_WITH_VERSION_1 // for backward compatibility 16 | #define WITH_URDFDOM 17 | #endif 18 | #endif 19 | 20 | #endif // ifndef __pinocchio_deprecated_macros_hpp__ 21 | -------------------------------------------------------------------------------- /include/pinocchio/deprecated-namespaces.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_deprecated_namespaces_hpp__ 6 | #define __pinocchio_deprecated_namespaces_hpp__ 7 | 8 | #if PINOCCHIO_ENABLE_COMPATIBILITY_WITH_VERSION_1 // do not warn 9 | namespace se3 = ::pinocchio; 10 | #endif 11 | 12 | #endif // ifndef __pinocchio_deprecated_namespaces_hpp__ 13 | -------------------------------------------------------------------------------- /include/pinocchio/deprecation.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_deprecation_hpp__ 6 | #define __pinocchio_deprecation_hpp__ 7 | 8 | #include "pinocchio/deprecated.hpp" 9 | #include "pinocchio/deprecated-macros.hpp" 10 | #include "pinocchio/deprecated-namespaces.hpp" 11 | 12 | #endif // ifndef __pinocchio_deprecation_hpp__ 13 | -------------------------------------------------------------------------------- /include/pinocchio/math/casadi.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_math_casadi_hpp__ 6 | #define __pinocchio_math_casadi_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/math/casadi.hpp, pinocchio/autodiff/casadi.hpp) 12 | // clang-format on 13 | 14 | #include "pinocchio/autodiff/casadi.hpp" 15 | 16 | #endif // #ifndef __pinocchio_math_casadi_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/math/cppad.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_math_ccpad_hpp__ 6 | #define __pinocchio_math_ccpad_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/math/cppad.hpp, pinocchio/autodiff/cppad.hpp) 12 | // clang-format on 13 | 14 | #include "pinocchio/autodiff/cppad.hpp" 15 | 16 | #endif // #ifndef __pinocchio_math_ccpad_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/math/cppadcg.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_math_ccpadcg_hpp__ 6 | #define __pinocchio_math_ccpadcg_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/math/cppadcg.hpp, pinocchio/codegen/cppadcg.hpp) 12 | // clang-format on 13 | 14 | #include "pinocchio/codegen/cppadcg.hpp" 15 | 16 | #endif // #ifndef __pinocchio_math_ccpadcg_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/math/sign.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_math_sign_hpp__ 6 | #define __pinocchio_math_sign_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | /// 11 | /// \brief Returns the robust sign of t 12 | /// 13 | template 14 | Scalar sign(const Scalar & t) 15 | { 16 | return (t > Scalar(0)) - (t < Scalar(0)); 17 | } 18 | } // namespace pinocchio 19 | 20 | #endif // #ifndef __pinocchio_math_sign_hpp__ 21 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/data.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_data_txx__ 6 | #define __pinocchio_multibody_data_txx__ 7 | 8 | #include "pinocchio/multibody/data.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI 14 | DataTpl::DataTpl(); 15 | 16 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI 17 | DataTpl::DataTpl( 18 | const context::Model &); 19 | 20 | } // namespace pinocchio 21 | 22 | #endif // ifndef __pinocchio_multibody_data_txx__ 23 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/geometry-object-filter.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_geometry_object_filter_hpp__ 6 | #define __pinocchio_multibody_geometry_object_filter_hpp__ 7 | 8 | #include "pinocchio/multibody/instance-filter.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | struct GeometryObjectFilterBase : InstanceFilterBase 14 | { 15 | 16 | }; // struct GeometryObjectFilterBase 17 | 18 | struct GeometryObjectFilterNothing final : GeometryObjectFilterBase 19 | { 20 | bool operator()(const GeometryObject &) const 21 | { 22 | return true; 23 | } 24 | 25 | }; // struct GeometryObjectFilterNothing 26 | 27 | struct GeometryObjectFilterSelectByJoint final : GeometryObjectFilterBase 28 | { 29 | GeometryObjectFilterSelectByJoint(const size_t joint_id) 30 | : joint_id(joint_id) 31 | { 32 | } 33 | 34 | bool operator()(const GeometryObject & geometry_object) const 35 | { 36 | return geometry_object.parentJoint == joint_id; 37 | } 38 | 39 | const size_t joint_id; 40 | 41 | }; // struct GeometryObjectFilterSelectByJoint 42 | 43 | } // namespace pinocchio 44 | 45 | #endif // #ifndef __pinocchio_multibody_geometry_object_filter_hpp__ 46 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/geometry-object.hxx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2022 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_geometry_object_hxx__ 6 | #define __pinocchio_multibody_geometry_object_hxx__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | 13 | inline std::ostream & operator<<(std::ostream & os, const GeometryObject & geom_object) 14 | { 15 | os << "Name: \t \n" 16 | << geom_object.name << "\n" 17 | << "Parent frame ID: \t \n" 18 | << geom_object.parentFrame << "\n" 19 | << "Parent joint ID: \t \n" 20 | << geom_object.parentJoint << "\n" 21 | << "Position in parent frame: \t \n" 22 | << geom_object.placement << "\n" 23 | << "Absolute path to mesh file: \t \n" 24 | << geom_object.meshPath << "\n" 25 | << "Scale for transformation of the mesh: \t \n" 26 | << geom_object.meshScale.transpose() << "\n" 27 | << "Disable collision: \t \n" 28 | << geom_object.disableCollision << "\n" 29 | << std::endl; 30 | return os; 31 | } 32 | 33 | } // namespace pinocchio 34 | 35 | #endif // ifndef __pinocchio_multibody_geometry_object_hxx__ 36 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/joint-motion-subspace.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_joint_motion_subspace_hpp__ 6 | #define __pinocchio_multibody_joint_motion_subspace_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | template 14 | struct JointMotionSubspaceTpl; 15 | 16 | typedef JointMotionSubspaceTpl<1, context::Scalar, context::Options> JointMotionSubspace1d; 17 | typedef JointMotionSubspaceTpl<3, context::Scalar, context::Options> JointMotionSubspace3d; 18 | typedef JointMotionSubspaceTpl<6, context::Scalar, context::Options> JointMotionSubspace6d; 19 | typedef JointMotionSubspaceTpl 20 | JointMotionSubspaceXd; 21 | 22 | } // namespace pinocchio 23 | 24 | #include "pinocchio/multibody/joint-motion-subspace-base.hpp" 25 | #include "pinocchio/multibody/joint-motion-subspace-generic.hpp" 26 | 27 | #endif // ifndef __pinocchio_multibody_joint_motion_subspace_hpp__ 28 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/joint/joint-base.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2019 CNRS INRIA 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. 4 | // 5 | 6 | #ifndef __pinocchio_multibody_joint_base_hpp__ 7 | #define __pinocchio_multibody_joint_base_hpp__ 8 | 9 | #include "pinocchio/multibody/joint/fwd.hpp" 10 | 11 | #include "pinocchio/multibody/joint/joint-model-base.hpp" 12 | #include "pinocchio/multibody/joint/joint-data-base.hpp" 13 | 14 | #endif // ifndef __pinocchio_multibody_joint_base_hpp__ 15 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/joint/joints.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_joint_joints_hpp__ 6 | #define __pinocchio_multibody_joint_joints_hpp__ 7 | 8 | #include "pinocchio/multibody/joint/joint-free-flyer.hpp" 9 | #include "pinocchio/multibody/joint/joint-planar.hpp" 10 | #include "pinocchio/multibody/joint/joint-prismatic.hpp" 11 | #include "pinocchio/multibody/joint/joint-prismatic-unaligned.hpp" 12 | #include "pinocchio/multibody/joint/joint-revolute.hpp" 13 | #include "pinocchio/multibody/joint/joint-revolute-unbounded.hpp" 14 | #include "pinocchio/multibody/joint/joint-revolute-unaligned.hpp" 15 | #include "pinocchio/multibody/joint/joint-revolute-unbounded-unaligned.hpp" 16 | #include "pinocchio/multibody/joint/joint-spherical-ZYX.hpp" 17 | #include "pinocchio/multibody/joint/joint-spherical.hpp" 18 | #include "pinocchio/multibody/joint/joint-translation.hpp" 19 | #include "pinocchio/multibody/joint/joint-mimic.hpp" 20 | #include "pinocchio/multibody/joint/joint-helical.hpp" 21 | #include "pinocchio/multibody/joint/joint-helical-unaligned.hpp" 22 | #include "pinocchio/multibody/joint/joint-universal.hpp" 23 | 24 | #endif // ifndef __pinocchio_multibody_joint_joints_hpp__ 25 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/liegroup/fwd.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_lie_group_fwd_hpp__ 6 | #define __pinocchio_lie_group_fwd_hpp__ 7 | 8 | #include "pinocchio/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | template 13 | struct LieGroupGenericTpl; 14 | } 15 | 16 | #endif // ifndef __pinocchio_lie_group_fwd_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/liegroup/liegroup-algo.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018-2020 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_liegroup_liegroup_algo_hpp__ 6 | #define __pinocchio_multibody_liegroup_liegroup_algo_hpp__ 7 | 8 | #include "pinocchio/multibody/joint/joint-base.hpp" 9 | #include "pinocchio/multibody/liegroup/liegroup.hpp" 10 | 11 | #include "pinocchio/multibody/liegroup/liegroup-algo.hxx" 12 | 13 | #endif // ifndef __pinocchio_multibody_liegroup_liegroup_algo_hpp__ 14 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/pool/fwd.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021-2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_pool_fwd_hpp__ 6 | #define __pinocchio_multibody_pool_fwd_hpp__ 7 | 8 | #include "pinocchio/multibody/joint/fwd.hpp" 9 | 10 | namespace pinocchio 11 | { 12 | 13 | template< 14 | typename Scalar, 15 | int Options = 0, 16 | template class JointCollectionTpl = JointCollectionDefaultTpl> 17 | class ModelPoolTpl; 18 | typedef ModelPoolTpl ModelPool; 19 | 20 | template< 21 | typename Scalar, 22 | int Options = 0, 23 | template class JointCollectionTpl = JointCollectionDefaultTpl> 24 | class GeometryPoolTpl; 25 | typedef GeometryPoolTpl GeometryPool; 26 | 27 | } // namespace pinocchio 28 | 29 | #endif // ifndef __pinocchio_multibody_pool_fwd_hpp__ 30 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/sample-models.txx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_parsers_sample_models_txx__ 6 | #define __pinocchio_multibody_parsers_sample_models_txx__ 7 | 8 | #ifndef PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS 9 | 10 | namespace pinocchio 11 | { 12 | namespace buildModels 13 | { 14 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 15 | manipulator( 16 | context::Model &, bool); 17 | 18 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 19 | humanoid(context::Model &, bool); 20 | 21 | extern template PINOCCHIO_EXPLICIT_INSTANTIATION_DECLARATION_DLLAPI void 22 | humanoidRandom( 23 | context::Model &, bool, bool); 24 | } // namespace buildModels 25 | } // namespace pinocchio 26 | 27 | #endif // PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS 28 | 29 | #endif // ifndef __pinocchio_multibody_parsers_sample_models_txx__ 30 | -------------------------------------------------------------------------------- /include/pinocchio/multibody/visitor.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2019 CNRS INRIA 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. 4 | // 5 | 6 | #ifndef __pinocchio_multibody_visitor_hpp__ 7 | #define __pinocchio_multibody_visitor_hpp__ 8 | 9 | #include "pinocchio/multibody/visitor/joint-unary-visitor.hpp" 10 | #include "pinocchio/multibody/visitor/joint-binary-visitor.hpp" 11 | 12 | #endif // ifndef __pinocchio_multibody_visitor_hpp__ 13 | -------------------------------------------------------------------------------- /include/pinocchio/parsers/meshloader-fwd.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_parsers_meshloader_fwd_hpp__ 6 | #define __pinocchio_parsers_meshloader_fwd_hpp__ 7 | 8 | #include 9 | 10 | #ifdef PINOCCHIO_WITH_HPP_FCL 11 | #include 12 | #endif // PINOCCHIO_WITH_HPP_FCL 13 | 14 | #ifdef COAL_VERSION 15 | namespace coal 16 | { 17 | class MeshLoader; 18 | typedef std::shared_ptr MeshLoaderPtr; 19 | } // namespace coal 20 | #else 21 | namespace hpp 22 | { 23 | namespace fcl 24 | { 25 | class MeshLoader; 26 | typedef std::shared_ptr MeshLoaderPtr; 27 | } // namespace fcl 28 | } // namespace hpp 29 | #endif // COAL_VERSION 30 | 31 | #endif // __pinocchio_parsers_meshloader_fwd_hpp__ 32 | -------------------------------------------------------------------------------- /include/pinocchio/parsers/mjcf/geometry.hxx: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2024 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_parsers_mjcf_geometry_hxx__ 6 | #define __pinocchio_parsers_mjcf_geometry_hxx__ 7 | 8 | #include "pinocchio/parsers/mjcf.hpp" 9 | #include "pinocchio/parsers/mjcf/mjcf-graph.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | namespace mjcf 14 | { 15 | template class JointCollectionTpl> 16 | GeometryModel & buildGeom( 17 | ModelTpl & model, 18 | const std::string & filename, 19 | const GeometryType type, 20 | GeometryModel & geomModel, 21 | ::hpp::fcl::MeshLoaderPtr meshLoader) 22 | { 23 | ::pinocchio::urdf::details::UrdfVisitor visitor(model); 24 | 25 | typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph; 26 | 27 | MjcfGraph graph(visitor, filename); 28 | 29 | graph.parseGraphFromXML(filename); 30 | 31 | // Use the Mjcf graph to create the geometry model 32 | graph.parseGeomTree(type, geomModel, meshLoader); 33 | 34 | return geomModel; 35 | } 36 | } // namespace mjcf 37 | } // namespace pinocchio 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /include/pinocchio/parsers/python.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2023 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_parser_python_hpp__ 6 | #define __pinocchio_parser_python_hpp__ 7 | 8 | #include "pinocchio/python_parser/config.hpp" 9 | #include "pinocchio/multibody/model.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | namespace python 14 | { 15 | /// \brief Load a model from a Python script. 16 | /// 17 | /// This function raises a Python error in case of inconsistency in the Python code. 18 | /// 19 | /// \input filename The full path to the model file. 20 | /// \input var_name Name of the Python variable which contains the model in the script. 21 | /// 22 | /// \returns The model constructed by the Python script. 23 | /// 24 | // TODO: look inside the context of Python and find an occurence of object Model 25 | PINOCCHIO_PYTHON_PARSER_DLLAPI 26 | Model buildModel(const std::string & filename, const std::string & var_name = "model"); 27 | 28 | } // namespace python 29 | 30 | } // namespace pinocchio 31 | 32 | #endif // ifndef __pinocchio_parser_python_hpp__ 33 | -------------------------------------------------------------------------------- /include/pinocchio/parsers/sample-models.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_parsers_sample_models_hpp__ 6 | #define __pinocchio_parsers_sample_models_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER(pinocchio/parsers/sample-models.hpp, pinocchio/multibody/sample-models.hpp) 12 | // clang-format on 13 | 14 | #include "pinocchio/multibody/sample-models.hpp" 15 | 16 | #endif // __pinocchio_parsers_sample_models_hpp__ 17 | -------------------------------------------------------------------------------- /include/pinocchio/parsers/urdf/utils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_parsers_urdf_utils_hpp__ 6 | #define __pinocchio_parsers_urdf_utils_hpp__ 7 | 8 | #include "pinocchio/spatial/fwd.hpp" 9 | #include "pinocchio/spatial/se3.hpp" 10 | #include 11 | 12 | namespace pinocchio 13 | { 14 | namespace urdf 15 | { 16 | namespace details 17 | { 18 | /// 19 | /// \brief Convert URDF Pose quantity to SE3. 20 | /// 21 | /// \param[in] M The input URDF Pose. 22 | /// 23 | /// \return The converted pose/transform pinocchio::SE3. 24 | /// 25 | SE3 convertFromUrdf(const ::urdf::Pose & M); 26 | } // namespace details 27 | } // namespace urdf 28 | } // namespace pinocchio 29 | 30 | #endif // __pinocchio_parsers_urdf_utils_hpp__ 31 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/csv.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_serialization_csv_hpp__ 6 | #define __pinocchio_serialization_csv_hpp__ 7 | 8 | #include "pinocchio/serialization/fwd.hpp" 9 | 10 | #include 11 | #include 12 | 13 | namespace pinocchio 14 | { 15 | 16 | template 17 | void toCSVfile(const std::string & filename, const Eigen::MatrixBase & matrix) 18 | { 19 | const Eigen::IOFormat CSVFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n"); 20 | std::ofstream file(filename.c_str()); 21 | file << matrix.format(CSVFormat); 22 | } 23 | } // namespace pinocchio 24 | 25 | #endif // ifndef __pinocchio_serialization_csv_hpp__ 26 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/fcl.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2024 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_multibody_fcl_serialization_hpp__ 6 | #define __pinocchio_multibody_fcl_serialization_hpp__ 7 | 8 | #include "pinocchio/multibody/fcl.hpp" 9 | 10 | namespace boost 11 | { 12 | namespace serialization 13 | { 14 | 15 | #ifndef PINOCCHIO_WITH_HPP_FCL 16 | 17 | template 18 | void serialize( 19 | Archive & /*ar*/, 20 | pinocchio::fcl::FakeCollisionGeometry & /*fake_collision_geometry*/, 21 | const unsigned int /*version*/) 22 | { 23 | } 24 | 25 | #endif // PINOCCHIO_WITH_HPP_FCL 26 | 27 | } // namespace serialization 28 | } // namespace boost 29 | 30 | #endif // __pinocchio_multibody_fcl_serialization_hpp__ 31 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/frame.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019-2021 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_serialization_frame_hpp__ 6 | #define __pinocchio_serialization_frame_hpp__ 7 | 8 | #include "pinocchio/multibody/frame.hpp" 9 | 10 | #include "pinocchio/serialization/fwd.hpp" 11 | #include "pinocchio/serialization/se3.hpp" 12 | #include "pinocchio/serialization/inertia.hpp" 13 | 14 | namespace boost 15 | { 16 | namespace serialization 17 | { 18 | 19 | template 20 | void 21 | serialize(Archive & ar, pinocchio::FrameTpl & f, const unsigned int version) 22 | { 23 | ar & make_nvp("name", f.name); 24 | ar & make_nvp("parent", f.parentJoint); 25 | ar & make_nvp("parentFrame", f.parentFrame); 26 | ar & make_nvp("placement", f.placement); 27 | ar & make_nvp("type", f.type); 28 | 29 | if (version > 0) 30 | ar & make_nvp("inertia", f.inertia); 31 | } 32 | 33 | template 34 | struct version> 35 | { 36 | enum 37 | { 38 | value = 1 39 | }; 40 | }; 41 | 42 | } // namespace serialization 43 | } // namespace boost 44 | 45 | #endif // ifndef __pinocchio_serialization_frame_hpp__ 46 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/fwd.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2017-2019 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_serialization_fwd_hpp__ 6 | #define __pinocchio_serialization_fwd_hpp__ 7 | 8 | #include "pinocchio/fwd.hpp" 9 | #include 10 | 11 | #include "pinocchio/serialization/eigen.hpp" 12 | 13 | #define BOOST_SERIALIZATION_MAKE_NVP(member) boost::serialization::make_nvp(##member, member) 14 | 15 | namespace pinocchio 16 | { 17 | template 18 | struct Serialize 19 | { 20 | template 21 | static void run(Archive & ar, T & object); 22 | }; 23 | } // namespace pinocchio 24 | 25 | #endif // ifndef __pinocchio_serialization_fwd_hpp__ 26 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/joints.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019-2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_serialization_joints_hpp__ 6 | #define __pinocchio_serialization_joints_hpp__ 7 | 8 | #include "pinocchio/multibody/joint/joints.hpp" 9 | #include "pinocchio/multibody/joint/joint-generic.hpp" 10 | #include "pinocchio/multibody/joint/joint-composite.hpp" 11 | #include "pinocchio/multibody/joint/joint-collection.hpp" 12 | 13 | #include "pinocchio/serialization/fwd.hpp" 14 | #include "pinocchio/serialization/eigen.hpp" 15 | #include "pinocchio/serialization/vector.hpp" 16 | #include "pinocchio/serialization/aligned-vector.hpp" 17 | 18 | #include 19 | #include 20 | 21 | #include "pinocchio/serialization/joints-transform.hpp" 22 | #include "pinocchio/serialization/joints-motion.hpp" 23 | #include "pinocchio/serialization/joints-motion-subspace.hpp" 24 | #include "pinocchio/serialization/joints-model.hpp" 25 | #include "pinocchio/serialization/joints-data.hpp" 26 | 27 | #endif // ifndef __pinocchio_serialization_joints_hpp__ 28 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/spatial.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_serialization_spatial_hpp__ 6 | #define __pinocchio_serialization_spatial_hpp__ 7 | 8 | #include "pinocchio/serialization/se3.hpp" 9 | #include "pinocchio/serialization/motion.hpp" 10 | #include "pinocchio/serialization/force.hpp" 11 | #include "pinocchio/serialization/symmetric3.hpp" 12 | #include "pinocchio/serialization/inertia.hpp" 13 | 14 | #endif // ifndef __pinocchio_serialization_spatial_hpp__ 15 | -------------------------------------------------------------------------------- /include/pinocchio/serialization/symmetric3.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_serialization_symmetric3_hpp__ 6 | #define __pinocchio_serialization_symmetric3_hpp__ 7 | 8 | #include "pinocchio/spatial/symmetric3.hpp" 9 | #include "pinocchio/serialization/fwd.hpp" 10 | 11 | #include 12 | #include 13 | 14 | namespace boost 15 | { 16 | namespace serialization 17 | { 18 | 19 | template 20 | void save( 21 | Archive & ar, 22 | const pinocchio::Symmetric3Tpl & S, 23 | const unsigned int /*version*/) 24 | { 25 | ar & make_nvp("data", make_array(S.data().data(), 6)); 26 | } 27 | 28 | template 29 | void load( 30 | Archive & ar, pinocchio::Symmetric3Tpl & S, const unsigned int /*version*/) 31 | { 32 | ar >> make_nvp("data", make_array(S.data().data(), 6)); 33 | } 34 | 35 | template 36 | void serialize( 37 | Archive & ar, pinocchio::Symmetric3Tpl & S, const unsigned int version) 38 | { 39 | split_free(ar, S, version); 40 | } 41 | 42 | } // namespace serialization 43 | } // namespace boost 44 | 45 | #endif // ifndef __pinocchio_serialization_symmetric3_hpp__ 46 | -------------------------------------------------------------------------------- /include/pinocchio/spatial/fcl-pinocchio-conversions.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2024 CNRS INRIA 3 | // 4 | // 5 | #ifndef __pinocchio_spatial_fcl_convertion_hpp__ 6 | #define __pinocchio_spatial_fcl_convertion_hpp__ 7 | 8 | #include "pinocchio/macros.hpp" 9 | 10 | // clang-format off 11 | PINOCCHIO_PRAGMA_DEPRECATED_HEADER( 12 | pinocchio/spatial/fcl-pinocchio-conversions.hpp, 13 | pinocchio/collision/fcl-pinocchio-conversions.hpp) 14 | // clang-format on 15 | 16 | #include "pinocchio/collision/fcl-pinocchio-conversions.hpp" 17 | 18 | #endif // ifndef __pinocchio_spatial_fcl_convertion_hpp__ 19 | -------------------------------------------------------------------------------- /include/pinocchio/spatial/log.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2021 CNRS INRIA 3 | 4 | #ifndef __pinocchio_spatial_log_hpp__ 5 | #define __pinocchio_spatial_log_hpp__ 6 | 7 | namespace pinocchio 8 | { 9 | 10 | template 11 | struct log3_impl; 12 | template 13 | struct Jlog3_impl; 14 | 15 | template 16 | struct log6_impl; 17 | template 18 | struct Jlog6_impl; 19 | 20 | template 21 | inline typename PINOCCHIO_EIGEN_PLAIN_TYPE(Matrix3) 22 | renormalize_rotation_matrix(const Eigen::MatrixBase & R); 23 | 24 | } // namespace pinocchio 25 | 26 | #endif // ifndef __pinocchio_spatial_log_hpp__ 27 | -------------------------------------------------------------------------------- /include/pinocchio/unsupported.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_unsupported_hpp__ 6 | #define __pinocchio_unsupported_hpp__ 7 | 8 | #include "pinocchio/deprecation.hpp" 9 | 10 | #define PINOCCHIO_UNSUPPORTED PINOCCHIO_DEPRECATED 11 | #define PINOCCHIO_UNSUPPORTED_MESSAGE PINOCCHIO_DEPRECATED_MESSAGE 12 | 13 | #endif // ifndef __pinocchio_unsupported_hpp__ 14 | -------------------------------------------------------------------------------- /include/pinocchio/utils/axis-label.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_axis_label_hpp__ 6 | #define __pinocchio_axis_label_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | 11 | /// 12 | /// \brief Generate the label (X, Y or Z) of the axis relative to its index. 13 | /// 14 | /// \tparam axis Index of the axis (either 0 for X, 1 for Y and Z for 2). 15 | /// 16 | /// \returns a char containing the label of the axis. 17 | /// 18 | template 19 | inline char axisLabel(); 20 | 21 | template<> 22 | inline char axisLabel<0>() 23 | { 24 | return 'X'; 25 | } 26 | template<> 27 | inline char axisLabel<1>() 28 | { 29 | return 'Y'; 30 | } 31 | template<> 32 | inline char axisLabel<2>() 33 | { 34 | return 'Z'; 35 | } 36 | } // namespace pinocchio 37 | 38 | #endif // __pinocchio_axis_label_hpp__ 39 | -------------------------------------------------------------------------------- /include/pinocchio/utils/cast.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_utils_cast_hpp__ 6 | #define __pinocchio_utils_cast_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | template 13 | NewScalar cast(const Scalar & value) 14 | { 15 | return Eigen::internal::cast_impl::run(value); 16 | } 17 | } // namespace pinocchio 18 | 19 | #endif // ifndef __pinocchio_utils_cast_hpp__ 20 | -------------------------------------------------------------------------------- /include/pinocchio/utils/helpers.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_utils_helpers_hpp__ 6 | #define __pinocchio_utils_helpers_hpp__ 7 | 8 | namespace pinocchio 9 | { 10 | namespace internal 11 | { 12 | template 13 | struct is_same_type 14 | { 15 | static const bool value = false; 16 | }; 17 | 18 | template 19 | struct is_same_type 20 | { 21 | static const bool value = true; 22 | }; 23 | } // namespace internal 24 | } // namespace pinocchio 25 | 26 | #endif // __pinocchio_utils_helpers_hpp__ 27 | -------------------------------------------------------------------------------- /include/pinocchio/utils/openmp.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_utils_openmp_hpp__ 6 | #define __pinocchio_utils_openmp_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | 13 | /// \brief Returns the number of thread defined by the environment variable OMP_NUM_THREADS. 14 | /// If this variable is not defined, this simply returns the default value 1. 15 | /// 16 | inline int getOpenMPNumThreadsEnv() 17 | { 18 | int num_threads = 1; 19 | 20 | if (const char * env_p = std::getenv("OMP_NUM_THREADS")) 21 | num_threads = atoi(env_p); 22 | 23 | return num_threads; 24 | } 25 | } // namespace pinocchio 26 | 27 | #endif // ifndef __pinocchio_utils_openmp_hpp__ 28 | -------------------------------------------------------------------------------- /include/pinocchio/utils/shared-ptr.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2023 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_utils_shared_ptr_hpp__ 6 | #define __pinocchio_utils_shared_ptr_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | 13 | /// \brief Compares two std::shared_ptr 14 | /// 15 | template 16 | bool compare_shared_ptr(const std::shared_ptr & ptr1, const std::shared_ptr & ptr2) 17 | { 18 | if (ptr1 == ptr2) 19 | return true; 20 | if (ptr1 && ptr2) 21 | return *ptr1.get() == *ptr2.get(); 22 | return false; 23 | } 24 | } // namespace pinocchio 25 | 26 | #endif // ifndef __pinocchio_utils_shared_ptr_hpp__ 27 | -------------------------------------------------------------------------------- /include/pinocchio/utils/string-generator.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2019 CNRS INRIA 3 | // 4 | 5 | #ifndef __pinocchio_utils_string_generator_hpp__ 6 | #define __pinocchio_utils_string_generator_hpp__ 7 | 8 | #include 9 | #include 10 | 11 | namespace pinocchio 12 | { 13 | 14 | /// 15 | /// \brief Generate a random string composed of alphanumeric symbols of a given length. 16 | /// 17 | /// \param[in] len The length of the output string. 18 | /// 19 | /// \returns a random string composed of alphanumeric symbols. 20 | /// 21 | inline std::string randomStringGenerator(const int len) 22 | { 23 | std::string res; 24 | static const char alphanum[] = "0123456789" 25 | "ABCDEFGHIJKLMNOPQRSTUVWXYZ" 26 | "abcdefghijklmnopqrstuvwxyz"; 27 | 28 | for (int i = 0; i < len; ++i) 29 | res += alphanum[((size_t)std::rand() % (sizeof(alphanum) - 1))]; 30 | return res; 31 | } 32 | } // namespace pinocchio 33 | 34 | #endif // __pinocchio_utils_string_generator_hpp__ 35 | -------------------------------------------------------------------------------- /include/pinocchio/utils/string.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 INRIA 3 | // 4 | 5 | #ifndef __pinocchio_utils_string_hpp__ 6 | #define __pinocchio_utils_string_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | 13 | /// 14 | /// \brief Replace string from with to in input_str. 15 | /// 16 | /// \param[in] input_str string on which replace operates. 17 | /// \param[in] from The string to replace. 18 | /// \param[in] to The string to replace the old value with. 19 | /// 20 | /// \returns true if from has been found within input_str 21 | /// 22 | inline bool replace(std::string & input_str, const std::string & from, const std::string & to) 23 | { 24 | bool has_from = false; 25 | size_t start_pos = input_str.find(from); 26 | 27 | while (start_pos != std::string::npos) 28 | { 29 | has_from = true; 30 | input_str.replace(start_pos, from.length(), to); 31 | start_pos = input_str.find(from); 32 | } 33 | 34 | return has_from; 35 | } 36 | } // namespace pinocchio 37 | 38 | #endif // __pinocchio_utils_string_hpp__ 39 | -------------------------------------------------------------------------------- /include/pinocchio/utils/timer2.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 LAAS-CNRS 3 | // 4 | 5 | #ifndef __pinocchio_utils_timer2_hpp__ 6 | #define __pinocchio_utils_timer2_hpp__ 7 | 8 | #include 9 | 10 | namespace pinocchio 11 | { 12 | 13 | class Timer 14 | { 15 | public: 16 | Timer() 17 | { 18 | clock_gettime(CLOCK_MONOTONIC, &start_); 19 | } 20 | 21 | inline void reset() 22 | { 23 | clock_gettime(CLOCK_MONOTONIC, &start_); 24 | } 25 | 26 | inline double get_duration() 27 | { 28 | clock_gettime(CLOCK_MONOTONIC, &finish_); 29 | duration_ = static_cast(finish_.tv_sec - start_.tv_sec) * 1000000; 30 | duration_ += static_cast(finish_.tv_nsec - start_.tv_nsec) / 1000; 31 | return duration_ / 1000.; 32 | } 33 | 34 | inline double get_us_duration() 35 | { 36 | clock_gettime(CLOCK_MONOTONIC, &finish_); 37 | duration_ = static_cast(finish_.tv_sec - start_.tv_sec) * 1000000; 38 | duration_ += static_cast(finish_.tv_nsec - start_.tv_nsec) / 1000; 39 | return duration_; 40 | } 41 | 42 | private: 43 | struct timespec start_; 44 | struct timespec finish_; 45 | double duration_; 46 | }; 47 | } // namespace pinocchio 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [tool.ruff] 2 | exclude = [ "cmake/*", "models/example-robot-data/*" ] 3 | 4 | [tool.ruff.lint] 5 | select = [ "E", "F", "I", "PTH", "RUF", "UP", "W" ] 6 | -------------------------------------------------------------------------------- /src/algorithm/center-of-mass-derivatives.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/algorithm/center-of-mass-derivatives.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | 10 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 11 | getCenterOfMassVelocityDerivatives< 12 | context::Scalar, 13 | context::Options, 14 | JointCollectionDefaultTpl, 15 | context::Matrix3x>( 16 | const context::Model &, context::Data &, const Eigen::MatrixBase &); 17 | } // namespace pinocchio 18 | -------------------------------------------------------------------------------- /src/algorithm/compute-all-terms.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/algorithm/compute-all-terms.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | namespace impl 10 | { 11 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void computeAllTerms< 12 | context::Scalar, 13 | context::Options, 14 | JointCollectionDefaultTpl, 15 | Eigen::Ref, 16 | Eigen::Ref>( 17 | const context::Model &, 18 | context::Data &, 19 | const Eigen::MatrixBase> &, 20 | const Eigen::MatrixBase> &); 21 | } // namespace impl 22 | } // namespace pinocchio 23 | -------------------------------------------------------------------------------- /src/algorithm/contact-dynamics.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | 7 | #ifndef PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS 8 | 9 | #include "pinocchio/algorithm/contact-dynamics.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | 14 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 15 | computeKKTContactDynamicMatrixInverse< 16 | context::Scalar, 17 | context::Options, 18 | JointCollectionDefaultTpl, 19 | context::VectorXs, 20 | context::MatrixXs, 21 | context::MatrixXs>( 22 | const context::Model &, 23 | context::Data &, 24 | const Eigen::MatrixBase &, 25 | const Eigen::MatrixBase &, 26 | const Eigen::MatrixBase &, 27 | const context::Scalar &); 28 | 29 | } // namespace pinocchio 30 | 31 | #endif // PINOCCHIO_SKIP_ALGORITHM_CONTACT_DYNAMICS 32 | -------------------------------------------------------------------------------- /src/algorithm/contact-jacobian.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | 7 | #ifndef PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN 8 | 9 | #include "pinocchio/algorithm/contact-jacobian.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void getConstraintJacobian< 14 | context::Scalar, 15 | context::Options, 16 | JointCollectionDefaultTpl, 17 | context::MatrixXs>( 18 | const context::Model &, 19 | const context::Data &, 20 | const context::RigidConstraintModel &, 21 | context::RigidConstraintData &, 22 | const Eigen::MatrixBase &); 23 | 24 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void getConstraintsJacobian< 25 | context::Scalar, 26 | context::Options, 27 | JointCollectionDefaultTpl, 28 | context::MatrixXs, 29 | typename context::RigidConstraintModelVector::allocator_type, 30 | typename context::RigidConstraintDataVector::allocator_type>( 31 | const context::Model &, 32 | const context::Data &, 33 | const context::RigidConstraintModelVector &, 34 | context::RigidConstraintDataVector &, 35 | const Eigen::MatrixBase &); 36 | 37 | } // namespace pinocchio 38 | 39 | #endif // PINOCCHIO_SKIP_ALGORITHM_CONTACT_JACOBIAN 40 | -------------------------------------------------------------------------------- /src/algorithm/crba.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/algorithm/crba.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::MatrixXs & crba< 10 | context::Scalar, 11 | context::Options, 12 | JointCollectionDefaultTpl, 13 | Eigen::Ref>( 14 | const context::Model &, 15 | context::Data &, 16 | const Eigen::MatrixBase> &, 17 | const Convention convention); 18 | } // namespace pinocchio 19 | -------------------------------------------------------------------------------- /src/algorithm/geometry.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | 7 | #ifndef PINOCCHIO_SKIP_ALGORITHM_GEOMETRY 8 | 9 | #include "pinocchio/algorithm/geometry.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | 14 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void updateGeometryPlacements< 15 | context::Scalar, 16 | context::Options, 17 | JointCollectionDefaultTpl, 18 | context::VectorXs>( 19 | const context::Model &, 20 | context::Data &, 21 | const GeometryModel &, 22 | GeometryData &, 23 | const Eigen::MatrixBase &); 24 | 25 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 26 | updateGeometryPlacements( 27 | const context::Model &, const context::Data &, const GeometryModel &, GeometryData &); 28 | 29 | } // namespace pinocchio 30 | 31 | #endif // PINOCCHIO_SKIP_ALGORITHM_GEOMETRY 32 | -------------------------------------------------------------------------------- /src/algorithm/impulse-dynamics.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | 7 | #ifndef PINOCCHIO_SKIP_ALGORITHM_IMPULSE_DYNAMICS 8 | 9 | #include "pinocchio/algorithm/impulse-dynamics.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | 14 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI const context::VectorXs & 15 | impulseDynamics< 16 | context::Scalar, 17 | context::Options, 18 | JointCollectionDefaultTpl, 19 | context::VectorXs, 20 | context::VectorXs, 21 | typename context::RigidConstraintModelVector::allocator_type, 22 | typename context::RigidConstraintDataVector::allocator_type>( 23 | const context::Model &, 24 | context::Data &, 25 | const Eigen::MatrixBase &, 26 | const Eigen::MatrixBase &, 27 | const context::RigidConstraintModelVector &, 28 | context::RigidConstraintDataVector &, 29 | const context::Scalar, 30 | const ProximalSettingsTpl &); 31 | } // namespace pinocchio 32 | 33 | #endif // PINOCCHIO_SKIP_ALGORITHM_IMPULSE_DYNAMICS 34 | -------------------------------------------------------------------------------- /src/algorithm/proximal.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/algorithm/proximal.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | 10 | template struct PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI 11 | ProximalSettingsTpl; 12 | 13 | } // namespace pinocchio 14 | -------------------------------------------------------------------------------- /src/collision/collision.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | #include "pinocchio/collision/collision.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | 11 | template PINOCCHIO_COLLISION_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI bool computeCollisions< 12 | context::Scalar, 13 | context::Options, 14 | JointCollectionDefaultTpl, 15 | context::VectorXs>( 16 | const context::Model &, 17 | context::Data &, 18 | const GeometryModel &, 19 | GeometryData &, 20 | const Eigen::MatrixBase &, 21 | const bool stopAtFirstCollision); 22 | 23 | template PINOCCHIO_COLLISION_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 24 | computeBodyRadius( 25 | const context::Model &, const GeometryModel &, GeometryData &); 26 | 27 | } // namespace pinocchio 28 | -------------------------------------------------------------------------------- /src/collision/distance.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | #include "pinocchio/collision/distance.hpp" 7 | 8 | namespace pinocchio 9 | { 10 | 11 | template PINOCCHIO_COLLISION_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI std::size_t 12 | computeDistances( 13 | const context::Model &, 14 | context::Data &, 15 | const GeometryModel &, 16 | GeometryData &, 17 | const Eigen::MatrixBase &); 18 | 19 | } // namespace pinocchio 20 | -------------------------------------------------------------------------------- /src/multibody/data.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/multibody/data.hpp" 6 | 7 | namespace pinocchio 8 | { 9 | 10 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI 11 | DataTpl::DataTpl(); 12 | 13 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI 14 | DataTpl::DataTpl( 15 | const context::Model &); 16 | 17 | } // namespace pinocchio 18 | -------------------------------------------------------------------------------- /src/multibody/sample-models.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include "pinocchio/spatial/fwd.hpp" 6 | 7 | #ifndef PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS 8 | 9 | #include "pinocchio/multibody/sample-models.hpp" 10 | 11 | namespace pinocchio 12 | { 13 | namespace buildModels 14 | { 15 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 16 | manipulator( 17 | context::Model &, bool); 18 | 19 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 20 | humanoid(context::Model &, bool); 21 | 22 | template PINOCCHIO_EXPLICIT_INSTANTIATION_DEFINITION_DLLAPI void 23 | humanoidRandom( 24 | context::Model &, bool, bool); 25 | 26 | } // namespace buildModels 27 | } // namespace pinocchio 28 | 29 | #endif // PINOCCHIO_SKIP_MULTIBODY_SAMPLE_MODELS 30 | -------------------------------------------------------------------------------- /src/parsers/urdf/utils.cpp: -------------------------------------------------------------------------------- 1 | #include "pinocchio/parsers/urdf/utils.hpp" 2 | 3 | namespace pinocchio 4 | { 5 | namespace urdf 6 | { 7 | namespace details 8 | { 9 | 10 | SE3 convertFromUrdf(const ::urdf::Pose & M) 11 | { 12 | const ::urdf::Vector3 & p = M.position; 13 | const ::urdf::Rotation & q = M.rotation; 14 | return SE3(SE3::Quaternion(q.w, q.x, q.y, q.z).matrix(), SE3::Vector3(p.x, p.y, p.z)); 15 | } 16 | } // namespace details 17 | } // namespace urdf 18 | } // namespace pinocchio 19 | -------------------------------------------------------------------------------- /unittest/algorithm/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_subdirectory(utils) 2 | -------------------------------------------------------------------------------- /unittest/algorithm/utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | add_pinocchio_unit_test(motion) 2 | add_pinocchio_unit_test(force) 3 | -------------------------------------------------------------------------------- /unittest/casadi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2021 INRIA 3 | # 4 | 5 | macro(ADD_CASADI_UNIT_TEST name) 6 | add_pinocchio_unit_test(${name} PACKAGES casadi) 7 | get_cpp_test_name(${name} ${CMAKE_CURRENT_SOURCE_DIR} TEST_NAME) 8 | set_tests_properties(${TEST_NAME} PROPERTIES TIMEOUT 3000) 9 | add_dependencies(${PROJECT_NAME}-test-casadi ${TEST_NAME}) 10 | endmacro() 11 | 12 | if(BUILD_WITH_CASADI_SUPPORT) 13 | add_custom_target(${PROJECT_NAME}-test-casadi) 14 | set_target_properties(${PROJECT_NAME}-test-casadi PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) 15 | 16 | add_casadi_unit_test(basic) 17 | add_casadi_unit_test(spatial) 18 | add_casadi_unit_test(explog) 19 | add_casadi_unit_test(joints) 20 | add_casadi_unit_test(algorithms) 21 | if(BUILD_ADVANCED_TESTING) 22 | add_casadi_unit_test(aba) 23 | add_casadi_unit_test(constraint-dynamics-derivatives) 24 | add_casadi_unit_test(constraint-dynamics) 25 | add_casadi_unit_test(aba-derivatives) 26 | endif() 27 | add_casadi_unit_test(integrate-derivatives) 28 | add_casadi_unit_test(rnea-derivatives) 29 | endif() 30 | -------------------------------------------------------------------------------- /unittest/casadi/casadi-utils.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 Inria 3 | // 4 | // Utils for tests, e.g. conversion between Eigen matrix 5 | // and DM without copy. 6 | #include "pinocchio/autodiff/casadi.hpp" 7 | #include "pinocchio/autodiff/casadi-algo.hpp" 8 | 9 | /// Without copy 10 | template 11 | casadi::DM eigenToDM(const Eigen::MatrixBase & x) 12 | { 13 | typedef Eigen::Map Map_t; 14 | std::vector x_vec((size_t)x.size()); 15 | Map_t(x_vec.data(), x.rows(), x.cols()) = x; 16 | casadi::DM out(x_vec); 17 | return reshape(out, x.rows(), x.cols()); 18 | } 19 | 20 | template 21 | casadi::DM SE3toCasadiDM(const pinocchio::SE3Base & M) 22 | { 23 | typedef pinocchio::SE3Base SE3; 24 | typedef typename Derived::Scalar Scalar; 25 | typename SE3::HomogeneousMatrixType M_mat = M.toHomogeneousMatrix(); 26 | std::vector flat_M_vec(M_mat.data(), M_mat.data() + M_mat.size()); 27 | casadi::DM out{flat_M_vec}; 28 | return reshape(out, 4, 4); 29 | } 30 | -------------------------------------------------------------------------------- /unittest/cppad/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2021 INRIA 3 | # 4 | 5 | # Automatic differentiation 6 | macro(ADD_CPPAD_UNIT_TEST name) 7 | include_directories(SYSTEM ${cppad_INCLUDE_DIR}) 8 | add_pinocchio_unit_test(${name} PACKAGES ${cppad_LIBRARY}) 9 | get_cpp_test_name(${name} ${CMAKE_CURRENT_SOURCE_DIR} TEST_NAME) 10 | add_dependencies(${PROJECT_NAME}-test-cpp-cppad ${TEST_NAME}) 11 | endmacro() 12 | 13 | if(BUILD_WITH_AUTODIFF_SUPPORT) 14 | add_custom_target(${PROJECT_NAME}-test-cpp-cppad) 15 | set_target_properties(${PROJECT_NAME}-test-cpp-cppad PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True) 16 | 17 | add_cppad_unit_test(basic) 18 | add_cppad_unit_test(spatial) 19 | add_cppad_unit_test(joints) 20 | add_cppad_unit_test(algorithms) 21 | add_cppad_unit_test(derivatives) 22 | add_cppad_unit_test(joint-configurations) 23 | endif() 24 | -------------------------------------------------------------------------------- /unittest/cppadcg/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | macro(ADD_CPPADCG_UNIT_TEST name) 2 | include_directories(SYSTEM ${cppad_INCLUDE_DIR}) 3 | include_directories(SYSTEM ${cppadcodegen_INCLUDE_DIR}) 4 | add_pinocchio_unit_test(${name} PACKAGES ${cppad_LIBRARY} ${CMAKE_DL_LIBS}) 5 | get_cpp_test_name(${name} ${CMAKE_CURRENT_SOURCE_DIR} TEST_NAME) 6 | add_dependencies(${PROJECT_NAME}-test-cpp-cppadcg ${TEST_NAME}) 7 | target_compile_definitions(${TEST_NAME} PUBLIC PINOCCHIO_CXX_COMPILER=\"${CMAKE_CXX_COMPILER}\") 8 | endmacro() 9 | 10 | if(BUILD_WITH_AUTODIFF_SUPPORT) 11 | if(BUILD_WITH_CODEGEN_SUPPORT) 12 | add_custom_target(${PROJECT_NAME}-test-cpp-cppadcg) 13 | set_target_properties(${PROJECT_NAME}-test-cpp-cppadcg PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD 14 | True) 15 | 16 | add_cppadcg_unit_test(basic) 17 | if(BUILD_ADVANCED_TESTING) 18 | add_cppadcg_unit_test(algorithms) 19 | add_cppadcg_unit_test(joint-configurations) 20 | endif() 21 | endif() 22 | endif() 23 | -------------------------------------------------------------------------------- /unittest/csv.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include // to avoid C99 warnings 10 | 11 | #include 12 | #include 13 | 14 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 15 | 16 | using namespace pinocchio; 17 | 18 | BOOST_AUTO_TEST_CASE(test_random_matrix) 19 | { 20 | const Eigen::DenseIndex mat_size = 20; 21 | const Eigen::MatrixXd matrix = Eigen::MatrixXd::Random(mat_size, mat_size); 22 | toCSVfile(TEST_SERIALIZATION_FOLDER "/test.csv", matrix); 23 | } 24 | 25 | BOOST_AUTO_TEST_SUITE_END() 26 | -------------------------------------------------------------------------------- /unittest/eigen-basic-op.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2019 INRIA 3 | // 4 | 5 | #include "pinocchio/multibody/model.hpp" 6 | 7 | #include 8 | #include "pinocchio/math/matrix.hpp" 9 | 10 | #include 11 | #include 12 | 13 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 14 | 15 | BOOST_AUTO_TEST_CASE(test_matrix_matrix_product) 16 | { 17 | using namespace pinocchio; 18 | using namespace Eigen; 19 | const Eigen::DenseIndex m = 20, n = 100; 20 | MatrixXd M1(MatrixXd::Ones(m, n)), M2(MatrixXd::Ones(n, m)); 21 | MatrixMatrixProduct::type res = M1 * M2; 22 | BOOST_CHECK(!res.eval().isZero()); 23 | } 24 | 25 | BOOST_AUTO_TEST_CASE(test_scalar_matrix_product) 26 | { 27 | using namespace pinocchio; 28 | using namespace Eigen; 29 | const Eigen::DenseIndex m = 20, n = 100; 30 | MatrixXd M(MatrixXd::Ones(m, n)); 31 | const double alpha = 0.; 32 | ScalarMatrixProduct::type res = alpha * M; 33 | BOOST_CHECK(res.eval().isZero()); 34 | } 35 | 36 | BOOST_AUTO_TEST_CASE(test_matrix_scalar_product) 37 | { 38 | using namespace pinocchio; 39 | using namespace Eigen; 40 | const Eigen::DenseIndex m = 20, n = 100; 41 | MatrixXd M(MatrixXd::Ones(m, n)); 42 | const double alpha = 1.; 43 | MatrixScalarProduct::type res = M * alpha; 44 | BOOST_CHECK(res.eval() == M); 45 | } 46 | 47 | BOOST_AUTO_TEST_SUITE_END() 48 | -------------------------------------------------------------------------------- /unittest/geometry-object.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2022 INRIA 3 | // 4 | 5 | #include 6 | 7 | #include "pinocchio/multibody/model.hpp" 8 | #include "pinocchio/multibody/data.hpp" 9 | 10 | #include "pinocchio/multibody/geometry-object.hpp" 11 | 12 | #include 13 | #include 14 | 15 | using namespace pinocchio; 16 | 17 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 18 | 19 | BOOST_AUTO_TEST_CASE(test_clone) 20 | { 21 | hpp::fcl::Sphere * sphere_ptr = new hpp::fcl::Sphere(0.5); 22 | GeometryObject::CollisionGeometryPtr sphere_shared_ptr(sphere_ptr); 23 | GeometryObject geom_obj("sphere", 0, 0, SE3::Random(), sphere_shared_ptr); 24 | 25 | const GeometryObject geom_obj_clone = geom_obj.clone(); 26 | BOOST_CHECK(geom_obj_clone == geom_obj); 27 | 28 | sphere_ptr->radius = 1.; 29 | BOOST_CHECK(geom_obj_clone != geom_obj); 30 | } 31 | 32 | BOOST_AUTO_TEST_SUITE_END() 33 | -------------------------------------------------------------------------------- /unittest/gram-schmidt-orthonormalisation.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2024 INRIA 3 | // 4 | 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include // to avoid C99 warnings 11 | 12 | #include 13 | #include 14 | 15 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 16 | 17 | using namespace pinocchio; 18 | 19 | BOOST_AUTO_TEST_CASE(test_random_matrix) 20 | { 21 | for (size_t i = 0; i < 100; ++i) 22 | { 23 | const Eigen::DenseIndex size = 20; 24 | const Eigen::MatrixXd random_mat = Eigen::MatrixXd::Random(size, size); 25 | const auto qr = random_mat.householderQr(); 26 | const Eigen::MatrixXd basis = qr.householderQ(); 27 | 28 | for (size_t k = 0; k < 1000; ++k) 29 | { 30 | const Eigen::VectorXd random_vec = Eigen::VectorXd::Random(size); 31 | orthonormalisation(basis.leftCols(10), random_vec); 32 | BOOST_CHECK((basis.leftCols(10).transpose() * random_vec).isZero()); 33 | } 34 | } 35 | } 36 | 37 | BOOST_AUTO_TEST_SUITE_END() 38 | -------------------------------------------------------------------------------- /unittest/joint-free-flyer.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2015-2020 CNRS INRIA 3 | // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France. 4 | // 5 | 6 | #include "pinocchio/math/fwd.hpp" 7 | #include "pinocchio/multibody/joint/joints.hpp" 8 | #include "pinocchio/algorithm/rnea.hpp" 9 | #include "pinocchio/algorithm/aba.hpp" 10 | #include "pinocchio/algorithm/crba.hpp" 11 | #include "pinocchio/algorithm/jacobian.hpp" 12 | #include "pinocchio/algorithm/compute-all-terms.hpp" 13 | 14 | #include 15 | #include 16 | 17 | using namespace pinocchio; 18 | 19 | BOOST_AUTO_TEST_SUITE(JointFreeFlyer) 20 | 21 | BOOST_AUTO_TEST_CASE(spatial) 22 | { 23 | Motion v(Motion::Random()); 24 | 25 | JointMotionSubspaceIdentityTpl constraint; 26 | Motion Sv = constraint * v.toVector(); 27 | 28 | BOOST_CHECK(Sv == v); 29 | } 30 | 31 | BOOST_AUTO_TEST_SUITE_END() 32 | -------------------------------------------------------------------------------- /unittest/macros.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2021 CNRS INRIA 3 | // 4 | 5 | #include "pinocchio/macros.hpp" 6 | 7 | #include 8 | #include 9 | 10 | using namespace pinocchio; 11 | 12 | std::string expected_msg; 13 | 14 | bool check_exception_msg(const std::exception & exception) 15 | { 16 | BOOST_CHECK_EQUAL(expected_msg, exception.what()); 17 | return expected_msg == exception.what(); 18 | } 19 | 20 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 21 | 22 | void function_1(std::vector v, size_t size) 23 | { 24 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), size); 25 | //"size of input vector should be " << size) 26 | } 27 | void function_2(std::vector v, size_t size) 28 | { 29 | PINOCCHIO_CHECK_ARGUMENT_SIZE(v.size(), size, "custom message " << "with stream"); 30 | } 31 | 32 | BOOST_AUTO_TEST_CASE(test_check_arguments) 33 | { 34 | expected_msg = "wrong argument size: expected 2, got 3\n" 35 | "hint: v.size() is different from size\n"; 36 | BOOST_CHECK_EXCEPTION( 37 | function_1(std::vector(3), 2), std::invalid_argument, check_exception_msg); 38 | expected_msg = "wrong argument size: expected 2, got 3\n" 39 | "hint: custom message with stream\n"; 40 | BOOST_CHECK_EXCEPTION( 41 | function_2(std::vector(3), 2), std::invalid_argument, check_exception_msg); 42 | } 43 | 44 | BOOST_AUTO_TEST_SUITE_END() 45 | -------------------------------------------------------------------------------- /unittest/models/link_and_joint_identical_name.urdf: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /unittest/models/test_composite.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /unittest/packaging/cmake/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | project(ExtraLib CXX) 4 | find_package(pinocchio REQUIRED) 5 | 6 | set(PINOCCHIO_MODEL_DIR "${PROJECT_SOURCE_DIR}/../../../models") 7 | add_definitions('-DPINOCCHIO_MODEL_DIR=\"${PINOCCHIO_MODEL_DIR}\"') 8 | 9 | add_executable(run_rnea ../run_rnea.cpp) 10 | target_link_libraries(run_rnea PUBLIC pinocchio::pinocchio) 11 | 12 | if(DEFINED PINOCCHIO_USE_URDFDOM) 13 | add_executable(load_urdf ../load_urdf.cpp) 14 | target_link_libraries(load_urdf PUBLIC pinocchio::pinocchio) 15 | endif(DEFINED PINOCCHIO_USE_URDFDOM) 16 | -------------------------------------------------------------------------------- /unittest/packaging/load_urdf.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | 6 | int main(int /*argc*/, char ** /*argv*/) 7 | { 8 | using namespace pinocchio; 9 | 10 | const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf"); 11 | Model model; 12 | pinocchio::urdf::buildModel(filename, JointModelFreeFlyer(), model); 13 | 14 | std::cout << "model.nq: " << model.nq << std::endl; 15 | std::cout << "model.nv: " << model.nv << std::endl; 16 | 17 | return 0; 18 | } 19 | -------------------------------------------------------------------------------- /unittest/packaging/pinocchio_header/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.6) 2 | 3 | project(ExtraLib CXX) 4 | find_package(pinocchio REQUIRED) 5 | 6 | add_executable(run_fk ../run_fk.cpp) 7 | target_link_libraries(run_fk PUBLIC pinocchio::pinocchio_headers) 8 | -------------------------------------------------------------------------------- /unittest/packaging/pinocchio_header/run_fk.cpp: -------------------------------------------------------------------------------- 1 | #include "pinocchio/multibody/sample-models.hpp" 2 | 3 | #include "pinocchio/algorithm/joint-configuration.hpp" 4 | #include "pinocchio/algorithm/aba.hpp" 5 | 6 | #include 7 | 8 | int main(int /*argc*/, char ** /*argv*/) 9 | { 10 | using namespace pinocchio; 11 | 12 | pinocchio::Model model; 13 | pinocchio::buildModels::humanoidRandom(model); 14 | 15 | // Build data related to model 16 | Data data(model); 17 | 18 | // Sample a random joint configuration as well as random joint velocity and torque 19 | Eigen::VectorXd q = randomConfiguration(model); 20 | Eigen::VectorXd v = Eigen::VectorXd::Random(model.nv); 21 | Eigen::VectorXd tau = Eigen::VectorXd::Random(model.nv); 22 | 23 | // Computes the forward dynamics (ABA) 24 | aba(model, data, q, v, tau, Convention::WORLD); 25 | 26 | // Get access to the joint acceleration 27 | std::cout << "Joint acceleration: " << data.ddq << std::endl; 28 | return 0; 29 | } 30 | -------------------------------------------------------------------------------- /unittest/packaging/pkgconfig/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.12) 2 | 3 | project(ExtraLib CXX) 4 | find_package(PkgConfig REQUIRED) 5 | pkg_check_modules(PINOCCHIO REQUIRED pinocchio) 6 | 7 | add_executable(run_rnea ../run_rnea.cpp) 8 | target_link_libraries(run_rnea PRIVATE ${PINOCCHIO_LDFLAGS}) 9 | target_include_directories(run_rnea SYSTEM PRIVATE ${PINOCCHIO_INCLUDE_DIRS}) 10 | target_compile_definitions(run_rnea PRIVATE ${PINOCCHIO_CFLAGS_OTHER}) 11 | 12 | if("-DPINOCCHIO_WITH_URDFDOM" IN_LIST PINOCCHIO_CFLAGS) 13 | set(PINOCCHIO_MODEL_DIR "${PROJECT_SOURCE_DIR}/../../../models") 14 | 15 | add_executable(load_urdf ../load_urdf.cpp) 16 | target_link_libraries(load_urdf PRIVATE ${PINOCCHIO_LDFLAGS}) 17 | target_include_directories(load_urdf SYSTEM PRIVATE ${PINOCCHIO_INCLUDE_DIRS}) 18 | target_compile_definitions(load_urdf PRIVATE ${PINOCCHIO_CFLAGS_OTHER} 19 | "PINOCCHIO_MODEL_DIR=\"${PINOCCHIO_MODEL_DIR}\"") 20 | endif() 21 | 22 | message(STATUS "LDFLAGS: ${PINOCCHIO_LDFLAGS}") 23 | message(STATUS "CFLAGS: ${PINOCCHIO_CFLAGS_OTHER}") 24 | message(STATUS "INCLUDE: ${PINOCCHIO_INCLUDE_DIRS}") 25 | -------------------------------------------------------------------------------- /unittest/packaging/run_rnea.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | int main(int /*argc*/, char ** /*argv*/) 8 | { 9 | using namespace pinocchio; 10 | 11 | Model model; 12 | pinocchio::buildModels::humanoid(model); 13 | 14 | Model::VectorXs q = randomConfiguration(model); 15 | Model::VectorXs v = Model::VectorXs::Random(model.nv); 16 | Model::VectorXs a = Model::VectorXs::Random(model.nv); 17 | 18 | Data data(model); 19 | rnea(model, data, q, v, a); 20 | 21 | return 0; 22 | } 23 | -------------------------------------------------------------------------------- /unittest/python/bindings_build_geom_from_urdf_memorycheck.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | from pathlib import Path 3 | 4 | import pinocchio as pin 5 | 6 | 7 | @unittest.skipUnless(pin.WITH_URDFDOM, "Needs URDFDOM") 8 | class TestBuildGeomFromUrdfMemoryCheck(unittest.TestCase): 9 | def setUp(self): 10 | self.current_dir = Path(__file__).parent 11 | self.model_dir = self.current_dir / "../../models" 12 | self.model_path = ( 13 | self.model_dir 14 | / "example-robot-data/robots/ur_description/urdf/ur5_robot.urdf" 15 | ) 16 | 17 | def test_load(self): 18 | model = pin.buildModelFromUrdf(self.model_path) 19 | for _ in range(2): 20 | pin.buildGeomFromUrdf( 21 | model, 22 | self.model_path, 23 | pin.COLLISION, 24 | package_dirs=self.model_dir, 25 | ) 26 | 27 | 28 | if __name__ == "__main__": 29 | unittest.main() 30 | -------------------------------------------------------------------------------- /unittest/python/bindings_fcl_transform.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | 5 | 6 | class TestFCLTransformConversion(unittest.TestCase): 7 | def test_from_SE3(self): 8 | M = pin.SE3.Random() 9 | fcl_transform = pin.hppfcl.Transform3f(M) 10 | 11 | self.assertTrue((M.rotation == fcl_transform.getRotation()).all()) 12 | self.assertTrue((M.translation == fcl_transform.getTranslation()).all()) 13 | 14 | def test_to_SE3(self): 15 | fcl_transform = pin.hppfcl.Transform3f() 16 | M = pin.SE3(fcl_transform) 17 | self.assertTrue(M.isIdentity()) 18 | 19 | 20 | if __name__ == "__main__": 21 | if pin.WITH_HPP_FCL_BINDINGS: 22 | unittest.main() 23 | -------------------------------------------------------------------------------- /unittest/python/bindings_geometry_model.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | 5 | 6 | class TestGeometryModelBindings(unittest.TestCase): 7 | def test_pair_equals(self): 8 | c1 = pin.CollisionPair(1, 2) 9 | c2 = pin.CollisionPair(1, 2) 10 | c3 = pin.CollisionPair(3, 4) 11 | 12 | self.assertEqual(c1, c2) 13 | self.assertTrue(c1 == c2) 14 | self.assertFalse(c1 != c2) 15 | 16 | self.assertNotEqual(c1, c3) 17 | self.assertTrue(c1 != c3) 18 | self.assertFalse(c1 == c3) 19 | 20 | def test_pair_copy(self): 21 | c1 = pin.CollisionPair(1, 2) 22 | c2 = c1.copy() 23 | 24 | self.assertEqual(c1, c2) 25 | 26 | c2.second = 3 27 | self.assertNotEqual(c1, c2) 28 | 29 | 30 | if __name__ == "__main__": 31 | unittest.main() 32 | -------------------------------------------------------------------------------- /unittest/python/bindings_kinematic_regressor.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | from test_case import PinocchioTestCase as TestCase 5 | 6 | 7 | class TestKinematicRegressorBindings(TestCase): 8 | def test_all(self): 9 | model = pin.buildSampleModelHumanoidRandom() 10 | 11 | joint_name = "larm6_joint" 12 | joint_id = model.getJointId(joint_name) 13 | frame_id = model.addBodyFrame("test_body", joint_id, pin.SE3.Identity(), -1) 14 | 15 | data = model.createData() 16 | 17 | model.lowerPositionLimit[:7] = -1.0 18 | model.upperPositionLimit[:7] = 1.0 19 | 20 | q = pin.randomConfiguration(model) 21 | pin.forwardKinematics(model, data, q) 22 | 23 | R1 = pin.computeJointKinematicRegressor( 24 | model, data, joint_id, pin.ReferenceFrame.LOCAL, pin.SE3.Identity() 25 | ) 26 | R2 = pin.computeJointKinematicRegressor( 27 | model, data, joint_id, pin.ReferenceFrame.LOCAL 28 | ) 29 | 30 | self.assertApprox(R1, R2) 31 | 32 | R3 = pin.computeFrameKinematicRegressor( 33 | model, data, frame_id, pin.ReferenceFrame.LOCAL 34 | ) 35 | self.assertApprox(R1, R3) 36 | 37 | 38 | if __name__ == "__main__": 39 | unittest.main() 40 | -------------------------------------------------------------------------------- /unittest/python/bindings_sample_models.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | from test_case import PinocchioTestCase as TestCase 5 | 6 | 7 | class TestSampleModels(TestCase): 8 | def setUp(self): 9 | pass 10 | 11 | def test_all_sampled_models(self): 12 | huamnoid_1 = pin.buildSampleModelHumanoidRandom() 13 | huamnoid_2 = pin.buildSampleModelHumanoidRandom(True) 14 | huamnoid_3 = pin.buildSampleModelHumanoidRandom(False) 15 | 16 | self.assertTrue(huamnoid_1 != huamnoid_2) 17 | self.assertTrue(huamnoid_1 != huamnoid_3) 18 | 19 | manipulator_1 = pin.buildSampleModelManipulator() 20 | 21 | if pin.WITH_HPP_FCL: 22 | _geometry_manipulator_1 = pin.buildSampleGeometryModelManipulator( 23 | manipulator_1 24 | ) 25 | 26 | humanoid_4 = pin.buildSampleModelHumanoid() 27 | humanoid_5 = pin.buildSampleModelHumanoid(True) 28 | humanoid_6 = pin.buildSampleModelHumanoid(False) 29 | 30 | self.assertTrue(humanoid_4 == humanoid_5) 31 | self.assertTrue(humanoid_4 != humanoid_6) 32 | 33 | if pin.WITH_HPP_FCL: 34 | _geometry_humanoid_2 = pin.buildSampleGeometryModelHumanoid(humanoid_4) 35 | 36 | 37 | if __name__ == "__main__": 38 | unittest.main() 39 | -------------------------------------------------------------------------------- /unittest/python/bindings_std_map.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import unittest 3 | from pathlib import Path 4 | 5 | import numpy as np 6 | import pinocchio as pin 7 | from test_case import PinocchioTestCase as TestCase 8 | 9 | 10 | class TestStdMap(TestCase): 11 | def setUp(self): 12 | pass 13 | 14 | def test_pickle(self): 15 | map = pin.StdMap_String_VectorXd() 16 | keys = [] 17 | for k in range(100): 18 | key_name = "key_" + str(k + 1) 19 | keys.append(key_name) 20 | map[key_name] = np.random.rand(10) 21 | 22 | pickle.dump(map, Path("save_std_map.p").open("wb")) 23 | 24 | map_loaded = pickle.load(Path("save_std_map.p").open("rb")) 25 | for key in keys: 26 | self.assertApprox(map[key], map_loaded[key]) 27 | 28 | 29 | if __name__ == "__main__": 30 | unittest.main() 31 | -------------------------------------------------------------------------------- /unittest/python/bindings_std_vector.py: -------------------------------------------------------------------------------- 1 | import pickle 2 | import unittest 3 | from pathlib import Path 4 | 5 | import numpy as np 6 | import pinocchio as pin 7 | from test_case import PinocchioTestCase as TestCase 8 | 9 | 10 | class TestStdMap(TestCase): 11 | def setUp(self): 12 | pass 13 | 14 | def test_pickle(self): 15 | vec = pin.StdVec_Vector3() 16 | for k in range(100): 17 | vec.append(np.random.rand(3)) 18 | 19 | pickle.dump(vec, Path("save_std_vec.p").open("wb")) 20 | 21 | vec_loaded = pickle.load(Path("save_std_vec.p").open("rb")) 22 | for k in range(len(vec)): 23 | self.assertApprox(vec[k], vec_loaded[k]) 24 | 25 | 26 | if __name__ == "__main__": 27 | unittest.main() 28 | -------------------------------------------------------------------------------- /unittest/python/bindings_visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "pinocchio/bindings/python/visualizers/visualizer-visitor.hpp" 2 | 3 | namespace bp = boost::python; 4 | namespace pin = pinocchio; 5 | using pin::python::VisualizerPythonVisitor; 6 | using pin::visualizers::BaseVisualizer; 7 | 8 | using pin::GeometryModel; 9 | using pin::Model; 10 | 11 | struct DummyVisualizer : public BaseVisualizer 12 | { 13 | using BaseVisualizer::BaseVisualizer; 14 | void loadViewerModel() override 15 | { 16 | } 17 | void displayImpl() override 18 | { 19 | } 20 | }; 21 | 22 | BOOST_PYTHON_MODULE(EXT_MODULE_NAME) 23 | { 24 | bp::import("pinocchio"); 25 | 26 | bp::class_("DummyVisualizer", bp::no_init) 27 | .def(bp::init()) 28 | .def(VisualizerPythonVisitor{}); 29 | } 30 | -------------------------------------------------------------------------------- /unittest/python/bindings_visualizer.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | from test_case import PinocchioTestCase as TestCase 5 | from test_ext_bindings_visualizer import DummyVisualizer 6 | 7 | 8 | class TestBindingsViz(TestCase): 9 | def setUp(self): 10 | self.model = pin.Model() 11 | self.visual = pin.GeometryModel() 12 | self.viz = DummyVisualizer(self.model, self.visual) 13 | 14 | def test_getters(self): 15 | self.assertEqual(self.model, self.viz.model) 16 | self.assertEqual(self.visual, self.viz.visualModel) 17 | 18 | 19 | if __name__ == "__main__": 20 | unittest.main() 21 | -------------------------------------------------------------------------------- /unittest/python/casadi/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2020-2021 INRIA 3 | # 4 | 5 | set(${PROJECT_NAME}_PYTHON_CASADI_TESTS bindings_main_algo) 6 | 7 | foreach(test ${${PROJECT_NAME}_PYTHON_CASADI_TESTS}) 8 | set(TEST_NAME "${PROJECT_NAME}-test-py-casadi-${test}") 9 | add_python_unit_test(${TEST_NAME} "unittest/python/casadi/${test}.py" "bindings/python") 10 | add_windows_dll_path_to_test(${TEST_NAME}) 11 | endforeach() 12 | -------------------------------------------------------------------------------- /unittest/python/serialization.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | from test_case import PinocchioTestCase as TestCase 5 | 6 | main_path = "./serialization-data" 7 | 8 | 9 | class TestSerialization(TestCase): 10 | def testTXT(self): 11 | model = pin.buildSampleModelHumanoidRandom() 12 | filename = main_path + "/model.txt" 13 | model.saveToText(filename) 14 | 15 | model2 = pin.Model() 16 | model2.loadFromText(filename) 17 | 18 | self.assertTrue(model == model2) 19 | 20 | def testXML(self): 21 | model = pin.buildSampleModelHumanoidRandom() 22 | filename = main_path + "/model.xml" 23 | tag_name = "Model" 24 | model.saveToXML(filename, tag_name) 25 | 26 | model2 = pin.Model() 27 | model2.loadFromXML(filename, tag_name) 28 | 29 | self.assertTrue(model == model2) 30 | 31 | def testBIN(self): 32 | model = pin.buildSampleModelHumanoidRandom() 33 | filename = main_path + "/model.bin" 34 | model.saveToBinary(filename) 35 | 36 | model2 = pin.Model() 37 | model2.loadFromBinary(filename) 38 | 39 | self.assertTrue(model == model2) 40 | 41 | 42 | if __name__ == "__main__": 43 | unittest.main() 44 | -------------------------------------------------------------------------------- /unittest/python/test_case.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | from pinocchio.utils import isapprox 4 | 5 | 6 | def tracefunc(frame, event, arg): 7 | print(f"{event}, {frame.f_code.co_filename}: {frame.f_lineno}") 8 | return tracefunc 9 | 10 | 11 | class PinocchioTestCase(unittest.TestCase): 12 | def assertApprox(self, a, b, eps=1e-6): 13 | return self.assertTrue( 14 | isapprox(a, b, eps), 15 | f"\n{a}\nis not approximately equal to\n{b}\nwith precision {eps:f}", 16 | ) 17 | -------------------------------------------------------------------------------- /unittest/python/version.py: -------------------------------------------------------------------------------- 1 | import unittest 2 | 3 | import pinocchio as pin 4 | from test_case import PinocchioTestCase as TestCase 5 | 6 | 7 | class TestVersion(TestCase): 8 | def test_version(self): 9 | print("version:", pin.__version__) 10 | print("raw_version:", pin.__raw_version__) 11 | self.assertTrue(pin.__version__) 12 | self.assertTrue(pin.__raw_version__) 13 | 14 | 15 | if __name__ == "__main__": 16 | unittest.main() 17 | -------------------------------------------------------------------------------- /unittest/python_parser.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2016-2020 CNRS INRIA 3 | // 4 | 5 | #include 6 | 7 | #include "pinocchio/multibody/model.hpp" 8 | #include "pinocchio/multibody/data.hpp" 9 | #include "pinocchio/parsers/python.hpp" 10 | 11 | #include 12 | 13 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 14 | 15 | BOOST_AUTO_TEST_CASE(buildModel) 16 | { 17 | std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_model.py"); 18 | 19 | #ifndef NDEBUG 20 | std::cout << "Parse filename \"" << filename << "\"" << std::endl; 21 | #endif 22 | pinocchio::Model model = pinocchio::python::buildModel(filename, "model"); 23 | #ifndef NDEBUG 24 | std::cout << "This model has \"" << model.nq << "\" DoF" << std::endl; 25 | #endif 26 | 27 | BOOST_CHECK(model.nq == 9); 28 | BOOST_CHECK(model.nv == 8); 29 | } 30 | 31 | BOOST_AUTO_TEST_SUITE_END() 32 | -------------------------------------------------------------------------------- /unittest/utils/macros.hpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018 CNRS 3 | // 4 | 5 | #ifndef __pinocchio_unit_test_utils_macros_hpp__ 6 | #define __pinocchio_unit_test_utils_macros_hpp__ 7 | 8 | #ifdef __clang__ 9 | #include // to avoid some warning with clang 10 | #endif 11 | 12 | // #ifdef __clang__ 13 | // #define BEGIN_UNIT_TEST_SECTION _Pragma("clang diagnostic push") _Pragma("clang diagnostic 14 | // ignored \"-Wc99-extensions\" ") 15 | // #else 16 | // #define BEGIN_UNIT_TEST_SECTION 17 | // #endif 18 | // 19 | // #ifdef __clang__ 20 | // #define END_UNIT_TEST_SECTION _Pragma("clang diagnostic pop") 21 | // #else 22 | // #define END_UNIT_TEST_SECTION 23 | // #endif 24 | 25 | #endif // ifndef __pinocchio_unit_test_utils_macros_hpp__ 26 | -------------------------------------------------------------------------------- /unittest/vector.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2020 INRIA 3 | // 4 | 5 | #include 6 | 7 | #include // to avoid C99 warnings 8 | 9 | #include 10 | #include 11 | 12 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 13 | 14 | BOOST_AUTO_TEST_CASE(test_isNormalized) 15 | { 16 | srand(0); 17 | 18 | using namespace pinocchio; 19 | typedef Eigen::Matrix Vector; 20 | 21 | const int max_size = 1000; 22 | #ifdef NDEBUG 23 | const int max_test = 1e6; 24 | #else 25 | const int max_test = 1e2; 26 | #endif 27 | for (int i = 0; i < max_test; ++i) 28 | { 29 | const Eigen::DenseIndex size = rand() % max_size + 1; // random vector size 30 | Vector vec; 31 | vec = Vector::Random(size) + Vector::Constant(size, 2.); 32 | BOOST_CHECK(!isNormalized(vec)); 33 | 34 | vec.normalize(); 35 | BOOST_CHECK(isNormalized(vec)); 36 | 37 | // Specific check for the Zero vector 38 | BOOST_CHECK(!isNormalized(Vector(Vector::Zero(size)))); 39 | } 40 | } 41 | 42 | BOOST_AUTO_TEST_SUITE_END() 43 | -------------------------------------------------------------------------------- /unittest/version.cpp: -------------------------------------------------------------------------------- 1 | // 2 | // Copyright (c) 2018-2019 CNRS INRIA 3 | // 4 | 5 | #include 6 | #include 7 | 8 | #include "utils/macros.hpp" 9 | 10 | #include 11 | #include 12 | 13 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 14 | 15 | BOOST_AUTO_TEST_CASE(test_version) 16 | { 17 | using namespace std; 18 | using namespace pinocchio; 19 | 20 | const string delimiter = "."; 21 | ostringstream version_ref; 22 | version_ref << PINOCCHIO_MAJOR_VERSION << delimiter << PINOCCHIO_MINOR_VERSION << delimiter 23 | << PINOCCHIO_PATCH_VERSION; 24 | 25 | BOOST_CHECK_EQUAL(version_ref.str().c_str(), printVersion()); 26 | 27 | BOOST_CHECK(checkVersionAtLeast(0, 0, 0)); 28 | BOOST_CHECK( 29 | checkVersionAtLeast(PINOCCHIO_MAJOR_VERSION, PINOCCHIO_MINOR_VERSION, PINOCCHIO_PATCH_VERSION)); 30 | BOOST_CHECK(!checkVersionAtLeast( 31 | PINOCCHIO_MAJOR_VERSION, PINOCCHIO_MINOR_VERSION, PINOCCHIO_PATCH_VERSION + 1)); 32 | BOOST_CHECK(!checkVersionAtLeast(99, 0, 0)); 33 | } 34 | 35 | BOOST_AUTO_TEST_SUITE_END() 36 | -------------------------------------------------------------------------------- /unittest/visualizer.cpp: -------------------------------------------------------------------------------- 1 | #include "pinocchio/visualizers/base-visualizer.hpp" 2 | #include "pinocchio/multibody/sample-models.hpp" 3 | 4 | #include 5 | 6 | using namespace pinocchio::visualizers; 7 | 8 | BOOST_AUTO_TEST_SUITE(BOOST_TEST_MODULE) 9 | 10 | struct TestViz : BaseVisualizer 11 | { 12 | using BaseVisualizer::BaseVisualizer; 13 | void loadViewerModel() override 14 | { 15 | } 16 | void displayImpl() override 17 | { 18 | } 19 | }; 20 | 21 | BOOST_AUTO_TEST_CASE(viz_ctor_create_datas) 22 | { 23 | pinocchio::Model model; 24 | pinocchio::buildModels::manipulator(model); 25 | pinocchio::GeometryModel geom_model; // empty 26 | TestViz viz{model, geom_model}; 27 | BOOST_CHECK(!viz.hasExternalData()); 28 | BOOST_CHECK(!viz.hasCollisionModel()); 29 | // dtor destroys internal datas 30 | } 31 | 32 | BOOST_AUTO_TEST_CASE(viz_ctor_borrow_datas) 33 | { 34 | pinocchio::Model model; 35 | pinocchio::buildModels::manipulator(model); 36 | pinocchio::GeometryModel geom_model; // empty 37 | pinocchio::Data data{model}; 38 | pinocchio::GeometryData geom_data{geom_model}; 39 | 40 | { 41 | TestViz viz{model, geom_model, nullptr, data, geom_data, nullptr}; 42 | BOOST_CHECK(viz.hasExternalData()); 43 | BOOST_CHECK(!viz.hasCollisionModel()); 44 | // viz destroyed, does not destroy datas 45 | } 46 | // no double free segfault 47 | } 48 | 49 | BOOST_AUTO_TEST_SUITE_END() 50 | -------------------------------------------------------------------------------- /utils/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # 2 | # Copyright (c) 2015-2020 CNRS INRIA 3 | # 4 | 5 | # --- MACROS ------------------------------------------------------------------ 6 | # --- MACROS ------------------------------------------------------------------ 7 | # --- MACROS ------------------------------------------------------------------ 8 | macro(ADD_UTIL NAME UTIL_SRC) 9 | 10 | if(BUILD_UTILS) 11 | add_executable(${NAME} "${UTIL_SRC}.cpp") 12 | else(BUILD_UTILS) 13 | add_executable(${NAME} EXCLUDE_FROM_ALL "${UTIL_SRC}.cpp") 14 | endif(BUILD_UTILS) 15 | 16 | target_link_libraries(${NAME} PUBLIC ${PROJECT_NAME}) 17 | 18 | add_dependencies(utils ${NAME}) 19 | 20 | if(BUILD_UTILS) 21 | install(TARGETS ${NAME} DESTINATION bin) 22 | endif(BUILD_UTILS) 23 | 24 | endmacro(ADD_UTIL) 25 | 26 | # --- RULES ------------------------------------------------------------------- 27 | # --- RULES ------------------------------------------------------------------- 28 | # --- RULES ------------------------------------------------------------------- 29 | add_custom_target(utils) 30 | 31 | if(urdfdom_FOUND) 32 | add_util(pinocchio_read_model pinocchio_read_model) 33 | endif(urdfdom_FOUND) 34 | --------------------------------------------------------------------------------