Simbody  3.5
AssemblyCondition_QValue.h
Go to the documentation of this file.
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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines