/
tutorialDominoes.cpp
357 lines (291 loc) · 10.1 KB
/
tutorialDominoes.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
/*
* Copyright (c) 2015, Georgia Tech Research Corporation
* All rights reserved.
*
* Author(s): Michael X. Grey <mxgrey@gatech.edu>
*
* Georgia Tech Graphics Lab and Humanoid Robotics Lab
*
* Directed by Prof. C. Karen Liu and Prof. Mike Stilman
* <karenliu@cc.gatech.edu> <mstilman@cc.gatech.edu>
*
* This file is provided under the following "BSD-style" License:
* Redistribution and use in source and binary forms, with or
* without modification, are permitted provided that the following
* conditions are met:
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES,
* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
* DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
* CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
* USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include "dart/dart.h"
const double default_domino_height = 0.3;
const double default_domino_width = 0.4 * default_domino_height;
const double default_domino_depth = default_domino_width / 5.0;
const double default_distance = default_domino_height / 2.0;
const double default_angle = 20.0 * M_PI / 180.0;
const double default_domino_density = 2.6e3; // kg/m^3
const double default_domino_mass =
default_domino_density
* default_domino_height
* default_domino_width
* default_domino_depth;
const double default_push_force = 8.0; // N
const int default_force_duration = 200; // # iterations
const int default_push_duration = 1000; // # iterations
const double defaultmEndEffectormOffset = 0.05;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::math;
class Controller
{
public:
Controller(const SkeletonPtr& manipulator, const SkeletonPtr& /*domino*/)
: mManipulator(manipulator)
{
// Grab the current joint angles to use them as desired angles
// Lesson 2b
// Set up the information needed for an operational space controller
// Lesson 3a
// Set PD control gains
mKpPD = 200.0;
mKdPD = 20.0;
// Set operational space control gains
mKpOS = 5.0;
mKdOS = 0.01;
}
/// Compute a stable PD controller that also compensates for gravity and
/// Coriolis forces
void setPDForces()
{
// Write a stable PD controller
// Lesson 2c
// Compensate for gravity and Coriolis forces
// Lesson 2d
}
/// Compute an operational space controller to push on the first domino
void setOperationalSpaceForces()
{
// Lesson 3b
}
protected:
/// The manipulator Skeleton that we will be controlling
SkeletonPtr mManipulator;
/// The target pose for the controller
SimpleFramePtr mTarget;
/// End effector for the manipulator
BodyNodePtr mEndEffector;
/// Desired joint positions when not applying the operational space controller
Eigen::VectorXd mQDesired;
/// The offset of the end effector from the body origin of the last BodyNode
/// in the manipulator
Eigen::Vector3d mOffset;
/// Control gains for the proportional error terms in the PD controller
double mKpPD;
/// Control gains for the derivative error terms in the PD controller
double mKdPD;
/// Control gains for the proportional error terms in the operational
/// space controller
double mKpOS;
/// Control gains for the derivative error terms in the operational space
/// controller
double mKdOS;
/// Joint forces for the manipulator (output of the Controller)
Eigen::VectorXd mForces;
};
class MyWindow : public dart::gui::SimWindow
{
public:
MyWindow(const WorldPtr& world)
: mTotalAngle(0.0),
mHasEverRun(false),
mForceCountDown(0),
mPushCountDown(0)
{
setWorld(world);
mFirstDomino = world->getSkeleton("domino");
mFloor = world->getSkeleton("floor");
mController = std::make_unique<Controller>(
world->getSkeleton("manipulator"), mFirstDomino);
}
// Attempt to create a new domino. If the new domino would be in collision
// with anything (other than the floor), then discard it.
void attemptToCreateDomino(double /*angle*/)
{
// Create a new domino
// Lesson 1a
// Look through the collisions to see if any dominoes are penetrating
// something
// Lesson 1b
}
// Delete the last domino that was added to the scene. (Do not delete the
// original domino)
void deleteLastDomino()
{
// Lesson 1c
}
void keyboard(unsigned char key, int x, int y) override
{
if(!mHasEverRun)
{
switch(key)
{
case 'q':
attemptToCreateDomino( default_angle);
break;
case 'w':
attemptToCreateDomino(0.0);
break;
case 'e':
attemptToCreateDomino(-default_angle);
break;
case 'd':
deleteLastDomino();
break;
case ' ':
mHasEverRun = true;
break;
}
}
else
{
switch(key)
{
case 'f':
mForceCountDown = default_force_duration;
break;
case 'r':
mPushCountDown = default_push_duration;
break;
}
}
SimWindow::keyboard(key, x, y);
}
void timeStepping() override
{
// If the user has pressed the 'f' key, apply a force to the first domino in
// order to push it over
if(mForceCountDown > 0)
{
// Lesson 1d
--mForceCountDown;
}
// Run the controller for the manipulator
if(mPushCountDown > 0)
{
mController->setOperationalSpaceForces();
--mPushCountDown;
}
else
{
mController->setPDForces();
}
SimWindow::timeStepping();
}
protected:
/// Base domino. Used to clone new dominoes.
SkeletonPtr mFirstDomino;
/// Floor of the scene
SkeletonPtr mFloor;
/// History of the dominoes that have been created
std::vector<SkeletonPtr> mDominoes;
/// History of the angles that the user has specified
std::vector<double> mAngles;
/// Sum of all angles so far
double mTotalAngle;
/// Set to true the first time spacebar is pressed
bool mHasEverRun;
/// The first domino will be pushed by a disembodied force while the value of
/// this is greater than zero
int mForceCountDown;
/// The manipulator will attempt to push on the first domino while the value
/// of this is greater than zero
int mPushCountDown;
std::unique_ptr<Controller> mController;
};
SkeletonPtr createDomino()
{
// Create a Skeleton with the name "domino"
SkeletonPtr domino = Skeleton::create("domino");
// Create a body for the domino
BodyNodePtr body =
domino->createJointAndBodyNodePair<FreeJoint>(nullptr).second;
// Create a shape for the domino
std::shared_ptr<BoxShape> box(
new BoxShape(Eigen::Vector3d(default_domino_depth,
default_domino_width,
default_domino_height)));
body->createShapeNodeWith<VisualAddon, CollisionAddon, DynamicsAddon>(box);
// Set up inertia for the domino
dart::dynamics::Inertia inertia;
inertia.setMass(default_domino_mass);
inertia.setMoment(box->computeInertia(default_domino_mass));
body->setInertia(inertia);
domino->getDof("Joint_pos_z")->setPosition(default_domino_height / 2.0);
return domino;
}
SkeletonPtr createFloor()
{
SkeletonPtr floor = Skeleton::create("floor");
// Give the floor a body
BodyNodePtr body =
floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;
// Give the body a shape
double floor_width = 10.0;
double floor_height = 0.01;
std::shared_ptr<BoxShape> box(
new BoxShape(Eigen::Vector3d(floor_width, floor_width, floor_height)));
auto shapeNode
= body->createShapeNodeWith<VisualAddon, CollisionAddon, DynamicsAddon>(box);
shapeNode->getVisualAddon()->setColor(dart::Color::Black());
// Put the body into position
Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height / 2.0);
body->getParentJoint()->setTransformFromParentBodyNode(tf);
return floor;
}
SkeletonPtr createManipulator()
{
// Lesson 2a
return Skeleton::create("manipulator");
}
int main(int argc, char* argv[])
{
SkeletonPtr domino = createDomino();
SkeletonPtr floor = createFloor();
SkeletonPtr manipulator = createManipulator();
WorldPtr world = std::make_shared<World>();
world->addSkeleton(domino);
world->addSkeleton(floor);
world->addSkeleton(manipulator);
MyWindow window(world);
std::cout << "Before simulation has started, you can create new dominoes:" << std::endl;
std::cout << "'w': Create new domino angled forward" << std::endl;
std::cout << "'q': Create new domino angled to the left" << std::endl;
std::cout << "'e': Create new domino angled to the right" << std::endl;
std::cout << "'d': Delete the last domino that was created" << std::endl;
std::cout << std::endl;
std::cout << "spacebar: Begin simulation (you can no longer create or remove dominoes)" << std::endl;
std::cout << "'p': replay simulation" << std::endl;
std::cout << "'f': Push the first domino with a disembodies force so that it falls over" << std::endl;
std::cout << "'r': Push the first domino with the manipulator so that it falls over" << std::endl;
std::cout << "'v': Turn contact force visualization on/off" << std::endl;
glutInit(&argc, argv);
window.initWindow(640, 480, "Dominoes");
glutMainLoop();
}