1 #include <OpenSim/OpenSim.h>
2 #include <OpenSim/Common/Reporter.h>
3
4 using namespace OpenSim;
5 using namespace SimTK;
6
7 class ComplexResponse : public ModelComponent {
8 OpenSim_DECLARE_CONCRETE_OBJECT(ComplexResponse, ModelComponent);
9 public:
10 OpenSim_DECLARE_PROPERTY(strength, double, "per-coord param.");
11 OpenSim_DECLARE_SOCKET(coord, Coordinate, "");
12
13 OpenSim_DECLARE_OUTPUT(term_1, double, getTerm1, SimTK::Stage::Position);
14 OpenSim_DECLARE_OUTPUT(term_2, double, getTerm2, SimTK::Stage::Velocity);
15 OpenSim_DECLARE_OUTPUT(sum, double, getSum, SimTK::Stage::Velocity);
16
ComplexResponse()17 ComplexResponse() {
18 constructProperties();
19 }
getTerm1(const State & s) const20 double getTerm1(const State& s) const {
21 if (!isCacheVariableValid(s, "term_1")) {
22 const auto& coord = getConnectee<Coordinate>("coord");
23 const auto value = coord.getValue(s);
24 setCacheVariableValue(s, "term_1", get_strength() * value);
25 }
26 return getCacheVariableValue<double>(s, "term_1");
27 }
getTerm2(const State & s) const28 double getTerm2(const State& s) const {
29 if (!isCacheVariableValid(s, "term_2")) {
30 const auto& coord = getConnectee<Coordinate>("coord");
31 const auto speed = coord.getSpeedValue(s);
32 setCacheVariableValue(s, "term_2", 2.0 * speed);
33 }
34 return getCacheVariableValue<double>(s, "term_2");
35 }
getSum(const State & s) const36 double getSum(const State& s) const {
37 if (!isCacheVariableValid(s, "sum")) {
38 setCacheVariableValue(s, "sum", getTerm1(s) + getTerm2(s));
39 }
40 return getCacheVariableValue<double>(s, "sum");
41 }
42 private:
constructProperties()43 void constructProperties() {
44 constructProperty_strength(3.0);
45 }
extendAddToSystem(MultibodySystem & system) const46 void extendAddToSystem(MultibodySystem& system) const override {
47 Super::extendAddToSystem(system);
48 addCacheVariable<double>("term_1",
49 0.0, SimTK::Stage::Velocity);
50 addCacheVariable<double>("term_2",
51 0.0, SimTK::Stage::Velocity);
52 addCacheVariable<double>("sum",
53 0.0, SimTK::Stage::Velocity);
54 }
55 };
56
57 class AggregateResponse : public ModelComponent {
58 OpenSim_DECLARE_CONCRETE_OBJECT(AggregateResponse, ModelComponent);
59 public:
AggregateResponse()60 AggregateResponse() { constructProperties(); }
61
62 // TODO propagate this scaling_factor to the ComplexResponses using
63 // Component::getParent.
64 OpenSim_DECLARE_PROPERTY(scaling_factor, double, "Affects each coord.");
65
66
67
68 OpenSim_DECLARE_OUTPUT(total_sum, double, getTotalSum,
69 SimTK::Stage::Position);
70 OpenSim_DECLARE_OUTPUT(total_term_1, double, getTotalTerm1,
71 SimTK::Stage::Velocity);
72 OpenSim_DECLARE_OUTPUT(total_term_2, double, getTotalTerm2,
73 SimTK::Stage::Velocity);
74
75
76
getTotalSum(const State & s) const77 double getTotalSum(const State& s) const {
78 const double basalRate(1.0);
79 double totalSum = basalRate;
80 for (const auto& response : getComponentList<ComplexResponse>()) {
81 totalSum += response.getOutputValue<double>(s, "sum");
82 }
83 return totalSum;
84 }
85
getTotalTerm1(const State & s) const86 double getTotalTerm1(const State& s) const {
87 double totalTerm1=0;
88 for (const auto& response : getComponentList<ComplexResponse>()) {
89 totalTerm1 += response.getOutputValue<double>(s, "term_1");
90 }
91 return totalTerm1;
92 }
93
getTotalTerm2(const State & s) const94 double getTotalTerm2(const State& s) const {
95 double totalTerm2=0;
96 for (const auto& response : getComponentList<ComplexResponse>()) {
97 totalTerm2 += response.getOutputValue<double>(s, "term_2");
98 }
99 return totalTerm2;
100 }
101
102 /*
103 ComplexResponse cresponse1 { constructSubcomponent<ComplexResponse>("complex_response_j1"); }
104 */
105
106 private:
107
constructProperties()108 void constructProperties() {
109 constructProperty_scaling_factor(1.0);
110 }
111 };
112
integrate(const System & system,Integrator & integrator,const State & initialState,Real finalTime)113 void integrate(const System& system, Integrator& integrator,
114 const State& initialState,
115 Real finalTime) {
116 TimeStepper ts(system, integrator);
117 ts.initialize(initialState);
118 ts.setReportAllSignificantStates(true);
119 integrator.setReturnEveryInternalStep(true);
120 while (ts.getState().getTime() < finalTime) {
121 ts.stepTo(finalTime);
122 system.realize(ts.getState(), SimTK::Stage::Report);
123 }
124 }
125
testComplexResponse()126 void testComplexResponse() {
127 Model model;
128
129 // Bodies and joints.
130 auto b1 = new OpenSim::Body("b1", 1, Vec3(0), Inertia(0));
131 auto b2 = new OpenSim::Body("b2", 1, Vec3(0), Inertia(0));
132 auto b3 = new OpenSim::Body("b3", 1, Vec3(0), Inertia(0));
133 auto j1 = new PinJoint("j1", model.getGround(), Vec3(0), Vec3(0),
134 *b1, Vec3(0, 1, 0), Vec3(0));
135 auto j2 = new PinJoint("j2", *b1, Vec3(0), Vec3(0),
136 *b2, Vec3(0, 1, 0), Vec3(0));
137 auto j3 = new PinJoint("j3", *b2, Vec3(0), Vec3(0),
138 *b3, Vec3(0, 1, 0), Vec3(0));
139
140 // Aggregate calculator.
141 auto aggregate = new AggregateResponse();
142 aggregate->setName("aggregate_response");
143 // Add individual responses to the aggregate response
144 auto response1 = new ComplexResponse();
145 response1->setName("complex_response_j1");
146 response1->updSocket<Coordinate>("coord").connect(
147 j1->getCoordinate(PinJoint::Coord::RotationZ));
148 // add to aggregate which takes ownership
149 aggregate->addComponent(response1);
150
151 // now create response 2
152 auto response2 = new ComplexResponse();
153 response2->setName("complex_response_j2");
154 response2->updSocket<Coordinate>("coord").connect(
155 j2->getCoordinate(PinJoint::Coord::RotationZ));
156 // add to aggregate which takes ownership
157 aggregate->addComponent(response2);
158
159 // Add in the components to the model.
160 model.addBody(b1);
161 model.addBody(b2);
162 model.addBody(b3);
163 model.addJoint(j1);
164 model.addJoint(j2);
165 model.addJoint(j3);
166 model.addModelComponent(aggregate);
167
168 auto* reporter = new ConsoleReporter();
169 reporter->setName("reporter");
170 reporter->updInput("inputs").connect(response1->getOutput("sum"));
171 reporter->updInput("inputs").connect(response2->getOutput("sum"));
172 reporter->updInput("inputs").connect(aggregate->getOutput("total_sum"));
173 reporter->updInput("inputs").connect(
174 j1->getCoordinate(PinJoint::Coord::RotationZ).getOutput("value"));
175 // TODO connect by path: reporter->getInput("input").connect("/complex_response/sum");
176 // multi input: reporter->getMultiInput("input").append_connect(cr->getOutput("sum"));
177
178 model.addComponent(reporter);
179
180 State& state = model.initSystem();
181
182 model.updCoordinateSet().get("j1_coord_0").setValue(state, 0.5 * Pi);
183
184 RungeKuttaMersonIntegrator integrator(model.getSystem());
185 integrate(model.getSystem(), integrator, state, 1);
186 }
187
main()188 int main() {
189 // TODO SimTK_START_TEST("futureMuscleMetabolicsResponse");
190 SimTK_SUBTEST(testComplexResponse);
191 //SimTK_END_TEST();
192 }
193