Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_H_ 00002 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm) * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK biosimulation toolkit originating from * 00008 * Simbios, the NIH National Center for Physics-Based Simulation of * 00009 * Biological Structures at Stanford, funded under the NIH Roadmap for * 00010 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2010-14 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00027 #include "SimTKcommon.h" 00028 #include "simbody/internal/common.h" 00029 #include "simbody/internal/Assembler.h" 00030 00031 #include <map> 00032 00033 namespace SimTK { 00034 00035 //------------------------------------------------------------------------------ 00036 // ASSEMBLY CONDITION 00037 //------------------------------------------------------------------------------ 00044 class SimTK_SIMBODY_EXPORT AssemblyCondition { 00045 public: 00046 00049 explicit AssemblyCondition(const String& name) 00050 : name(name), assembler(0) {} 00051 00053 virtual ~AssemblyCondition() {} 00054 00061 virtual int initializeCondition() const {return 0;} 00062 00065 virtual void uninitializeCondition() const {} 00066 00075 virtual int calcErrors(const State& state, Vector& err) const 00076 { return -1; } 00077 00087 virtual int calcErrorJacobian(const State& state, Matrix& jacobian) const 00088 { return -1; } 00089 00097 virtual int getNumErrors(const State& state) const 00098 { Vector err; 00099 const int status = calcErrors(state, err); 00100 if (status == 0) 00101 return err.size(); 00102 SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::getNumErrors()", 00103 "The default implementation of getNumErrors() depends on" 00104 " calcErrors() but that method was not implemented for assembly" 00105 " condition '%s'.", name.c_str()); 00106 SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::getNumErrors()", 00107 "The default implementation of getNumErrors() uses calcErrors()" 00108 " which returned status %d (assembly condition '%s').", 00109 status, name.c_str()); 00110 return -1; // NOTREACHED 00111 } 00112 00117 virtual int calcGoal(const State& state, Real& goal) const 00118 { static Vector err; 00119 const int status = calcErrors(state, err); 00120 if (status == 0) 00121 { goal = err.normSqr() / std::max(1,err.size()); 00122 return 0; } 00123 SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::calcGoal()", 00124 "The default implementation of calcGoal() depends on calcErrors()" 00125 " but that method was not implemented for assembly condition '%s'.", 00126 name.c_str()); 00127 SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::calcGoal()", 00128 "The default implementation of calcGoal() uses calcErrors() which" 00129 " returned status %d (assembly condition '%s').", 00130 status, name.c_str()); 00131 return -1; // NOTREACHED 00132 } 00133 00140 virtual int calcGoalGradient(const State& state, Vector& gradient) const 00141 { return -1; } 00142 00144 const char* getName() const {return name.c_str();} 00145 00148 bool isInAssembler() const {return assembler != 0;} 00152 const Assembler& getAssembler() const 00153 { assert(assembler); return *assembler;} 00157 AssemblyConditionIndex getAssemblyConditionIndex() const 00158 { return myAssemblyConditionIndex; } 00159 00160 //------------------------------------------------------------------------------ 00161 protected: 00162 //------------------------------------------------------------------------------ 00163 // These are useful when writing concrete AssemblyConditions. 00164 00167 int getNumFreeQs() const {return getAssembler().getNumFreeQs();} 00171 QIndex getQIndexOfFreeQ(Assembler::FreeQIndex fx) const 00172 { return getAssembler().getQIndexOfFreeQ(fx); } 00176 Assembler::FreeQIndex getFreeQIndexOfQ(QIndex qx) const 00177 { return getAssembler().getFreeQIndexOfQ(qx); } 00179 const MultibodySystem& getMultibodySystem() const 00180 { return getAssembler().getMultibodySystem(); } 00183 const SimbodyMatterSubsystem& getMatterSubsystem() const 00184 { return getMultibodySystem().getMatterSubsystem(); } 00185 00188 void initializeAssembler() const { 00189 // The Assembler will in turn invoke initializeCondition(). 00190 if (isInAssembler()) getAssembler().initialize(); 00191 else initializeCondition(); 00192 } 00193 00197 void uninitializeAssembler() const { 00198 // The Assembler will in turn invoke uninitializeCondition(). 00199 if (isInAssembler()) getAssembler().uninitialize(); 00200 else uninitializeCondition(); 00201 } 00202 00203 //------------------------------------------------------------------------------ 00204 private: 00205 //------------------------------------------------------------------------------ 00206 // This method is used by the Assembler when the AssemblyCondition object 00207 // is adopted. 00208 friend class Assembler; 00209 void setAssembler(const Assembler& assembler, AssemblyConditionIndex acx) { 00210 assert(!this->assembler); 00211 this->assembler = &assembler; 00212 this->myAssemblyConditionIndex = acx; 00213 } 00214 00215 String name; // assembly condition name 00216 const Assembler* assembler; 00217 AssemblyConditionIndex myAssemblyConditionIndex; 00218 }; 00219 00220 } // namespace SimTK 00221 00222 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_H_