opensim-core/OpenSim/Examples/Wrapping/exampleScholz2015GeometryPath.cpp at e623ed031f65f719272c9acd2cbb27e1bc16171a · opensim-org/opensim-core · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
/* -------------------------------------------------------------------------- *
 *                OpenSim:  exampleScholz2015GeometryPath.cpp                 *
 * -------------------------------------------------------------------------- *
 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
 * See http://opensim.stanford.edu and the NOTICE file for more information.  *
 * OpenSim is developed at Stanford University and supported by the US        *
 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
 * through the Warrior Web program.                                           *
 *                                                                            *
 * Copyright (c) 2005-2025 Stanford University and the Authors                *
 * Author(s): Nicholas Bianco                                                 *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

/// This example demonstrates the basic elements of creating a geometry-based
/// path using the Scholz2015GeometryPath class. The path is used to define the
/// length and speed of a PathSpring, which applies forces to the bodies of a
/// double pendulum model. The path contains three path points and wraps around
/// a cylindrical obstacle.

#include <OpenSim/OpenSim.h>

using namespace OpenSim;

int main() {

    Model model = ModelFactory::createDoublePendulum();
    model.setUseVisualizer(true);

    // Create a PathSpring with a Scholz2015GeometryPath.
    auto* spring = new PathSpring();
    spring->setName("path_spring");
    spring->setRestingLength(0.25);
    spring->setDissipation(0.75);
    spring->setStiffness(10.0);
    spring->set_path(Scholz2015GeometryPath());
    model.addComponent(spring);

    // Configure the Scholz2015GeometryPath. We will update the path after
    // adding it to the PathSpring, so that the Socket connections in each
    // Station (i.e., path point) remain valid.
    Scholz2015GeometryPath& path = spring->updPath<Scholz2015GeometryPath>();
    path.setName("path");

    // Add a path point connected to ground. Since this is the first path point,
    // it defines the origin of the path.
    path.appendPathPoint(model.getGround(), SimTK::Vec3(0.05, 0.05, 0.));

    // Append a second path point, creating a straight line segment between the
    // ground and body "b0".
    path.appendPathPoint(model.getComponent<Body>("/bodyset/b0"),
            SimTK::Vec3(-0.5, 0.1, 0.));

    // Create a ContactCylinder to use as a wrapping obstacle to the path. The
    // cylinder has radius 0.15 m and is attached to body "b0".
    auto* obstacle = new ContactCylinder(0.15,
            SimTK::Vec3(-0.2, 0.2, 0.), SimTK::Vec3(0),
            model.getComponent<Body>("/bodyset/b0"));
    model.addComponent(obstacle);

    // Before we add the obstacle to the path, we must provide a "contact hint"
    // to initialize the wrapping solver. The contact hint is a point on the
    // surface of the obstacle, expressed in the obstacle's frame. The point
    // does not have to lie on the contact geometry's surface, nor does it have
    // to belong to a valid cable path. The choice of the contact hint will
    // determine which side of the cylinder the path wraps around.
    SimTK::Vec3 contact_hint(0., 0.15, 0.);
    path.appendObstacle(*obstacle, contact_hint);

    // At least one path point must follow an obstacle (or list of obstacles)
    // in a Scholz2015GeometryPath. Since this is the last path point we are
    // adding, it defines the insertion of the path.
    path.appendPathPoint(model.getComponent<Body>("/bodyset/b1"),
            SimTK::Vec3(-0.5, 0.1, 0.));

    // Enable warm starts in the wrapping solver. At each time step, the
    // wrapping solver will use the solution from the previous time step as an
    // initial guess for the current time step.
    path.setUseWarmStart(true);

    // Initialize the system.
    SimTK::State state = model.initSystem();
    model.updVisualizer().updSimbodyVisualizer().setBackgroundType(
            SimTK::Visualizer::SolidColor);
    model.updVisualizer().updSimbodyVisualizer().setCameraTransform(
            SimTK::Vec3(0.2, -0.8, 3.0));
    model.updVisualizer().updSimbodyVisualizer().setCameraFieldOfView(0.9);

    // Simulate.
    Manager manager(model);
    manager.initialize(state);
    manager.integrate(20.0);
}