pymomentum-cpu 0.1.93.post0__cp312-cp312-macosx_14_0_arm64.whl

include/momentum/character_solver/skeleton_solver_function.h

@@ -0,0 +1,94 @@
+/*
+ * 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/character.h>
+#include <momentum/character/fwd.h>
+#include <momentum/character/parameter_transform.h>
+#include <momentum/character/types.h>
+#include <momentum/character_solver/fwd.h>
+#include <momentum/solver/solver_function.h>
+#include <span>
+
+namespace momentum {
+
+template <typename T>
+class SkeletonSolverFunctionT : public SolverFunctionT<T> {
+ public:
+  SkeletonSolverFunctionT(
+      const Character& character,
+      const ParameterTransformT<T>& parameterTransform,
+      std::span<const std::shared_ptr<SkeletonErrorFunctionT<T>>> errorFunctions = {});
+  ~SkeletonSolverFunctionT() override;
+
+  double getError(const Eigen::VectorX<T>& parameters) final;
+
+  double getGradient(const Eigen::VectorX<T>& parameters, Eigen::VectorX<T>& gradient) final;
+
+  double getJacobian(
+      const Eigen::VectorX<T>& parameters,
+      Eigen::MatrixX<T>& jacobian,
+      Eigen::VectorX<T>& residual,
+      size_t& actualRows) final;
+
+  double getJtJR(
+      const Eigen::VectorX<T>& parameters,
+      Eigen::MatrixX<T>& jtj,
+      Eigen::VectorX<T>& jtr) override;
+
+  // overriding this to get a mix of JtJs and analytical Hessians from skeleton_ errorFunctions_
+  double getSolverDerivatives(
+      const Eigen::VectorX<T>& parameters,
+      Eigen::MatrixX<T>& hess,
+      Eigen::VectorX<T>& grad) override;
+
+  void updateParameters(Eigen::VectorX<T>& parameters, const Eigen::VectorX<T>& delta) final;
+  void setEnabledParameters(const ParameterSet& ps) final;
+
+  void addErrorFunction(std::shared_ptr<SkeletonErrorFunctionT<T>> solvable);
+  void clearErrorFunctions();
+
+  [[nodiscard]] const std::vector<std::shared_ptr<SkeletonErrorFunctionT<T>>>& getErrorFunctions()
+      const;
+
+  /// Provides access to the full character (replaces getSkeleton and getParameterTransform)
+  [[nodiscard]] const Character& getCharacter() const {
+    return character_;
+  }
+
+  /// Legacy compatibility (delegates to character_.skeleton)
+  [[nodiscard]] const Skeleton* getSkeleton() const {
+    return &character_.skeleton;
+  }
+
+  /// Provides access to the parameter transform
+  [[nodiscard]] const ParameterTransformT<T>* getParameterTransform() const {
+    return &parameterTransform_;
+  }
+
+  /// Updates mesh state if any error functions require it
+  void updateMeshState(const ModelParametersT<T>& parameters, const SkeletonStateT<T>& state);
+
+  /// Checks if any error functions require mesh state
+  [[nodiscard]] bool needsMeshState() const;
+
+ private:
+  const Character& character_;
+  const ParameterTransformT<T>& parameterTransform_;
+  std::unique_ptr<SkeletonStateT<T>> state_;
+  std::unique_ptr<MeshStateT<T>> meshState_;
+  bool needsMeshState_;
+  VectorX<bool> activeJointParams_;
+
+  Eigen::MatrixX<T> tJacobian_;
+  Eigen::VectorX<T> tResidual_;
+
+  std::vector<std::shared_ptr<SkeletonErrorFunctionT<T>>> errorFunctions_;
+};
+
+} // namespace momentum

include/momentum/character_solver/skinned_locator_error_function.h

@@ -0,0 +1,166 @@
+/*
+ * 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/skinned_locator.h>
+#include <momentum/character_solver/error_function_utils.h>
+#include <momentum/character_solver/fwd.h>
+#include <momentum/character_solver/plane_error_function.h>
+#include <momentum/character_solver/position_error_function.h>
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/math/fwd.h>
+
+namespace momentum {
+
+/// Constraint structure for skinned locator error function.
+///
+/// Represents a constraint that pulls a skinned locator towards a target position
+/// in world space with a specified weight.
+template <typename T>
+struct SkinnedLocatorConstraintT {
+  int locatorIndex = -1;
+  T weight = 1;
+  Eigen::Vector3<T> targetPosition;
+
+  template <typename T2>
+  SkinnedLocatorConstraintT<T2> cast() const {
+    return {
+        this->locatorIndex,
+        static_cast<T2>(this->weight),
+        this->targetPosition.template cast<T2>()};
+  }
+};
+
+/// Error function for skinned locator constraints.
+///
+/// This error function computes the error between skinned locator positions and their
+/// target positions. Skinned locators are points that are deformed by the character's
+/// skeleton and can be used to constrain the character to specific world positions.
+/// The error is computed as the weighted squared distance between the current skinned
+/// locator position and the target position.
+template <typename T>
+class SkinnedLocatorErrorFunctionT : public SkeletonErrorFunctionT<T> {
+ public:
+  /// Constructs a skinned locator error function for the given character.
+  explicit SkinnedLocatorErrorFunctionT(const Character& character_in);
+  ~SkinnedLocatorErrorFunctionT() override;
+
+  SkinnedLocatorErrorFunctionT(const SkinnedLocatorErrorFunctionT<T>& other) = delete;
+  SkinnedLocatorErrorFunctionT<T>& operator=(const SkinnedLocatorErrorFunctionT<T>& other) = delete;
+  SkinnedLocatorErrorFunctionT(SkinnedLocatorErrorFunctionT<T>&& other) = delete;
+  SkinnedLocatorErrorFunctionT<T>& operator=(SkinnedLocatorErrorFunctionT<T>&& other) = delete;
+
+  /// Computes the total error for all skinned locator constraints.
+  ///
+  /// @param modelParameters The current model parameters
+  /// @param state The current skeleton state
+  /// @return The sum of weighted squared distances between locators and targets
+  [[nodiscard]] double getError(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */) final;
+
+  /// Computes the gradient of the error function with respect to model parameters.
+  ///
+  /// @param modelParameters The current model parameters
+  /// @param state The current skeleton state
+  /// @param gradient Output gradient vector to be filled
+  /// @return The total error value
+  double getGradient(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */,
+      Eigen::Ref<Eigen::VectorX<T>> gradient) final;
+
+  /// Computes the Jacobian matrix and residual vector for the error function.
+  ///
+  /// @param modelParameters The current model parameters
+  /// @param state The current skeleton state
+  /// @param jacobian Output Jacobian matrix to be filled
+  /// @param residual Output residual vector to be filled
+  /// @param usedRows Number of rows used in the Jacobian (output parameter)
+  /// @return The total error value
+  double getJacobian(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& /* meshState */,
+      Eigen::Ref<Eigen::MatrixX<T>> jacobian,
+      Eigen::Ref<Eigen::VectorX<T>> residual,
+      int& usedRows) final;
+
+  /// Returns the number of rows needed for the Jacobian matrix.
+  [[nodiscard]] size_t getJacobianSize() const final;
+
+  /// Adds a new skinned locator constraint.
+  ///
+  /// @param locatorIndex Index of the skinned locator in the character
+  /// @param weight Weight for this constraint
+  /// @param targetPosition Target position in world space
+  void addConstraint(int locatorIndex, T weight, const Eigen::Vector3<T>& targetPosition);
+
+  /// Removes all constraints.
+  void clearConstraints();
+
+  /// Sets all constraints at once, replacing any existing constraints.
+  ///
+  /// @param constraints Vector of constraints to set
+  void setConstraints(const std::vector<SkinnedLocatorConstraintT<T>>& constraints);
+
+  /// Returns the current list of constraints.
+  [[nodiscard]] const std::vector<SkinnedLocatorConstraintT<T>>& getConstraints() const {
+    return constraints_;
+  }
+
+  /// Returns the character associated with this error function.
+  [[nodiscard]] const Character* getCharacter() const override {
+    return &character_;
+  }
+
+ private:
+  // Get the parameter index for a skinned locator, or -1 if not parameterized
+  [[nodiscard]] int getSkinnedLocatorParameterIndex(int locatorIndex) const {
+    if (locatorIndex <
+        static_cast<int>(character_.parameterTransform.skinnedLocatorParameters.size())) {
+      return character_.parameterTransform.skinnedLocatorParameters[locatorIndex];
+    }
+    return -1;
+  }
+
+  // Calculate the derivative of the world position with respect to the rest position
+  void calculateDWorldPos(
+      const SkeletonStateT<T>& state,
+      const SkinnedLocatorConstraintT<T>& constr,
+      const Eigen::Vector3<T>& d_restPos,
+      Eigen::Vector3<T>& d_worldPos) const;
+  double calculatePositionJacobian(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const SkinnedLocatorConstraintT<T>& constr,
+      Ref<Eigen::MatrixX<T>> jac,
+      Ref<Eigen::VectorX<T>> res) const;
+
+  double calculatePositionGradient(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const SkinnedLocatorConstraintT<T>& constr,
+      Eigen::Ref<Eigen::VectorX<T>> gradient) const;
+
+  // Calculate the skinned locator position in world space
+  Eigen::Vector3<T> calculateSkinnedLocatorPosition(
+      const SkeletonStateT<T>& state,
+      int locatorIndex,
+      const Eigen::Vector3<T>& locatorRestPos) const;
+  Eigen::Vector3<T> getLocatorRestPosition(const ModelParametersT<T>& modelParams, int locatorIndex)
+      const;
+
+  const Character& character_;
+
+  std::vector<SkinnedLocatorConstraintT<T>> constraints_;
+};
+
+} // namespace momentum

include/momentum/character_solver/skinned_locator_triangle_error_function.h

@@ -0,0 +1,146 @@
+/*
+ * 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/skinned_locator.h>
+#include <momentum/character_solver/error_function_utils.h>
+#include <momentum/character_solver/fwd.h>
+#include <momentum/character_solver/skeleton_error_function.h>
+#include <momentum/character_solver/vertex_error_function.h>
+#include <momentum/math/fwd.h>
+
+namespace momentum {
+
+template <typename T>
+struct SkinnedLocatorTriangleConstraintT {
+  int locatorIndex = -1;
+  Eigen::Vector3i tgtTriangleIndices;
+  Eigen::Vector3<T> tgtTriangleBaryCoords;
+  T depth = 0;
+  T weight = 1;
+
+  template <typename T2>
+  SkinnedLocatorTriangleConstraintT<T2> cast() const {
+    return {
+        this->locatorIndex,
+        this->tgtTriangleIndices,
+        this->tgtTriangleBaryCoords.template cast<T2>(),
+        static_cast<T2>(this->depth),
+        static_cast<T2>(this->weight)};
+  }
+};
+
+/// Support constraining a skinned locator to a location on the mesh defined by a target triangle
+/// and its normal. The target point is specified as sum_i (bary_i * target_triangle_vertex_i) +
+/// depth * target_triangle_normal. This constraint applies "forces" both to the skinned locator and
+/// every vertex in the mesh target triangle, so it will actually try to pull both toward each
+/// other.
+template <typename T>
+class SkinnedLocatorTriangleErrorFunctionT : public SkeletonErrorFunctionT<T> {
+ public:
+  explicit SkinnedLocatorTriangleErrorFunctionT(
+      const Character& character,
+      VertexConstraintType type = VertexConstraintType::Position);
+  ~SkinnedLocatorTriangleErrorFunctionT() override;
+
+  SkinnedLocatorTriangleErrorFunctionT(const SkinnedLocatorTriangleErrorFunctionT& other) = delete;
+  SkinnedLocatorTriangleErrorFunctionT& operator=(const SkinnedLocatorTriangleErrorFunctionT&) =
+      delete;
+  SkinnedLocatorTriangleErrorFunctionT(SkinnedLocatorTriangleErrorFunctionT&&) = delete;
+  SkinnedLocatorTriangleErrorFunctionT& operator=(SkinnedLocatorTriangleErrorFunctionT&&) = delete;
+
+  [[nodiscard]] double getError(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& meshState) final;
+
+  double getGradient(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& meshState,
+      Eigen::Ref<Eigen::VectorX<T>> gradient) final;
+
+  double getJacobian(
+      const ModelParametersT<T>& modelParameters,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& meshState,
+      Eigen::Ref<Eigen::MatrixX<T>> jacobian,
+      Eigen::Ref<Eigen::VectorX<T>> residual,
+      int& usedRows) final;
+
+  [[nodiscard]] size_t getJacobianSize() const final;
+
+  void addConstraint(
+      int locatorIndex,
+      const Eigen::Vector3i& triangleIndices,
+      const Eigen::Vector3<T>& triangleBaryCoords,
+      float depth = 0,
+      T weight = 1);
+  void clearConstraints();
+  void setConstraints(const std::vector<SkinnedLocatorTriangleConstraintT<T>>& constraints);
+
+  [[nodiscard]] const std::vector<SkinnedLocatorTriangleConstraintT<T>>& getConstraints() const {
+    return constraints_;
+  }
+
+  [[nodiscard]] const Character* getCharacter() const override {
+    return &character_;
+  }
+
+  /// Override to indicate this function requires mesh state
+  [[nodiscard]] bool needsMesh() const override {
+    return true;
+  }
+
+ private:
+  // Get the parameter index for a skinned locator, or -1 if not parameterized
+  [[nodiscard]] int getSkinnedLocatorParameterIndex(int locatorIndex) const {
+    if (locatorIndex <
+        static_cast<int>(character_.parameterTransform.skinnedLocatorParameters.size())) {
+      return character_.parameterTransform.skinnedLocatorParameters[locatorIndex];
+    }
+    return -1;
+  }
+
+  double calculatePositionJacobian(
+      const ModelParametersT<T>& modelParameters,
+      const MeshStateT<T>& meshState,
+      const SkinnedLocatorTriangleConstraintT<T>& constr,
+      Ref<Eigen::MatrixX<T>> jac,
+      Ref<Eigen::VectorX<T>> res) const;
+
+  double calculatePlaneJacobian(
+      const ModelParametersT<T>& modelParameters,
+      const MeshStateT<T>& meshState,
+      const SkinnedLocatorTriangleConstraintT<T>& constr,
+      Ref<Eigen::MatrixX<T>> jac,
+      T& res) const;
+
+  double calculatePositionGradient(
+      const ModelParametersT<T>& modelParameters,
+      const MeshStateT<T>& meshState,
+      const SkinnedLocatorTriangleConstraintT<T>& constr,
+      Eigen::Ref<Eigen::VectorX<T>> gradient) const;
+
+  double calculatePlaneGradient(
+      const ModelParametersT<T>& modelParameters,
+      const MeshStateT<T>& meshState,
+      const SkinnedLocatorTriangleConstraintT<T>& constr,
+      Eigen::Ref<Eigen::VectorX<T>> gradient) const;
+
+  Eigen::Vector3<T> getLocatorRestPosition(const ModelParametersT<T>& modelParams, int locatorIndex)
+      const;
+
+  const Character& character_;
+
+  std::vector<SkinnedLocatorTriangleConstraintT<T>> constraints_;
+
+  const VertexConstraintType constraintType_;
+};
+
+} // namespace momentum

include/momentum/character_solver/skinning_weight_iterator.h

@@ -0,0 +1,80 @@
+/*
+ * 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/fwd.h>
+#include <momentum/character/skin_weights.h>
+#include <momentum/math/fwd.h>
+
+namespace momentum {
+
+template <typename T>
+struct BoneWeightT {
+  size_t parentBone{};
+  float weight{};
+  Eigen::Vector3<T> weightedWorldSpacePoint;
+
+  bool operator<(const BoneWeightT<T>& rhs) const {
+    return parentBone < rhs.parentBone;
+  }
+
+  bool operator>(const BoneWeightT<T>& rhs) const {
+    return parentBone > rhs.parentBone;
+  }
+
+  BoneWeightT<T>& operator+=(const BoneWeightT<T>& rhs) {
+    this->weight += rhs.weight;
+    this->weightedWorldSpacePoint += rhs.weightedWorldSpacePoint;
+    return *this;
+  }
+};
+
+// When we compute the derivatives of skinned points, we usually
+// end up with a loop like this:
+//   for each skinned point i
+//     for each skinning bone and weight B_ij, w_ij
+//       for each ancestor k
+//          compute the weighted change in the skinned point wrt the ancestor
+// The redundancy here is that most points are skinned to a set of bones in the
+// same hierarchy (for example, two parts of the arm) and so it's redundant to
+// walk up the whole hierarchy from scratch for each skinned bone.
+//
+// This class instead does a single pass up the tree from leaf-most to root-most
+// skinned bones, accumulating skinning weights along the way.  By packaging it
+// in this class we hide the complexity and simplify the jacobian/gradient
+// calculation.
+template <typename T>
+class SkinningWeightIteratorT {
+ public:
+  SkinningWeightIteratorT(
+      const Character& character,
+      const MeshT<T>& restMesh,
+      const SkeletonStateT<T>& skelState,
+      Eigen::Index vertexIndex);
+
+  SkinningWeightIteratorT(
+      const Character& character,
+      const SkinnedLocator& locator,
+      const Eigen::Vector3<T>& locatorPosition,
+      const SkeletonStateT<T>& skelState);
+
+  [[nodiscard]] bool finished() const;
+
+  // Returns the tuple <parent bone index, bone weight, vertex position in world space wrt the
+  // current bone>
+  std::tuple<size_t, T, Eigen::Vector3<T>> next();
+
+ private:
+  void checkInvariants();
+
+  std::array<BoneWeightT<T>, kMaxSkinJoints> boneWeights;
+  int nBoneWeights;
+  const Character& character;
+};
+
+} // namespace momentum

include/momentum/character_solver/state_error_function.h

@@ -0,0 +1,119 @@
+/*
+ * 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/skeleton_state.h>
+#include <momentum/character_solver/fwd.h>
+#include <momentum/character_solver/skeleton_error_function.h>
+
+namespace momentum {
+
+/// Defines different methods for computing rotation error between two quaternions.
+enum class RotationErrorType {
+  /// Frobenius norm of rotation matrix difference: ||R1 - R2||_F^2
+  ///
+  /// This is the default method. It computes the squared Frobenius norm of the
+  /// difference between two rotation matrices. While not a geodesic distance,
+  /// it has smooth derivatives everywhere and is computationally efficient.
+  RotationMatrixDifference,
+
+  /// Logarithmic map of relative rotation: ||log(R1^{-1} * R2)||^2
+  ///
+  /// This method computes the squared norm of the logarithmic map of the relative
+  /// rotation quaternion. This gives the squared geodesic distance on SO(3), which
+  /// has a clear geometric interpretation. It uses numerically robust logmap
+  /// computation with Taylor series for small angles.
+  QuaternionLogMap,
+};
+
+template <typename T>
+class StateErrorFunctionT : public SkeletonErrorFunctionT<T> {
+ public:
+  StateErrorFunctionT(
+      const Skeleton& skel,
+      const ParameterTransform& pt,
+      RotationErrorType rotationErrorType = RotationErrorType::RotationMatrixDifference);
+  explicit StateErrorFunctionT(
+      const Character& character,
+      RotationErrorType rotationErrorType = RotationErrorType::RotationMatrixDifference);
+
+  [[nodiscard]] 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,
+      Eigen::Ref<Eigen::VectorX<T>> gradient) final;
+
+  double getJacobian(
+      const ModelParametersT<T>& params,
+      const SkeletonStateT<T>& state,
+      const MeshStateT<T>& meshState,
+      Eigen::Ref<Eigen::MatrixX<T>> jacobian,
+      Eigen::Ref<Eigen::VectorX<T>> residual,
+      int& usedRows) final;
+
+  [[nodiscard]] size_t getJacobianSize() const final;
+
+  void reset();
+  void setTargetState(const SkeletonStateT<T>* target);
+  void setTargetState(const SkeletonStateT<T>& target);
+  void setTargetState(TransformListT<T> target);
+  void setTargetWeight(const Eigen::VectorX<T>& weights);
+  void setTargetWeights(const Eigen::VectorX<T>& posWeight, const Eigen::VectorX<T>& rotWeight);
+
+  /// Set the target position weights for each joint.
+  /// @param posWeight Per-joint position weights.
+  void setPositionTargetWeights(const Eigen::VectorX<T>& posWeight);
+
+  /// Set the target rotation weights for each joint.
+  /// @param rotWeight Per-joint rotation weights.
+  void setRotationTargetWeights(const Eigen::VectorX<T>& rotWeight);
+
+  void setWeights(const float posWeight, const float rotationWeight) {
+    posWgt_ = posWeight;
+    rotWgt_ = rotationWeight;
+  }
+
+  [[nodiscard]] const TransformListT<T>& getTargetState() const {
+    return this->targetState_;
+  }
+
+  [[nodiscard]] const Eigen::VectorX<T>& getPositionWeights() const {
+    return targetPositionWeights_;
+  }
+  [[nodiscard]] const Eigen::VectorX<T>& getRotationWeights() const {
+    return targetRotationWeights_;
+  }
+  [[nodiscard]] const T& getPositionWeight() const {
+    return posWgt_;
+  }
+  [[nodiscard]] const T& getRotationWeight() const {
+    return rotWgt_;
+  }
+
+ private:
+  TransformListT<T> targetState_;
+  Eigen::VectorX<T> targetPositionWeights_;
+  Eigen::VectorX<T> targetRotationWeights_;
+
+  T posWgt_;
+  T rotWgt_;
+
+  const RotationErrorType rotationErrorType_;
+
+ public:
+  // weights for the error functions
+  static constexpr T kPositionWeight = 1e-3f;
+  static constexpr T kOrientationWeight = 1e+0f;
+};
+
+} // namespace momentum

include/momentum/character_solver/transform_pose.h

@@ -0,0 +1,80 @@
+/*
+ * 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/character.h>
+#include <momentum/character_solver/fwd.h>
+#include <momentum/math/random.h>
+#include <momentum/math/transform.h>
+#include <momentum/math/types.h>
+#include <momentum/solver/fwd.h>
+
+namespace momentum {
+
+/// Function which computes a new set of model parameters such that the character pose is a rigidly
+/// transformed version of the original pose.  While there is technically a closed form solution
+/// for any given skeleton, this is complicated in momentum because different
+/// characters attach the rigid parameters to different joints, so a fully general solution uses IK.
+/// However, getting it right requires dealing with local minima issues in Euler angles and other
+/// challenges.  So centralizing this functionality in a single place is useful.
+///
+/// @param[in] ensureContinuousOutput If true, the output model parameters will be solved
+/// sequentially so that the resulting output is continuous where possible.  This helps to avoid
+/// issues with Euler flipping.
+template <typename T>
+std::vector<ModelParametersT<T>> transformPose(
+    const Character& character,
+    const std::vector<ModelParametersT<T>>& modelParameters,
+    const std::vector<TransformT<T>>& transforms,
+    bool ensureContinuousOutput = true);
+
+template <typename T>
+std::vector<ModelParametersT<T>> transformPose(
+    const Character& character,
+    const std::vector<ModelParametersT<T>>& modelParameters,
+    const TransformT<T>& transforms);
+
+/// Solver which can be reused to repeatedly apply the transformPose function listed above.
+template <typename T>
+class PoseTransformSolverT {
+ public:
+  explicit PoseTransformSolverT(const Character& character);
+  ~PoseTransformSolverT();
+
+  /// Applies a rigid transform to the given model parameters.
+  /// @param[in,out] modelParameters The model parameters to be transformed.
+  /// @param[in] transform The transform to apply.
+  /// @param[in] modelParametersPrev The previous pose, which is used to ensure continuity (the
+  /// solver will initialize using the previous frame's rigid parameters).
+  void transformPose(
+      ModelParametersT<T>& modelParameters,
+      const TransformT<T>& transform,
+      const ModelParametersT<T>& modelParametersPrev = ModelParametersT<T>{});
+
+ private:
+  Eigen::Index numModelParametersFull_;
+
+  momentum::Character characterSimplified_;
+  momentum::ParameterTransformT<T> parameterTransformSimplified_;
+  size_t rootJointSimplified_;
+  momentum::ParameterSet rigidParametersSimplified_;
+  std::vector<size_t> simplifiedParamToFullParamIdx_;
+
+  momentum::ModelParametersT<T> solvedParametersSimplified_;
+  momentum::SkeletonStateT<T> skelStateSimplified_;
+
+  std::shared_ptr<momentum::PositionErrorFunctionT<T>> positionError_;
+  std::shared_ptr<momentum::OrientationErrorFunctionT<T>> orientationError_;
+
+  std::unique_ptr<momentum::SkeletonSolverFunctionT<T>> solverFunction_;
+  std::unique_ptr<momentum::SolverT<T>> solver_;
+
+  momentum::Random<> rng_;
+};
+
+} // namespace momentum