1 #ifndef OPENSIM_ASSEMBLY_SOLVER_H_
2 #define OPENSIM_ASSEMBLY_SOLVER_H_
3 /* -------------------------------------------------------------------------- *
4  *                         OpenSim:  AssemblySolver.h                         *
5  * -------------------------------------------------------------------------- *
6  * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
7  * See http://opensim.stanford.edu and the NOTICE file for more information.  *
8  * OpenSim is developed at Stanford University and supported by the US        *
9  * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
10  * through the Warrior Web program.                                           *
11  *                                                                            *
12  * Copyright (c) 2005-2017 Stanford University and the Authors                *
13  * Author(s): Ajay Seth                                                       *
14  *                                                                            *
15  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
16  * not use this file except in compliance with the License. You may obtain a  *
17  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
18  *                                                                            *
19  * Unless required by applicable law or agreed to in writing, software        *
20  * distributed under the License is distributed on an "AS IS" BASIS,          *
21  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
22  * See the License for the specific language governing permissions and        *
23  * limitations under the License.                                             *
24  * -------------------------------------------------------------------------- */
25 
26 #include "Solver.h"
27 #include "OpenSim/Simulation/CoordinateReference.h"
28 #include "simbody/internal/Assembler.h"
29 
30 namespace SimTK {
31 class QValue;
32 class State;
33 }
34 
35 namespace OpenSim {
36 
37 class Model;
38 
39 //=============================================================================
40 //=============================================================================
41 /**
42  * Solve for the coordinates (degrees-of-freedom) of the model that satisfy the
43  * set of constraints imposed on the model as well as set of desired coordinate
44  * values.  The AssembleSolver provides the option to convert the problem to an
45  * approximate one where the constraint violations are treated as penalties to
46  * to be minimized rather than strictly enforced. This can speed up the time
47  * solution and can be used to seed the constrained problem near to a solution.
48  *
49  * The assembly objective: min A = sum(Wq_i*(q_i-qd_i)^2)) + [Wc*sum(c_err)^2]
50  * iff Wc == Infinity, second term is not included, but
51  *  A is subject to the constraint equations:  G(q)-Go = 0
52  *
53  * When the model (and the number of goals) is guaranteed not to change and the
54  * the initial state is close to the assembly solution (from initial assembly(),
55  * then track() is a efficient method for updating the configuration to track
56  * the small change to the desired coordinate value.
57  *
58  * See SimTK::Assembler for more algorithmic details of the underlying solver.
59  *
60  * @author Ajay Seth
61  * @version 1.0
62  */
63 class OSIMSIMULATION_API AssemblySolver: public Solver {
64 OpenSim_DECLARE_CONCRETE_OBJECT(AssemblySolver, Solver);
65 //=============================================================================
66 // METHODS
67 //=============================================================================
68 public:
69     //--------------------------------------------------------------------------
70     // CONSTRUCTION
71     //--------------------------------------------------------------------------
72 
73     /** Construct an Assembly solver with the coordinate references as the goal
74         of the assembly and (optional)constraint weight. Default is infinity
75         constraint weighting (i.e. rigidly enforced) during assembly. */
76     AssemblySolver(const Model &model,
77                    const SimTK::Array_<CoordinateReference> &coordinateReferences,
78                    double constraintWeight = SimTK::Infinity);
79 
~AssemblySolver()80     virtual ~AssemblySolver() {}
81 
82     /** %Set the unitless accuracy of the assembly solution, which dictates
83         the number of significant digits the solution should be resolved to.
84         Note, setting the accuracy will invalidate the AssemblySolver and one
85         must call assemble() before being able to track().*/
86     void setAccuracy(double accuracy);
87 
88     /** %Set the relative weighting for constraints. Use Infinity to identify the
89         strict enforcement of constraints, otherwise any positive weighting will
90         append the constraint errors to the assembly cost which the solver will
91         minimize.*/
setConstraintWeight(double weight)92     void setConstraintWeight(double weight) {_constraintWeight = weight; }
93 
94     /** Specify which coordinates to match, each with a desired value and a
95         relative weighting. */
getCoordinateReferences()96     const SimTK::Array_<CoordinateReference>& getCoordinateReferences() const
97     {   return _coordinateReferencesp; };
98     /** Once a set of coordinates has been specified its reference value and
99         weight can be updated directly */
100     void updateCoordinateReference(const std::string &coordName, double value,
101                                    double weight=1.0);
102 
103     /** Assemble a model configuration that meets the assembly conditions
104         (desired values and constraints) and accuracy, starting from an initial
105         state that does not have to satisfy the constraints. */
106     virtual void assemble(SimTK::State &s);
107 
108     /** Obtain a model configuration that meets the assembly conditions
109         (desired values and constraints) given a state that satisfies or
110         is close to satisfying the constraints. Note there can be no change
111         in the number of constraints or desired coordinates. Desired
112         coordinate values can and should be updated between repeated calls
113         to track a desired trajectory of coordinate values. Use assemble()
114         first to obtain the first solution and use track() to efficiently
115         find a nearby solution due to a small change in the desired value.*/
116     virtual void track(SimTK::State &s);
117 
118     /** Read access to the underlying SimTK::Assembler. */
119     const SimTK::Assembler& getAssembler() const;
120 
121 protected:
122     /** Internal method to convert the CoordinateReferences into goals of the
123         assembly solver. Subclasses, can add and override to include other goals
124         such as point of interest matching (Marker tracking). This method is
125         automatically called by assemble. */
126     virtual void setupGoals(SimTK::State &s);
127     /** Internal method to update the time, reference values and/or their
128         weights that define the goals, based on the passed in state. This method
129         is called at the end of setupGoals() and beginning of track()*/
130     virtual void updateGoals(const SimTK::State &s);
131 
132     /** Write access to the underlying SimTK::Assembler. */
133     SimTK::Assembler& updAssembler();
134 
135 private:
136 
137     // The assembly solution accuracy
138     double _accuracy;
139 
140     // Weight for built-in constraints to be satisfied
141     double _constraintWeight;
142 
143     // The coordinates reference value and weighting. This is full copy rather than ref.
144     SimTK::Array_<CoordinateReference> _coordinateReferencesp;
145 
146     // Underlying SimTK::Assembler that will perform the assembly
147     SimTK::ResetOnCopy< std::unique_ptr<SimTK::Assembler>> _assembler;
148 
149     SimTK::Array_<SimTK::QValue*> _coordinateAssemblyConditions;
150 //=============================================================================
151 };  // END of class AssemblySolver
152 //=============================================================================
153 } // namespace
154 
155 #endif // OPENSIM_ASSEMBLY_SOLVER_H_
156