diff --git a/CHANGELOG.md b/CHANGELOG.md index 3944d22c2b..abdd7ca512 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -20,6 +20,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). - Add ``collision_color`` parameter to `MeshcatVisualizer.loadViewerModel` ([#2350](https://github.com/stack-of-tasks/pinocchio/pull/2350)) - Add ``BuildFromMJCF`` function to RobotWrapper ([#2363](https://github.com/stack-of-tasks/pinocchio/pull/2363)) +### Removed +- Remove deprecated headers related to joint constraints ([#2382](https://github.com/stack-of-tasks/pinocchio/pull/2382)) + ### Changed - Use eigenpy to expose `GeometryObject::meshMaterial` variant ([#2315](https://github.com/stack-of-tasks/pinocchio/pull/2315)) - GepettoViewer is no more the default viewer for RobotWrapper ([#2331](https://github.com/stack-of-tasks/pinocchio/pull/2331)) diff --git a/cmake b/cmake index 91b8f5f216..b3c2af1b68 160000 --- a/cmake +++ b/cmake @@ -1 +1 @@ -Subproject commit 91b8f5f2168b123a198da079b8e6c09fd1f60285 +Subproject commit b3c2af1b68686dc9d5f459fb617647e37a15a76d diff --git a/include/pinocchio/multibody/constraint-base.hpp b/include/pinocchio/multibody/constraint-base.hpp deleted file mode 100644 index ea9880cb20..0000000000 --- a/include/pinocchio/multibody/constraint-base.hpp +++ /dev/null @@ -1,202 +0,0 @@ -// -// Copyright (c) 2015-2020 CNRS INRIA -// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. -// - -#ifndef __pinocchio_multibody_constraint_base_hpp__ -#define __pinocchio_multibody_constraint_base_hpp__ - -#include "pinocchio/macros.hpp" -#include "pinocchio/spatial/fwd.hpp" -#include "pinocchio/spatial/motion.hpp" -#include "pinocchio/spatial/act-on-set.hpp" - -#include - -// S : v \in M^6 -> v_J \in lie(Q) ~= R^nv -// S^T : f_J \in lie(Q)^* ~= R^nv -> f \in F^6 - -#define PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, TYPENAME) \ - typedef TYPENAME traits::Scalar Scalar; \ - typedef TYPENAME traits::JointMotion JointMotion; \ - typedef TYPENAME traits::JointForce JointForce; \ - typedef TYPENAME traits::DenseBase DenseBase; \ - typedef TYPENAME traits::MatrixReturnType MatrixReturnType; \ - typedef TYPENAME traits::ConstMatrixReturnType ConstMatrixReturnType; \ - enum \ - { \ - LINEAR = traits::LINEAR, \ - ANGULAR = traits::ANGULAR \ - }; \ - enum \ - { \ - Options = traits::Options \ - }; - -#define PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(DERIVED) \ - PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, typename) -#define PINOCCHIO_CONSTRAINT_TYPEDEF(DERIVED) \ - PINOCCHIO_CONSTRAINT_TYPEDEF_GENERIC(DERIVED, PINOCCHIO_EMPTY_ARG) - -namespace pinocchio -{ - - /// \brief Return type of the Constraint::Transpose * Force operation - template - struct ConstraintForceOp - { - typedef ReturnTypeNotDefined ReturnType; - }; - - /// \brief Return type of the Constraint::Transpose * ForceSet operation - template - struct ConstraintForceSetOp - { - typedef ReturnTypeNotDefined ReturnType; - }; - - template - class JointMotionSubspaceBase : public NumericalBase - { - protected: - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(Derived) - - public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - Derived & derived() - { - return *static_cast(this); - } - const Derived & derived() const - { - return *static_cast(this); - } - - template - JointMotion operator*(const Eigen::MatrixBase & vj) const - { - return derived().__mult__(vj); - } - - MatrixReturnType matrix() - { - return derived().matrix_impl(); - } - ConstMatrixReturnType matrix() const - { - return derived().matrix_impl(); - } - - int nv() const - { - return derived().nv_impl(); - } - - static int rows() - { - return 6; - } - int cols() const - { - return nv(); - } - - template - bool isApprox( - const JointMotionSubspaceBase & other, - const Scalar & prec = Eigen::NumTraits::dummy_precision()) const - { - return matrix().isApprox(other.matrix(), prec); - } - - void disp(std::ostream & os) const - { - derived().disp_impl(os); - } - friend std::ostream & operator<<(std::ostream & os, const JointMotionSubspaceBase & X) - { - X.disp(os); - return os; - } - - typename SE3GroupAction::ReturnType se3Action(const SE3Tpl & m) const - { - return derived().se3Action(m); - } - - typename SE3GroupAction::ReturnType - se3ActionInverse(const SE3Tpl & m) const - { - return derived().se3ActionInverse(m); - } - - template - typename MotionAlgebraAction::ReturnType - motionAction(const MotionDense & v) const - { - return derived().motionAction(v); - } - - bool operator==(const JointMotionSubspaceBase & other) const - { - return derived().isEqual(other.derived()); - } - - }; // class JointMotionSubspaceBase - - ///  \brief Operation Y * S used in the CRBA algorithm for instance - template - typename MultiplicationOp, ConstraintDerived>::ReturnType operator*( - const InertiaTpl & Y, - const JointMotionSubspaceBase & constraint) - { - return impl::LhsMultiplicationOp, ConstraintDerived>::run( - Y, constraint.derived()); - } - - ///  \brief Operation Y_matrix * S used in the ABA algorithm for instance - template - typename MultiplicationOp, ConstraintDerived>::ReturnType - operator*( - const Eigen::MatrixBase & Y, - const JointMotionSubspaceBase & constraint) - { - return impl::LhsMultiplicationOp, ConstraintDerived>::run( - Y.derived(), constraint.derived()); - } - - namespace details - { - template - struct StDiagonalMatrixSOperation - { - typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; - typedef typename traits::ReducedSquaredMatrix ReducedSquaredMatrix; - - static ReturnType run(const JointMotionSubspaceBase & /*constraint*/) - { - return ReducedSquaredMatrix::Identity(Constraint::NV, Constraint::NV); - } - }; - } // namespace details - - template - struct JointMotionSubspaceTransposeBase - { - typedef typename traits::StDiagonalMatrixSOperationReturnType - StDiagonalMatrixSOperationReturnType; - }; - - template - typename JointMotionSubspaceTransposeBase::StDiagonalMatrixSOperationReturnType - operator*( - const JointMotionSubspaceTransposeBase & /*S_transpose*/, - const JointMotionSubspaceBase & S) - { - return details::StDiagonalMatrixSOperation::run(S.derived()); - } - -} // namespace pinocchio - -#endif // ifndef __pinocchio_multibody_constraint_base_hpp__ diff --git a/include/pinocchio/multibody/constraint-generic.hpp b/include/pinocchio/multibody/constraint-generic.hpp deleted file mode 100644 index 7ef1026b50..0000000000 --- a/include/pinocchio/multibody/constraint-generic.hpp +++ /dev/null @@ -1,212 +0,0 @@ -// -// Copyright (c) 2015-2020 CNRS INRIA -// Copyright (c) 2016 Wandercraft, 86 rue de Paris 91400 Orsay, France. -// - -#ifndef __pinocchio_multibody_constraint_generic_hpp__ -#define __pinocchio_multibody_constraint_generic_hpp__ - -namespace pinocchio -{ - - template - struct traits> - { - typedef _Scalar Scalar; - enum - { - LINEAR = 0, - ANGULAR = 3, - Options = _Options, - Dim = _Dim - }; - - typedef MotionTpl JointMotion; - typedef Eigen::Matrix JointForce; - typedef Eigen::Matrix DenseBase; - typedef Eigen::Matrix ReducedSquaredMatrix; - - typedef typename PINOCCHIO_EIGEN_REF_CONST_TYPE(DenseBase) ConstMatrixReturnType; - typedef typename PINOCCHIO_EIGEN_REF_TYPE(DenseBase) MatrixReturnType; - - typedef ReducedSquaredMatrix StDiagonalMatrixSOperationReturnType; - }; // traits JointMotionSubspaceTpl - - template - struct SE3GroupAction> - { - typedef Eigen::Matrix ReturnType; - }; - - template - struct MotionAlgebraAction, MotionDerived> - { - typedef Eigen::Matrix ReturnType; - }; - - template - struct JointMotionSubspaceTpl - : public JointMotionSubspaceBase> - { - EIGEN_MAKE_ALIGNED_OPERATOR_NEW - - typedef JointMotionSubspaceBase Base; - - friend class JointMotionSubspaceBase; - PINOCCHIO_CONSTRAINT_TYPEDEF_TPL(JointMotionSubspaceTpl) - - enum - { - NV = _Dim - }; - - using Base::nv; - - template - explicit JointMotionSubspaceTpl(const Eigen::MatrixBase & _S) - : S(_S) - { - // There is currently a bug in Eigen/Core/util/StaticAssert.h in the use of the full namespace - // path - // TODO - EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(DenseBase, D); - } - - JointMotionSubspaceTpl() - : S() - { - EIGEN_STATIC_ASSERT( - _Dim != Eigen::Dynamic, YOU_CALLED_A_DYNAMIC_SIZE_METHOD_ON_A_FIXED_SIZE_MATRIX_OR_VECTOR) - } - - // It is only valid for dynamics size - explicit JointMotionSubspaceTpl(const int dim) - : S(6, dim) - { - EIGEN_STATIC_ASSERT( - _Dim == Eigen::Dynamic, YOU_CALLED_A_FIXED_SIZE_METHOD_ON_A_DYNAMIC_SIZE_MATRIX_OR_VECTOR) - } - - static JointMotionSubspaceTpl Zero(const int dim) - { - return JointMotionSubspaceTpl(dim); - } - - template - JointMotion __mult__(const Eigen::MatrixBase & vj) const - { - return JointMotion(S * vj); - } - - struct Transpose : JointMotionSubspaceTransposeBase - { - const JointMotionSubspaceTpl & ref; - Transpose(const JointMotionSubspaceTpl & ref) - : ref(ref) - { - } - - template - JointForce operator*(const ForceDense & f) const - { - return (ref.S.transpose() * f.toVector()).eval(); - } - - template - typename Eigen::Matrix operator*(const Eigen::MatrixBase & F) - { - return (ref.S.transpose() * F).eval(); - } - }; - - Transpose transpose() const - { - return Transpose(*this); - } - - MatrixReturnType matrix_impl() - { - return S; - } - ConstMatrixReturnType matrix_impl() const - { - return S; - } - - int nv_impl() const - { - return (int)S.cols(); - } - - template - friend typename JointMotionSubspaceTpl<_Dim, _Scalar, _Options>::DenseBase - operator*(const InertiaTpl & Y, const JointMotionSubspaceTpl & S) - { - typedef typename JointMotionSubspaceTpl::DenseBase ReturnType; - ReturnType res(6, S.nv()); - motionSet::inertiaAction(Y, S.S, res); - return res; - } - - template - friend Eigen::Matrix<_Scalar, 6, _Dim> - operator*(const Eigen::Matrix & Ymatrix, const JointMotionSubspaceTpl & S) - { - typedef Eigen::Matrix<_Scalar, 6, _Dim> ReturnType; - return ReturnType(Ymatrix * S.matrix()); - } - - DenseBase se3Action(const SE3Tpl & m) const - { - DenseBase res(6, nv()); - motionSet::se3Action(m, S, res); - return res; - } - - DenseBase se3ActionInverse(const SE3Tpl & m) const - { - DenseBase res(6, nv()); - motionSet::se3ActionInverse(m, S, res); - return res; - } - - template - DenseBase motionAction(const MotionDense & v) const - { - DenseBase res(6, nv()); - motionSet::motionAction(v, S, res); - return res; - } - - void disp_impl(std::ostream & os) const - { - os << "S =\n" << S << std::endl; - } - - bool isEqual(const JointMotionSubspaceTpl & other) const - { - return S == other.S; - } - - protected: - DenseBase S; - }; // class JointMotionSubspaceTpl - - namespace details - { - template - struct StDiagonalMatrixSOperation> - { - typedef JointMotionSubspaceTpl Constraint; - typedef typename traits::StDiagonalMatrixSOperationReturnType ReturnType; - - static ReturnType run(const JointMotionSubspaceBase & constraint) - { - return constraint.matrix().transpose() * constraint.matrix(); - } - }; - } // namespace details - -} // namespace pinocchio - -#endif // ifndef __pinocchio_multibody_constraint_generic_hpp__ diff --git a/include/pinocchio/serialization/joints-constraint.hpp b/include/pinocchio/serialization/joints-constraint.hpp deleted file mode 100644 index 46a3571be1..0000000000 --- a/include/pinocchio/serialization/joints-constraint.hpp +++ /dev/null @@ -1,143 +0,0 @@ -// -// Copyright (c) 2019-2020 INRIA -// - -#ifndef __pinocchio_serialization_joints_motion_subspace_hpp__ -#define __pinocchio_serialization_joints_motion_subspace_hpp__ - -#include "pinocchio/serialization/fwd.hpp" - -#include -#include - -namespace boost -{ - namespace serialization - { - - template - void serialize( - Archive & /*ar*/, - pinocchio::JointMotionSubspaceRevoluteTpl & /*S*/, - const unsigned int /*version*/) - { - } - - template - void serialize( - Archive & /*ar*/, - pinocchio::JointMotionSubspacePrismaticTpl & /*S*/, - const unsigned int /*version*/) - { - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspaceHelicalTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("h", S.h()); - } - - template - void serialize( - Archive & /*ar*/, - pinocchio::JointMotionSubspaceSphericalTpl & /*S*/, - const unsigned int /*version*/) - { - } - - template - void serialize( - Archive & /*ar*/, - pinocchio::JointMotionSubspaceTranslationTpl & /*S*/, - const unsigned int /*version*/) - { - } - - template - void serialize( - Archive & /*ar*/, - pinocchio::JointMotionSubspaceIdentityTpl & /*S*/, - const unsigned int /*version*/) - { - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspaceRevoluteUnalignedTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("axis", S.axis()); - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspacePrismaticUnalignedTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("axis", S.axis()); - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspaceHelicalUnalignedTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("axis", S.axis()); - ar & make_nvp("h", S.h()); - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspaceUniversalTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("angularSubspace", S.angularSubspace()); - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspaceTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("matrix", S.matrix()); - } - - template - void serialize( - Archive & ar, - pinocchio::ScaledJointMotionSubspace & S, - const unsigned int /*version*/) - { - ar & make_nvp("scaling", S.scaling()); - ar & make_nvp("constraint", S.constraint()); - } - - template - void serialize( - Archive & /*ar*/, - pinocchio::JointMotionSubspacePlanarTpl & /*S*/, - const unsigned int /*version*/) - { - } - - template - void serialize( - Archive & ar, - pinocchio::JointMotionSubspaceSphericalZYXTpl & S, - const unsigned int /*version*/) - { - ar & make_nvp("angularSubspace", S.angularSubspace()); - } - - } // namespace serialization -} // namespace boost - -#endif // ifndef __pinocchio_serialization_joints_motion_subspace_hpp__ diff --git a/sources.cmake b/sources.cmake index 991984996b..dda8d0d52d 100644 --- a/sources.cmake +++ b/sources.cmake @@ -153,8 +153,6 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/math/tensor.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/math/triangular-matrix.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/math/tridiagonal-matrix.hpp - ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/constraint-base.hpp - ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/constraint-generic.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/data.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/data.hxx ${PROJECT_SOURCE_DIR}/include/pinocchio/multibody/fcl.hpp @@ -236,7 +234,6 @@ set(${PROJECT_NAME}_CORE_PUBLIC_HEADERS ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/fwd.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/geometry.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/inertia.hpp - ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/joints-constraint.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/joints-data.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/joints.hpp ${PROJECT_SOURCE_DIR}/include/pinocchio/serialization/joints-model.hpp