Debian Codegraph @

tutorials/tutorial_multi_pendulum_finished/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 const double default_torque = 15.0; // N-m
41 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 for (std::size_t i = 0; i < mPendulum->getNumDofs(); ++i)
108 {
109 DegreeOfFreedom* dof = mPendulum->getDof(i);
110 double q0 = dof->getRestPosition() + delta;
111
112 // The system becomes numerically unstable when the rest position exceeds
113 // 90 degrees
114 if (std::abs(q0) > dart::math::toRadian(90.0))
115 q0 = (q0 > 0) ? dart::math::toRadian(90.0)
116 : dart::math::toRadian(-90.0);
117
118 dof->setRestPosition(q0);
119 }
120
121 // Only curl up along one axis in the BallJoint
122 mPendulum->getDof(0)->setRestPosition(0.0);
123 mPendulum->getDof(2)->setRestPosition(0.0);
124 }
125
126 void changeStiffness(double delta)
127 {
128 for (std::size_t i = 0; i < mPendulum->getNumDofs(); ++i)
129 {
130 DegreeOfFreedom* dof = mPendulum->getDof(i);
131 double stiffness = dof->getSpringStiffness() + delta;
132 if (stiffness < 0.0)
133 stiffness = 0.0;
134 dof->setSpringStiffness(stiffness);
135 }
136 }
137
138 void changeDamping(double delta)
139 {
140 for (std::size_t i = 0; i < mPendulum->getNumDofs(); ++i)
141 {
142 DegreeOfFreedom* dof = mPendulum->getDof(i);
143 double damping = dof->getDampingCoefficient() + delta;
144 if (damping < 0.0)
145 damping = 0.0;
146 dof->setDampingCoefficient(damping);
147 }
148 }
149
150 /// Add a constraint to attach the final link to the world
151 void addConstraint()
152 {
153 // Get the last body in the pendulum
154 BodyNode* tip = mPendulum->getBodyNode(mPendulum->getNumBodyNodes() - 1);
155
156 // Attach the last link to the world
157 Eigen::Vector3d location
158 = tip->getTransform() * Eigen::Vector3d(0.0, 0.0, default_height);
159 mBallConstraint = std::make_shared<dart::constraint::BallJointConstraint>(
160 tip, location);
161 mWorld->getConstraintSolver()->addConstraint(mBallConstraint);
162 }
163
164 /// Remove any existing constraint, allowing the pendulum to flail freely
165 void removeConstraint()
166 {
167 mWorld->getConstraintSolver()->removeConstraint(mBallConstraint);
168 mBallConstraint = nullptr;
169 }
170
171 /// Handle keyboard input
172 void keyboard(unsigned char key, int x, int y) override
173 {
174 switch (key)
175 {
176 case '-':
177 changeDirection();
178 break;
179
180 case '1':
181 applyForce(0);
182 break;
183 case '2':
184 applyForce(1);
185 break;
186 case '3':
187 applyForce(2);
188 break;
189 case '4':
190 applyForce(3);
191 break;
192 case '5':
193 applyForce(4);
194 break;
195 case '6':
196 applyForce(5);
197 break;
198 case '7':
199 applyForce(6);
200 break;
201 case '8':
202 applyForce(7);
203 break;
204 case '9':
205 applyForce(8);
206 break;
207 case '0':
208 applyForce(9);
209 break;
210
211 case 'q':
212 changeRestPosition(delta_rest_position);
213 break;
214 case 'a':
215 changeRestPosition(-delta_rest_position);
216 break;
217
218 case 'w':
219 changeStiffness(delta_stiffness);
220 break;
221 case 's':
222 changeStiffness(-delta_stiffness);
223 break;
224
225 case 'e':
226 changeDamping(delta_damping);
227 break;
228 case 'd':
229 changeDamping(-delta_damping);
230 break;
231
232 case 'r': {
233 if (mBallConstraint)
234 removeConstraint();
235 else
236 addConstraint();
237 break;
238 }
239
240 case 'f':
241 mBodyForce = !mBodyForce;
242 break;
243
244 default:
245 SimWindow::keyboard(key, x, y);
246 }
247 }
248
249 void timeStepping() override
250 {
251 // Reset all the shapes to be Blue
252 for (std::size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i)
253 {
254 BodyNode* bn = mPendulum->getBodyNode(i);
255
256 // If we have three visualization shapes, that means the arrow is
257 // attached. We should remove it in case this body is no longer
258 // experiencing a force
259 if (bn->getNumShapeNodesWith<VisualAspect>() == 3u)
260 {
261 assert(bn->getShapeNodeWith<VisualAspect>(2)->getShape() == mArrow);
262 bn->getShapeNodeWith<VisualAspect>(2)->remove();
263 }
264
265 bn->setColor(dart::Color::Blue());
266 }
267
268 if (!mBodyForce)
269 {
270 // Apply joint torques based on user input, and color the Joint shape red
271 for (std::size_t i = 0; i < mPendulum->getNumDofs(); ++i)
272 {
273 if (mForceCountDown[i] > 0)
274 {
275 DegreeOfFreedom* dof = mPendulum->getDof(i);
276 dof->setForce(mPositiveSign ? default_torque : -default_torque);
277
278 BodyNode* bn = dof->getChildBodyNode();
279 bn->getShapeNodeWith<VisualAspect>(0)->getVisualAspect()->setColor(
280 dart::Color::Red());
281
282 --mForceCountDown[i];
283 }
284 }
285 }
286 else
287 {
288 // Apply body forces based on user input, and color the body shape red
289 for (std::size_t i = 0; i < mPendulum->getNumBodyNodes(); ++i)
290 {
291 if (mForceCountDown[i] > 0)
292 {
293 BodyNode* bn = mPendulum->getBodyNode(i);
294
295 Eigen::Vector3d force = default_force * Eigen::Vector3d::UnitX();
296 Eigen::Vector3d location(
297 -default_width / 2.0, 0.0, default_height / 2.0);
298 if (!mPositiveSign)
299 {
300 force = -force;
301 location[0] = -location[0];
302 }
303 bn->addExtForce(force, location, true, true);
304 bn->getShapeNodeWith<VisualAspect>(1)->getVisualAspect()->setColor(
305 dart::Color::Red());
306 bn->createShapeNodeWith<VisualAspect>(mArrow);
307
308 --mForceCountDown[i];
309 }
310 }
311 }
312
313 // Step the simulation forward
314 SimWindow::timeStepping();
315 }
316
317 protected:
318 /// An arrow shape that we will use to visualize applied forces
319 std::shared_ptr<ArrowShape> mArrow;
320
321 /// The pendulum that we will be perturbing
322 SkeletonPtr mPendulum;
323
324 /// Pointer to the ball constraint that we will be turning on and off
325 dart::constraint::BallJointConstraintPtr mBallConstraint;
326
327 /// Number of iterations before clearing a force entry
328 std::vector<int> mForceCountDown;
329
330 /// Whether a force should be applied in the positive or negative direction
331 bool mPositiveSign;
332
333 /// True if 1-9 should be used to apply a body force. Otherwise, 1-9 will be
334 /// used to apply a joint torque.
335 bool mBodyForce;
336 };
337
338 void setGeometry(const BodyNodePtr& bn)
339 {
340 // Create a BoxShape to be used for both visualization and collision checking
341 std::shared_ptr<BoxShape> box(new BoxShape(
342 Eigen::Vector3d(default_width, default_depth, default_height)));
343
344 // Create a shape node for visualization and collision checking
345 auto shapeNode
346 = bn->createShapeNodeWith<VisualAspect, CollisionAspect, DynamicsAspect>(
347 box);
348 shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
349
350 // Set the location of the shape node
351 Eigen::Isometry3d box_tf(Eigen::Isometry3d::Identity());
352 Eigen::Vector3d center = Eigen::Vector3d(0, 0, default_height / 2.0);
353 box_tf.translation() = center;
354 shapeNode->setRelativeTransform(box_tf);
355
356 // Move the center of mass to the center of the object
357 bn->setLocalCOM(center);
358 }
359
360 BodyNode* makeRootBody(const SkeletonPtr& pendulum, const std::string& name)
361 {
362 BallJoint::Properties properties;
363 properties.mName = name + "_joint";
364 properties.mRestPositions = Eigen::Vector3d::Constant(default_rest_position);
365 properties.mSpringStiffnesses = Eigen::Vector3d::Constant(default_stiffness);
366 properties.mDampingCoefficients = Eigen::Vector3d::Constant(default_damping);
367
368 auto bodyProp = BodyNode::Properties(BodyNode::AspectProperties(name));
369 BodyNodePtr bn = pendulum
370 ->createJointAndBodyNodePair<BallJoint>(
371 nullptr, properties, bodyProp)
372 .second;
373
374 // Make a shape for the Joint
375 const double& R = default_width;
376 std::shared_ptr<EllipsoidShape> ball(
377 new EllipsoidShape(sqrt(2) * Eigen::Vector3d(R, R, R)));
378 auto shapeNode = bn->createShapeNodeWith<VisualAspect>(ball);
379 shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
380
381 // Set the geometry of the Body
382 setGeometry(bn);
383
384 return bn;
385 }
386
387 BodyNode* addBody(
388 const SkeletonPtr& pendulum, BodyNode* parent, const std::string& name)
389 {
390 // Set up the properties for the Joint
391 RevoluteJoint::Properties properties;
392 properties.mName = name + "_joint";
393 properties.mAxis = Eigen::Vector3d::UnitY();
394 properties.mT_ParentBodyToJoint.translation()
395 = Eigen::Vector3d(0, 0, default_height);
396 properties.mRestPositions[0] = default_rest_position;
397 properties.mSpringStiffnesses[0] = default_stiffness;
398 properties.mDampingCoefficients[0] = default_damping;
399
400 // Create a new BodyNode, attached to its parent by a RevoluteJoint
401 BodyNodePtr bn = pendulum
402 ->createJointAndBodyNodePair<RevoluteJoint>(
403 parent, properties, BodyNode::AspectProperties(name))
404 .second;
405
406 // Make a shape for the Joint
407 const double R = default_width / 2.0;
408 const double h = default_depth;
409 std::shared_ptr<CylinderShape> cyl(new CylinderShape(R, h));
410
411 // Line up the cylinder with the Joint axis
412 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
413 tf.linear() = dart::math::eulerXYZToMatrix(
414 Eigen::Vector3d(dart::math::toRadian(90.0), 0, 0));
415
416 auto shapeNode = bn->createShapeNodeWith<VisualAspect>(cyl);
417 shapeNode->getVisualAspect()->setColor(dart::Color::Blue());
418 shapeNode->setRelativeTransform(tf);
419
420 // Set the geometry of the Body
421 setGeometry(bn);
422
423 return bn;
424 }
425
426 int main(int argc, char* argv[])
427 {
428 // Create an empty Skeleton with the name "pendulum"
429 SkeletonPtr pendulum = Skeleton::create("pendulum");
430
431 // Add each body to the last BodyNode in the pendulum
432 BodyNode* bn = makeRootBody(pendulum, "body1");
433 bn = addBody(pendulum, bn, "body2");
434 bn = addBody(pendulum, bn, "body3");
435 bn = addBody(pendulum, bn, "body4");
436 bn = addBody(pendulum, bn, "body5");
437
438 // Set the initial position of the first DegreeOfFreedom so that the pendulum
439 // starts to swing right away
440 pendulum->getDof(1)->setPosition(dart::math::toRadian(120.0));
441
442 // Create a world and add the pendulum to the world
443 WorldPtr world = World::create();
444 world->addSkeleton(pendulum);
445
446 // Create a window for rendering the world and handling user input
447 MyWindow window(world);
448
449 // Print instructions
450 std::cout << "space bar: simulation on/off" << std::endl;
451 std::cout << "'p': replay simulation" << std::endl;
452 std::cout << "'1' -> '9': apply torque to a pendulum body" << std::endl;
453 std::cout << "'-': Change sign of applied joint torques" << std::endl;
454 std::cout << "'q': Increase joint rest positions" << std::endl;
455 std::cout << "'a': Decrease joint rest positions" << std::endl;
456 std::cout << "'w': Increase joint spring stiffness" << std::endl;
457 std::cout << "'s': Decrease joint spring stiffness" << std::endl;
458 std::cout << "'e': Increase joint damping" << std::endl;
459 std::cout << "'d': Decrease joint damping" << std::endl;
460 std::cout << "'r': add/remove constraint on the end of the chain"
461 << std::endl;
462 std::cout << "'f': switch between applying joint torques and body forces"
463 << std::endl;
464
465 // Initialize glut, initialize the window, and begin the glut event loop
466 glutInit(&argc, argv);
467 window.initWindow(640, 480, "Multi-Pendulum Tutorial");
468 glutMainLoop();
469 }