Python bindings for VertexVertexDistanceErrorFunction. (#578)

Summary:
Pull Request resolved: #578

### Added Python Bindings for VertexVertexDistanceErrorFunction
#### Summary

This diff adds Python bindings for the `VertexVertexDistanceErrorFunction` class, allowing it to be used directly from Python.  This change enables the use of the `VertexVertexDistanceErrorFunction` in Python, expanding the functionality of the `pymomentum` library.

#### Example Use Case

With this change, you can now create and use `VertexVertexDistanceErrorFunction` objects directly from Python, like this:
```python
import pymomentum
error_function = pymomentum.solver2.VertexVertexDistanceErrorFunction()
```

Reviewed By: jeongseok-meta

Differential Revision: D82848959

fbshipit-source-id: d36b4dc85b5d3578dd8f67066ed3022717822995

Author: cdtwigg

pymomentum/solver2/solver2_error_functions.cpp

@@ -24,6 +24,7 @@
 #include <momentum/character_solver/skeleton_error_function.h>
 #include <momentum/character_solver/state_error_function.h>
 #include <momentum/character_solver/vertex_projection_error_function.h>
+#include <momentum/character_solver/vertex_vertex_distance_error_function.h>
 #include <pymomentum/solver2/solver2_utility.h>
 
 #include <fmt/format.h>
@@ -1238,6 +1239,162 @@ distance is greater than zero (ie. the point being above).)")
           py::arg("name") = std::optional<std::vector<std::string>>{});
 }
 
+void defVertexVertexDistanceErrorFunction(py::module_& m) {
+  py::class_<mm::VertexVertexDistanceConstraintT<float>>(
+      m, "VertexVertexDistanceConstraint")
+      .def(
+          "__repr__",
+          [](const mm::VertexVertexDistanceConstraintT<float>& self) {
+            return fmt::format(
+                "VertexVertexDistanceConstraint(vertex_index1={}, vertex_index2={}, weight={}, target_distance={})",
+                self.vertexIndex1,
+                self.vertexIndex2,
+                self.weight,
+                self.targetDistance);
+          })
+      .def_readonly(
+          "vertex_index1",
+          &mm::VertexVertexDistanceConstraintT<float>::vertexIndex1,
+          "The index of the first vertex")
+      .def_readonly(
+          "vertex_index2",
+          &mm::VertexVertexDistanceConstraintT<float>::vertexIndex2,
+          "The index of the second vertex")
+      .def_readonly(
+          "weight",
+          &mm::VertexVertexDistanceConstraintT<float>::weight,
+          "The weight of the constraint")
+      .def_readonly(
+          "target_distance",
+          &mm::VertexVertexDistanceConstraintT<float>::targetDistance,
+          "The target distance between the two vertices");
+
+  py::class_<
+      mm::VertexVertexDistanceErrorFunctionT<float>,
+      mm::SkeletonErrorFunction,
+      std::shared_ptr<mm::VertexVertexDistanceErrorFunctionT<float>>>(
+      m,
+      "VertexVertexDistanceErrorFunction",
+      R"(Error function that minimizes the distance between pairs of vertices on the character's mesh.
+
+This is useful for constraints where you want to maintain specific distances between
+different parts of the character mesh, such as keeping fingers at a certain distance
+or maintaining the width of body parts.)")
+      .def(
+          "__repr__",
+          [](const mm::VertexVertexDistanceErrorFunctionT<float>& self) {
+            return fmt::format(
+                "VertexVertexDistanceErrorFunction(weight={}, num_constraints={})",
+                self.getWeight(),
+                self.numConstraints());
+          })
+      .def(
+          py::init<>(
+              [](const mm::Character& character, float weight)
+                  -> std::shared_ptr<
+                      mm::VertexVertexDistanceErrorFunctionT<float>> {
+                validateWeight(weight, "weight");
+                auto result = std::make_shared<
+                    mm::VertexVertexDistanceErrorFunctionT<float>>(character);
+                result->setWeight(weight);
+                return result;
+              }),
+          R"(Initialize a VertexVertexDistanceErrorFunction.
+
+            :param character: The character to use.
+            :param weight: The weight applied to the error function.)",
+          py::keep_alive<1, 2>(),
+          py::arg("character"),
+          py::kw_only(),
+          py::arg("weight") = 1.0f)
+      .def(
+          "add_constraint",
+          [](mm::VertexVertexDistanceErrorFunctionT<float>& self,
+             int vertexIndex1,
+             int vertexIndex2,
+             float weight,
+             float targetDistance) {
+            validateVertexIndex(
+                vertexIndex1, "vertex_index1", self.getCharacter());
+            validateVertexIndex(
+                vertexIndex2, "vertex_index2", self.getCharacter());
+            validateWeight(weight, "weight");
+            self.addConstraint(
+                vertexIndex1, vertexIndex2, weight, targetDistance);
+          },
+          R"(Adds a vertex-to-vertex distance constraint to the error function.
+
+        :param vertex_index1: The index of the first vertex.
+        :param vertex_index2: The index of the second vertex.
+        :param weight: The weight of the constraint.
+        :param target_distance: The desired distance between the two vertices.)",
+          py::arg("vertex_index1"),
+          py::arg("vertex_index2"),
+          py::arg("weight"),
+          py::arg("target_distance"))
+      .def(
+          "add_constraints",
+          [](mm::VertexVertexDistanceErrorFunctionT<float>& self,
+             const py::array_t<int>& vertexIndex1,
+             const py::array_t<int>& vertexIndex2,
+             const py::array_t<float>& weight,
+             const py::array_t<float>& targetDistance) {
+            ArrayShapeValidator validator;
+            const int nConsIdx = -1;
+            validator.validate(
+                vertexIndex1, "vertex_index1", {nConsIdx}, {"n_cons"});
+            validateVertexIndex(
+                vertexIndex1, "vertex_index1", self.getCharacter());
+            validator.validate(
+                vertexIndex2, "vertex_index2", {nConsIdx}, {"n_cons"});
+            validateVertexIndex(
+                vertexIndex2, "vertex_index2", self.getCharacter());
+            validator.validate(weight, "weight", {nConsIdx}, {"n_cons"});
+            validateWeights(weight, "weight");
+            validator.validate(
+                targetDistance, "target_distance", {nConsIdx}, {"n_cons"});
+
+            auto vertexIndex1Acc = vertexIndex1.unchecked<1>();
+            auto vertexIndex2Acc = vertexIndex2.unchecked<1>();
+            auto weightAcc = weight.unchecked<1>();
+            auto targetDistanceAcc = targetDistance.unchecked<1>();
+
+            py::gil_scoped_release release;
+
+            for (py::ssize_t i = 0; i < vertexIndex1.shape(0); ++i) {
+              self.addConstraint(
+                  vertexIndex1Acc(i),
+                  vertexIndex2Acc(i),
+                  weightAcc(i),
+                  targetDistanceAcc(i));
+            }
+          },
+          R"(Adds multiple vertex-to-vertex distance constraints to the error function.
+
+        :param vertex_index1: A numpy array of indices for the first vertices.
+        :param vertex_index2: A numpy array of indices for the second vertices.
+        :param weight: A numpy array of weights for the constraints.
+        :param target_distance: A numpy array of desired distances between vertex pairs.)",
+          py::arg("vertex_index1"),
+          py::arg("vertex_index2"),
+          py::arg("weight"),
+          py::arg("target_distance"))
+      .def(
+          "clear_constraints",
+          &mm::VertexVertexDistanceErrorFunctionT<float>::clearConstraints,
+          "Clears all vertex-to-vertex distance constraints from the error function.")
+      .def_property_readonly(
+          "constraints",
+          [](const mm::VertexVertexDistanceErrorFunctionT<float>& self) {
+            return self.getConstraints();
+          },
+          "Returns the list of vertex-to-vertex distance constraints.")
+      .def(
+          "num_constraints",
+          &mm::VertexVertexDistanceErrorFunctionT<float>::numConstraints,
+          "Returns the number of constraints.");
+}
+
 } // namespace
 
 void addErrorFunctions(py::module_& m) {
@@ -2237,6 +2394,9 @@ rotation matrix to a target rotation.)")
 
   // Vertex Projection error function
   defVertexProjectionErrorFunction(m);
+
+  // Vertex-to-vertex distance error function
+  defVertexVertexDistanceErrorFunction(m);
 }
 
 } // namespace pymomentum

pymomentum/test/test_solver2.py

@@ -1448,6 +1448,144 @@ def test_vertex_sequence_error_function(self) -> None:
         self.assertIn("VertexSequenceErrorFunction", repr_str)
         self.assertIn("num_constraints=2", repr_str)
 
+    def test_vertex_vertex_distance_constraint(self) -> None:
+        """Test VertexVertexDistanceErrorFunction to ensure vertices are pulled to target distance."""
+
+        # Create a test character
+        character = pym_geometry.create_test_character(num_joints=4)
+
+        n_params = character.parameter_transform.size
+
+        # Ensure repeatability in the rng:
+        torch.manual_seed(0)
+        model_params_init = torch.zeros(n_params, dtype=torch.float32)
+
+        # Choose two vertices to constrain - use vertices that are initially far apart
+        vertex_index1 = 0
+        vertex_index2 = character.mesh.vertices.shape[0] - 1  # Last vertex
+        target_distance = 0.5  # Target distance between the two vertices
+        weight = 1.0
+
+        # Get initial positions of the vertices
+        skel_state_init = pym_geometry.model_parameters_to_skeleton_state(
+            character, model_params_init
+        )
+        initial_mesh = character.skin_points(skel_state_init)
+        initial_pos1 = initial_mesh[vertex_index1, :3]
+        initial_pos2 = initial_mesh[vertex_index2, :3]
+        initial_distance = torch.norm(initial_pos2 - initial_pos1).item()
+
+        # Create VertexVertexDistanceErrorFunction
+        vertex_distance_error = pym_solver2.VertexVertexDistanceErrorFunction(character)
+
+        # Test basic properties
+        self.assertEqual(vertex_distance_error.num_constraints(), 0)
+        self.assertEqual(len(vertex_distance_error.constraints), 0)
+
+        # Add a single constraint
+        vertex_distance_error.add_constraint(
+            vertex_index1=vertex_index1,
+            vertex_index2=vertex_index2,
+            weight=weight,
+            target_distance=target_distance,
+        )
+
+        # Verify constraint was added
+        self.assertEqual(vertex_distance_error.num_constraints(), 1)
+        self.assertEqual(len(vertex_distance_error.constraints), 1)
+
+        constraint = vertex_distance_error.constraints[0]
+        self.assertEqual(constraint.vertex_index1, vertex_index1)
+        self.assertEqual(constraint.vertex_index2, vertex_index2)
+        self.assertAlmostEqual(constraint.weight, weight)
+        self.assertAlmostEqual(constraint.target_distance, target_distance)
+
+        # Create solver function with the vertex distance error
+        solver_function = pym_solver2.SkeletonSolverFunction(
+            character, [vertex_distance_error]
+        )
+
+        # Set solver options
+        solver_options = pym_solver2.GaussNewtonSolverOptions()
+        solver_options.max_iterations = 100
+        solver_options.regularization = 1e-5
+
+        # Create and run the solver
+        solver = pym_solver2.GaussNewtonSolver(solver_function, solver_options)
+        model_params_final = solver.solve(model_params_init.numpy())
+
+        # Convert final model parameters to skeleton state
+        skel_state_final = pym_geometry.model_parameters_to_skeleton_state(
+            character, torch.from_numpy(model_params_final)
+        )
+
+        # Compute final mesh and vertex positions
+        final_mesh = character.skin_points(skel_state_final)
+        final_pos1 = final_mesh[vertex_index1, :3]
+        final_pos2 = final_mesh[vertex_index2, :3]
+        final_distance = torch.norm(final_pos2 - final_pos1).item()
+
+        # Assert that the final distance is close to the target distance
+        self.assertAlmostEqual(
+            final_distance,
+            target_distance,
+            delta=1e-3,
+            msg=f"Final distance {final_distance} does not match target {target_distance}",
+        )
+
+        # Verify that the distance actually changed from the initial distance
+        self.assertNotAlmostEqual(
+            initial_distance,
+            final_distance,
+            delta=1e-1,
+            msg=f"Distance did not change significantly from initial {initial_distance} to final {final_distance}",
+        )
+
+        # Test multiple constraints using add_constraints
+        vertex_distance_error.clear_constraints()
+        self.assertEqual(vertex_distance_error.num_constraints(), 0)
+
+        # Add multiple constraints
+        vertex_indices1 = np.array([0, 1], dtype=np.int32)
+        vertex_indices2 = np.array([2, 3], dtype=np.int32)
+        weights = np.array([1.0, 2.0], dtype=np.float32)
+        target_distances = np.array([0.3, 0.7], dtype=np.float32)
+
+        vertex_distance_error.add_constraints(
+            vertex_index1=vertex_indices1,
+            vertex_index2=vertex_indices2,
+            weight=weights,
+            target_distance=target_distances,
+        )
+
+        # Verify multiple constraints were added
+        self.assertEqual(vertex_distance_error.num_constraints(), 2)
+        constraints = vertex_distance_error.constraints
+        self.assertEqual(len(constraints), 2)
+
+        # Check first constraint
+        self.assertEqual(constraints[0].vertex_index1, 0)
+        self.assertEqual(constraints[0].vertex_index2, 2)
+        self.assertAlmostEqual(constraints[0].weight, 1.0)
+        self.assertAlmostEqual(constraints[0].target_distance, 0.3)
+
+        # Check second constraint
+        self.assertEqual(constraints[1].vertex_index1, 1)
+        self.assertEqual(constraints[1].vertex_index2, 3)
+        self.assertAlmostEqual(constraints[1].weight, 2.0)
+        self.assertAlmostEqual(constraints[1].target_distance, 0.7)
+
+        # Test string representation
+        repr_str = repr(vertex_distance_error)
+        self.assertIn("VertexVertexDistanceErrorFunction", repr_str)
+        self.assertIn("num_constraints=2", repr_str)
+
+        # Test constraint string representation
+        constraint_repr = repr(constraints[0])
+        self.assertIn("VertexVertexDistanceConstraint", constraint_repr)
+        self.assertIn("vertex_index1=0", constraint_repr)
+        self.assertIn("vertex_index2=2", constraint_repr)
+
     def test_weight_validation(self) -> None:
         """Test that error functions throw ValueError when negative weights are passed."""