Simbody  3.5
AssemblyCondition.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_H_
2 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2010-14 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKcommon.h"
30 
31 #include <map>
32 
33 namespace SimTK {
34 
35 //------------------------------------------------------------------------------
36 // ASSEMBLY CONDITION
37 //------------------------------------------------------------------------------
45 public:
46 
49 explicit AssemblyCondition(const String& name)
50 : name(name), assembler(0) {}
51 
53 virtual ~AssemblyCondition() {}
54 
61 virtual int initializeCondition() const {return 0;}
62 
65 virtual void uninitializeCondition() const {}
66 
75 virtual int calcErrors(const State& state, Vector& err) const
76 { return -1; }
77 
87 virtual int calcErrorJacobian(const State& state, Matrix& jacobian) const
88 { return -1; }
89 
97 virtual int getNumErrors(const State& state) const
98 { Vector err;
99  const int status = calcErrors(state, err);
100  if (status == 0)
101  return err.size();
102  SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::getNumErrors()",
103  "The default implementation of getNumErrors() depends on"
104  " calcErrors() but that method was not implemented for assembly"
105  " condition '%s'.", name.c_str());
106  SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::getNumErrors()",
107  "The default implementation of getNumErrors() uses calcErrors()"
108  " which returned status %d (assembly condition '%s').",
109  status, name.c_str());
110  return -1; // NOTREACHED
111 }
112 
117 virtual int calcGoal(const State& state, Real& goal) const
118 { static Vector err;
119  const int status = calcErrors(state, err);
120  if (status == 0)
121  { goal = err.normSqr() / std::max(1,err.size());
122  return 0; }
123  SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::calcGoal()",
124  "The default implementation of calcGoal() depends on calcErrors()"
125  " but that method was not implemented for assembly condition '%s'.",
126  name.c_str());
127  SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::calcGoal()",
128  "The default implementation of calcGoal() uses calcErrors() which"
129  " returned status %d (assembly condition '%s').",
130  status, name.c_str());
131  return -1; // NOTREACHED
132 }
133 
140 virtual int calcGoalGradient(const State& state, Vector& gradient) const
141 { return -1; }
142 
144 const char* getName() const {return name.c_str();}
145 
148 bool isInAssembler() const {return assembler != 0;}
152 const Assembler& getAssembler() const
153 { assert(assembler); return *assembler;}
157 AssemblyConditionIndex getAssemblyConditionIndex() const
158 { return myAssemblyConditionIndex; }
159 
160 //------------------------------------------------------------------------------
161  protected:
162 //------------------------------------------------------------------------------
163 // These are useful when writing concrete AssemblyConditions.
164 
167 int getNumFreeQs() const {return getAssembler().getNumFreeQs();}
171 QIndex getQIndexOfFreeQ(Assembler::FreeQIndex fx) const
172 { return getAssembler().getQIndexOfFreeQ(fx); }
176 Assembler::FreeQIndex getFreeQIndexOfQ(QIndex qx) const
177 { return getAssembler().getFreeQIndexOfQ(qx); }
180 { return getAssembler().getMultibodySystem(); }
184 { return getMultibodySystem().getMatterSubsystem(); }
185 
188 void initializeAssembler() const {
189  // The Assembler will in turn invoke initializeCondition().
190  if (isInAssembler()) getAssembler().initialize();
191  else initializeCondition();
192 }
193 
197 void uninitializeAssembler() const {
198  // The Assembler will in turn invoke uninitializeCondition().
199  if (isInAssembler()) getAssembler().uninitialize();
200  else uninitializeCondition();
201 }
202 
203 //------------------------------------------------------------------------------
204  private:
205 //------------------------------------------------------------------------------
206 // This method is used by the Assembler when the AssemblyCondition object
207 // is adopted.
208 friend class Assembler;
209 void setAssembler(const Assembler& assembler, AssemblyConditionIndex acx) {
210  assert(!this->assembler);
211  this->assembler = &assembler;
212  this->myAssemblyConditionIndex = acx;
213 }
214 
215 String name; // assembly condition name
216 const Assembler* assembler;
217 AssemblyConditionIndex myAssemblyConditionIndex;
218 };
219 
220 } // namespace SimTK
221 
222 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_H_
void initializeAssembler() const
Call this method before doing anything that logically requires the Assembler, or at least this Assemb...
Definition: AssemblyCondition.h:188
QIndex getQIndexOfFreeQ(Assembler::FreeQIndex fx) const
Ask the assembler where to find the actual q in the State that corresponds to a given free q; only va...
Definition: AssemblyCondition.h:171
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
ScalarNormSq normSqr() const
This is the scalar Frobenius norm, and its square.
Definition: MatrixBase.h:723
bool isInAssembler() const
Test whether this AssemblyCondition has already been adopted by an Assembler.
Definition: AssemblyCondition.h:148
int size() const
Definition: VectorBase.h:396
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
AssemblyCondition(const String &name)
Base class constructor just takes the assembly condition name and saves it.
Definition: AssemblyCondition.h:49
Every Simbody header and source file should include this header before any other Simbody header...
This Study attempts to find a configuration (set of joint coordinates q) of a Simbody MultibodySystem...
Definition: Assembler.h:148
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
virtual int initializeCondition() const
This is called whenever the Assembler is initialized in case this assembly condition wants to do some...
Definition: AssemblyCondition.h:61
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
virtual ~AssemblyCondition()
Destructor is virtual for use by derived classes.
Definition: AssemblyCondition.h:53
const SimbodyMatterSubsystem & getMatterSubsystem() const
Ask the assembler for the MultibodySystem with which it is associated and extract the SimbodyMatterSu...
Definition: AssemblyCondition.h:183
AssemblyConditionIndex getAssemblyConditionIndex() const
Return the AssemblyConditionIndex of this concrete AssemblyCondition within the Assembler that has ad...
Definition: AssemblyCondition.h:157
virtual int getNumErrors(const State &state) const
Override to supply an efficient method for determining how many errors will be returned by calcErrors...
Definition: AssemblyCondition.h:97
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
const Assembler & getAssembler() const
Return the Assembler that has adopted this AssemblyCondition.
Definition: AssemblyCondition.h:152
const MultibodySystem & getMultibodySystem() const
Ask the assembler for the MultibodySystem with which it is associated.
Definition: AssemblyCondition.h:179
void uninitializeAssembler() const
Call this when modifying any parameter of the concrete AssemblyCondition that would require reinitial...
Definition: AssemblyCondition.h:197
ELEM max(const VectorBase< ELEM > &v)
Definition: VectorMath.h:251
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
virtual int calcGoalGradient(const State &state, Vector &gradient) const
Override to supply an analytic gradient for this assembly condition&#39;s goal.
Definition: AssemblyCondition.h:140
Unique integer type for Subsystem-local q indexing.
virtual void uninitializeCondition() const
This is called whenever the containing Assembler is uninitialized in case this assembly condition has...
Definition: AssemblyCondition.h:65
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition: String.h:62
int getNumFreeQs() const
Ask the assembler how many free q&#39;s there are; only valid after initialization but does not invoke in...
Definition: AssemblyCondition.h:167
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
virtual int calcErrorJacobian(const State &state, Matrix &jacobian) const
Override to supply an analytic Jacobian for the assembly errors returned by calcErrors().
Definition: AssemblyCondition.h:87
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
const char * getName() const
Return the name assigned to this AssemblyCondition on construction.
Definition: AssemblyCondition.h:144
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
virtual int calcErrors(const State &state, Vector &err) const
Calculate the amount by which this assembly condition is violated by the q values in the given state...
Definition: AssemblyCondition.h:75
virtual int calcGoal(const State &state, Real &goal) const
Calculate the current contribution (>= 0) of this assembly condition to the goal value that is being ...
Definition: AssemblyCondition.h:117
Assembler::FreeQIndex getFreeQIndexOfQ(QIndex qx) const
Ask the assembler where to find the free q (if any) that corresponds to a given q in the State; only ...
Definition: AssemblyCondition.h:176