Scholz2015 wrapping algorithm

[pepbos]

Scholz2015 Wrapping Algorithm

Implementation of a new algorithm for computing the path of a weightless, and frictionless cable spanning over multiple smooth obstacle surfaces[^fn].

[^fn]: Scholz, A., Sherman, M., Stavness, I. et al (2016). A fast multi-obstacle muscle wrapping method using natural geodesic variations. Multibody System Dynamics 36, 195–219.

Algorithm Overview

Creating a cable A new subsystem class CableSubsystem is added to manage the cables in a simulation:

MultibodySystem system;
CableSubsystem cables(system);

A cable is represented by the new class CableSpan, and allows a path to be spanned between two points:

CableSpan cable(
        cables,
        body1, Vec3(0.), // Path origin
        body2, Vec3(0.)); // Path termination

Obstacles can be added to the cable to make it more interesting:

cable.addObstacle(obstacleBody, obstacleOffset, obstacleGeometry);

For example, we can add a torus to the scene:

https://github.com/simbody/simbody/assets/46680867/49ea834d-bf1b-49fd-9d11-b90ef7dfeb69

Initializing the Path To help control the cable's path you can choose a contact point hint for each obstacle. For example, placing the contact point within the torus:

cable.setContactPointHint(obsIndex, -torusRadius * Vec3{YAxis} / 2);

allows spanning the cable through the torus:

Obstacle Ordering Currently, the order in which the cable can come into contact with the obstacle is ordered based on the order in which the obstacles are added to the cable. This was done to reduce both the algorithm complexity and simplify building your path, while being useful in a large set of problems.

For the previous example this means that a CableSpan will not collide with the torus twice:

https://github.com/simbody/simbody/assets/46680867/94917a9f-1a10-485c-be53-dad9d78cfa4f

or in the case of two obstacles, that collision with the first obstacle will not be detected after contact with the second obstacle:

https://github.com/simbody/simbody/assets/46680867/38173be5-de17-44e5-ae77-48ee69d7a3a0

It is still possible to make contact with a single surface geometry multiple times. But you have to add that surface as an obstacle multiple times. For example, if we wish to make contact with a torus twice, it can be added as an obstacle twice:

// Start with torus obstacle
cable.addObstacle(ground, offset, torusGeometry, contactPoint1);
// Add other obstacles
...
// End with same torus obstacle.
cable.addObstacle(ground, offset, torusGeometry, contactPoint2);

which allows the cable to make contact with the same torus as the initial and final obstacle:

Contact Detection The cable can lose and regain contact with an obstacle, by lifting-off or touching down on the surface. Lift-off is detected when:

1. The length of the geodesic is zero, and
2. the path point before and after the geodesic are above the surface

https://github.com/simbody/simbody/assets/46680867/7668045d-727d-4482-811b-8bb7c5990886

After lifting off, the cable misses the obstacle in a straight line. Touchdown is detected by tracking the point on the line that is nearest the surface. This tracking point is visualized by a red line. For example, a cable that misses all obstacles:

https://github.com/simbody/simbody/assets/46680867/5bf31b47-8ec0-45a2-b676-8e05b5bfa05b

A common approach is to put the tracking point on the obstacle, and not the line. I found this to be less robust and more computationally expensive. Especially for a torus it can be hard to jump over the "hole" (which happens above).

Implicit, Parametric, Analytic Surfaces Whenever the cable touches down on a suface this adds a curved segment to the path. Each curved segment is a geodesic for that obstacle surface. The geodesic is computed analytically if possible, and otherwise using the implicit surface equations. The parametric form is not used in this PR, and would be part of future work.

Continuous Wrapping When solving for the path, the corrections are applied to the path locally. This means that the path will not jump to the other surface side, and allows for winding over an obstacle multiple times:

https://github.com/simbody/simbody/assets/46680867/b9a920e8-4ca8-4a15-96a8-dd590ad73c94

The CableSpan stores the last computed geodesic in local obstacle coordinates in a discrete cache variable. This is then used as a warm-start at the next realization. For example, after computing the initial path, the path below is computed in zero solver iterations:

https://github.com/simbody/simbody/assets/46680867/179cc2c2-d985-4b38-84d4-81a3198766e5

During the above rotation the only computation done by CableSpan is to transform the cached geodesic to ground frame, which is considerably cheaper than recomputing the path.

Added Example A new example is added examples/ExampleCableSpan.cpp, that combines the currently tested surfaces:

https://github.com/user-attachments/assets/49c6dd87-a9af-43f4-8f6e-97dc223d593a

Test Cases

The CableSubsystemTestHelper is added and can be used to assert that:

1. all curve segments in a path are correct geodesics, and
2. the computed path error Jacobian is correct.

This test can be used during a simulation to check your currently computed path:

CableSubsystemTestHelper().testCurrentPath(s, cables, std::cout);

This allows testing all computed paths during a test case simulation. Additionally it will catch bugs in geodesic related math in ContactGeometry, which has been modified in this PR. See description of CableSubsystemTestHelper in CableSpan.h for details.

Additionally, the following test cases have been added at Simbody/tests/TestCableSpan.cpp:

Test Case 1: A test case using all currently supported surfaces, and with different cable end point locations:

https://github.com/user-attachments/assets/cd714afa-e904-46cd-bb8d-a54ef1425a85

For each computed path, the test checks that:

1. The curve segments are correct geodesics.
2. The path error jacobian is correct.
3. The computed length derivative matches the change in length.

Test Case 2: A test case verifying touchdown and liftoff on a torus, ellipsoid, sphere and cylinder surface:

https://github.com/user-attachments/assets/2a54f76d-3e90-4954-a846-3120de312811

This test verifies that the path loses/regains contact with the obstacles when you would expect it to.

Test Case 3: A test case for which the path is simple enough such that we can check the result by hand:

Here we verify that the following computations give the expected result:

1. Curve segment lengths, and total cable length.
2. Computed forces and torques.
3. Computed Frenet frames at the obstacle contact points.

Future Work

Some issues have been spotted, and will still be collected. They are important to address, but should not effect the currently proposed framework. The idea is to create new PRs for each, and not make them part of this PR.

Configuration Parameters: There are some issues regarding the configuration parameters:

1. The surface constraint tolerance could/should be related to the integrator accuracy.
2. The path solver accuracy could/should be related to the integrator accuracy.
3. The max number of iterations allowed during surface projection is not exposed.

Geodesic Math Speedup The original implementation of ContactGeometry::calcSurfaceValue(), ContactGeometry::calcSurfaceGradient(), ContactGeometry::calcSurfaceHessian() and ContactGeometry::calcSurfaceCurvatureInDirection() was taking up most of the time during the computation of a path. These have been overridden for some surfaces (ellipsoid, sphere, cylinder), but not yet for others (torus, bicubic-patch). This will make some surfaces significantly slower than others. Similarly, the proposed touchdown detection is using the implicit equations for all surfaces. For the analytic surfaces this could be done much more efficiently.

Path Initialization Currently the path has trouble initializing when the initial guess is too far (angular distance > 90 degrees) from the optimal path.

Touchdown Misses The path can miss touching down on the surface if the configuration changes too much between states. Here we escape the torus when forcing large translation of the cable origin:

https://github.com/user-attachments/assets/6392f6e2-c3dc-4070-8358-119b38d595d8

Alternate Cost Function Currently the optimal path is found by minimizing the angular discontinuities at the segment connection points. This is not always equivalent to minimizing the path length.

Visualizer Path Points The interface required for visualizing the path points in OpenSim GUI could be improved. For example: If needed interpolation could be done efficiently by the CableSpan.

Changes Summary

The following files have been changed/added:

The new classes CableSpan and CableSubsystem are:

• Declared in: Simbody/include/simbody/internal/CableSpan.h
• Defined in: Simbody/src/CableSpan.cpp

The helper class CableSubsystemTestHelper is:

• Declared in: Simbody/include/simbody/internal/CableSpan.h
• Implementation class declared in: Simbody/src/CableSpan_SubsystemTestHelper_Impl.h
• Defined in: Simbody/src/CableSpan_SubsystemTestHelper_Impl.cpp

The following functions are added to ContactGeometry:

• calcSurfaceTorsionInDirection: Computing the geodesic torsion of the surface.
• calcNearestPointOnLineImplicitly: Computing a point on a line that is nearest to the surface.
• shootGeodesicInDirectionImplicitly: Computing a new geodesic implicitly.
• shootGeodesicInDirectionAnalytically: Computing a new geodesic analytically.
• isAnalyticFormAvailable: Returns whether an analytic form is available for this geometry.
• isSurfaceDefined: Returns whether a surface is defined at a given point (used to check domain of the bicubic surface)

The following functions in ContactGeometry are virtualized:

• calcSurfaceValue
• calcSurfaceGradient
• calcSurfaceHessian
• calcSurfaceCurvatureInDirection and are overridden for the sphere, cylinder and ellipsoid.

The analytic geodesic shootGeodesicInDirectionAnalytically is implemented in:

• SimTKmath/Geometry/src/ContactGeometry_Sphere.cpp: for the sphere
• SimTKmath/Geometry/src/ContactGeometry_Cylinder.cpp: for the cylinder

An example is added: examples/ExampleCableSpan.cpp Unit tests are collected in: Simbody/tests/TestCableSpan.cpp

---

This change is