forked from MarcToussaint/robotics-course
-
Notifications
You must be signed in to change notification settings - Fork 5
/
Copy pathsimlab2.html
368 lines (253 loc) · 17.7 KB
/
simlab2.html
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
358
359
360
361
362
363
364
365
366
367
368
<!DOCTYPE html>
<html class="writer-html5" lang="en" >
<head>
<meta charset="utf-8" />
<meta name="viewport" content="width=device-width, initial-scale=1.0" />
<title>SimLab Exercise 2 - Motion — Robotics Course documentation</title>
<link rel="stylesheet" href="_static/css/theme.css" type="text/css" />
<link rel="stylesheet" href="_static/pygments.css" type="text/css" />
<!--[if lt IE 9]>
<script src="_static/js/html5shiv.min.js"></script>
<![endif]-->
<script type="text/javascript" id="documentation_options" data-url_root="./" src="_static/documentation_options.js"></script>
<script src="_static/jquery.js"></script>
<script src="_static/underscore.js"></script>
<script src="_static/doctools.js"></script>
<script async="async" src="https://cdnjs.cloudflare.com/ajax/libs/mathjax/2.7.7/latest.js?config=TeX-AMS-MML_HTMLorMML"></script>
<script type="text/javascript" src="_static/js/theme.js"></script>
<link rel="index" title="Index" href="genindex.html" />
<link rel="search" title="Search" href="search.html" />
<link rel="next" title="SimLab Exercise 3 - OpenCV" href="simlab3.html" />
<link rel="prev" title="SimLab Exercise 1 - Basic Setup" href="simlab1.html" />
</head>
<body class="wy-body-for-nav">
<div class="wy-grid-for-nav">
<nav data-toggle="wy-nav-shift" class="wy-nav-side">
<div class="wy-side-scroll">
<div class="wy-side-nav-search" >
<a href="index.html" class="icon icon-home"> Robotics Course
</a>
<div role="search">
<form id="rtd-search-form" class="wy-form" action="search.html" method="get">
<input type="text" name="q" placeholder="Search docs" />
<input type="hidden" name="check_keywords" value="yes" />
<input type="hidden" name="area" value="default" />
</form>
</div>
</div>
<div class="wy-menu wy-menu-vertical" data-spy="affix" role="navigation" aria-label="main navigation">
<p class="caption"><span class="caption-text">Contents:</span></p>
<ul class="current">
<li class="toctree-l1 current"><a class="reference internal" href="simlab.html">Lab Course in Simulation</a><ul class="current">
<li class="toctree-l2"><a class="reference internal" href="simlab1.html">SimLab Exercise 1 - Basic Setup</a></li>
<li class="toctree-l2 current"><a class="current reference internal" href="#">SimLab Exercise 2 - Motion</a><ul>
<li class="toctree-l3"><a class="reference internal" href="#a-basic-ik">a) Basic IK</a></li>
<li class="toctree-l3"><a class="reference internal" href="#b-path-optimization-velocity-acceleration-objectives">b) Path Optimization & velocity/acceleration objectives</a></li>
<li class="toctree-l3"><a class="reference internal" href="#c-explore-collision-features-and-enforce-touch">c) Explore collision features, and enforce touch</a></li>
<li class="toctree-l3"><a class="reference internal" href="#d-interacting-with-real-objects">d) Interacting with “real” objects</a></li>
<li class="toctree-l3"><a class="reference internal" href="#e-advanced-tricky-use-of-inequalities-scaling-and-target">e) Advanced: Tricky use of inequalities, scaling, and target</a></li>
<li class="toctree-l3"><a class="reference internal" href="#f-advanced-reactive-operational-space-control">f) Advanced: Reactive Operational Space Control</a></li>
</ul>
</li>
<li class="toctree-l2"><a class="reference internal" href="simlab3.html">SimLab Exercise 3 - OpenCV</a></li>
<li class="toctree-l2"><a class="reference internal" href="simlab4.html">SimLab Exercise 4 - Grasp</a></li>
<li class="toctree-l2"><a class="reference internal" href="simlab6.html">SimLab - Project Proposal and Final Presentation</a></li>
</ul>
</li>
<li class="toctree-l1"><a class="reference internal" href="rai.html">Robotics Code Overview</a></li>
<li class="toctree-l1"><a class="reference internal" href="komoTutorial.html">Learning KOMO (K-Order Markov Optimization)</a></li>
</ul>
</div>
</div>
</nav>
<section data-toggle="wy-nav-shift" class="wy-nav-content-wrap">
<nav class="wy-nav-top" aria-label="top navigation">
<i data-toggle="wy-nav-top" class="fa fa-bars"></i>
<a href="index.html">Robotics Course</a>
</nav>
<div class="wy-nav-content">
<div class="rst-content">
<div role="navigation" aria-label="breadcrumbs navigation">
<ul class="wy-breadcrumbs">
<li><a href="index.html" class="icon icon-home"></a> »</li>
<li><a href="simlab.html">Lab Course in Simulation</a> »</li>
<li>SimLab Exercise 2 - Motion</li>
<li class="wy-breadcrumbs-aside">
</li>
</ul>
<hr/>
</div>
<div role="main" class="document" itemscope="itemscope" itemtype="http://schema.org/Article">
<div itemprop="articleBody">
<div class="section" id="simlab-exercise-2-motion">
<h1>SimLab Exercise 2 - Motion<a class="headerlink" href="#simlab-exercise-2-motion" title="Permalink to this headline">¶</a></h1>
<p>Note: Before new exercises, always update the repo:</p>
<div class="highlight-default notranslate"><div class="highlight"><pre><span></span>cd $HOME/git/robotics-course
git pull
git submodule update
cd build
make -j4
</pre></div>
</div>
<div class="section" id="a-basic-ik">
<h2>a) Basic IK<a class="headerlink" href="#a-basic-ik" title="Permalink to this headline">¶</a></h2>
<p>The first example in <code class="docutils literal notranslate"><span class="pre">course3-Simulation/03-motion</span></code> demonstrates a
minimalistic setup to use optimization for IK. The initial example
creates a KOMO instance setup to solve a 1-time-step optimization
problem (i.e., and IK problem). Start from this example to generate
more interesting motion. The specific tasks are:</p>
<ol class="arabic simple">
<li><p>Vary between the left gripper and right gripper reaching for the
object. Is there a difference to “the object reaching for the right
gripper” vs. the other way around? And test the left gripper
reaching for the right gripper.</p></li>
<li><p>Also constrain the gripper orientation when reaching for the
object. For a start, try to add a <code class="docutils literal notranslate"><span class="pre">FS_quaternionDiff</span></code>
constraint. Why does this not work immediately? Try to change the
object pose so that the constraint can be fulfilled. Be able to
explain the result.</p></li>
<li><p>There are (in my view better) alternatives to apriori fixing the
gripper orientation (the full quaternion). Instead add a
<code class="docutils literal notranslate"><span class="pre">FS_scalarProductXZ</span></code> constraint and try to understand. (Zoom into
the little coordinate frame in the gripper center to understand
conventions.) Play around with all combinations of
<code class="docutils literal notranslate"><span class="pre">scalarProduct??</span></code> and understand the effect. Further, add one
more argument <code class="docutils literal notranslate"><span class="pre">target={.1}</span></code> to the <code class="docutils literal notranslate"><span class="pre">addObjective</span></code> method, and
understand the result. Using multiple scalar product features, how
could you also impose a full orientation constraint, and how would
this differ to constraining the quaternion directly?</p></li>
<li><p>Think more holistically about grasping: How could it be realized
properly? For example, think about optimizing a series of two or
three poses, where the first might be a so-called <em>pre-grasp</em>, and
the others model approach and final grap (from which the
gripper-close command could be triggered). Create such a
sequence. Note: Do all of this without yet explicitly using
collision (<code class="docutils literal notranslate"><span class="pre">pairCollision</span></code>) features.</p></li>
<li><p>For your information, <code class="docutils literal notranslate"><span class="pre">tutorials/2-features.ipynb</span></code> gives an
impression about what alternative features one can use to design
motion.</p></li>
</ol>
</div>
<div class="section" id="b-path-optimization-velocity-acceleration-objectives">
<h2>b) Path Optimization & velocity/acceleration objectives<a class="headerlink" href="#b-path-optimization-velocity-acceleration-objectives" title="Permalink to this headline">¶</a></h2>
<p>The second example in <code class="docutils literal notranslate"><span class="pre">course3-Simulation/03-motion</span></code> demonstrates a
full path optimization example. Note the differences: We now specify the <code class="docutils literal notranslate"><span class="pre">times</span></code> argument when adding an objective, which is an single time slice or interval specified as a floating number. We defined the path to have 1 phase with 40 steps-per-phase; the <code class="docutils literal notranslate"><span class="pre">times={1.}</span></code> means the objective only holds at phase 1 (end of the path).</p>
<p>Further note the <code class="docutils literal notranslate"><span class="pre">qItself</span></code> objective with <code class="docutils literal notranslate"><span class="pre">order=1</span></code>, which constrains the joint velocity to be zero at the end of the path.</p>
<ol class="arabic simple">
<li><p>Remove the qItself objective - why is the result optimal?</p></li>
<li><p>Add the qItself objective again. Additionally, constrain the
gripper to have constant(!) acceleration (order=2) equal to
<span class="math notranslate nohighlight">\((0,0,.1)\)</span> during the interval [0.7,1.]. What is this
doing?</p></li>
<li><p>Modify the previous to impose a reasonable grasp approach to the
object, that works for any orientation of the object.</p></li>
</ol>
</div>
<div class="section" id="c-explore-collision-features-and-enforce-touch">
<h2>c) Explore collision features, and enforce touch<a class="headerlink" href="#c-explore-collision-features-and-enforce-touch" title="Permalink to this headline">¶</a></h2>
<p>So far we neglected collisions – and it is generally a fair approach
to first try to design motions that inherently stay away from
collisions even without using collision features, as the latter imply
local optima.</p>
<p>The <code class="docutils literal notranslate"><span class="pre">distance</span></code> feature returns the <em>negative</em> distance between the
given pair of frames (where the frames need to be convex shapes). You
should impose an inequality (lower-equal zero) to force the solver to avoid
penetrations. By changing the target you can also add a margin.</p>
<ol class="arabic simple">
<li><p>Add an additional obstacle, e.g. a sphere, to the scene, with which
your moving gripper (from exercise b) collides. Then add a
<code class="docutils literal notranslate"><span class="pre">distance</span></code> inequality objective between the new sphere and the
<code class="docutils literal notranslate"><span class="pre">"R_gripper"</span></code> object. In addition, also add the same between
<code class="docutils literal notranslate"><span class="pre">R_gripper</span></code> and <code class="docutils literal notranslate"><span class="pre">object</span></code>, which should modify the grasp
approach. You can also try analogously for <code class="docutils literal notranslate"><span class="pre">R_finger1</span></code> and
<code class="docutils literal notranslate"><span class="pre">R_finger2</span></code>.</p></li>
<li><p>Now, remove previous grasp or collision objectives, and only add a
<code class="docutils literal notranslate"><span class="pre">distance</span></code> equal to zero objective on the <code class="docutils literal notranslate"><span class="pre">R_finger1</span></code> and
<code class="docutils literal notranslate"><span class="pre">object</span></code> as the goal constraint. This should generate a touching
motion. This is a typical ingredient in generating pushing
interactions.</p></li>
</ol>
</div>
<div class="section" id="d-interacting-with-real-objects">
<h2>d) Interacting with “real” objects<a class="headerlink" href="#d-interacting-with-real-objects" title="Permalink to this headline">¶</a></h2>
<p>The two examples in <code class="docutils literal notranslate"><span class="pre">course3-Simulation/03-motion</span></code> do not really interact with the “real” world (Simulation, in our case), but only compute some motion in your model configuration. Let us change that:</p>
<ol class="arabic simple">
<li><p>Add a new object named “myObj” of shape type <code class="docutils literal notranslate"><span class="pre">ssBox</span></code> (see note
below).</p></li>
<li><p>Setup a “real” world loop, and shortcut perception by always
querying <code class="docutils literal notranslate"><span class="pre">RealWorld["obj1"]->getPosition()</span></code> to get the position
of the object “obj1”. Set the position of “myObj” to the same
position.</p></li>
<li><p>Try to use a finger of either of the arms to continuously touch the
falling object in the real world loop.</p></li>
</ol>
<p>Note: <code class="docutils literal notranslate"><span class="pre">ssBox</span></code> means sphere-swept box. This is a box with rounded
corners. This should be your default primitive shape. The shape is
determined by 4 numbers: x-size, y-size, z-size, radius of
corners. The 2nd most important shape type is <code class="docutils literal notranslate"><span class="pre">ssCvx</span></code> (sphere-swept
convex), which is determined by a set of 3D points, and sphere radius
that is added to the points’ convex hull. (E.g., a capsule can also be
described as simple ssCvx: 2 points with a sweeping radius.) The
sphere-swept shape primitives allow for well-defined Jacobians of
collision features.</p>
</div>
<div class="section" id="e-advanced-tricky-use-of-inequalities-scaling-and-target">
<h2>e) Advanced: Tricky use of inequalities, scaling, and target<a class="headerlink" href="#e-advanced-tricky-use-of-inequalities-scaling-and-target" title="Permalink to this headline">¶</a></h2>
<p>This is a bit tricky to figure out, but if you do, you really understood the use of the scaling, target and inequalities.</p>
<p>As in a), consider IK for grasping a cylinder again. Let’s care only
about the gripper position, not it’s orientation. The position needs
to be in the interval <span class="math notranslate nohighlight">\([-l/2,l/2]\)</span> along the z-axis of the
cylinder, if it has length <span class="math notranslate nohighlight">\(l\)</span>. We can model this with 4
constraints:
* The x-component of the <code class="docutils literal notranslate"><span class="pre">positionRel(gripperCenter,object)</span></code> needs to be equal 0
* The y-component of the <code class="docutils literal notranslate"><span class="pre">positionRel(gripperCenter,object)</span></code> needs to be equal 0
* The z-component of the <code class="docutils literal notranslate"><span class="pre">positionRel(gripperCenter,object)</span></code> needs to be lower-equal l/2
* The z-component of the <code class="docutils literal notranslate"><span class="pre">positionRel(gripperCenter,object)</span></code> needs to be greater-equal l/2</p>
<p>Can you figure out how to realize this? Tip: Choosing a scale
<code class="docutils literal notranslate"><span class="pre">arr({1,</span> <span class="pre">3},</span> <span class="pre">{0,0,1})</span></code> picks out the z-component of a 3D feature (as
it means multiplication with <span class="math notranslate nohighlight">\((0,0,1)^T\)</span>.) But note, the target
always needs to live in the original 3D feature space!</p>
</div>
<div class="section" id="f-advanced-reactive-operational-space-control">
<h2>f) Advanced: Reactive Operational Space Control<a class="headerlink" href="#f-advanced-reactive-operational-space-control" title="Permalink to this headline">¶</a></h2>
<p>Realize a simplest possible instance of Operational Space Control
using KOMO. [The python interfaces are not ready for this yet.]</p>
<ol class="arabic simple">
<li><p>Setup a minimal KOMO problem of order <span class="math notranslate nohighlight">\(k=2\)</span>. Add a
add_qControlObjective to penalize accelerations. Add another
add_qControlObjective to penalize also velocities! Add a weak
objective on the hand position. Solve and make a single step
forward.</p></li>
<li><p>Repeat the above, always recreating KOMO from the new current
configuration.</p></li>
</ol>
</div>
</div>
</div>
</div>
<footer>
<div class="rst-footer-buttons" role="navigation" aria-label="footer navigation">
<a href="simlab3.html" class="btn btn-neutral float-right" title="SimLab Exercise 3 - OpenCV" accesskey="n" rel="next">Next <span class="fa fa-arrow-circle-right" aria-hidden="true"></span></a>
<a href="simlab1.html" class="btn btn-neutral float-left" title="SimLab Exercise 1 - Basic Setup" accesskey="p" rel="prev"><span class="fa fa-arrow-circle-left" aria-hidden="true"></span> Previous</a>
</div>
<hr/>
<div role="contentinfo">
<p>
© Copyright 2020, Marc Toussaint.
</p>
</div>
Built with <a href="https://www.sphinx-doc.org/">Sphinx</a> using a
<a href="https://github.com/readthedocs/sphinx_rtd_theme">theme</a>
provided by <a href="https://readthedocs.org">Read the Docs</a>.
</footer>
</div>
</div>
</section>
</div>
<script type="text/javascript">
jQuery(function () {
SphinxRtdTheme.Navigation.enable(true);
});
</script>
</body>
</html>