diff --git a/CHANGELOG.md b/CHANGELOG.md index 60dac6c9d9..8ab5b11bc6 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -12,6 +12,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Fixed - Remove a lot of warnings ([#2139](https://github.com/stack-of-tasks/pinocchio/pull/2139)) - `MeshcatVisualizer` doesn't crash anymore when there is no collision model defined ([#2147](https://github.com/stack-of-tasks/pinocchio/pull/2147)) +- Fix MSVC build ([#2155](https://github.com/stack-of-tasks/pinocchio/pull/2155)) ### Added - Add `examples/floating-base-velocity-viewer.py` to visualize floating base velocity [#2143](https://github.com/stack-of-tasks/pinocchio/pull/2143) diff --git a/include/pinocchio/multibody/liegroup/special-euclidean.hpp b/include/pinocchio/multibody/liegroup/special-euclidean.hpp index bffd27ab37..04c4985bf9 100644 --- a/include/pinocchio/multibody/liegroup/special-euclidean.hpp +++ b/include/pinocchio/multibody/liegroup/special-euclidean.hpp @@ -362,10 +362,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH @@ -384,10 +384,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); MotionTpl nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; @@ -410,9 +410,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, - const Eigen::MatrixBase & J) const + const Eigen::MatrixBase & J) { Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH @@ -431,9 +431,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, - const Eigen::MatrixBase & J) const + const Eigen::MatrixBase & J) { Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J); MotionTpl nu; nu.toVector() << v.template head<2>(), 0, 0, 0, v[2]; @@ -470,16 +470,15 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void random_impl(const Eigen::MatrixBase & qout) const + static void random_impl(const Eigen::MatrixBase & qout) { R2crossSO2_t().random(qout); } template - void randomConfiguration_impl(const Eigen::MatrixBase & lower, + static void randomConfiguration_impl(const Eigen::MatrixBase & lower, const Eigen::MatrixBase & upper, const Eigen::MatrixBase & qout) - const { R2crossSO2_t ().randomConfiguration(lower, upper, qout); } @@ -581,9 +580,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP /// \cheatsheet \f$ \frac{\partial\ominus}{\partial q_1} {}^1X_0 = - \frac{\partial\ominus}{\partial q_0} \f$ template - void dDifference_impl (const Eigen::MatrixBase & q0, + static void dDifference_impl (const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, - const Eigen::MatrixBase & J) const + const Eigen::MatrixBase & J) { typedef typename SE3::Vector3 Vector3; typedef typename SE3::Matrix3 Matrix3; @@ -741,10 +740,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); Eigen::Matrix Jtmp6; @@ -759,10 +758,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH @@ -781,9 +780,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); Eigen::Matrix Jtmp6; @@ -799,9 +798,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); PINOCCHIO_COMPILER_DIAGNOSTIC_PUSH @@ -847,16 +846,15 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void random_impl (const Eigen::MatrixBase& qout) const + static void random_impl (const Eigen::MatrixBase& qout) { R3crossSO3_t().random(qout); } template - void randomConfiguration_impl(const Eigen::MatrixBase & lower, + static void randomConfiguration_impl(const Eigen::MatrixBase & lower, const Eigen::MatrixBase & upper, const Eigen::MatrixBase & qout) - const { R3crossSO3_t ().randomConfiguration(lower, upper, qout); } diff --git a/include/pinocchio/multibody/liegroup/special-orthogonal.hpp b/include/pinocchio/multibody/liegroup/special-orthogonal.hpp index b6e81b5918..50ecc72d65 100644 --- a/include/pinocchio/multibody/liegroup/special-orthogonal.hpp +++ b/include/pinocchio/multibody/liegroup/special-orthogonal.hpp @@ -143,9 +143,9 @@ namespace pinocchio } template - void dDifference_impl(const Eigen::MatrixBase & q0, + static void dDifference_impl(const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, - const Eigen::MatrixBase & J) const + const Eigen::MatrixBase & J) { Matrix2 R; // R0.transpose() * R1; R(0,0) = R(1,1) = q0.dot(q1); @@ -176,7 +176,7 @@ namespace pinocchio // See quaternion::firstOrderNormalize for equations. const Scalar norm2 = out.squaredNorm(); out *= (3 - norm2) / 2; - assert (isNormalized(out)); + assert (pinocchio::isNormalized(out)); } template @@ -237,32 +237,32 @@ namespace pinocchio } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & Jout) const + const Eigen::MatrixBase & Jout) { PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin; } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & Jout) const + const Eigen::MatrixBase & Jout) { PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin; } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, - const Eigen::MatrixBase & /*J*/) const {} + const Eigen::MatrixBase & /*J*/) {} template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, - const Eigen::MatrixBase & /*J*/) const {} + const Eigen::MatrixBase & /*J*/) {} @@ -316,7 +316,7 @@ namespace pinocchio } template - void random_impl (const Eigen::MatrixBase& qout) const + static void random_impl (const Eigen::MatrixBase& qout) { Config_t & out = PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout); @@ -326,9 +326,9 @@ namespace pinocchio } template - void randomConfiguration_impl(const Eigen::MatrixBase &, + static void randomConfiguration_impl(const Eigen::MatrixBase &, const Eigen::MatrixBase &, - const Eigen::MatrixBase & qout) const + const Eigen::MatrixBase & qout) { random_impl(qout); } @@ -387,9 +387,9 @@ namespace pinocchio } template - void dDifference_impl (const Eigen::MatrixBase & q0, + static void dDifference_impl (const Eigen::MatrixBase & q0, const Eigen::MatrixBase & q1, - const Eigen::MatrixBase & J) const + const Eigen::MatrixBase & J) { typedef typename SE3::Matrix3 Matrix3; @@ -510,10 +510,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { typedef typename SE3::Matrix3 Matrix3; JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); @@ -522,10 +522,10 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { typedef typename SE3::Matrix3 Matrix3; JacobianOut_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,J_out); @@ -538,9 +538,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { typedef typename SE3::Matrix3 Matrix3; Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); @@ -549,9 +549,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & v, - const Eigen::MatrixBase & J_out) const + const Eigen::MatrixBase & J_out) { typedef typename SE3::Matrix3 Matrix3; Jacobian_t & Jout = PINOCCHIO_EIGEN_CONST_CAST(Jacobian_t,J_out); @@ -610,7 +610,7 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void random_impl(const Eigen::MatrixBase & qout) const + static void random_impl(const Eigen::MatrixBase & qout) { QuaternionMap_t quat_map(PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).data()); quaternion::uniformRandom(quat_map); @@ -619,9 +619,9 @@ PINOCCHIO_COMPILER_DIAGNOSTIC_POP } template - void randomConfiguration_impl(const Eigen::MatrixBase &, + static void randomConfiguration_impl(const Eigen::MatrixBase &, const Eigen::MatrixBase &, - const Eigen::MatrixBase & qout) const + const Eigen::MatrixBase & qout) { random_impl(qout); } diff --git a/include/pinocchio/multibody/liegroup/vector-space.hpp b/include/pinocchio/multibody/liegroup/vector-space.hpp index a452817c14..0fe6afe476 100644 --- a/include/pinocchio/multibody/liegroup/vector-space.hpp +++ b/include/pinocchio/multibody/liegroup/vector-space.hpp @@ -95,12 +95,12 @@ namespace pinocchio } template - void dDifference_product_impl(const ConfigL_t &, + static void dDifference_product_impl(const ConfigL_t &, const ConfigR_t &, const JacobianIn_t & Jin, JacobianOut_t & Jout, bool, - const AssignmentOperatorType op) const + const AssignmentOperatorType op) { switch (op) { case SETTO: @@ -182,13 +182,13 @@ namespace pinocchio } template - void dIntegrate_product_impl(const Config_t &, + static void dIntegrate_product_impl(const Config_t &, const Tangent_t &, const JacobianIn_t & Jin, JacobianOut_t & Jout, bool, const ArgumentPosition, - const AssignmentOperatorType op) const + const AssignmentOperatorType op) { switch(op) { case SETTO: @@ -207,32 +207,32 @@ namespace pinocchio } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & Jout) const + const Eigen::MatrixBase & Jout) { PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin; } template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, const Eigen::MatrixBase & Jin, - const Eigen::MatrixBase & Jout) const + const Eigen::MatrixBase & Jout) { PINOCCHIO_EIGEN_CONST_CAST(JacobianOut_t,Jout) = Jin; } template - void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dq_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, - const Eigen::MatrixBase & /*J*/) const {} + const Eigen::MatrixBase & /*J*/) {} template - void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, + static void dIntegrateTransport_dv_impl(const Eigen::MatrixBase & /*q*/, const Eigen::MatrixBase & /*v*/, - const Eigen::MatrixBase & /*J*/) const {} + const Eigen::MatrixBase & /*J*/) {} @@ -251,7 +251,7 @@ namespace pinocchio } template - void random_impl (const Eigen::MatrixBase& qout) const + static void random_impl (const Eigen::MatrixBase& qout) { PINOCCHIO_EIGEN_CONST_CAST(Config_t,qout).setRandom(); }