Simbody 3.7
Loading...
Searching...
No Matches
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
32namespace SimTK {
33
34
35//------------------------------------------------------------------------------
36// Q VALUE
37//------------------------------------------------------------------------------
41class QValue : public AssemblyCondition {
42public:
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
114private:
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:606