pymomentum-cpu 0.1.82.post0__cp313-cp313-macosx_14_0_arm64.whl

include/momentum/character/skeleton_state.h

@@ -0,0 +1,181 @@
+/*
+ * 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/joint_state.h>
+#include <momentum/character/types.h>
+#include <momentum/math/types.h>
+
+namespace momentum {
+
+/// Measures of similarity between two skeleton states
+///
+/// Contains both per-joint error metrics and aggregate statistics
+struct StateSimilarity {
+  /// Position error for each joint (in distance units)
+  VectorXf positionError;
+
+  /// Orientation error for each joint (in radians)
+  VectorXf orientationError;
+
+  /// Root mean square of position errors across all joints
+  float positionRMSE;
+
+  /// Root mean square of orientation errors across all joints
+  float orientationRMSE;
+
+  /// Maximum position error across all joints
+  float positionMax;
+
+  /// Maximum orientation error across all joints
+  float orientationMax;
+};
+
+/// Represents the complete state of a skeleton
+///
+/// Stores joint parameters and computed joint states for an entire skeleton.
+/// Provides methods for updating the state based on new parameters and
+/// comparing different skeleton states.
+template <typename T>
+struct SkeletonStateT {
+  /// Joint parameters for all joints in the skeleton
+  JointParametersT<T> jointParameters;
+
+  /// Computed joint states for all joints in the skeleton
+  JointStateListT<T> jointState;
+
+  /// Creates an empty skeleton state
+  SkeletonStateT() noexcept = default;
+
+  /// Creates a skeleton state from joint parameters and a reference skeleton
+  ///
+  /// @param parameters Joint parameters for all joints in the skeleton
+  /// @param referenceSkeleton The skeleton structure defining joint hierarchy
+  /// @param computeDeriv Whether to compute derivative information for the joints
+  SkeletonStateT(
+      const JointParametersT<T>& parameters,
+      const Skeleton& referenceSkeleton,
+      bool computeDeriv = true);
+
+  /// Creates a skeleton state from rvalue joint parameters and a reference skeleton
+  ///
+  /// @param parameters Joint parameters for all joints in the skeleton (moved from)
+  /// @param referenceSkeleton The skeleton structure defining joint hierarchy
+  /// @param computeDeriv Whether to compute derivative information for the joints
+  SkeletonStateT(
+      JointParametersT<T>&& parameters,
+      const Skeleton& referenceSkeleton,
+      bool computeDeriv = true);
+
+  /// Creates a skeleton state from another skeleton state with a different scalar type
+  ///
+  /// @tparam T2 Source scalar type
+  /// @param rhs Source skeleton state to copy from
+  template <typename T2>
+  explicit SkeletonStateT(const SkeletonStateT<T2>& rhs);
+
+  /// Updates the skeleton state with new joint parameters
+  ///
+  /// @param jointParameters New joint parameters for all joints
+  /// @param referenceSkeleton The skeleton structure defining joint hierarchy
+  /// @param computeDeriv Whether to compute derivative information for the joints
+  void set(
+      const JointParametersT<T>& jointParameters,
+      const Skeleton& referenceSkeleton,
+      bool computeDeriv = true);
+
+  /// Updates the skeleton state with new rvalue joint parameters
+  ///
+  /// @param jointParameters New joint parameters for all joints (moved from)
+  /// @param referenceSkeleton The skeleton structure defining joint hierarchy
+  /// @param computeDeriv Whether to compute derivative information for the joints
+  void set(
+      JointParametersT<T>&& jointParameters,
+      const Skeleton& referenceSkeleton,
+      bool computeDeriv = true);
+
+  /// Updates the skeleton state from another skeleton state with a different scalar type
+  ///
+  /// @tparam T2 Source scalar type
+  /// @param rhs Source skeleton state to copy from
+  template <typename T2>
+  void set(const SkeletonStateT<T2>& rhs);
+
+  /// Compares two skeleton states and returns similarity metrics
+  ///
+  /// @param state1 First skeleton state to compare
+  /// @param state2 Second skeleton state to compare
+  /// @return Similarity metrics between the two states
+  [[nodiscard]] static StateSimilarity compare(
+      const SkeletonStateT<T>& state1,
+      const SkeletonStateT<T>& state2);
+
+  /// Extracts global transforms for all joints in the skeleton
+  ///
+  /// @return List of global transforms for all joints
+  [[nodiscard]] TransformListT<T> toTransforms() const;
+
+  /// Converts the skeleton state to a different scalar type
+  ///
+  /// @tparam T2 Target scalar type
+  /// @return Skeleton state with the target scalar type
+  template <typename T2>
+  [[nodiscard]] SkeletonStateT<T2> cast() const {
+    SkeletonStateT<T2> result;
+    result.set(*this);
+    return result;
+  }
+
+ private:
+  /// Updates the joint states based on the current joint parameters
+  ///
+  /// @param referenceSkeleton The skeleton structure defining joint hierarchy
+  /// @param computeDeriv Whether to compute derivative information for the joints
+  void set(const Skeleton& referenceSkeleton, bool computeDeriv);
+
+  /// Copies joint states from another skeleton state with a different scalar type
+  ///
+  /// @tparam T2 Source scalar type
+  /// @param rhs Source skeleton state to copy from
+  template <typename T2>
+  void copy(const SkeletonStateT<T2>& rhs);
+};
+
+/// Computes the relative transform between two joints in a skeleton
+///
+/// This transform maps points from joint A's local space to joint B's local space.
+/// It is computed by finding the common ancestor of both joints and combining
+/// the transforms along the path, which is more numerically stable than
+/// computing (T_B)^{-1} * T_A directly.
+///
+/// @param jointA Source joint index
+/// @param jointB Target joint index
+/// @param referenceSkeleton The skeleton structure defining joint hierarchy
+/// @param skelState Current state of the skeleton
+/// @return Transform from joint A's local space to joint B's local space
+template <typename T>
+TransformT<T> transformAtoB(
+    size_t jointA,
+    size_t jointB,
+    const Skeleton& referenceSkeleton,
+    const SkeletonStateT<T>& skelState);
+
+/// Invert the skeleton state (global transforms in world space) back to joint parameters (Euler
+/// angles in local space).  Note that this conversion is not unique due to the non-uniqueness of
+/// Euler angle conversion.
+template <typename T>
+[[nodiscard]] JointParametersT<T> skeletonStateToJointParameters(
+    const SkeletonStateT<T>& state,
+    const Skeleton& skeleton);
+
+template <typename T>
+[[nodiscard]] JointParametersT<T> skeletonStateToJointParameters(
+    const TransformListT<T>& state,
+    const Skeleton& skeleton);
+
+} // namespace momentum

include/momentum/character/skeleton_utility.h

@@ -0,0 +1,38 @@
+/*
+ * 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/types.h>
+#include <momentum/math/types.h>
+
+namespace momentum {
+
+inline constexpr float kDefaultExtrapolateFactor = 0.8f;
+inline constexpr float kDefaultExtrapolateMaxDelta = 0.4f; // ~23 degrees
+
+/// Extrapolates model parameters by first clamping the difference between current and previous
+/// parameters to the range [-maxDelta, maxDelta], and then scaling this clamped difference by
+/// `factor`. Returns the current parameters when sizes mismatch.
+[[nodiscard]] ModelParameters extrapolateModelParameters(
+    const ModelParameters& previous,
+    const ModelParameters& current,
+    float factor = kDefaultExtrapolateFactor,
+    float maxDelta = kDefaultExtrapolateMaxDelta);
+
+/// Extrapolates model parameters considering active parameters. The extrapolation is done by first
+/// clamping the difference between current and previous parameters to the range [-maxDelta,
+/// maxDelta] for each active parameter, and then scaling this clamped difference by `factor`.
+/// Returns the current parameters when sizes mismatch.
+[[nodiscard]] ModelParameters extrapolateModelParameters(
+    const ModelParameters& previous,
+    const ModelParameters& current,
+    const ParameterSet& activeParams,
+    float factor = kDefaultExtrapolateFactor,
+    float maxDelta = kDefaultExtrapolateMaxDelta);
+
+} // namespace momentum

include/momentum/character/skin_weights.h

@@ -0,0 +1,67 @@
+/*
+ * 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/types.h>
+#include <momentum/math/types.h>
+
+#include <cstdint>
+#include <vector>
+
+namespace momentum {
+
+/// Maximum number of joints that can influence a single vertex
+inline static constexpr uint32_t kMaxSkinJoints = 8;
+
+/// Matrix type for storing joint indices that influence each vertex
+///
+/// Each row represents a vertex, and each column represents a joint influence.
+/// The matrix has a fixed number of columns (kMaxSkinJoints) and a dynamic number of rows.
+using IndexMatrix =
+    Eigen::Matrix<uint32_t, Eigen::Dynamic, kMaxSkinJoints, Eigen::AutoAlign | Eigen::RowMajor>;
+
+/// Matrix type for storing weights of joint influences on each vertex
+///
+/// Each row represents a vertex, and each column represents the weight of a joint influence.
+/// The matrix has a fixed number of columns (kMaxSkinJoints) and a dynamic number of rows.
+using WeightMatrix =
+    Eigen::Matrix<float, Eigen::Dynamic, kMaxSkinJoints, Eigen::AutoAlign | Eigen::RowMajor>;
+
+/// Stores skinning weights and joint indices for character mesh deformation
+struct SkinWeights {
+  /// Joint indices that influence each vertex
+  ///
+  /// Each row corresponds to a vertex, and each column contains the index of a joint
+  /// that influences that vertex. Unused influences are set to 0.
+  IndexMatrix index;
+
+  /// Weight of each joint's influence on each vertex
+  ///
+  /// Each row corresponds to a vertex, and each column contains the weight of a joint's
+  /// influence on that vertex. Weights for a vertex typically sum to 1.0. Unused influences
+  /// are set to 0.0.
+  WeightMatrix weight;
+
+  /// Sets the skin weights from vectors of joint indices and weights
+  ///
+  /// @param ind Vector of vectors containing joint indices for each vertex
+  /// @param wgt Vector of vectors containing weights for each vertex
+  /// @throws If ind.size() != wgt.size() (via MT_CHECK)
+  void set(const std::vector<std::vector<size_t>>& ind, const std::vector<std::vector<float>>& wgt);
+
+  /// Compares two SkinWeights objects for equality
+  ///
+  /// Two SkinWeights objects are considered equal if their index and weight matrices
+  /// are approximately equal (using Eigen's isApprox method).
+  ///
+  /// @param skinWeights The SkinWeights object to compare with
+  /// @return True if the objects are equal, false otherwise
+  bool operator==(const SkinWeights& skinWeights) const;
+};
+
+} // namespace momentum

include/momentum/character/skinned_locator.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/skin_weights.h>
+#include <momentum/character/types.h>
+#include <momentum/math/utility.h>
+
+namespace momentum {
+
+/// A skinned locator is a locator which can be attached to multiple bones.
+/// The locator's position is defined relative to the rest pose of the character (and not
+/// local to a single parent bone as with a regular Locator) and its position at runtime is
+/// determined by blending the skinning transforms (using LBS).
+///
+/// The purpose of the SkinnedLocator is to model mocap markers attached to the actor's skin:
+/// the location of points on e.g. the shoulder is more accurately modeled by blending
+/// multiple transforms.  In addition, the locator can be constrained to slide along the
+/// surface of the mesh using e.g. the SkinnedLocatorTriangleErrorFunction.
+///
+/// Locators can be used for various purposes such as tracking specific points
+/// on a character, defining constraints, or serving as targets for inverse kinematics.
+struct SkinnedLocator {
+  /// Name identifier for the locator
+  std::string name;
+
+  /// Index of the parent joints in the skeleton.  The final position of the locator
+  /// is determined by blending the transforms from each (valid) parent.
+  Eigen::Matrix<uint32_t, kMaxSkinJoints, 1> parents;
+
+  /// Skinning weight for each parent joint.
+  Eigen::Matrix<float, kMaxSkinJoints, 1> skinWeights;
+
+  /// Position relative to rest pose of the character
+  Vector3f position;
+
+  /// Influence weight of this locator when used in constraints
+  float weight;
+
+  /// Creates a locator with the specified properties
+  ///
+  /// @param name Identifier for the locator
+  /// @param parents Indices of the parent joints
+  /// @param weights Skinning weights for the parent joints
+  /// @param weight Influence weight in constraints
+  SkinnedLocator(
+      const std::string& name = "uninitialized",
+      const Eigen::Matrix<uint32_t, kMaxSkinJoints, 1>& parents =
+          Eigen::Matrix<uint32_t, kMaxSkinJoints, 1>::Zero(),
+      const Eigen::Matrix<float, kMaxSkinJoints, 1>& skinWeights =
+          Eigen::Matrix<float, kMaxSkinJoints, 1>::Zero(),
+      const Vector3f& position = Vector3f::Zero(),
+      const float weight = 1.0f)
+      : name(name),
+        parents(parents),
+        skinWeights(skinWeights),
+        position(position),
+        weight(weight) {}
+
+  /// Compares two locators for equality, using approximate comparison for floating-point values
+  ///
+  /// @param locator The locator to compare with
+  /// @return True if all properties are equal (or approximately equal for floating-point values)
+  inline bool operator==(const SkinnedLocator& locator) const {
+    return (
+        (name == locator.name) && (parents == locator.parents) &&
+        skinWeights.isApprox(locator.skinWeights) && position.isApprox(locator.position) &&
+        isApprox(weight, locator.weight));
+  }
+};
+
+/// A collection of locators attached to a skeleton
+using SkinnedLocatorList = std::vector<SkinnedLocator>;
+
+} // namespace momentum

include/momentum/character/types.h

@@ -0,0 +1,202 @@
+/*
+ * 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
+
+//------------------------------------------------------------------------------
+// Momentum specific Eigen defines
+//------------------------------------------------------------------------------
+
+#include <momentum/character/fwd.h>
+
+#include <Eigen/Dense>
+
+#include <array>
+#include <string>
+#include <tuple>
+#include <vector>
+
+namespace momentum {
+
+inline constexpr size_t kParametersPerJoint = 7;
+enum JointParameterNames { TX = 0, TY, TZ, RX, RY, RZ, SC };
+inline constexpr std::array<const char*, kParametersPerJoint>
+    kJointParameterNames{"tx", "ty", "tz", "rx", "ry", "rz", "sc"};
+
+template <typename T>
+using JointVectorT = Eigen::Matrix<T, kParametersPerJoint, 1>;
+using JointVector = JointVectorT<float>;
+
+template <template <typename> typename Derived, typename EigenType>
+struct EigenStrongType {
+  using Scalar = typename EigenType::Scalar;
+
+  EigenType v;
+
+  EigenStrongType() : v() {}
+
+  explicit EigenStrongType(::Eigen::Index size) : v(size) {}
+
+  template <typename Other>
+  /* implicit */ EigenStrongType(const ::Eigen::EigenBase<Other>& o) : v(o) {}
+
+  template <typename Other>
+  /* implicit */ EigenStrongType(::Eigen::EigenBase<Other>&& o) : v(std::move(o)) {}
+
+  template <typename Other>
+  EigenStrongType& operator=(const ::Eigen::EigenBase<Other>& o) {
+    v = o;
+    return *this;
+  }
+
+  template <typename Other>
+  EigenStrongType& operator=(::Eigen::EigenBase<Other>&& o) {
+    v = std::move(o);
+    return *this;
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE
+  auto&& operator()(::Eigen::Index i) {
+    return v(i);
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE
+  auto&& operator()(::Eigen::Index i) const {
+    return v(i);
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE
+  auto&& operator[](::Eigen::Index i) {
+    return v[i];
+  }
+
+  EIGEN_DEVICE_FUNC
+  EIGEN_STRONG_INLINE
+  auto&& operator[](::Eigen::Index i) const {
+    return v[i];
+  }
+
+  template <typename NewScalar>
+  [[nodiscard]] EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE Derived<NewScalar> cast() const {
+    return v.template cast<NewScalar>();
+  }
+
+  [[nodiscard]] EIGEN_DEVICE_FUNC EIGEN_STRONG_INLINE auto size() const {
+    return v.size();
+  }
+
+  EIGEN_DEVICE_FUNC
+  static Derived<Scalar> Zero(::Eigen::Index size) {
+    return Derived<Scalar>(EigenType::Zero(size));
+  }
+};
+
+// a vector describing model parameters
+template <typename T>
+struct ModelParametersT : public EigenStrongType<ModelParametersT, ::Eigen::VectorX<T>> {
+  using t = ::Eigen::VectorX<T>;
+  using EigenStrongType<ModelParametersT, t>::EigenStrongType;
+  using EigenStrongType<ModelParametersT, t>::operator=;
+};
+
+using ModelParameters = ModelParametersT<float>;
+using ModelParametersd = ModelParametersT<double>;
+
+template <typename T>
+struct BlendWeightsT : public EigenStrongType<BlendWeightsT, ::Eigen::VectorX<T>> {
+  using t = ::Eigen::VectorX<T>;
+  using EigenStrongType<BlendWeightsT, t>::EigenStrongType;
+  using EigenStrongType<BlendWeightsT, t>::operator=;
+};
+
+using BlendWeights = BlendWeightsT<float>;
+using BlendWeightsd = BlendWeightsT<double>;
+
+static_assert(
+    sizeof(BlendWeights) == sizeof(Eigen::VectorXf),
+    "Missing Empty Base Class Optimization");
+static_assert(
+    sizeof(BlendWeightsd) == sizeof(Eigen::VectorXd),
+    "Missing Empty Base Class Optimization");
+
+// A vector describing joint parameters, the vector has size numSkeletonJoints * kParametersPerJoint
+// e.g. in case of a character body model with 159 joints, the joint parameters vector will have the
+// size 159 * 7 = 1113
+template <typename T>
+struct JointParametersT : public EigenStrongType<JointParametersT, ::Eigen::VectorX<T>> {
+  using t = ::Eigen::VectorX<T>;
+  using EigenStrongType<JointParametersT, t>::EigenStrongType;
+  using EigenStrongType<JointParametersT, t>::operator=;
+
+  [[nodiscard]] static ::Eigen::Vector3<T> fromRotationMatrix(const ::Eigen::Matrix3<T>& m) {
+    // From JointState::set(), we can see that localRotation = rz * ry * rx, but the order in
+    // JointParameters is [rx, ry, rz]. Therefore, the conversion should be
+    // rotationMatrixToEulerZYX.reverse.
+    return rotationMatrixToEulerZYX(m).reverse();
+  }
+
+  [[nodiscard]] static ::Eigen::Vector3<T> fromQuaternion(const ::Eigen::Quaternion<T>& q) {
+    return fromRotationMatrix(q.toRotationMatrix());
+  }
+};
+
+using JointParameters = JointParametersT<float>;
+using JointParametersd = JointParametersT<double>;
+
+static_assert(
+    sizeof(JointParameters) == sizeof(Eigen::VectorXf),
+    "Missing Empty Base Class Optimization");
+static_assert(
+    sizeof(JointParametersd) == sizeof(Eigen::VectorXd),
+    "Missing Empty Base Class Optimization");
+
+template <typename T>
+using JointStateListT = std::vector<JointStateT<T>>;
+
+using JointStateList = JointStateListT<float>;
+using JointStateListd = JointStateListT<double>;
+
+/// A struct that encapsulates both pose and identity parameters for a character.
+///
+/// @note This structure implies, but does not enforce, that the pose vector should contain only
+/// pose information with identity-related elements set to zero. Similarly, the identity vector
+/// should exclusively contain bone length information, excluding any pose data.
+template <typename T>
+struct CharacterParametersT {
+  /// The model parameter vector representing the pose of the character. This vector should have a
+  /// size of numModelParams.
+
+  ModelParametersT<T> pose;
+  /// The joint parameter vector representing the unique bone lengths of the character, defining
+  /// the character's identity. This vector should have a size of numSkeletonJoints *
+  /// kParametersPerJoint.
+  JointParametersT<T> offsets;
+};
+
+using CharacterParameters = CharacterParametersT<float>;
+using CharacterParametersd = CharacterParametersT<double>;
+
+// The tuple of model parameter names and corresponding matrix representing the pose in a sequence
+// of frames The poses are ordered in columns and the expected shape of motion matrix is
+// (numModelParams, numFrames)
+
+using MotionParameters = std::tuple<std::vector<std::string>, Eigen::MatrixXf>;
+
+// The tuple containing the skeleton joint names and identity parameters. The identity parameters
+// represent bone offsets and bone scales that are added to the joint states (local transform for
+// each joint wrt parent joint) during FK step The identity parameters are expressed as a vector of
+// size (numSkeletonJoints * momentum::kParametersPerJoint)
+
+using IdentityParameters = std::tuple<std::vector<std::string>, Eigen::VectorXf>;
+
+// define static kInvalidIndex for size_t
+inline constexpr size_t kInvalidIndex = std::numeric_limits<size_t>::max();
+
+} // namespace momentum