Debian Codegraph @

tutorials/tutorial_multi_pendulum/main.cpp

raw
1 /*
2 * Copyright (c) 2011-2022, The DART development contributors
3 * All rights reserved.
4 *
5 * The list of contributors can be found at:
6 * https://github.com/dartsim/dart/blob/master/LICENSE
7 *
8 * This file is provided under the following "BSD-style" License:
9 * Redistribution and use in source and binary forms, with or
10 * without modification, are permitted provided that the following
11 * conditions are met:
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
19 * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
20 * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
22 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
23 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
24 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
25 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
26 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30 * POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #include <dart/dart.hpp>
34 #include <dart/gui/gui.hpp>
35
36 const double default_height = 1.0; // m
37 const double default_width = 0.2; // m
38 const double default_depth = 0.2; // m
39
40 [[maybe_unused]] const double default_torque = 15.0; // N-m
41 [[maybe_unused]] const double default_force = 15.0; // N
42 const int default_countdown = 200; // Number of timesteps for applying force
43
44 const double default_rest_position = 0.0;
45 const double delta_rest_position = dart::math::toRadian(10.0);
46
47 const double default_stiffness = 0.0;
48 const double delta_stiffness = 10;
49
50 const double default_damping = 5.0;
51 const double delta_damping = 1.0;
52
53 using namespace dart::dynamics;
54 using namespace dart::simulation;
55
56 class MyWindow : public dart::gui::glut::SimWindow
57 {
58 public:
59 /// Constructor
60 MyWindow(WorldPtr world)
61 : mBallConstraint(nullptr), mPositiveSign(true), mBodyForce(false)
62 {
63 setWorld(world);
64
65 // Find the Skeleton named "pendulum" within the World
66 mPendulum = world->getSkeleton("pendulum");
67
68 // Make sure that the pendulum was found in the World
69 assert(mPendulum != nullptr);
70
71 mForceCountDown.resize(mPendulum->getNumDofs(), 0);
72
73 ArrowShape::Properties arrow_properties;
74 arrow_properties.mRadius = 0.05;
75 mArrow = std::shared_ptr<ArrowShape>(new ArrowShape(
76 Eigen::Vector3d(-default_height, 0.0, default_height / 2.0),
77 Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0),
78 arrow_properties,
79 dart::Color::Orange(1.0)));
80 }
81
82 void changeDirection()
83 {
84 mPositiveSign = !mPositiveSign;
85 if (mPositiveSign)
86 {
87 mArrow->setPositions(
88 Eigen::Vector3d(-default_height, 0.0, default_height / 2.0),
89 Eigen::Vector3d(-default_width / 2.0, 0.0, default_height / 2.0));
90 }
91 else
92 {
93 mArrow->setPositions(
94 Eigen::Vector3d(default_height, 0.0, default_height / 2.0),
95 Eigen::Vector3d(default_width / 2.0, 0.0, default_height / 2.0));
96 }
97 }
98
99 void applyForce(std::size_t index)
100 {
101 if (index < mForceCountDown.size())
102 mForceCountDown[index] = default_countdown;
103 }
104
105 void changeRestPosition(double /*delta*/)
106 {
107 // Lesson 2a
108 }
109
110 void changeStiffness(double /*delta*/)
111 {
112 // Lesson 2b
113 }
114
115 void changeDamping(double /*delta*/)
116 {
117 // Lesson 2c
118 }
119
120 /// Add a constraint to attach the final link to the world
121 void addConstraint()
122 {
123 // Lesson 3
124 }
125
126 /// Remove any existing constraint, allowing the pendulum to flail freely
127 void removeConstraint()
128 {
129 // Lesson 3
130 }
131
132 /// Handle keyboard input
133 void keyboard(unsigned char key, int x, int y) override
134 {
135 switch (key)
136 {
137 case '-':
138 changeDirection();
139 break;
140
141 case '1':
142 applyForce(0);
143 break;
144 case '2':
145 applyForce(1);
146 break;
147 case '3':
148 applyForce(2);
149 break;
150 case '4':
151 applyForce(3);
152 break;
153 case '5':
154 applyForce(4);
155 break;
156 case '6':
157 applyForce(5);
158 break;
159 case '7':
160 applyForce(6);
161 break;
162 case '8':
163 applyForce(7);
164 break;
165 case '9':
166 applyForce(8);
167 break;
168 case '0':
169 applyForce(9);
170 break;
171
172 case 'q':
173 changeRestPosition(delta_rest_position);
174 break;
175 case 'a':
176 changeRestPosition(-delta_rest_position);
177 break;
178
179 case 'w':
180 changeStiffness(delta_stiffness);
181 break;
182 case 's':
183 changeStiffness(-delta_stiffness);
184 break;
185
186 case 'e':
187 changeDamping(delta_damping);
188 break;
189 case 'd':
190 changeDamping(-delta_damping);
191 break;
192
193 case 'r': {
194 if (mBallConstraint)
195 removeConstraint();
196 else
197 addConstraint();
198 break;
199 }
200
201 case 'f':
202 mBodyForce = !mBodyForce;
203 break;
204
205 default:
206 SimWindow::keyboard(key, x, y);
207 }
208 }
209
210 void timeStepping() override
211 {
212 // Reset all the shapes to be Blue
213 // Lesson 1a
214
215 if (!mBodyForce)
216 {
217 // Apply joint torques based on user input, and color the Joint shape red
218 for (std::size_t i = 0; i < mPendulum->getNumDofs(); ++i)
219 {
220 if (mForceCountDown[i] > 0)
221 {
222 // Lesson 1b
223
224 --mForceCountDown[i];
225 }
226 }
227 }
228 else
229 {
230 // Apply body forces based on user input, and color the body shape red
231 for (std::size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i)
232 {
233 if (mForceCountDown[i] > 0)
234 {
235 // Lesson 1c
236
237 --mForceCountDown[i];
238 }
239 }
240 }
241
242 // Step the simulation forward
243 SimWindow::timeStepping();
244 }
245
246 protected:
247 /// An arrow shape that we will use to visualize applied forces
248 std::shared_ptr<ArrowShape> mArrow;
249
250 /// The pendulum that we will be perturbing
251 SkeletonPtr mPendulum;
252
253 /// Pointer to the ball constraint that we will be turning on and off
254 dart::constraint::BallJointConstraint* mBallConstraint;
255
256 /// Number of iterations before clearing a force entry
257 std::vector<int> mForceCountDown;
258
259 /// Whether a force should be applied in the positive or negative direction
260 bool mPositiveSign;
261
262 /// True if 1-9 should be used to apply a body force. Otherwise, 1-9 will be
263 /// used to apply a joint torque.
264 bool mBodyForce;
265 };
266
267 void setGeometry(const BodyNodePtr& bn)
268 {
269 // Create a BoxShape to be used for both visualization and collision checking
270 std::shared_ptr<BoxShape> box(new BoxShape(
271 Eigen::Vector3d(default_width, default_depth, default_height)));
272
273 // Create a shape node for visualization and collision checking
274 auto shapeNode
275 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
276 box);
277 shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
278
279 // Set the location of the shape node
280 Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity());
281 Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0);
282 box_tf.translation() = center;
283 shapeNode->setRelativeTransform(box_tf);
284
285 // Move the center of mass to the center of the object
286 bn->setLocalCOM(center);
287 }
288
289 BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name)
290 {
291 BallJoint::Properties properties;
292 properties.mName = name + "_joint";
293 properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position);
294 properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness);
295 properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping);
296
297 BodyNodePtr bn
298 = pendulum
299 ->createJointAndBodyNodePair<BallJoint>(
300 nullptr, properties, BodyNode::AspectProperties(name))
301 .second;
302
303 // Make a shape for the Joint
304 const double& R = default_width;
305 std::shared_ptr<EllipsoidShape> ball(
306 new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R)));
307 auto shapeNode = bn->createShapeNodeWith<VisualAspect>(ball);
308 shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
309
310 // Set the geometry of the Body
311 setGeometry(bn);
312
313 return bn;
314 }
315
316 BodyNode* addBody(
317 const SkeletonPtr& pendulum, BodyNode* parent, const std::string& name)
318 {
319 // Set up the properties for the Joint
320 RevoluteJoint::Properties properties;
321 properties.mName = name + "_joint";
322 properties.mAxis = Eigen::Vector3d::UnitY();
323 properties.mT_ParentBodyToJoint.translation()
324 = Eigen::Vector3d(0, 0, default_height);
325 properties.mRestPositions[0] = default_rest_position;
326 properties.mSpringStiffnesses[0] = default_stiffness;
327 properties.mDampingCoefficients[0] = default_damping;
328
329 // Create a new BodyNode, attached to its parent by a RevoluteJoint
330 BodyNodePtr bn = pendulum
331 ->createJointAndBodyNodePair<RevoluteJoint>(
332 parent, properties, BodyNode::AspectProperties(name))
333 .second;
334
335 // Make a shape for the Joint
336 const double R = default_width / 2.0;
337 const double h = default_depth;
338 std::shared_ptr<CylinderShape> cyl(new CylinderShape(R, h));
339
340 // Line up the cylinder with the Joint axis
341 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
342 tf.linear() = dart::math::eulerXYZToMatrix(
343 Eigen::Vector3d(dart::math::toRadian(90.0), 0, 0));
344
345 auto shapeNode = bn->createShapeNodeWith<VisualAspect>(cyl);
346 shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
347 shapeNode->setRelativeTransform(tf);
348
349 // Set the geometry of the Body
350 setGeometry(bn);
351
352 return bn;
353 }
354
355 int main(int argc, char* argv[])
356 {
357 // Create an empty Skeleton with the name "pendulum"
358 SkeletonPtr pendulum = Skeleton::create("pendulum");
359
360 // Add each body to the last BodyNode in the pendulum
361 BodyNode* bn = makeRootBody(pendulum, "body1");
362 bn = addBody(pendulum, bn, "body2");
363 bn = addBody(pendulum, bn, "body3");
364 bn = addBody(pendulum, bn, "body4");
365 bn = addBody(pendulum, bn, "body5");
366
367 // Set the initial position of the first DegreeOfFreedom so that the pendulum
368 // starts to swing right away
369 pendulum->getDof(1)->setPosition(dart::math::toRadian(120.0));
370
371 // Create a world and add the pendulum to the world
372 WorldPtr world = World::create();
373 world->addSkeleton(pendulum);
374
375 // Create a window for rendering the world and handling user input
376 MyWindow window(world);
377
378 // Print instructions
379 std::cout << "space bar: simulation on/off" << std::endl;
380 std::cout << "'p': replay simulation" << std::endl;
381 std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl;
382 std::cout << "'-': Change sign of applied joint torques" << std::endl;
383 std::cout << "'q': Increase joint rest positions" << std::endl;
384 std::cout << "'a': Decrease joint rest positions" << std::endl;
385 std::cout << "'w': Increase joint spring stiffness" << std::endl;
386 std::cout << "'s': Decrease joint spring stiffness" << std::endl;
387 std::cout << "'e': Increase joint damping" << std::endl;
388 std::cout << "'d': Decrease joint damping" << std::endl;
389 std::cout << "'r': add/remove constraint on the end of the chain"
390 << std::endl;
391 std::cout << "'f': switch between applying joint torques and body forces"
392 << std::endl;
393
394 // Initialize glut, initialize the window, and begin the glut event loop
395 glutInit(&argc, argv);
396 window.initWindow(640, 480, "Multi-Pendulum Tutorial");
397 glutMainLoop();
398 }