pymomentum-cpu 0.1.77.post30__cp313-cp313-manylinux_2_39_x86_64.whl

include/momentum/diff_ik/fully_differentiable_orientation_error_function.h

@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) Meta Platforms, Inc. and affiliates.
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+#pragma once
+
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/diff_ik/fully_differentiable_skeleton_error_function.h>
+#include <momentum/diff_ik/fwd.h>
+
+#include <vector>
+
+namespace momentum {
+
+template <typename T>
+struct OrientationConstraintT {
+  Quaternion<T> offset; ///< Constraint rotation offset in local joint space
+  Quaternion<T> target; ///< Target orientation in global space
+  size_t parent; ///< Parent joint index this constraint is under
+  float weight; ///< Weight of the constraint
+  std::string name; ///< Name of the constraint
+
+  explicit OrientationConstraintT(
+      const Quaternion<T>& offset,
+      const Quaternion<T>& target,
+      size_t parent,
+      float w,
+      const std::string& n = "")
+      : offset(offset), target(target), parent(parent), weight(w), name(n) {
+    // Empty
+  }
+};
+
+template <typename T>
+class FullyDifferentiableOrientationErrorFunctionT
+    : public FullyDifferentiableSkeletonErrorFunctionT<T>,
+      public SkeletonErrorFunctionT<T> {
+ public:
+  static constexpr T kOrientationWeight = 1e-1f;
+  static constexpr const char* kParents = "parents";
+  static constexpr const char* kOffsets = "offsets";
+  static constexpr const char* kWeights = "weights";
+  static constexpr const char* kTargets = "targets";
+
+  FullyDifferentiableOrientationErrorFunctionT(const Skeleton& skel, const ParameterTransform& pt);
+  ~FullyDifferentiableOrientationErrorFunctionT() override = default;
+
+  [[nodiscard]] std::vector<std::string> inputs() const final;
+  [[nodiscard]] Eigen::Index getInputSize(const std::string& name) const final;
+  [[nodiscard]] const char* name() const final {
+    return "OrientationErrorFunction";
+  }
+
+  Eigen::VectorX<T> d_gradient_d_input_dot(
+      const std::string& inputName,
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) final;
+
+  double getError(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */) final;
+
+  double getGradient(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */,
+      Ref<Eigen::VectorX<T>> gradient) final;
+
+  double getJacobian(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */,
+      Ref<Eigen::MatrixX<T>> jacobian,
+      Ref<Eigen::VectorX<T>> residual,
+      int& usedRows) final;
+
+  [[nodiscard]] size_t getJacobianSize() const final;
+
+  void addConstraint(const OrientationConstraintT<T>& constr);
+
+ private:
+  void getInputImp(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> value) const final;
+  void setInputImp(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value) final;
+
+  T calculateOrientationGradient(
+      const SkeletonStateT<T>& state,
+      const OrientationConstraintT<T>& constr,
+      Eigen::VectorX<T>& jGrad) const;
+
+  template <typename JetType>
+  JetType calculateOrientationGradient_dot(
+      const SkeletonStateT<T>& state,
+      size_t constrParent,
+      const JetType& constrWeight,
+      const Eigen::Quaternion<JetType>& constrOrientationOffset,
+      const Eigen::Quaternion<JetType>& constrOrientationTarget,
+      Eigen::Ref<const Eigen::VectorX<T>> vec) const;
+
+  [[nodiscard]] T calculateOrientationJacobian(
+      const SkeletonStateT<T>& state,
+      const OrientationConstraintT<T>& constr,
+      Ref<Eigen::MatrixX<T>> jac,
+      Ref<Eigen::VectorX<T>> res) const;
+
+  std::vector<OrientationConstraintT<T>> constraints_;
+  Eigen::VectorX<T> jointGrad_;
+};
+
+} // namespace momentum

include/momentum/diff_ik/fully_differentiable_pose_prior_error_function.h

@@ -0,0 +1,76 @@
+/*
+ * Copyright (c) Meta Platforms, Inc. and affiliates.
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+#pragma once
+
+#include <momentum/character_solver/pose_prior_error_function.h>
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/diff_ik/fully_differentiable_skeleton_error_function.h>
+#include <momentum/diff_ik/fwd.h>
+
+#include <Eigen/Core>
+
+namespace momentum {
+
+template <typename T>
+class FullyDifferentiablePosePriorErrorFunctionT
+    : public FullyDifferentiableSkeletonErrorFunctionT<T>,
+      public PosePriorErrorFunctionT<T> {
+ public:
+  FullyDifferentiablePosePriorErrorFunctionT(
+      const Skeleton& skel,
+      const ParameterTransform& pt,
+      std::vector<std::string> names);
+  ~FullyDifferentiablePosePriorErrorFunctionT() override;
+
+  void setPosePrior(
+      const VectorX<T>& pi,
+      const MatrixX<T>& mmu,
+      const std::vector<MatrixX<T>>& W,
+      const VectorX<T>& sigma);
+
+  static constexpr const char* kPi = "pi";
+  static constexpr const char* kMu = "mu";
+  static constexpr const char* kW = "W";
+  static constexpr const char* kSigma = "sigma";
+  static constexpr const char* kParameterIndices = "parameter_indices";
+
+  [[nodiscard]] std::vector<std::string> inputs() const final;
+  [[nodiscard]] Eigen::Index getInputSize(const std::string& name) const final;
+  [[nodiscard]] const char* name() const final {
+    return "MarkerErrorFunction";
+  }
+
+  Eigen::VectorX<T> d_gradient_d_input_dot(
+      const std::string& inputName,
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) final;
+
+ protected:
+  void getInputImp(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> value) const final;
+  void setInputImp(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value) final;
+
+  // pi_ are the mixture weights
+  Eigen::VectorX<T> pi_;
+
+  // The columns of mu are the means for the different mixtures
+  Eigen::MatrixX<T> mmu_;
+
+  // Each matrix W is the basis for a mixture:
+  std::vector<Eigen::MatrixX<T>> W_;
+
+  // The covariance matrix is (sigma^2 * I + W^T * W)^{-1}
+  // Note that this is sigma, not sigma squared, because the scaling should
+  // be more reasonable (sigma is the same order of magnitude as the mu and
+  // W arrays).
+  Eigen::VectorX<T> sigma_;
+
+  const std::vector<std::string> names_;
+};
+
+} // namespace momentum

include/momentum/diff_ik/fully_differentiable_position_error_function.h

@@ -0,0 +1,138 @@
+/*
+ * Copyright (c) Meta Platforms, Inc. and affiliates.
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+#pragma once
+
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/diff_ik/fully_differentiable_skeleton_error_function.h>
+#include <momentum/diff_ik/fwd.h>
+
+#include <vector>
+
+namespace momentum {
+
+template <typename T>
+struct PositionConstraintT {
+  Vector3<T> offset; ///< Positional offset in the parent joint space
+  Vector3<T> target; ///< Target position
+  size_t parent; ///< Parent joint index this constraint is under
+  float weight; ///< Weight of the constraint
+  std::string name; ///< Name of the constraint
+
+  explicit PositionConstraintT(
+      const Vector3<T>& offset,
+      const Vector3<T>& target,
+      size_t parent,
+      float w,
+      const std::string& n = "")
+      : offset(offset), target(target), parent(parent), weight(w), name(n) {
+    // Empty
+  }
+};
+
+template <typename T>
+struct PositionConstraintStateT {
+  std::vector<Eigen::Vector3<T>>
+      position; // the current position of all locators according to a given skeleton state
+
+  PositionConstraintStateT() = default;
+
+  PositionConstraintStateT(
+      const SkeletonStateT<T>& skeletonState,
+      const std::vector<PositionConstraintT<T>>& referenceConstraints) {
+    update(skeletonState, referenceConstraints);
+  }
+
+  void update(
+      const SkeletonStateT<T>& skeletonState,
+      const std::vector<PositionConstraintT<T>>& referenceConstraints);
+};
+
+template <typename T>
+class FullyDifferentiablePositionErrorFunctionT
+    : public FullyDifferentiableSkeletonErrorFunctionT<T>,
+      public SkeletonErrorFunctionT<T> {
+ public:
+  static constexpr T kPositionWeight = 1e-4f;
+  static constexpr const char* kParents = "parents";
+  static constexpr const char* kOffsets = "offsets";
+  static constexpr const char* kWeights = "weights";
+  static constexpr const char* kTargets = "targets";
+
+  FullyDifferentiablePositionErrorFunctionT(const Skeleton& skel, const ParameterTransform& pt);
+  ~FullyDifferentiablePositionErrorFunctionT() override = default;
+
+  [[nodiscard]] std::vector<std::string> inputs() const final;
+  [[nodiscard]] Eigen::Index getInputSize(const std::string& name) const final;
+  [[nodiscard]] const char* name() const final {
+    return "PositionErrorFunction";
+  }
+
+  Eigen::VectorX<T> d_gradient_d_input_dot(
+      const std::string& inputName,
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) final;
+
+  double getError(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */) final;
+
+  double getGradient(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */,
+      Ref<Eigen::VectorX<T>> gradient) final;
+
+  double getJacobian(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */,
+      Ref<Eigen::MatrixX<T>> jacobian,
+      Ref<Eigen::VectorX<T>> residual,
+      int& usedRows) final;
+
+  [[nodiscard]] size_t getJacobianSize() const final;
+
+  void addConstraint(const PositionConstraintT<T>& constr);
+
+  [[nodiscard]] const std::vector<PositionConstraintT<T>>& getConstraints() const;
+
+ private:
+  void getInputImp(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> value) const final;
+  void setInputImp(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value) final;
+
+  T calculatePositionGradient(
+      const SkeletonStateT<T>& state,
+      const PositionConstraintT<T>& constr,
+      const Eigen::Vector3<T>& pos,
+      Eigen::VectorX<T>& jGrad) const;
+
+  template <typename JetType>
+  [[nodiscard]] JetType calculatePositionGradient_dot(
+      const SkeletonStateT<T>& state,
+      size_t iConstr,
+      size_t constrParent,
+      const JetType& constrWeight,
+      const Eigen::Vector3<JetType>& constr_offset,
+      const Eigen::Vector3<JetType>& constr_target,
+      Eigen::Ref<const Eigen::VectorX<T>> vec) const;
+
+  [[nodiscard]] T calculatePositionJacobian(
+      const SkeletonStateT<T>& state,
+      const PositionConstraintT<T>& constr,
+      const Eigen::Vector3<T>& pos,
+      Ref<Eigen::MatrixX<T>> jac,
+      Ref<Eigen::VectorX<T>> res) const;
+
+  std::vector<PositionConstraintT<T>> constraints_;
+  PositionConstraintStateT<T> constraintsState_;
+  Eigen::VectorX<T> jointGrad_;
+};
+
+} // namespace momentum

include/momentum/diff_ik/fully_differentiable_projection_error_function.h

@@ -0,0 +1,65 @@
+/*
+ * Copyright (c) Meta Platforms, Inc. and affiliates.
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+#pragma once
+#include <momentum/character/locator.h>
+#include <momentum/character_solver/projection_error_function.h>
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/diff_ik/fully_differentiable_skeleton_error_function.h>
+#include <momentum/diff_ik/fwd.h>
+
+namespace momentum {
+
+MOMENTUM_FWD_DECLARE_TEMPLATE_CLASS(FullyDifferentiableProjectionErrorFunction);
+
+template <typename T>
+class FullyDifferentiableProjectionErrorFunctionT
+    : public momentum::ProjectionErrorFunctionT<T>,
+      public virtual momentum::FullyDifferentiableSkeletonErrorFunctionT<T> {
+ public:
+  static constexpr const char* kParents = "parents";
+  static constexpr const char* kOffsets = "offsets";
+  static constexpr const char* kWeights = "weights";
+  static constexpr const char* kTargets = "targets";
+  static constexpr const char* kProjections = "projections";
+
+  FullyDifferentiableProjectionErrorFunctionT(
+      const momentum::Skeleton& skel,
+      const momentum::ParameterTransform& pt,
+      T nearClip = T(1));
+
+  [[nodiscard]] std::vector<std::string> inputs() const override;
+  [[nodiscard]] Eigen::Index getInputSize(const std::string& name) const override;
+  void getInputImp(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> result) const override;
+  void setInputImp(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value) override;
+  [[nodiscard]] const char* name() const override {
+    return "ProjectionErrorFunction";
+  }
+
+  Eigen::VectorX<T> d_gradient_d_input_dot(
+      const std::string& inputName,
+      const momentum::ModelParametersT<T>& modelParams,
+      const momentum::SkeletonStateT<T>& state,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) override;
+
+ protected:
+  template <typename JetType>
+  JetType constraintGradient_dot(
+      const momentum::ModelParametersT<T>& modelParams,
+      const momentum::SkeletonStateT<T>& skelState,
+      int parentJointIndex_cons,
+      const Eigen::Vector3<JetType>& offset_cons,
+      const JetType& weight_cons,
+      const Eigen::Matrix<JetType, 3, 4>& projection_cons,
+      const Eigen::Vector2<JetType>& target_cons,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) const;
+
+  using momentum::ProjectionErrorFunctionT<T>::_nearClip;
+  using momentum::ProjectionErrorFunctionT<T>::kProjectionWeight;
+};
+
+} // namespace momentum

include/momentum/diff_ik/fully_differentiable_skeleton_error_function.h

@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) Meta Platforms, Inc. and affiliates.
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+#pragma once
+
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/diff_ik/fwd.h>
+
+#include <Eigen/SparseCore>
+#include <vector>
+
+namespace momentum {
+
+// An error function which can compute the gradients wrt all its inputs.
+// For example, a "normal" position error function can compute the gradient
+// of the error wrt the solved modelParameters.  A "FullyDifferentiable"
+// error function also knows how to compute the derivative of the gradient
+// wrt the per-constraint weights, the per-constraint targets, etc.
+//
+// To make this work, we need a mechanism to specify which inputs we can
+// differentiate wrt.  Thus the inputs()/getInput()/setInput() stuff below.
+// This design is a bit "string-heavy" but ensures that we can use the various
+// derivatives in code that doesn't know a priori which inputs a given
+// constraint has.
+//
+// Note that the resulting inputs are all specified as Eigen::VectorXf.  This
+// tends to mean that the resulting error functions are going to operate on
+// big homogenous Tensor types instead of the mixed-struct types like
+// MarkerErrorFunction uses.
+template <typename T>
+class FullyDifferentiableSkeletonErrorFunctionT {
+ public:
+  virtual ~FullyDifferentiableSkeletonErrorFunctionT() = default;
+
+  [[nodiscard]] virtual const char* name() const = 0;
+
+  // Get a list of all the differentiable inputs for this error function
+  // (excluding the modelParameters).
+  [[nodiscard]] virtual std::vector<std::string> inputs() const = 0;
+
+  // Get the current input values.  A given input will be unrolled into a flat vector,
+  // so, for example, if the input was an n x 3 vector of constraint targets, getInput
+  // would return a (3n x 1) VectorXf with the targets packed (t1x t1y t1z t2x t2y t2z ...)
+  [[nodiscard]] Eigen::VectorX<T> getInput(const std::string& name) const;
+  void getInput(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> value) const;
+
+  // Efficiently get the size of the input (e.g., getInput(name).size()).
+  [[nodiscard]] virtual Eigen::Index getInputSize(const std::string& name) const = 0;
+
+  // Set the current input values:
+  void setInput(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value);
+
+  // Compute the quantity
+  //  d/dInput(dE/dTheta) . v
+  // where dE/dTheta is the gradient of the energy computed in getGradient.
+  // By applying the dot product inside this function we avoid dealing
+  // with tensor types, and it is possible to compute very efficiently
+  // in reverse-mode differentiation such as used by PyTorch.
+  //
+  // Note that because v is a constant here, we can rewrite this as
+  //   d/dInput(dE/dTheta . v)
+  // Now it's a lot easier to understand (it's the derivative wrt the given input of a scalar,
+  // no tensors required) and very amenable to autodiff as described below.
+  virtual Eigen::VectorX<T> d_gradient_d_input_dot(
+      const std::string& inputName,
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) = 0;
+
+ protected:
+  virtual void getInputImp(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> value) const = 0;
+  virtual void setInputImp(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value) = 0;
+
+  // Notes on implementing d_gradient_d_input_dot: After some experimentation, I have settled on the
+  // following procedure for implementing the d_gradient_d_input_dot() function.  By using ceres
+  // autodiff, we can compute the derivatives for all the inputs without having to hand-implement
+  // them all.
+  //
+  // Note that I spent some time trying to convince ceres autodiff to compute both getGradient() and
+  // d_gradient_d_input_dot() straight from the error function; while I still think that might be
+  // possible it gets really nasty.  This is partly because Ceres isn't really engineered to deal
+  // with second derivatives (you can do it with jets of jets but the tooling gets pretty poor) and
+  // partly because getting it to respect the right sparsity patterns gets really complicated.
+  //
+  // 1. For a typical constraint like a projection constraint, the inputs are sparse; that is,
+  //    a given input only affects a few rows of the residual.  For example: a projection constraint
+  //    returns a (2n x 1) residual, but a given block of two is only affected by a single weight
+  //    w_i, a single offset o_i, and a single target t_i.
+  // 2. Split the gradient function out into a separate function for the ith term of the residual.
+  // Example:
+  //      void getGradient(...) {
+  //        for (int i = 0; i < constraints.size(); ++i) {
+  //          getConstraintGradient(constraints[i], gradient);
+  //        }
+  //      }
+  //      void getContraintGradient(const Constraint& cons, Eigen::Ref<Eigen::VectorX<T>> gradient)
+  //      {
+  //         ...
+  //      }
+  //    See e.g. MarkerErrorFunction for an example.
+  // 3. Copy the getConstraintGradient function and make the following changes:
+  //      a. Make it template <typename T>, where T will be a ceres::Jet<float, N>.
+  //      b. Turn it into getConstraintGradient_dot(..., Eigen::Ref<const Eigen::VectorX<T>> vec).
+  //         You will be applying the dot of the computed gradient against the vec as you go, and
+  //         the ceres::Jet types will accumulate the derivatives.
+  //      c. Pass all the constraint inputs in explicitly, templated on T.  That is, the
+  //         getConstraintGradient() function above becomes:
+  //         template <typename T>
+  //         void getConstraintGradient_dot(const T cons_weight,
+  //                                        const Vector3<T> cons_target,
+  //                                        Eigen::Ref<const Eigen::VectorX<T>> vec)
+  //      d. Replace all the Vector3f/float with Vector3<T>/T or just use auto and .eval() a lot.
+  //         Be wary of Eigen's use of temporaries.
+  //              https://eigen.tuxfamily.org/dox/TopicPitfalls.html
+  //         This is bad due to Eigen temporaries:
+  //           auto foo = bar.cross(baz).normalized();
+  //         This is okay:
+  //           auto foo = bar.cross(baz).normalized().eval();
+  //      e. Replace jointState.getRotationDerivative(...) with getRotationDerivative(jointState,
+  //      ...) and
+  //         jointState.getScaleDerivative(...) with getScaleDerivative(jointState, ...) from
+  //         ceres_utility.h.
+  //      f. Add the (grad . vec) dot product in the appropriate spot.  There is a function
+  //         times_parameterTransform_times_v() in CeresUtility too apply the dot product if your
+  //         function computes the gradient wrt the joint parameters instead of the model
+  //         parameters.
+  //             auto result = T();
+  //             for (size_t d = 0; d < 3; d++) {
+  //             {
+  //                 const float grad_jointParam = ...
+  //                 result += times_parametersTransform_times_v(grad_jointParam, jointIndex *
+  //                 kParametersPerJoint + d, vec)
+  //             }
+  //
+  // 4. Now, implement your d_gradient_d_input_dot() function.
+  //       if(input == "target") {
+  //           Eigen::VectorX<T> result(constraints.size()*3);
+  //           for (int iCons = 0; iCons < constraints.size(); ++iCons)
+  //           {
+  //               typedef ceres::Jet<float,3> JetType;
+  //               const auto& cons = constraints[iCons];
+  //               // We don't actually care about the value of the dot product here, we only
+  //               // care about the derivative, which is stored in the Jet dual number .v.
+  //               auto dotProd = getConstraintGradient_dot<JetType>(JetType(cons.weight),
+  //               buildJetVec(cons.target), vec); result.segment<3>(3*iCons) = dotProd.v;
+  //           }
+  //       }
+  //    The tricky part is deciding what you're differentiating wrt to.  The variables that are
+  //    constant can get converted to jets in this way:
+  //        v.cast<JetType>(), JetType(c)
+  //    The variables that we _are_ differentiating wrt need to be converted to unit jets, like
+  //    this:
+  //        buildJetVec(v), JetType(c, 0)
+};
+
+} // namespace momentum

include/momentum/diff_ik/fully_differentiable_state_error_function.h

@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) Meta Platforms, Inc. and affiliates.
+ *
+ * This source code is licensed under the MIT license found in the
+ * LICENSE file in the root directory of this source tree.
+ */
+
+#pragma once
+
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/character_solver/state_error_function.h>
+#include <momentum/diff_ik/fully_differentiable_skeleton_error_function.h>
+#include <momentum/diff_ik/fwd.h>
+
+#include <Eigen/Core>
+
+namespace momentum {
+
+template <typename T>
+class FullyDifferentiableStateErrorFunctionT : public FullyDifferentiableSkeletonErrorFunctionT<T>,
+                                               public StateErrorFunctionT<T> {
+ public:
+  FullyDifferentiableStateErrorFunctionT(const Skeleton& skel, const ParameterTransform& pt);
+  ~FullyDifferentiableStateErrorFunctionT() override;
+
+  [[nodiscard]] std::vector<std::string> inputs() const final;
+  [[nodiscard]] Eigen::Index getInputSize(const std::string& name) const final;
+  [[nodiscard]] const char* name() const final {
+    return "StateErrorFunction";
+  }
+
+  Eigen::VectorX<T> d_gradient_d_input_dot(
+      const std::string& inputName,
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      Eigen::Ref<const Eigen::VectorX<T>> inputVec) final;
+
+ protected:
+  void getInputImp(const std::string& name, Eigen::Ref<Eigen::VectorX<T>> value) const final;
+  void setInputImp(const std::string& name, Eigen::Ref<const Eigen::VectorX<T>> value) final;
+
+ private:
+  template <typename JetType>
+  JetType calculateGradient_dot(
+      const SkeletonStateT<T>& state,
+      size_t iJoint,
+      const Eigen::Vector3<JetType>& targetTranslation,
+      const Eigen::Quaternion<JetType>& targetRotation,
+      const JetType& targetPositionWeight,
+      const JetType& targetRotationWeight,
+      Eigen::Ref<const Eigen::VectorX<T>> vec);
+};
+
+} // namespace momentum