Debian Codegraph @

tutorials/tutorial_biped_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 #include <dart/utils/utils.hpp>
36
37 const double default_speed_increment = 0.5;
38
39 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) : mBiped(biped), mSpeed(0.0)
57 {
58 int nDofs = mBiped->getNumDofs();
59
60 mForces = Eigen::VectorXd::Zero(nDofs);
61
62 mKp = Eigen::MatrixXd::Identity(nDofs, nDofs);
63 mKd = Eigen::MatrixXd::Identity(nDofs, nDofs);
64
65 for (std::size_t i = 0; i < 6; ++i)
66 {
67 mKp(i, i) = 0.0;
68 mKd(i, i) = 0.0;
69 }
70
71 for (std::size_t i = 6; i < mBiped->getNumDofs(); ++i)
72 {
73 mKp(i, i) = 1000;
74 mKd(i, i) = 50;
75 }
76
77 setTargetPositions(mBiped->getPositions());
78 }
79
80 /// Reset the desired dof position to the current position
81 void setTargetPositions(const Eigen::VectorXd& pose)
82 {
83 mTargetPositions = pose;
84 }
85
86 /// Clear commanding forces
87 void clearForces()
88 {
89 mForces.setZero();
90 }
91
92 /// Add commanding forces from PD controllers (Lesson 2 Answer)
93 void addPDForces()
94 {
95 Eigen::VectorXd q = mBiped->getPositions();
96 Eigen::VectorXd dq = mBiped->getVelocities();
97
98 Eigen::VectorXd p = -mKp * (q - mTargetPositions);
99 Eigen::VectorXd d = -mKd * dq;
100
101 mForces += p + d;
102 mBiped->setForces(mForces);
103 }
104
105 /// Add commanind forces from Stable-PD controllers (Lesson 3 Answer)
106 void addSPDForces()
107 {
108 Eigen::VectorXd q = mBiped->getPositions();
109 Eigen::VectorXd dq = mBiped->getVelocities();
110
111 Eigen::MatrixXd invM
112 = (mBiped->getMassMatrix() + mKd * mBiped->getTimeStep()).inverse();
113 Eigen::VectorXd p
114 = -mKp * (q + dq * mBiped->getTimeStep() - mTargetPositions);
115 Eigen::VectorXd d = -mKd * dq;
116 Eigen::VectorXd qddot = invM
117 * (-mBiped->getCoriolisAndGravityForces() + p + d
118 + mBiped->getConstraintForces());
119
120 mForces += p + d - mKd * qddot * mBiped->getTimeStep();
121 mBiped->setForces(mForces);
122 }
123
124 /// add commanding forces from ankle strategy (Lesson 4 Answer)
125 void addAnkleStrategyForces()
126 {
127 Eigen::Vector3d COM = mBiped->getCOM();
128 // Approximated center of pressure in sagittal axis
129 Eigen::Vector3d offset(0.05, 0, 0);
130 Eigen::Vector3d COP
131 = mBiped->getBodyNode("h_heel_left")->getTransform() * offset;
132 double diff = COM[0] - COP[0];
133
134 Eigen::Vector3d dCOM = mBiped->getCOMLinearVelocity();
135 Eigen::Vector3d dCOP
136 = mBiped->getBodyNode("h_heel_left")->getLinearVelocity(offset);
137 double dDiff = dCOM[0] - dCOP[0];
138
139 int lHeelIndex = mBiped->getDof("j_heel_left_1")->getIndexInSkeleton();
140 int rHeelIndex = mBiped->getDof("j_heel_right_1")->getIndexInSkeleton();
141 int lToeIndex = mBiped->getDof("j_toe_left")->getIndexInSkeleton();
142 int rToeIndex = mBiped->getDof("j_toe_right")->getIndexInSkeleton();
143 if (diff < 0.1 && diff >= 0.0)
144 {
145 // Feedback rule for recovering forward push
146 double k1 = 200.0;
147 double k2 = 100.0;
148 double kd = 10;
149 mForces[lHeelIndex] += -k1 * diff - kd * dDiff;
150 mForces[lToeIndex] += -k2 * diff - kd * dDiff;
151 mForces[rHeelIndex] += -k1 * diff - kd * dDiff;
152 mForces[rToeIndex] += -k2 * diff - kd * dDiff;
153 }
154 else if (diff > -0.2 && diff < -0.05)
155 {
156 // Feedback rule for recovering backward push
157 double k1 = 2000.0;
158 double k2 = 100.0;
159 double kd = 100;
160 mForces[lHeelIndex] += -k1 * diff - kd * dDiff;
161 mForces[lToeIndex] += -k2 * diff - kd * dDiff;
162 mForces[rHeelIndex] += -k1 * diff - kd * dDiff;
163 mForces[rToeIndex] += -k2 * diff - kd * dDiff;
164 }
165 mBiped->setForces(mForces);
166 }
167
168 // Send velocity commands on wheel actuators (Lesson 6 Answer)
169 void setWheelCommands()
170 {
171 int wheelFirstIndex
172 = mBiped->getDof("joint_front_left_1")->getIndexInSkeleton();
173 for (std::size_t i = wheelFirstIndex; i < mBiped->getNumDofs(); ++i)
174 {
175 mKp(i, i) = 0.0;
176 mKd(i, i) = 0.0;
177 }
178
179 int index1 = mBiped->getDof("joint_front_left_2")->getIndexInSkeleton();
180 int index2 = mBiped->getDof("joint_front_right_2")->getIndexInSkeleton();
181 int index3 = mBiped->getDof("joint_back_left")->getIndexInSkeleton();
182 int index4 = mBiped->getDof("joint_back_right")->getIndexInSkeleton();
183 mBiped->setCommand(index1, mSpeed);
184 mBiped->setCommand(index2, mSpeed);
185 mBiped->setCommand(index3, mSpeed);
186 mBiped->setCommand(index4, mSpeed);
187 }
188
189 void changeWheelSpeed(double increment)
190 {
191 mSpeed += increment;
192 std::cout << "wheel speed = " << mSpeed << std::endl;
193 }
194
195 protected:
196 /// The biped Skeleton that we will be controlling
197 SkeletonPtr mBiped;
198
199 /// Joint forces for the biped (output of the Controller)
200 Eigen::VectorXd mForces;
201
202 /// Control gains for the proportional error terms in the PD controller
203 Eigen::MatrixXd mKp;
204
205 /// Control gains for the derivative error terms in the PD controller
206 Eigen::MatrixXd mKd;
207
208 /// Target positions for the PD controllers
209 Eigen::VectorXd mTargetPositions;
210
211 /// For velocity actuator: Current speed of the skateboard
212 double mSpeed;
213 };
214
215 class MyWindow : public SimWindow
216 {
217 public:
218 /// Constructor
219 MyWindow(const WorldPtr& world) : mForceCountDown(0), mPositiveSign(true)
220 {
221 setWorld(world);
222
223 mController = std::make_unique<Controller>(mWorld->getSkeleton("biped"));
224 }
225
226 /// Handle keyboard input
227 void keyboard(unsigned char key, int x, int y) override
228 {
229 switch (key)
230 {
231 case ',':
232 mForceCountDown = default_countdown;
233 mPositiveSign = false;
234 break;
235 case '.':
236 mForceCountDown = default_countdown;
237 mPositiveSign = true;
238 break;
239 case 'a':
240 mController->changeWheelSpeed(default_speed_increment);
241 break;
242 case 's':
243 mController->changeWheelSpeed(-default_speed_increment);
244 break;
245 default:
246 SimWindow::keyboard(key, x, y);
247 }
248 }
249
250 void timeStepping() override
251 {
252 mController->clearForces();
253
254 // Lesson 3
255 mController->addSPDForces();
256
257 // Lesson 4
258 mController->addAnkleStrategyForces();
259
260 // Lesson 6
261 mController->setWheelCommands();
262
263 // Apply body forces based on user input, and color the body shape red
264 if (mForceCountDown > 0)
265 {
266 BodyNode* bn = mWorld->getSkeleton("biped")->getBodyNode("h_abdomen");
267 bn->setColor(dart::Color::Red());
268
269 if (mPositiveSign)
270 bn->addExtForce(
271 default_force * Eigen::Vector3d::UnitX(),
272 bn->getCOM(),
273 false,
274 false);
275 else
276 bn->addExtForce(
277 -default_force * Eigen::Vector3d::UnitX(),
278 bn->getCOM(),
279 false,
280 false);
281
282 --mForceCountDown;
283 }
284
285 // Step the simulation forward
286 SimWindow::timeStepping();
287 }
288
289 protected:
290 std::unique_ptr<Controller> mController;
291
292 /// Number of iterations before clearing a force entry
293 int mForceCountDown;
294
295 /// Whether a force should be applied in the positive or negative direction
296 bool mPositiveSign;
297 };
298
299 // Load a biped model and enable joint limits and self-collision
300 // (Lesson 1 Answer)
301 SkeletonPtr loadBiped()
302 {
303 // Create the world with a skeleton
304 WorldPtr world = SkelParser::readWorld("dart://sample/skel/biped.skel");
305 assert(world != nullptr);
306
307 SkeletonPtr biped = world->getSkeleton("biped");
308
309 // Set joint limits
310 for (std::size_t i = 0; i < biped->getNumJoints(); ++i)
311 biped->getJoint(i)->setLimitEnforcement(true);
312
313 // Enable self collision check but ignore adjacent bodies
314 biped->enableSelfCollisionCheck();
315 biped->disableAdjacentBodyCheck();
316
317 return biped;
318 }
319
320 // Set initial configuration (Lesson 2 Answer)
321 void setInitialPose(SkeletonPtr biped)
322 {
323 biped->setPosition(
324 biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15);
325 biped->setPosition(
326 biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15);
327 biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4);
328 biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4);
329 biped->setPosition(
330 biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25);
331 biped->setPosition(
332 biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25);
333 }
334
335 // Load a skateboard model and connect it to the biped model via an Euler joint
336 // (Lesson 5 Answer)
337 void modifyBipedWithSkateboard(SkeletonPtr biped)
338 {
339 // Load the Skeleton from a file
340 WorldPtr world = SkelParser::readWorld("dart://sample/skel/skateboard.skel");
341
342 SkeletonPtr skateboard = world->getSkeleton(0);
343
344 EulerJoint::Properties properties = EulerJoint::Properties();
345 properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
346
347 skateboard->getRootBodyNode()->moveTo<EulerJoint>(
348 biped->getBodyNode("h_heel_left"), properties);
349 }
350
351 // Set the actuator type for four wheel joints to "VELOCITY" (Lesson 6 Answer)
352 void setVelocityAccuators(SkeletonPtr biped)
353 {
354 Joint* wheel1 = biped->getJoint("joint_front_left");
355 Joint* wheel2 = biped->getJoint("joint_front_right");
356 Joint* wheel3 = biped->getJoint("joint_back_left");
357 Joint* wheel4 = biped->getJoint("joint_back_right");
358 wheel1->setActuatorType(Joint::VELOCITY);
359 wheel2->setActuatorType(Joint::VELOCITY);
360 wheel3->setActuatorType(Joint::VELOCITY);
361 wheel4->setActuatorType(Joint::VELOCITY);
362 }
363
364 // Solve for a balanced pose using IK (Lesson 7 Answer)
365 Eigen::VectorXd solveIK(SkeletonPtr biped)
366 {
367 // Modify the intial pose to one-foot stance before IK
368 biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -1.4);
369 biped->setPosition(
370 biped->getDof("j_bicep_left_x")->getIndexInSkeleton(), 0.8);
371 biped->setPosition(
372 biped->getDof("j_bicep_right_x")->getIndexInSkeleton(), -0.8);
373
374 Eigen::VectorXd newPose = biped->getPositions();
375 BodyNodePtr leftHeel = biped->getBodyNode("h_heel_left");
376 BodyNodePtr leftToe = biped->getBodyNode("h_toe_left");
377 double initialHeight = -0.8;
378
379 for (std::size_t i = 0; i < default_ik_iterations; ++i)
380 {
381 Eigen::Vector3d deviation = biped->getCOM() - leftHeel->getCOM();
382 Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel);
383 LinearJacobian jacobian = biped->getCOMLinearJacobian()
384 - biped->getLinearJacobian(leftHeel, localCOM);
385
386 // Sagittal deviation
387 double error = deviation[0];
388 Eigen::VectorXd gradient = jacobian.row(0);
389 Eigen::VectorXd newDirection = -0.2 * error * gradient;
390
391 // Lateral deviation
392 error = deviation[2];
393 gradient = jacobian.row(2);
394 newDirection += -0.2 * error * gradient;
395
396 // Position constraint on four (approximated) corners of the left foot
397 Eigen::Vector3d offset(0.0, -0.04, -0.03);
398 error = (leftHeel->getTransform() * offset)[1] - initialHeight;
399 gradient = biped->getLinearJacobian(leftHeel, offset).row(1);
400 newDirection += -0.2 * error * gradient;
401
402 offset[2] = 0.03;
403 error = (leftHeel->getTransform() * offset)[1] - initialHeight;
404 gradient = biped->getLinearJacobian(leftHeel, offset).row(1);
405 newDirection += -0.2 * error * gradient;
406
407 offset[0] = 0.04;
408 error = (leftToe->getTransform() * offset)[1] - initialHeight;
409 gradient = biped->getLinearJacobian(leftToe, offset).row(1);
410 newDirection += -0.2 * error * gradient;
411
412 offset[2] = -0.03;
413 error = (leftToe->getTransform() * offset)[1] - initialHeight;
414 gradient = biped->getLinearJacobian(leftToe, offset).row(1);
415 newDirection += -0.2 * error * gradient;
416
417 newPose += newDirection;
418 biped->setPositions(newPose);
419 biped->computeForwardKinematics(true, false, false);
420 }
421 return newPose;
422 }
423
424 SkeletonPtr createFloor()
425 {
426 SkeletonPtr floor = Skeleton::create("floor");
427
428 // Give the floor a body
429 BodyNodePtr body
430 = floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
431
432 // Give the body a shape
433 double floor_width = 10.0;
434 double floor_height = 0.01;
435 std::shared_ptr<BoxShape> box(
436 new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width)));
437 auto shapeNode = body->createShapeNodeWith<
438 VisualAspect,
439 CollisionAspect,
440 DynamicsAspect>(box);
441 shapeNode->getVisualAspect()->setColor(dart::Color::Black());
442
443 // Put the body into position
444 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
445 tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0);
446 body->getParentJoint()->setTransformFromParentBodyNode(tf);
447
448 return floor;
449 }
450
451 int main(int argc, char* argv[])
452 {
453 SkeletonPtr floor = createFloor();
454
455 // Lesson 1
456 SkeletonPtr biped = loadBiped();
457
458 // Lesson 2
459 setInitialPose(biped);
460
461 // Lesson 5
462 modifyBipedWithSkateboard(biped);
463
464 // Lesson 6
465 setVelocityAccuators(biped);
466
467 // Lesson 7
468 Eigen::VectorXd balancedPose = solveIK(biped);
469 biped->setPositions(balancedPose);
470
471 WorldPtr world = std::make_shared<World>();
472 world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
473
474 if (dart::collision::CollisionDetector::getFactory()->canCreate("bullet"))
475 {
476 world->getConstraintSolver()->setCollisionDetector(
477 dart::collision::CollisionDetector::getFactory()->create("bullet"));
478 }
479
480 world->addSkeleton(floor);
481 world->addSkeleton(biped);
482
483 // Create a window for rendering the world and handling user input
484 MyWindow window(world);
485
486 // Print instructions
487 std::cout << "'.': forward push" << std::endl;
488 std::cout << "',': backward push" << std::endl;
489 std::cout << "'s': increase skateboard forward speed" << std::endl;
490 std::cout << "'a': increase skateboard backward speed" << std::endl;
491 std::cout << "space bar: simulation on/off" << std::endl;
492 std::cout << "'p': replay simulation" << std::endl;
493 std::cout << "'v': Turn contact force visualization on/off" << std::endl;
494 std::cout << "'[' and ']': replay one frame backward and forward"
495 << std::endl;
496
497 // Initialize glut, initialize the window, and begin the glut event loop
498 glutInit(&argc, argv);
499 window.initWindow(640, 480, "Multi-Pendulum Tutorial");
500 glutMainLoop();
501 }