Simbody  3.5
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_11 {
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_11 {
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_11 {
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_11 {
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_
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...
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
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...
int getNumEquations(const State &) const
Definition: AssemblyCondition_QValue.h:59
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
Every Simbody header and source file should include this header before any other Simbody header...
const MobilizedBody & getMobilizedBody(MobilizedBodyIndex) const
Given a MobilizedBodyIndex, return a read-only (const) reference to the corresponding MobilizedBody w...
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:593
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
unsigned char square(unsigned char u)
Definition: Scalar.h:351
const SimbodyMatterSubsystem & getMatterSubsystem() const
Ask the assembler for the MultibodySystem with which it is associated and extract the SimbodyMatterSu...
Definition: AssemblyCondition.h:183
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
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
#define OVERRIDE_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:267
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
Unique integer type for Subsystem-local q indexing.
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
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 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
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:167
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
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
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
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
This AssemblyCondition requests that a particular generalized coordinate end up with a specified valu...
Definition: AssemblyCondition_QValue.h:41
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
Real getValue() const
Return the currently set value to be used for this generalized coordinate.
Definition: AssemblyCondition_QValue.h:53