1 /*
2 * Copyright (c) 2011-2021, 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
Controller(const SkeletonPtr & biped)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
setTargetPositions(const Eigen::VectorXd & pose)81 void setTargetPositions(const Eigen::VectorXd& pose)
82 {
83 mTargetPositions = pose;
84 }
85
86 /// Clear commanding forces
clearForces()87 void clearForces()
88 {
89 mForces.setZero();
90 }
91
92 /// Add commanding forces from PD controllers (Lesson 2 Answer)
addPDForces()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)
addSPDForces()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)
addAnkleStrategyForces()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)
setWheelCommands()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
changeWheelSpeed(double increment)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
MyWindow(const WorldPtr & world)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
keyboard(unsigned char key,int x,int y)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
timeStepping()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 auto shapeNodes = bn->getShapeNodesWith<VisualAspect>();
268 shapeNodes[0]->getVisualAspect()->setColor(dart::Color::Red());
269
270 if (mPositiveSign)
271 bn->addExtForce(
272 default_force * Eigen::Vector3d::UnitX(),
273 bn->getCOM(),
274 false,
275 false);
276 else
277 bn->addExtForce(
278 -default_force * Eigen::Vector3d::UnitX(),
279 bn->getCOM(),
280 false,
281 false);
282
283 --mForceCountDown;
284 }
285
286 // Step the simulation forward
287 SimWindow::timeStepping();
288 }
289
290 protected:
291 std::unique_ptr<Controller> mController;
292
293 /// Number of iterations before clearing a force entry
294 int mForceCountDown;
295
296 /// Whether a force should be applied in the positive or negative direction
297 bool mPositiveSign;
298 };
299
300 // Load a biped model and enable joint limits and self-collision
301 // (Lesson 1 Answer)
loadBiped()302 SkeletonPtr loadBiped()
303 {
304 // Create the world with a skeleton
305 WorldPtr world = SkelParser::readWorld("dart://sample/skel/biped.skel");
306 assert(world != nullptr);
307
308 SkeletonPtr biped = world->getSkeleton("biped");
309
310 // Set joint limits
311 for (std::size_t i = 0; i < biped->getNumJoints(); ++i)
312 biped->getJoint(i)->setLimitEnforcement(true);
313
314 // Enable self collision check but ignore adjacent bodies
315 biped->enableSelfCollisionCheck();
316 biped->disableAdjacentBodyCheck();
317
318 return biped;
319 }
320
321 // Set initial configuration (Lesson 2 Answer)
setInitialPose(SkeletonPtr biped)322 void setInitialPose(SkeletonPtr biped)
323 {
324 biped->setPosition(
325 biped->getDof("j_thigh_left_z")->getIndexInSkeleton(), 0.15);
326 biped->setPosition(
327 biped->getDof("j_thigh_right_z")->getIndexInSkeleton(), 0.15);
328 biped->setPosition(biped->getDof("j_shin_left")->getIndexInSkeleton(), -0.4);
329 biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -0.4);
330 biped->setPosition(
331 biped->getDof("j_heel_left_1")->getIndexInSkeleton(), 0.25);
332 biped->setPosition(
333 biped->getDof("j_heel_right_1")->getIndexInSkeleton(), 0.25);
334 }
335
336 // Load a skateboard model and connect it to the biped model via an Euler joint
337 // (Lesson 5 Answer)
modifyBipedWithSkateboard(SkeletonPtr biped)338 void modifyBipedWithSkateboard(SkeletonPtr biped)
339 {
340 // Load the Skeleton from a file
341 WorldPtr world = SkelParser::readWorld("dart://sample/skel/skateboard.skel");
342
343 SkeletonPtr skateboard = world->getSkeleton(0);
344
345 EulerJoint::Properties properties = EulerJoint::Properties();
346 properties.mT_ChildBodyToJoint.translation() = Eigen::Vector3d(0, 0.1, 0);
347
348 skateboard->getRootBodyNode()->moveTo<EulerJoint>(
349 biped->getBodyNode("h_heel_left"), properties);
350 }
351
352 // Set the actuator type for four wheel joints to "VELOCITY" (Lesson 6 Answer)
setVelocityAccuators(SkeletonPtr biped)353 void setVelocityAccuators(SkeletonPtr biped)
354 {
355 Joint* wheel1 = biped->getJoint("joint_front_left");
356 Joint* wheel2 = biped->getJoint("joint_front_right");
357 Joint* wheel3 = biped->getJoint("joint_back_left");
358 Joint* wheel4 = biped->getJoint("joint_back_right");
359 wheel1->setActuatorType(Joint::VELOCITY);
360 wheel2->setActuatorType(Joint::VELOCITY);
361 wheel3->setActuatorType(Joint::VELOCITY);
362 wheel4->setActuatorType(Joint::VELOCITY);
363 }
364
365 // Solve for a balanced pose using IK (Lesson 7 Answer)
solveIK(SkeletonPtr biped)366 Eigen::VectorXd solveIK(SkeletonPtr biped)
367 {
368 // Modify the intial pose to one-foot stance before IK
369 biped->setPosition(biped->getDof("j_shin_right")->getIndexInSkeleton(), -1.4);
370 biped->setPosition(
371 biped->getDof("j_bicep_left_x")->getIndexInSkeleton(), 0.8);
372 biped->setPosition(
373 biped->getDof("j_bicep_right_x")->getIndexInSkeleton(), -0.8);
374
375 Eigen::VectorXd newPose = biped->getPositions();
376 BodyNodePtr leftHeel = biped->getBodyNode("h_heel_left");
377 BodyNodePtr leftToe = biped->getBodyNode("h_toe_left");
378 double initialHeight = -0.8;
379
380 for (std::size_t i = 0; i < default_ik_iterations; ++i)
381 {
382 Eigen::Vector3d deviation = biped->getCOM() - leftHeel->getCOM();
383 Eigen::Vector3d localCOM = leftHeel->getCOM(leftHeel);
384 LinearJacobian jacobian = biped->getCOMLinearJacobian()
385 - biped->getLinearJacobian(leftHeel, localCOM);
386
387 // Sagittal deviation
388 double error = deviation[0];
389 Eigen::VectorXd gradient = jacobian.row(0);
390 Eigen::VectorXd newDirection = -0.2 * error * gradient;
391
392 // Lateral deviation
393 error = deviation[2];
394 gradient = jacobian.row(2);
395 newDirection += -0.2 * error * gradient;
396
397 // Position constraint on four (approximated) corners of the left foot
398 Eigen::Vector3d offset(0.0, -0.04, -0.03);
399 error = (leftHeel->getTransform() * offset)[1] - initialHeight;
400 gradient = biped->getLinearJacobian(leftHeel, offset).row(1);
401 newDirection += -0.2 * error * gradient;
402
403 offset[2] = 0.03;
404 error = (leftHeel->getTransform() * offset)[1] - initialHeight;
405 gradient = biped->getLinearJacobian(leftHeel, offset).row(1);
406 newDirection += -0.2 * error * gradient;
407
408 offset[0] = 0.04;
409 error = (leftToe->getTransform() * offset)[1] - initialHeight;
410 gradient = biped->getLinearJacobian(leftToe, offset).row(1);
411 newDirection += -0.2 * error * gradient;
412
413 offset[2] = -0.03;
414 error = (leftToe->getTransform() * offset)[1] - initialHeight;
415 gradient = biped->getLinearJacobian(leftToe, offset).row(1);
416 newDirection += -0.2 * error * gradient;
417
418 newPose += newDirection;
419 biped->setPositions(newPose);
420 biped->computeForwardKinematics(true, false, false);
421 }
422 return newPose;
423 }
424
createFloor()425 SkeletonPtr createFloor()
426 {
427 SkeletonPtr floor = Skeleton::create("floor");
428
429 // Give the floor a body
430 BodyNodePtr body
431 = floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
432
433 // Give the body a shape
434 double floor_width = 10.0;
435 double floor_height = 0.01;
436 std::shared_ptr<BoxShape> box(
437 new BoxShape(Eigen::Vector3d(floor_width, floor_height, floor_width)));
438 auto shapeNode = body->createShapeNodeWith<
439 VisualAspect,
440 CollisionAspect,
441 DynamicsAspect>(box);
442 shapeNode->getVisualAspect()->setColor(dart::Color::Black());
443
444 // Put the body into position
445 Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
446 tf.translation() = Eigen::Vector3d(0.0, -1.0, 0.0);
447 body->getParentJoint()->setTransformFromParentBodyNode(tf);
448
449 return floor;
450 }
451
main(int argc,char * argv[])452 int main(int argc, char* argv[])
453 {
454 SkeletonPtr floor = createFloor();
455
456 // Lesson 1
457 SkeletonPtr biped = loadBiped();
458
459 // Lesson 2
460 setInitialPose(biped);
461
462 // Lesson 5
463 modifyBipedWithSkateboard(biped);
464
465 // Lesson 6
466 setVelocityAccuators(biped);
467
468 // Lesson 7
469 Eigen::VectorXd balancedPose = solveIK(biped);
470 biped->setPositions(balancedPose);
471
472 WorldPtr world = std::make_shared<World>();
473 world->setGravity(Eigen::Vector3d(0.0, -9.81, 0.0));
474
475 if (dart::collision::CollisionDetector::getFactory()->canCreate("bullet"))
476 {
477 world->getConstraintSolver()->setCollisionDetector(
478 dart::collision::CollisionDetector::getFactory()->create("bullet"));
479 }
480
481 world->addSkeleton(floor);
482 world->addSkeleton(biped);
483
484 // Create a window for rendering the world and handling user input
485 MyWindow window(world);
486
487 // Print instructions
488 std::cout << "'.': forward push" << std::endl;
489 std::cout << "',': backward push" << std::endl;
490 std::cout << "'s': increase skateboard forward speed" << std::endl;
491 std::cout << "'a': increase skateboard backward speed" << std::endl;
492 std::cout << "space bar: simulation on/off" << std::endl;
493 std::cout << "'p': replay simulation" << std::endl;
494 std::cout << "'v': Turn contact force visualization on/off" << std::endl;
495 std::cout << "'[' and ']': replay one frame backward and forward"
496 << std::endl;
497
498 // Initialize glut, initialize the window, and begin the glut event loop
499 glutInit(&argc, argv);
500 window.initWindow(640, 480, "Multi-Pendulum Tutorial");
501 glutMainLoop();
502 }
503