Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_QVALUE_H_ 00002 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_QVALUE_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 #include "simbody/internal/AssemblyCondition.h" 00031 00032 namespace SimTK { 00033 00034 00035 //------------------------------------------------------------------------------ 00036 // Q VALUE 00037 //------------------------------------------------------------------------------ 00041 class QValue : public AssemblyCondition { 00042 public: 00046 QValue(MobilizedBodyIndex mbx, MobilizerQIndex qx, 00047 Real value) 00048 : AssemblyCondition("QValue"), 00049 mobodIndex(mbx), qIndex(qx), value(value) {} 00050 00053 Real getValue() const {return value;} 00056 void setValue(Real newValue) {value=newValue;} 00057 00058 // For constraint: 00059 int getNumEquations(const State&) const {return 1;} 00060 int calcErrors(const State& state, Vector& error) const OVERRIDE_11 { 00061 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00062 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00063 error.resize(1); 00064 error[0] = mobod.getOneQ(state, qIndex) - value; 00065 return 0; 00066 } 00067 // Error jacobian is a zero-row except for a 1 in this q's entry (if 00068 // this q is free). 00069 int calcErrorJacobian(const State& state, Matrix& J) const OVERRIDE_11 { 00070 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00071 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00072 J.resize(1, getNumFreeQs()); 00073 J = 0; // will have at most one non-zero 00074 00075 // Find the FreeQIndex corresponding to this q. 00076 const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex); 00077 const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx); 00078 00079 // If this q isn't free then there is no way to affect the error 00080 // so the Jacobian stays all-zero. 00081 if (thisFreeIx.isValid()) 00082 J(0,thisFreeIx) = 1; 00083 00084 return 0; 00085 } 00086 00087 // For goal: goal = (q-value)^2 / 2 (the /2 is for gradient beauty) 00088 int calcGoal(const State& state, Real& goal) const OVERRIDE_11 { 00089 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00090 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00091 goal = square(mobod.getOneQ(state, qIndex) - value) / 2; 00092 return 0; 00093 } 00094 // Return a gradient with only this q's entry non-zero (if 00095 // this q is free). 00096 int calcGoalGradient(const State& state, Vector& grad) const OVERRIDE_11 { 00097 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00098 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00099 grad.resize(getNumFreeQs()); 00100 grad = 0; // will have at most one non-zero 00101 00102 // Find the FreeQIndex corresponding to this q. 00103 const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex); 00104 const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx); 00105 00106 // If this q isn't free then there is no way to affect the goal 00107 // so the gradient stays all-zero. 00108 if (thisFreeIx.isValid()) 00109 grad[thisFreeIx] = mobod.getOneQ(state, qIndex) - value; 00110 00111 return 0; 00112 } 00113 00114 private: 00115 MobilizedBodyIndex mobodIndex; 00116 MobilizerQIndex qIndex; 00117 Real value; 00118 }; 00119 00120 } // namespace SimTK 00121 00122 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_QVALUE_H_