1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) Example: Pendulum *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2007-12 Stanford University and the Authors. *
10 * Authors: Ajay Seth *
11 * Contributors: Michael Sherman *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24
25 // Define two identical double pendulums, one modeled the easy way using
26 // two pin mobilizers, the other modeled with free mobilizers plus a ball
27 // constraint, plus two "constant angle" constraints to get rid of the extra
28 // rotational degrees of freedom.
29 // We're going to show that the resulting reaction forces are identical.
30
31
32 #include "Simbody.h"
33
34 using namespace SimTK;
35 using std::cout; using std::endl;
36
main()37 int main() {
38 try {
39 // Create the system, with subsystems for the bodies and some forces.
40 MultibodySystem system;
41 SimbodyMatterSubsystem matter(system);
42 GeneralForceSubsystem forces(system);
43
44 // Add gravity as a force element.
45 Rotation x45(Pi/4, XAxis);
46 Rotation y45(Pi/4, YAxis);
47 Rotation z45(Pi/4, ZAxis);
48 Force::UniformGravity gravity(forces, matter, Vec3(10, Real(-9.8), 3));
49 // Create the body and some artwork for it.
50 Body::Rigid pendulumBody(MassProperties(1.0, Vec3(0), Inertia(1)));
51 pendulumBody.addDecoration(Transform(),
52 DecorativeSphere(Real(0.1)).setColor(Red));
53
54 // Add an instance of the body to the multibody system by connecting
55 // it to Ground via a pin mobilizer.
56 MobilizedBody::Pin pendulum1(matter.updGround(),
57 Transform(/*x45,*/Vec3(0,-1,0)),
58 pendulumBody,
59 Transform(Vec3(0, 1, 0)));
60 MobilizedBody::Pin pendulum1b(pendulum1,
61 Transform(/*x45,*/Vec3(0,-1,0)),
62 pendulumBody,
63 Transform(Vec3(0, 1, 0)));
64
65 // Now make an identical system with pin joints faked up using a
66 // free mobilizer + ball constraint + two angle constraints.
67 MobilizedBody::Free pendulum2(matter.updGround(),
68 Transform(/*x45,*/Vec3(2,-1,0)),
69 pendulumBody,
70 Transform(Vec3(0,1,0)));
71 Constraint::Ball ballcons2(matter.updGround(), Vec3(2,-1,0),
72 pendulum2, Vec3(0,1,0));
73 const Transform& X_GF2 = pendulum2.getDefaultInboardFrame();
74 const Transform& X_P2M = pendulum2.getDefaultOutboardFrame();
75 Constraint::ConstantAngle angx2(matter.Ground(), X_GF2.x(),
76 pendulum2, X_P2M.z());
77 Constraint::ConstantAngle angy2(matter.Ground(), X_GF2.y(),
78 pendulum2, X_P2M.z());
79
80 MobilizedBody::Free pendulum2b(pendulum2,
81 Transform(/*x45,*/Vec3(0,-1,0)),
82 pendulumBody,
83 Transform(Vec3(0,1,0)));
84 Constraint::Ball ballcons2b(pendulum2, Vec3(0,-1,0),
85 pendulum2b, Vec3(0,1,0));
86 const Transform& X_GF2b = pendulum2b.getDefaultInboardFrame();
87 const Transform& X_P2Mb = pendulum2b.getDefaultOutboardFrame();
88 Constraint::ConstantAngle angx2b(pendulum2, X_GF2b.x(),
89 pendulum2b, X_P2Mb.z());
90 Constraint::ConstantAngle angy2b(pendulum2, X_GF2b.y(),
91 pendulum2b, X_P2Mb.z());
92
93
94 // Visualize with default options; ask for a report every 1/30 of a second
95 // to match the Visualizer's default 30 frames per second rate.
96 Visualizer viz(system);
97 system.addEventReporter(new Visualizer::Reporter(viz, Real(1./30)));
98
99 // Initialize the system and state.
100
101 system.realizeTopology();
102 State state = system.getDefaultState();
103 pendulum1.setOneQ(state, 0, Pi/4);
104 //pendulum1.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
105
106 //pendulum1b.setOneU(state, 0, 1.0); // initial velocity 1 rad/sec
107 pendulum1b.setOneQ(state, 0, Pi/4);
108
109 pendulum2.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
110 //pendulum2.setUToFitAngularVelocity(state, Vec3(0,0,1));
111 pendulum2b.setQToFitRotation(state, Rotation(Pi/4, ZAxis));
112 //pendulum2b.setUToFitAngularVelocity(state, Vec3(0,0,1));
113
114 system.realize(state);
115 const Vector lambda = state.getMultipliers();
116 Vector_<SpatialVec> consBodyForcesInG;
117 Vector consMobForces;
118 matter.calcConstraintForcesFromMultipliers(state, -lambda, consBodyForcesInG,
119 consMobForces);
120 const MobodIndex p2x = pendulum2.getMobilizedBodyIndex();
121 const MobodIndex p2bx = pendulum2b.getMobilizedBodyIndex();
122 const Rotation& R_G2 = pendulum2.getBodyTransform(state).R();
123 //consBodyForcesInG[p2x] = shiftForceFromTo(consBodyForcesInG[p2x],
124 // Vec3(0), R_G2*Vec3(0,1,0));
125
126 const int nb = matter.getNumBodies();
127 Vector_<SpatialVec> forcesAtMInG;
128 matter.calcMobilizerReactionForces(state, forcesAtMInG);
129
130 // The above method returns reactions *on the child body* at the outboard
131 // mobilizer frame M (that is, the frame fixed on the child body).
132 // Calculate the same reactions, but *on the parent body* and at the
133 // inboard mobilizer frame F (that is, the frame fixed on the parent body).
134 // This is done by shifting the reaction forces across the mobilizer from
135 // M to F, and negating.
136 // Note that for the example here the mobilizers aren't translating so
137 // the origins Mo and Fo are identical. Nevertheless we're using the
138 // generic code here that should work for any system.
139
140 Vector_<SpatialVec> forcesAtFInG(nb); // to hold the result
141 forcesAtFInG[0] = -forcesAtMInG[0]; // Ground is "welded" at origin
142 for (MobilizedBodyIndex i(1); i < matter.getNumBodies(); ++i) {
143 const MobilizedBody& body = matter.getMobilizedBody(i);
144 const MobilizedBody& parent = body.getParentMobilizedBody();
145 // Want to shift negated reaction by p_MF_G, the vector from M
146 // to F across the mobilizer, expressed in Ground. We can get p_FM,
147 // then re-express in Ground for the shift and negate.
148 const Vec3& p_FM = body.getMobilizerTransform(state).p();
149 const Rotation& R_PF = body.getInboardFrame(state).R(); // In parent.
150 const Rotation& R_GP = parent.getBodyTransform(state).R();
151 Rotation R_GF = R_GP*R_PF; // F frame orientation in Ground.
152 Vec3 p_MF_G = -(R_GF*p_FM); // Re-express and negate shift vector.
153 forcesAtFInG[i] = -shiftForceBy(forcesAtMInG[i], p_MF_G);
154 }
155
156 std::cout << "Reactions @M: " << forcesAtMInG << "\n";
157 std::cout << "Reactions @F: " << forcesAtFInG << "\n";
158 std::cout << "norm of difference: " << (forcesAtMInG+forcesAtFInG).norm()
159 << "\n";
160
161 const MobodIndex p1x = pendulum1.getMobilizedBodyIndex();
162 const MobodIndex p1bx = pendulum1b.getMobilizedBodyIndex();
163 const Rotation& R_G1 = pendulum1.getBodyTransform(state).R();
164 const Rotation& R_G1b = pendulum1b.getBodyTransform(state).R();
165
166 // This time shift the child reactions from the mobilizer frames M to the
167 // child body frames B so we can compare with the constraint forces that
168 // are returned at the child body frame.
169 Vector_<SpatialVec> forcesAtBInG(nb);
170 for (MobodIndex i(0); i < nb; ++i) {
171 const Mobod& body = matter.getMobilizedBody(i);
172 const Vec3& p_BM = body.getOutboardFrame(state).p();
173 const Rotation& R_GB = body.getBodyTransform(state).R();
174 forcesAtBInG[i] = shiftForceFromTo(forcesAtMInG[i],
175 R_GB*p_BM, Vec3(0));
176 }
177
178 std::cout << "Pin mobilizer reaction forces:\n";
179 std::cout << "FB_G=" << forcesAtBInG[p1x]
180 << " " << forcesAtBInG[p1bx] << "\n";
181
182 std::cout << "Constraint reaction forces (should be the same):\n";
183 cout << "FC_G=" << -(ballcons2.getConstrainedBodyForcesAsVector(state)
184 + angx2.getConstrainedBodyForcesAsVector(state)
185 + angy2.getConstrainedBodyForcesAsVector(state))[1] << " ";
186 cout << -(ballcons2b.getConstrainedBodyForcesAsVector(state)
187 + angx2b.getConstrainedBodyForcesAsVector(state)
188 + angy2b.getConstrainedBodyForcesAsVector(state))[1] << endl;
189
190 viz.report(state);
191 // Simulate it.
192 cout << "Hit ENTER to run a short simulation ...";
193 getchar();
194
195 RungeKuttaMersonIntegrator integ(system);
196 TimeStepper ts(system, integ);
197 ts.initialize(state);
198 ts.stepTo(1.0);
199 state = integ.getState();
200 system.realize(state);
201 matter.calcMobilizerReactionForces(state, forcesAtMInG);
202 const Transform& X_GP = pendulum1.getBodyTransform(state);
203 //forcesAtMInG[1][1] = X_GP.R()*forcesAtMInG[1][1];
204 std::cout << "FM_G=" << forcesAtMInG << "\n";
205 ts.stepTo(Real(1.2));
206 state = integ.getState();
207 system.realize(state);
208 matter.calcMobilizerReactionForces(state, forcesAtMInG);
209 std::cout << "FM_G=" << forcesAtMInG << "\n";
210
211 } catch (const std::exception& e) {
212 std::cout << "ERROR: " << e.what() << std::endl;
213 return 1;
214 } catch (...) {
215 std::cout << "UNKNOWN EXCEPTION\n";
216 return 1;
217 }
218
219 return 0;
220 }
221