Simbody  3.8
AssemblyCondition_QValue.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_QVALUE_H_
2 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_QVALUE_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"
31 
32 namespace SimTK {
33 
34 
35 //------------------------------------------------------------------------------
36 // Q VALUE
37 //------------------------------------------------------------------------------
41 class QValue : public AssemblyCondition {
42 public:
47  Real value)
48  : AssemblyCondition("QValue"),
49  mobodIndex(mbx), qIndex(qx), value(value) {}
50 
53  Real getValue() const {return value;}
56  void setValue(Real newValue) {value=newValue;}
57 
58  // For constraint:
59  int getNumEquations(const State&) const {return 1;}
60  int calcErrors(const State& state, Vector& error) const override {
62  const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
63  error.resize(1);
64  error[0] = mobod.getOneQ(state, qIndex) - value;
65  return 0;
66  }
67  // Error jacobian is a zero-row except for a 1 in this q's entry (if
68  // this q is free).
69  int calcErrorJacobian(const State& state, Matrix& J) const override {
71  const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
72  J.resize(1, getNumFreeQs());
73  J = 0; // will have at most one non-zero
74 
75  // Find the FreeQIndex corresponding to this q.
76  const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex);
77  const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx);
78 
79  // If this q isn't free then there is no way to affect the error
80  // so the Jacobian stays all-zero.
81  if (thisFreeIx.isValid())
82  J(0,thisFreeIx) = 1;
83 
84  return 0;
85  }
86 
87  // For goal: goal = (q-value)^2 / 2 (the /2 is for gradient beauty)
88  int calcGoal(const State& state, Real& goal) const override {
90  const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
91  goal = square(mobod.getOneQ(state, qIndex) - value) / 2;
92  return 0;
93  }
94  // Return a gradient with only this q's entry non-zero (if
95  // this q is free).
96  int calcGoalGradient(const State& state, Vector& grad) const override {
98  const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
99  grad.resize(getNumFreeQs());
100  grad = 0; // will have at most one non-zero
101 
102  // Find the FreeQIndex corresponding to this q.
103  const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex);
104  const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx);
105 
106  // If this q isn't free then there is no way to affect the goal
107  // so the gradient stays all-zero.
108  if (thisFreeIx.isValid())
109  grad[thisFreeIx] = mobod.getOneQ(state, qIndex) - value;
110 
111  return 0;
112  }
113 
114 private:
115  MobilizedBodyIndex mobodIndex;
116  MobilizerQIndex qIndex;
117  Real value;
118 };
119 
120 } // namespace SimTK
121 
122 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_QVALUE_H_
Includes internal headers providing declarations for the basic SimTK Core classes,...
Every Simbody header and source file should include this header before any other Simbody header.
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
int getNumFreeQs() const
Ask the assembler how many free q's there are; only valid after initialization but does not invoke in...
Definition: AssemblyCondition.h:167
const SimbodyMatterSubsystem & getMatterSubsystem() const
Ask the assembler for the MultibodySystem with which it is associated and extract the SimbodyMatterSu...
Definition: AssemblyCondition.h:183
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
MatrixBase & resize(int m, int n)
Change the size of this matrix.
Definition: MatrixBase.h:773
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:169
Real getOneQ(const State &state, int which) const
Return one of the generalized coordinates q from this mobilizer's partition of the matter subsystem's...
QIndex getFirstQIndex(const State &state) const
Return the global QIndex of the first q for this mobilizer; all the q's range from getFirstQIndex() t...
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Unique integer type for Subsystem-local q indexing.
This AssemblyCondition requests that a particular generalized coordinate end up with a specified valu...
Definition: AssemblyCondition_QValue.h:41
int calcErrorJacobian(const State &state, Matrix &J) const override
Override to supply an analytic Jacobian for the assembly errors returned by calcErrors().
Definition: AssemblyCondition_QValue.h:69
int calcGoalGradient(const State &state, Vector &grad) const override
Override to supply an analytic gradient for this assembly condition's goal.
Definition: AssemblyCondition_QValue.h:96
void setValue(Real newValue)
Change the value to be used for this generalized coordinate; this can be done repeatedly during track...
Definition: AssemblyCondition_QValue.h:56
Real getValue() const
Return the currently set value to be used for this generalized coordinate.
Definition: AssemblyCondition_QValue.h:53
int getNumEquations(const State &) const
Definition: AssemblyCondition_QValue.h:59
int calcErrors(const State &state, Vector &error) const override
Calculate the amount by which this assembly condition is violated by the q values in the given state,...
Definition: AssemblyCondition_QValue.h:60
QValue(MobilizedBodyIndex mbx, MobilizerQIndex qx, Real value)
Construct an assembly condition that requests that the specified generalized coordinate be brought to...
Definition: AssemblyCondition_QValue.h:46
int calcGoal(const State &state, Real &goal) const override
Calculate the current contribution (>= 0) of this assembly condition to the goal value that is being ...
Definition: AssemblyCondition_QValue.h:88
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
const MobilizedBody & getMobilizedBody(MobilizedBodyIndex) const
Given a MobilizedBodyIndex, return a read-only (const) reference to the corresponding MobilizedBody w...
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
VectorBase & resize(int m)
Definition: VectorBase.h:451
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
unsigned char square(unsigned char u)
Definition: Scalar.h:349
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:607