Debian Codegraph @

tutorials/tutorial_biped/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 #include <dart/utils/utils.hpp>
36
37 const double default_speed_increment = 0.5;
38
39 [[maybe_unused]] const int default_ik_iterations = 4500;
40
41 const double default_force = 50.0; // N
42 const int default_countdown = 100; // Number of timesteps for applying force
43
44 using namespace dart::common;
45 using namespace dart::dynamics;
46 using namespace dart::simulation;
47 using namespace dart::gui;
48 using namespace dart::gui::glut;
49 using namespace dart::utils;
50 using namespace dart::math;
51
52 class Controller
53 {
54 public:
55 /// Constructor
56 Controller(const SkeletonPtr& biped)
57 : mBiped(biped), mPreOffset(0.0), mSpeed(0.0)
58 {
59 int nDofs = mBiped->getNumDofs();
60
61 mForces = Eigen::VectorXd::Zero(nDofs);
62
63 mKp = Eigen::MatrixXd::Identity(nDofs, nDofs);
64 mKd = Eigen::MatrixXd::Identity(nDofs, nDofs);
65
66 for (std::size_t i = 0; i < 6; ++i)
67 {
68 mKp(i, i) = 0.0;
69 mKd(i, i) = 0.0;
70 }
71
72 for (std::size_t i = 6; i < biped->getNumDofs(); ++i)
73 {
74 mKp(i, i) = 1000;
75 mKd(i, i) = 50;
76 }
77
78 setTargetPositions(mBiped->getPositions());
79 }
80
81 /// Reset the desired dof position to the current position
82 void setTargetPositions(const Eigen::VectorXd& pose)
83 {
84 mTargetPositions = pose;
85 }
86
87 /// Clear commanding forces
88 void clearForces()
89 {
90 mForces.setZero();
91 }
92
93 /// Add commanding forces from PD controllers
94 void addPDForces()
95 {
96 // Lesson 2
97 }
98
99 /// Add commanind forces from Stable-PD controllers
100 void addSPDForces()
101 {
102 // Lesson 3
103 }
104
105 /// add commanding forces from ankle strategy
106 void addAnkleStrategyForces()
107 {
108 // Lesson 4
109 }
110
111 // Send velocity commands on wheel actuators
112 void setWheelCommands()
113 {
114 // Lesson 6
115 }
116
117 void changeWheelSpeed(double increment)
118 {
119 mSpeed += increment;
120 std::cout << "wheel speed = " << mSpeed << std::endl;
121 }
122
123 protected:
124 /// The biped Skeleton that we will be controlling
125 SkeletonPtr mBiped;
126
127 /// Joint forces for the biped (output of the Controller)
128 Eigen::VectorXd mForces;
129
130 /// Control gains for the proportional error terms in the PD controller
131 Eigen::MatrixXd mKp;
132
133 /// Control gains for the derivative error terms in the PD controller
134 Eigen::MatrixXd mKd;
135
136 /// Target positions for the PD controllers
137 Eigen::VectorXd mTargetPositions;
138
139 /// For ankle strategy: Error in the previous timestep
140 double mPreOffset;
141
142 /// For velocity actuator: Current speed of the skateboard
143 double mSpeed;
144 };
145
146 class MyWindow : public SimWindow
147 {
148 public:
149 /// Constructor
150 MyWindow(const WorldPtr& world) : mForceCountDown(0), mPositiveSign(true)
151 {
152 setWorld(world);
153
154 mController = std::make_unique<Controller>(mWorld->getSkeleton("biped"));
155 }
156
157 /// Handle keyboard input
158 void keyboard(unsigned char key, int x, int y) override
159 {
160 switch (key)
161 {
162 case ',':
163 mForceCountDown = default_countdown;
164 mPositiveSign = false;
165 break;
166 case '.':
167 mForceCountDown = default_countdown;
168 mPositiveSign = true;
169 break;
170 case 'a':
171 mController->changeWheelSpeed(default_speed_increment);
172 break;
173 case 's':
174 mController->changeWheelSpeed(-default_speed_increment);
175 break;
176 default:
177 SimWindow::keyboard(key, x, y);
178 }
179 }
180
181 void timeStepping() override
182 {
183 mController->clearForces();
184
185 // Lesson 3
186 mController->addSPDForces();
187
188 // Lesson 4
189 mController->addAnkleStrategyForces();
190
191 // Lesson 6
192 mController->setWheelCommands();
193
194 // Apply body forces based on user input, and color the body shape red
195 if (mForceCountDown > 0)
196 {
197 BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen");
198 bn->setColor(dart::Color::Red());
199
200 if (mPositiveSign)
201 bn->addExtForce(
202 default_force * Eigen::Vector3d::UnitX(),
203 bn->getCOM(),
204 false,
205 false);
206 else
207 bn->addExtForce(
208 -default_force * Eigen::Vector3d::UnitX(),
209 bn->getCOM(),
210 false,
211 false);
212
213 --mForceCountDown;
214 }
215
216 // Step the simulation forward
217 SimWindow::timeStepping();
218 }
219
220 protected:
221 std::unique_ptr<Controller> mController;
222
223 /// Number of iterations before clearing a force entry
224 int mForceCountDown;
225
226 /// Whether a force should be applied in the positive or negative direction
227 bool mPositiveSign;
228 };
229
230 // Load a biped model and enable joint limits and self-collision
231 SkeletonPtr loadBiped()
232 {
233 // Lesson 1
234
235 // Create the world with a skeleton
236 WorldPtr world = SkelParser::readWorld("dart://sample/skel/biped.skel");
237 assert(world != nullptr);
238
239 SkeletonPtr biped = world->getSkeleton("biped");
240
241 return biped;
242 }
243
244 // Load a skateboard model and connect it to the biped model via an Euler joint
245 void modifyBipedWithSkateboard(SkeletonPtr /*biped*/)
246 {
247 // Lesson 5
248 }
249
250 // Set the actuator type for four wheel joints to "VELOCITY"
251 void setVelocityAccuators(SkeletonPtr /*biped*/)
252 {
253 // Lesson 6
254 }
255
256 // Solve for a balanced pose using IK
257 Eigen::VectorXd solveIK(SkeletonPtr biped)
258 {
259 // Lesson 7
260 Eigen::VectorXd newPose = biped->getPositions();
261 return newPose;
262 }
263
264 SkeletonPtr createFloor()
265 {
266 SkeletonPtr floor = Skeleton::create("floor");
267
268 // Give the floor a body
269 BodyNodePtr body
270 = floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
271
272 // Give the body a shape
273 double floor_width = 10.0;
274 double floor_height = 0.01;
275 std::shared_ptr<BoxShape> box(
276 new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width)));
277 auto shapeNode = body->createShapeNodeWith<
278 VisualAspect,
279 CollisionAspect,
280 DynamicsAspect>(box);
281 shapeNode->getVisualAspect()->setColor(dart::Color::Black());
282
283 // Put the body into position
284 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
285 tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0);
286 body->getParentJoint()->setTransformFromParentBodyNode(tf);
287
288 return floor;
289 }
290
291 int main(int argc, char* argv[])
292 {
293 SkeletonPtr floor = createFloor();
294
295 // Lesson 1
296 SkeletonPtr biped = loadBiped();
297
298 // Lesson 5
299 modifyBipedWithSkateboard(biped);
300
301 // Lesson 6
302 setVelocityAccuators(biped);
303
304 // Lesson 7
305 Eigen::VectorXd balancedPose = solveIK(biped);
306 biped->setPositions(balancedPose);
307
308 WorldPtr world = std::make_shared<World>();
309 world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
310
311 if (dart::collision::CollisionDetector::getFactory()->canCreate("bullet"))
312 {
313 world->getConstraintSolver()->setCollisionDetector(
314 dart::collision::CollisionDetector::getFactory()->create("bullet"));
315 }
316
317 world->addSkeleton(floor);
318 world->addSkeleton(biped);
319
320 // Create a window for rendering the world and handling user input
321 MyWindow window(world);
322
323 // Print instructions
324 std::cout << "'.': forward push" << std::endl;
325 std::cout << "',': backward push" << std::endl;
326 std::cout << "'s': increase skateboard forward speed" << std::endl;
327 std::cout << "'a': increase skateboard backward speed" << std::endl;
328 std::cout << "space bar: simulation on/off" << std::endl;
329 std::cout << "'p': replay simulation" << std::endl;
330 std::cout << "'v': Turn contact force visualization on/off" << std::endl;
331 std::cout << "'[' and ']': replay one frame backward and forward"
332 << std::endl;
333
334 // Initialize glut, initialize the window, and begin the glut event loop
335 glutInit(&argc, argv);
336 window.initWindow(640, 480, "Multi-Pendulum Tutorial");
337 glutMainLoop();
338 }