Simbody  3.6
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_
virtual void uninitializeCondition() const
This is called whenever the containing Assembler is uninitialized in case this assembly condition has...
Definition: AssemblyCondition.h:65
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_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
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
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
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
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:722
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
void initializeAssembler() const
Call this method before doing anything that logically requires the Assembler, or at least this Assemb...
Definition: AssemblyCondition.h:188
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:606
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
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
virtual ~AssemblyCondition()
Destructor is virtual for use by derived classes.
Definition: AssemblyCondition.h:53
const char * getName() const
Return the name assigned to this AssemblyCondition on construction.
Definition: AssemblyCondition.h:144
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
void uninitializeAssembler() const
Call this when modifying any parameter of the concrete AssemblyCondition that would require reinitial...
Definition: AssemblyCondition.h:197
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
AssemblyConditionIndex getAssemblyConditionIndex() const
Return the AssemblyConditionIndex of this concrete AssemblyCondition within the Assembler that has ad...
Definition: AssemblyCondition.h:157
const SimbodyMatterSubsystem & getMatterSubsystem() const
Ask the assembler for the MultibodySystem with which it is associated and extract the SimbodyMatterSu...
Definition: AssemblyCondition.h:183
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
Unique integer type for Subsystem-local q indexing.
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition: String.h:62
const MultibodySystem & getMultibodySystem() const
Ask the assembler for the MultibodySystem with which it is associated.
Definition: AssemblyCondition.h:179
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
bool isInAssembler() const
Test whether this AssemblyCondition has already been adopted by an Assembler.
Definition: AssemblyCondition.h:148
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
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
int size() const
Definition: VectorBase.h:396
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
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
const Assembler & getAssembler() const
Return the Assembler that has adopted this AssemblyCondition.
Definition: AssemblyCondition.h:152