|
10 | 10 | #include <momentum/character/skeleton_state.h> |
11 | 11 | #include <momentum/character_sequence_solver/acceleration_sequence_error_function.h> |
12 | 12 | #include <momentum/character_sequence_solver/jerk_sequence_error_function.h> |
| 13 | +#include <momentum/character_sequence_solver/joint_to_joint_sequence_error_function.h> |
13 | 14 | #include <momentum/character_sequence_solver/model_parameters_sequence_error_function.h> |
14 | 15 | #include <momentum/character_sequence_solver/sequence_solver.h> |
15 | 16 | #include <momentum/character_sequence_solver/state_sequence_error_function.h> |
|
21 | 22 | #include <pybind11/stl.h> |
22 | 23 | #include <Eigen/Core> |
23 | 24 |
|
| 25 | +#include <pymomentum/python_utility/eigen_quaternion.h> |
24 | 26 | #include <pymomentum/solver2/solver2_sequence_error_functions.h> |
25 | 27 | #include <pymomentum/solver2/solver2_utility.h> |
26 | 28 |
|
@@ -272,6 +274,92 @@ the gradient contribution is set to zero. |
272 | 274 | &mm::VelocityMagnitudeSequenceErrorFunction::getTargetSpeeds, |
273 | 275 | "Returns the per-joint target speeds."); |
274 | 276 |
|
| 277 | + py::class_< |
| 278 | + mm::JointToJointSequenceErrorFunction, |
| 279 | + mm::SequenceErrorFunction, |
| 280 | + std::shared_ptr<mm::JointToJointSequenceErrorFunction>>( |
| 281 | + m, "JointToJointSequenceErrorFunction") |
| 282 | + .def( |
| 283 | + py::init<>([](const mm::Character& character, |
| 284 | + float weight, |
| 285 | + float positionWeight, |
| 286 | + float orientationWeight) { |
| 287 | + validateWeight(weight, "weight"); |
| 288 | + validateWeight(positionWeight, "position_weight"); |
| 289 | + validateWeight(orientationWeight, "orientation_weight"); |
| 290 | + |
| 291 | + auto result = std::make_shared<mm::JointToJointSequenceErrorFunction>(character); |
| 292 | + result->setWeight(weight); |
| 293 | + result->setPositionWeight(positionWeight); |
| 294 | + result->setOrientationWeight(orientationWeight); |
| 295 | + |
| 296 | + return result; |
| 297 | + }), |
| 298 | + R"(A sequence error function that penalizes frame-to-frame changes in the relative |
| 299 | +transform between two joints. |
| 300 | +
|
| 301 | +For each constraint with source joint A and reference joint B, this function computes |
| 302 | +the relative transform at two consecutive frames and penalizes any change between them: |
| 303 | +
|
| 304 | +- Position error: ``relPos_{i+1} - relPos_i`` |
| 305 | +- Orientation error: ``relRot_{i+1} - relRot_i`` |
| 306 | +
|
| 307 | +This keeps the relative transform (e.g. a hand's grip on an object) stable from frame |
| 308 | +to frame. |
| 309 | +
|
| 310 | +:param character: The character to use. |
| 311 | +:param weight: The weight of the error function. Defaults to 1.0. |
| 312 | +:param position_weight: The weight of the position error. Defaults to 1.0. |
| 313 | +:param orientation_weight: The weight of the orientation error. Defaults to 1.0.)", |
| 314 | + py::arg("character"), |
| 315 | + py::kw_only(), |
| 316 | + py::arg("weight") = 1.0f, |
| 317 | + py::arg("position_weight") = 1.0f, |
| 318 | + py::arg("orientation_weight") = 1.0f) |
| 319 | + .def( |
| 320 | + "add_constraint", |
| 321 | + [](mm::JointToJointSequenceErrorFunction& self, |
| 322 | + size_t sourceJoint, |
| 323 | + size_t referenceJoint, |
| 324 | + float constraintWeight, |
| 325 | + const std::string& name) { |
| 326 | + self.addConstraint(sourceJoint, referenceJoint, constraintWeight, name); |
| 327 | + }, |
| 328 | + R"(Add a constraint between two joints. |
| 329 | +
|
| 330 | +:param source_joint: Index of the source joint (the joint whose relative pose is tracked). |
| 331 | +:param reference_joint: Index of the reference joint (the frame of reference). |
| 332 | +:param weight: Weight for this constraint. Defaults to 1.0. |
| 333 | +:param name: Name for debugging. Defaults to empty string.)", |
| 334 | + py::arg("source_joint"), |
| 335 | + py::arg("reference_joint"), |
| 336 | + py::kw_only(), |
| 337 | + py::arg("weight") = 1.0f, |
| 338 | + py::arg("name") = "") |
| 339 | + .def( |
| 340 | + "clear_constraints", |
| 341 | + &mm::JointToJointSequenceErrorFunction::clearConstraints, |
| 342 | + "Clears all constraints from the error function.") |
| 343 | + .def_property_readonly( |
| 344 | + "num_constraints", |
| 345 | + &mm::JointToJointSequenceErrorFunction::getNumConstraints, |
| 346 | + "Returns the number of constraints.") |
| 347 | + .def_property( |
| 348 | + "weight", |
| 349 | + &mm::JointToJointSequenceErrorFunction::getWeight, |
| 350 | + &mm::JointToJointSequenceErrorFunction::setWeight, |
| 351 | + "The weight applied to the error function.") |
| 352 | + .def_property( |
| 353 | + "position_weight", |
| 354 | + &mm::JointToJointSequenceErrorFunction::getPositionWeight, |
| 355 | + &mm::JointToJointSequenceErrorFunction::setPositionWeight, |
| 356 | + "The weight of the position error.") |
| 357 | + .def_property( |
| 358 | + "orientation_weight", |
| 359 | + &mm::JointToJointSequenceErrorFunction::getOrientationWeight, |
| 360 | + &mm::JointToJointSequenceErrorFunction::setOrientationWeight, |
| 361 | + "The weight of the orientation error."); |
| 362 | + |
275 | 363 | py::class_< |
276 | 364 | mm::StateSequenceErrorFunction, |
277 | 365 | mm::SequenceErrorFunction, |
|
0 commit comments