15 |
16 | # Notes on Maths
17 | The content of this course relies heavily on these mathematical knowledge, namely Group, Differential Geometry, and Lie Group. Of course you can finish the entire course without knowing all these maths tools by dint of solvitur ambulando, after all, the prerequisite knowledge for this course is "freshman mathematics" and "basic physics". But familiarizing yourself with these maths tools can do more with less.
18 |
19 |
14 |
15 | ## Useful Notes and Equations
16 | Before diving into the quesitons, some of the most handy notes and equations will be summarized in this section.
17 |
18 | ##### Configuration & C-space
19 | > **Definition 2.1.** The **configuration** of a robot is a complete specification of the position of every point of the robot. The minimum number n of **real-valued** coordinates needed to represent the configuration is the number of **degrees of freedom (dof)** of the robot. The n-dimensional space containing all possible configurations of the robot is called the **configuration space (C-space)**. The configuration of a robot is represented by a point in its C-space.
20 |
21 | ##### Task Space
22 | Is the space containing all the configurations specified by the tasks. This space is independent of the physical robot mechanism.
23 |
24 | ##### Workspace
25 | Is the C-space of the end-effector of the robot. The workspace can be interpreted as the reacheable space of the end-effector.
26 |
27 | It is also worth noticing that a dof has to be real-valued coordinates. For instance, a discrete coordinate of a coin, $${head, tail}$$, cannot be a dof, because its range is not real.
28 |
29 | ### Tables
30 | All pictures, tables, charts, unless noted otherwise, are taken from \[1].
31 |
32 |
33 |
34 |
35 | ### Equations
36 | General idea about degree of freedom (DoF):
37 | $$
38 | \begin{align}
39 | \begin{split}
40 | \text{DoF} &= (\text{sum of freedoms of the points}) - (\text{No. of independent constraints})\\
41 | &= (\text{sum of freedoms of the bodies}) - (\text{No. of independent constraints})\\
42 | \end{split}
43 | \end{align}
44 | $$
45 | #### Grübler's Formula
46 | $$
47 | \begin{align}
48 | \begin{split}
49 | \text{DoF} &= m(N-1)-\sum_{i=1}^{J}c_{i}\\
50 | &= m(N-1-J)+\sum_{i=1}^{J}f_{i}\\
51 | \text{where } m&=\text{DoF of a rigid body. For planar, m=3; for spatial, m=6}\\
52 | N&=\text{No. of links, always add 1 to account the ground}\\
53 | J&=\text{No. of joints}\\
54 | c_{i}&=\text{No. of constraints provided by ith joint}\\
55 | f_{i}&=\text{No. of DoF provided by ith joint}
56 | \end{split}
57 | \end{align}
58 | $$
59 |
60 | N.B. that:
61 | - Watch out for overlapping joints. If three links are connected to the same joint, then there are actually 2 joints!
62 | - When dealing with constraints, try to model them as a special kind of joints:
63 | - A point constraint (e.g., the end-effector must pass through a given point in space) is equivalent to a SP joint;
64 | - A surface constraint (e.g., a block sliding on a surface) is equivalent to a RP2 joint;
65 | - If a mechanism is consisted of purely revolute joints and have their axes of rotation meeting at a single point, then the system is actually confined to move on a surface of sphere. In this case, choose $$m = 3$$.
66 |
67 | ***
68 |
69 | ## Textbook Exercises Attempts
70 | > _**Exercise 2.1**_ Using the methods of Section 2.1 derive a formula, in terms of n, for the number of degrees of freedom of a rigid body in n-dimensional space. Indicate how many of these dof are translational and how many are rotational. Describe the topology of the C-space (e.g., for n = 2, the topology is $$\mathbb{R}^{2}\times \mathbb{S}^{1}$$).
71 |
72 | Recall that in 3D we first choose an arbitrary point in space, which has a linear DoF of 3, then for all of the following rotational DoF, the preceding choice provides one constraint. Therefore in n=3 we have the topology $$\mathbb{R}^{3}\times \mathbb{S}^{2}\times \mathbb{S}^{1}$$.
73 | Generalize this idea we have $$\mathbb{R}^{n}\times \mathbb{S}^{n-1}\times \mathbb{S}^{n-2}\times \text{...}\times \mathbb{S}^{1}$$.
74 |
75 | > _**Exercise 2.4**_ Assume each of your arms has n degrees of freedom. You are driving a car, your torso is stationary relative to the car (owing to a tight seatbelt!), and both hands are firmly grasping the wheel, so that your hands do not move relative to the wheel. How many degrees of freedom does your arms-plus-steering wheel system have? Explain your answer.
76 |
77 | The wheel is a fixed rigid body in space, therefore it adds 6 constraints to the system. Then, each of your hand has a $$DoF = n-6 $$. Together you have $$DoF = 2n-12$$. However, if the wheel is free to rotate, then it adds 1 extra DoF to the system. In this case you have $$DoF = 2n-11$$.
78 |
79 | > _**Exercise 2.5**_ Figure 2.15 shows a robot used for human arm rehabilitation. Determine the number of degrees of freedom of the chain formed by the human arm and the robot.
80 |
81 |
82 |
83 |
84 |
85 |
86 | $$
87 | \begin{align}
88 | \begin{split}
89 | m &= 6\\
90 | N &= 6(links) + 1(ground)=7\\
91 | J &= 5R + 2S=7\\
92 | \Sigma f_{i}&=5+2\cdot 3=11\\
93 | DoF&=m(N-1-J)+\Sigma f_{i}\\
94 | &= 6\cdot(7-1-7)+11\\
95 | &=5\\
96 | \end{split}
97 | \end{align}
98 | $$
99 |
100 | > _**Exercise 2.6**_ The mobile manipulator of Figure 2.16 consists of a 6R arm and multi-fingered hand mounted on a mobile base with a single wheel. You can think of the wheeled base as the same as the rolling coin in Figure 2.11 – the wheel (and base) can spin together about an axis perpendicular to the ground, and the wheel rolls without slipping. The base always remains horizontal. (Left unstated are the means to keep the base horizontal and to spin the wheel and base about an axis perpendicular to the ground.)
101 | > - (a) Ignoring the multi-fingered hand, describe the configuration space of the mobile manipulator.
102 | > - (b) Now suppose that the robot hand rigidly grasps a refrigerator door handle and, with its wheel and base completely stationary, opens the door using only its arm. With the door open, how many degrees of freedom does the mechanism formed by the arm and open door have?
103 | > - (c) A second identical mobile manipulator comes along, and after parking its mobile base, also rigidly grasps the refrigerator door handle. How many degrees of freedom does the mechanism formed by the two arms and the open refrigerator door have?
104 |
105 |
106 |
107 |
108 | - (a)
109 | The base is kinematically equivalent to a rolling coin, then its C-space is $$\mathbb{R}^{2}\times \mathbb{T}^{2}$$. The C-space of a standard 6R robot is just plainly 6 rotational DoF, i.e. $$\mathbb{S}^{1}\times ...\times \mathbb{S}^{1}=\mathbb{T}^{6}$$. Altogether we have the system C-space as $$\mathbb{R}^{2}\times \mathbb{T}^{2}\times \mathbb{T}^{6}=\mathbb{R}^{2}\times \mathbb{T}^{8}$$.
110 | - (b) $$ \begin{align}
111 | \begin{split}
112 | DoF &= 10(\text{total DoF})-4(\text{stationary base})\\
113 | &-6(\text{rigid body in space})+1(\text{door been free to rotate})\\
114 | &=1\\
115 | \end{split}
116 | \end{align}
117 | $$
118 | - (c) $$DoF=2\cdot(10-4-6)+1=1$$
119 |
120 | > _**Exercise 2.7**_ Three identical SRS open-chain arms are grasping a common object, as shown in Figure 2.17.
121 | > - (a) Find the number of degrees of freedom of this system.
122 | > - (b) Suppose there are now a total of n such arms grasping the object. How many degrees of freedom does this system have?
123 | > - (c) Suppose the spherical wrist joint in each of the n arms is now replaced by a universal joint. How many degrees of freedom does this system have?
124 |
125 |
126 |
127 |
128 |
129 | - (a) $$\begin{align}
130 | \begin{split}
131 | N &= 3\cdot(2)+1\\
132 | J &= 3\cdot(2S+R)\\
133 | \Sigma f_{i}&=3\cdot(2\cdot 3 +1)\\
134 | DoF&=6\cdot (8-1+9)+21\\
135 | &=9\\
136 | \end{split}
137 | \end{align}
138 | $$
139 | - (b)$$\begin{align}
140 | \begin{split}
141 | N&=2n+2\\
142 | J&=n\cdot (2S+R)=3n\\
143 | \Sigma f_{i}&=7n\\
144 | DoF&=6\cdot (2n+2-1-3n)+7n\\
145 | &=6+n\\
146 | \end{split}
147 | \end{align}
148 | $$
149 | - (c) S=>U, 3=>2
150 | $$
151 | \begin{align}
152 | \begin{split}
153 | \Sigma f_{i}&=n\cdot (U+S+R)=6n\\
154 | DoF&=6\cdot (2n+2-1-3n)+5n\\
155 | &=6\\
156 | \end{split}
157 | \end{align}
158 | $$
159 |
160 | > _**Exercise 2.8**_ Consider a spatial parallel mechanism consisting of a moving plate connected to a fixed plate by n identical legs. For the moving plate to have six degrees of freedom, how many degrees of freedom should each leg have, as a function of n? For example, if n = 3 then the moving plate and fixed plate are connected by three legs; how many degrees of freedom should each leg have for the moving plate to move with six degrees of freedom? Solve for arbitrary n.
161 |
162 | $$\begin{align}
163 | \begin{split}
164 | N &= 1(moving plate) + 1(fix plate/ground)\\
165 | J &= n\\
166 | \text{Let x be the No. of DoF of one leg.}\\
167 | \text{Then, }\Sigma f_{i}&=x\cdot n\\
168 | \text{DoF}&=6\cdot (2-1-n)+x\cdot n\\
169 | &=6\\
170 | \end{split}
171 | \end{align}
172 | $$
173 | It seems that the DoF is a constant.
174 |
175 | > _**Exercise 2.9**_ Use the planar version of Grübler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.18. Comment on whether your results agree with your intuition about the possible motions of these mechanisms.
176 |
177 |
178 |
179 |
180 |
181 | > _**Exercise 2.10**_ Use the planar version of Grübler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.19. Comment on whether your results agree with your intuition about the possible motions of these mechanisms.
182 |
183 |
184 |
185 |
186 |
187 | > _**Exercise 2.11**_ Use the spatial version of Grübler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.20. Comment on whether your results agree with your intuition about the possible motions of these mechanisms.
188 |
189 |
190 |
191 |
192 |
193 | > _**Exercise 2.12**_ Use the spatial version of Grübler’s formula to determine the number of degrees of freedom of the mechanisms shown in Figure 2.21. Comment on whether your results agree with your intuition about the possible motions of these mechanisms.
194 |
195 |
196 |
197 |
198 |
199 | For part (d), The RRRR mechanism at the bottom is called a scissor linkage (or lazy Tongs), which is a kind of planar four-bar linkage. Such structure provides 1 DoF.
200 |
201 | > _**Exercise 2.13**_ In the parallel mechanism shown in Figure 2.22, six legs of identical length are connected to a fixed and moving platform via spherical joints. Determine the number of degrees of freedom of this mechanism using Grübler’s formula. Illustrate all possible motions of the upper platform.
202 |
203 |
204 |
205 |
206 |
207 | > _**Exercise 2.14**_ The 3×UPU platform of Figure 2.23 consists of two platforms – the lower one stationary, the upper one mobile–connected by three UPU legs.
208 | > - (a) Using the spatial version of Grübler’s formula, verify that it has three degrees of freedom.
209 | > - (b) Construct a physical model of the 3×UPU platform to see if it indeed has three degrees of freedom. In particular, lock the three P joints in place; does the robot become a rigid structure as predicted by Grübler’s formula, or does it move?
210 |
211 |
212 |
213 |
214 |
215 | > _**Exercise 2.15**_ The mechanisms of Figures 2.24(a) and 2.24(b).
216 | > - (a) The mechanism of Figure 2.24(a) consists of six identical squares arranged in a single closed loop, connected by revolute joints. The bottom square is fixed to ground. Determine the number of degrees of freedom using Grübler’s formula.
217 | > - (b) The mechanism of Figure 2.24(b) also consists of six identical squares connected by revolute joints, but arranged differently (as shown). Determine the number of degrees of freedom using Grübler’s formula. Does your result agree with your intuition about the possible motions of this mechanism?
218 |
219 |
220 |
221 |
222 |
223 | > _**Exercise 2.16**_ Figure 2.25 shows a spherical four-bar linkage, in which four links (one of the links is the ground link) are connected by four revolute joints to form a single-loop closed chain. The four revolute joints are arranged so that they lie on a sphere such that their joint axes intersect at a common point.
224 | > - (a) Use Grübler’s formula to find the number of degrees of freedom. Justify your choice of formula.
225 | > - (b) Describe the configuration space.
226 | > - (c) Assuming that a reference frame is attached to the center link, describe its workspace.
227 |
228 |
229 |
230 |
231 |
232 | > _**Exercise 2.17**_ Figure 2.26 shows a parallel robot used for surgical applications. As shown in Figure 2.26(a), leg A is an RRRP chain, while legs B and C are RRRUR chains. A surgical tool is rigidly attached to the end-effector.
233 | > - (a) Use Grübler’s formula to find the number of degrees of freedom of the mechanism in Figure 2.26(a).
234 | > - (b) Now suppose that the surgical tool must always pass through point A in Figure 2.26(a). How many degrees of freedom does the manipulator have?
235 | > - (c) Legs A, B, and C are now replaced by three identical RRRR legs as shown in Figure 2.26(b). Furthermore, the axes of all R joints pass through point A. Use Gru ̈bler’s formula to derive the number of degrees of freedom of this mechanism.
236 |
237 |
238 |
239 |
240 |
241 | > _**Exercise 2.18**_ Figure 2.27 shows a 3×PUP platform, in which three identical PUP legs connect a fixed base to a moving platform. The P joints on both the fixed base and moving platform are arranged symmetrically. Use Grbler’s formula to find the number of degrees of freedom. Does your answer agree with your intuition about this mechanism? If not, try to explain any discrepancies without resorting to a detailed kinematic analysis.
242 |
243 |
244 |
245 |
246 |
247 | > _**Exercise 2.19**_ The dual-arm robot of Figure 2.28 is rigidly grasping a box. The box can only slide on the table; the bottom face of the box must always be in contact with the table. How many degrees of freedom does this system have?
248 |
249 |
250 |
251 |
252 |
253 | > _**Exercise 2.20**_ The dragonfly robot of Figure 2.29 has a body, four legs, and four wings as shown. Each leg is connected to each adjacent leg by a USP linkage. Use Grübler’s formula to answer the following questions.
254 | > - (a) Suppose the body is fixed and only the legs and wings can move. How many degrees of freedom does the robot have?
255 | > - (b) Now suppose the robot is flying in the air. How many degrees of freedom does the robot have?
256 | > - (c) Now suppose the robot is standing with all four feet in contact with the ground. Assume that the ground is uneven and that each foot–ground contact can be modeled as a point contact with no slip. How many degrees of freedom does the robot have? Explain your answer.
257 |
258 |
259 |
260 |
261 |
262 | - (c) The legs on the ground can be viewed as a point constraint that does not allow translation, therefore it can be modeled as merely a S joint. In doing so, by Grübler's formula we have:
263 |
264 | $$
265 | \begin{align*}
266 | \begin{split}
267 | N&=18\\
268 | J&=20+4S=24\\
269 | \Sigma f_{i}&=32+4\cdot (3)=44\\
270 | DoF &= 6\cdot (18-1-24)+44\\
271 | &=2\\
272 | \end{split}
273 | \end{align*}
274 | $$
275 |
276 | It should be pointed out that the wings of the Dragonfly are free to move. This means that the wings have 4 DoF, while the entire system only has 2. It might imply that the rest of the system has negative DoF. I am not sure whether this is showing that the existence of dependent joints (in this case the Grübler's formula will give negative DoF), or it is showing that there are extra constraints applied to the system.
277 |
278 | > _**Exercise 2.23**_ Consider the slider–crank mechanism of Figure 2.4(b). A rotational motion at the revolute joint fixed to ground (the “crank”) causes a translational motion at the prismatic joint (the “slider”). Suppose that the two links connected to the crank and slider are of equal length. Determine the configuration space of this mechanism, and draw its projected version on the space defined by the crank and slider joint variables.
279 |
280 |
281 |
282 |
283 |
284 | Let $$(x_{1}, y_{1})$$, $$(x_{2}, y_{2})$$, $$(x_{3}, y_{3})$$ be the centre of the three links respectively. Let x be
285 |
286 | $$
287 | x =
288 | \begin{bmatrix}
289 | x_{1} \\
290 | y_{1} \\
291 | x_{2} \\
292 | y_{2} \\
293 | x_{3} \\
294 | y_{3} \\
295 | \theta_{1} \\
296 | \theta_{2}
297 | \end{bmatrix}
298 | $$
299 |
300 | Then, let the constraint equations $$g_{i}(x)$$ be
301 |
302 | $$
303 | g_{i}(x) =
304 | \begin{bmatrix}
305 | x_{1}-\frac{L}{2}\cos{\theta_{1}} \\
306 | y_{1}-\frac{L}{2}\sin{\theta_{1}} \\
307 | x_{2}-L \cos{\theta_1} - \frac{L}{2}\cos{\theta_{2}} \\
308 | y_{2}-L \sin{\theta_1} + \frac{L}{2}\sin{\theta_{2}} \\
309 | x_{3} - L \cos{\theta_{1}} - L \cos{\theta_{2}} \\
310 | y_{3} - L \sin{\theta_{1}} + L \sin{\theta_{2}} \\
311 | y_{1}
312 | \end{bmatrix}
313 | = \vec{0}$$
314 |
315 | The C-space is then defined as
316 |
317 | $$ \text{C-space} = \{x | g_{i}(x)\} $$
318 |
319 | Not sure of what is the question asking for the projection.
320 |
321 | > _**Exercise 2.26**_ The tip coordinates for the two-link planar 2R robot of Figure 2.33 are given by
322 | > $$ \begin{align} \begin{split} x&=2\cos{\theta_{1}} +\cos(\theta_{1}+\theta_{2})\\ y &= 2 \sin{\theta_{1}} + \sin{\theta_{1} + \theta_{2}}\\ \end{split} \end{align}$$
323 | > - (a) What is the robot’s configuration space?
324 | > - (b) What is the robot’s workspace (i.e., the set of all points reachable by the tip)?
325 | > - (c) Suppose infinitely long vertical barriers are placed at x = 1 and x = −1. What is the free C-space of the robot (i.e., the portion of the C-space that does not result in any collisions with the vertical barriers)?
326 |
327 |
328 |
329 |
330 |
331 | - (a) Let $$(x_{1},y_{1})$$ and $$(x_{2},y_{2})$$ be the centre of the two links respectively. Define vector $$q$$ as
332 |
333 | $$
334 | q =
335 | \begin{bmatrix}
336 | x_{1}\\
337 | y_{1}\\
338 | x_{2}\\
339 | y_{2}\\
340 | \theta_{1}\\
341 | \theta_{2}
342 | \end{bmatrix}
343 | $$
344 |
345 | The constraint equations $$g_{i}(x)$$ can be defined as
346 |
347 | $$
348 | g_{i}(x) =
349 | \begin{bmatrix}
350 | x_{1} - \frac{2}{2} \cos{\theta_{1}} \\
351 | y_{1} - \frac{2}{2} \sin{\theta_{2}} \\
352 | x_{2} - 2 \cos{\theta_{1}} - \frac{1}{2} \cos{\theta_{1}+\theta_{2}} \\
353 | y_{2} - 2 \sin{\theta_{1}} - \frac{1}{2} \sin{\theta_{1}+\theta_{2}} \\
354 | \end{bmatrix}
355 | = \vec{0}
356 | $$
357 |
358 | The C-space can therefore be defined as
359 |
360 | $$
361 | \text{C-space} = \{q | g_{i}(x)\}
362 | $$
363 |
364 | - (b) The workspace can be defined as
365 | $$
366 | \text{Workspace} = \{ (x,y) | x=2c_{1}+c_{12}\text{, } y=2s_{1}+s_{12}, \theta_{i} \in (0,2\pi) \}
367 | $$
368 |
369 | - (c) The reacheable space is shown below in the sketch:
370 |
371 |
372 |
373 |
374 |
375 | With some math, the ratio of free C-space on original C-space can be calculated. The calculation is skipped here.
376 |
377 | > _**Exercise 2.28**_ Task space.
378 | > - (a) Describe the task space for a robot arm writing on a blackboard.
379 | > - (b) Describe the task space for a robot arm twirling a baton.
380 |
381 | - (a) A chalk in contact with the blackboard is an object on surface, therefore immediately we have $$\mathbb{R}^{2}$$. Then, the orientation of the chalk will affect the sharpness of the drawing, therefore we now have $$\mathbb{R}^{2}\times \mathbb{S}^{2}$$. Trivially, rotating the chalk about its central axis will not affect the drawing, thus the final task space is just $$\mathbb{R}^{2}\times \mathbb{S}^{2}$$.
382 | - (b) Not sure what does 'twirling a baton' refer to.
383 |
384 | > _**Exercise 2.29**_ Give a mathematical description of the topologies of the C-spaces of the following systems. Use cross products, as appropriate, of spaces such as a closed interval $$[a,b]$$ of a line and $$\mathbb{R}^{k}$$, $$\mathbb{S}^{m}$$, and $$\mathbb{T}^{n}$$, where k, m, and n are chosen appropriately.
385 | > - (a) The chassis of a car-like mobile robot rolling on an infinite plane.
386 | > - (b) The car-like mobile robot (chassis only) driving around on a spherical asteroid.
387 | > - (c) The car-like mobile robot (chassis only) on an infinite plane with an RRPR robot arm mounted on it. The prismatic joint has joint limits, but the revolute joints do not.
388 | > - (d) A free-flying spacecraft with a 6R arm mounted on it and no joint limits.
389 |
390 | - (a) $$\mathbb{R}^{2}\times \mathbb{S}^{1}$$
391 | - (b) $$\mathbb{S}^{2}\times \mathbb{S}^{1}$$
392 | - (c) It will be (chassis on plane) times (the RRR joints) times (the limited P joint): $$\mathbb{R}^{2}\times \mathbb{S}^{1} \times \mathbb{S}^{1}\times \mathbb{S}^{1}\times \mathbb{S}^{1} \times [a,b]= \mathbb{R}^{2}\times \mathbb{T}^{4}\times [a,b] $$
393 | - (d) It will be (rigid body in space) times (6R arm): $$\mathbb{R}^{3}\times \mathbb{S}^{2} \times \mathbb{S}^{1} \times \mathbb{T}^{6} = \mathbb{R}^{3}\times \mathbb{S}^{2} \times \mathbb{T}^{7}$$
394 |
395 | > _**Exercise 2.31**_ A differential-drive mobile robot has two wheels that do not steer but whose speeds can be controlled independently. The robot goes forward and backward by spinning the wheels in the same direction at the same speed, and it turns by spinning the wheels at different speeds. The configuration of the robot is given by five variables: the $$(x, y)$$ location of the point halfway between the wheels, the heading direction $$\theta$$ of the robot’s chassis relative to the x-axis of the world frame, and the rotation angles $$\phi_{1}$$ and $$\phi_{2}$$ of the two wheels about the axis through the centers of the wheels (Figure 2.34). Assume that the radius of each wheel is r and the distance between the wheels is 2d.
396 | > - (a) Let q = (x, y, $$\theta$$, $$\phi_{1}$$, $$\phi_{2}$$) be the configuration of the robot. If the two control inputs are the angular velocities of the wheels $$\omega_{1} = \dot{\phi_{1}}$$ and $$\omega_{2} = \dot{\phi_{2}}$$, write down the vector differential equation $$\dot{q} = g_{1}(q)\omega_{1} + g_{2}(q)\omega_{2}$$. The vector fields $$g_{1}(q)$$ and $$g_{2}(q)$$ are called control vector fields (see Section 13.3) and express how the system moves when the respective unit control signal is applied.
397 | > - (b) Write the corresponding Pfaffian constraints $$A(q)\dot{q} = \theta$$ for this system. How many Pfaffian constraints are there?
398 | > - (c) Are the constraints holonomic or nonholonomic? Or how many are holo- nomic and how many nonholonomic?
399 |
400 |
401 |
402 |
403 | - (a) The derivation of vector q in terms of $$\phi_{1}$$ and $$\phi_{2}$$ is shown below in the sketch:
404 |
405 |
406 |
407 |
408 |
409 | For a more detailed derivation, check this [note](http://planning.cs.uiuc.edu/node659.html).
410 |
411 | Based on that, we get
412 |
413 | $$
414 | \begin{align}
415 | \begin{split}
416 | \dot{q} &=
417 | \begin{bmatrix}
418 | \dot{x}\\
419 | \dot{y}\\
420 | \dot{\theta}\\
421 | \dot{\phi_{1}}\\
422 | \dot{\phi_{2}}
423 | \end{bmatrix}
424 | =
425 | \begin{bmatrix}
426 | \frac{r}{2}(\omega_{1}+\omega_{2})\cos{\theta}\\
427 | \frac{r}{2}(\omega_{1}+\omega_{2})\sin{\theta}\\
428 | \frac{r}{2d}(\omega_{2}-\omega_{1})\\
429 | \omega_{1}\\
430 | \omega_{2}
431 | \end{bmatrix}\\
432 | \end{split}
433 | \end{align}
434 | $$
435 |
436 | Rearrange the above equation as
437 |
438 | $$
439 | \dot{q} =
440 | \begin{bmatrix}
441 | \frac{r}{2}\cos{\theta} & \frac{r}{2}\cos{\theta}\\
442 | \frac{r}{2}\sin{\theta} & \frac{r}{2}\sin{\theta}\\
443 | -\frac{r}{2d} & \frac{r}{2d}\\
444 | 1 & 0\\
445 | 0 & 1\\
446 | \end{bmatrix}
447 | \cdot
448 | \begin{bmatrix}
449 | \omega_{1}\\
450 | \omega_{2}\\
451 | \end{bmatrix}
452 | $$
453 |
454 | - (b) The Pfaffian form can be expressed as
455 |
456 | $$
457 | A(q)\dot{q} =
458 | \begin{bmatrix}
459 | 1 & 0 & 0 & -\frac{r}{2}\cos{\theta} & -\frac{r}{2}\cos{\theta} \\
460 | 0 & 1 & 0 & -\frac{r}{2}\sin{\theta} & -\frac{r}{2}\sin{\theta} \\
461 | 0 & 0 & 1 & \frac{r}{2d} & -\frac{r}{2d}\\
462 | \end{bmatrix}
463 | \cdot
464 | \begin{bmatrix}
465 | \dot{x}\\
466 | \dot{y}\\
467 | \dot{\theta}\\
468 | \dot{\phi_{1}}\\
469 | \dot{\phi_{2}}
470 | \end{bmatrix}
471 | = \vec{0}
472 | $$
473 |
474 | There are 3 Pfaffian constraits.
475 |
476 | - (c) Not sure, it seems to me that all 3 Pfaffian constraints are integratable, since they are derived by differentiation. If it is really so, then all constraints are holonomic.
477 |
478 |
479 | _This is the end of Kapitel II Exercise Attempts._
480 |
481 |
482 | $$\begin{align*}
483 | \begin{split}
484 | \text{Valete discipulae et discipuli}
485 | \end{split}
486 | \end{align*}
487 | $$
488 |
489 | ***
490 |
491 | ## References
492 |
493 | [1] Modern Robotics Textbook.
494 |
495 |
15 |
16 | ## Useful Notes and Equations
17 | For a comprehensive and thorough summary of the theory, check MuChenSun's wonderful note [here](https://muchensun.github.io/ModernRoboticsCourseNotes/ch3.html).
18 |
19 | My own notes:
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 | ### Useful Equations:
34 |
35 | ***
36 |
37 | ## Textbook Exercises Attempts
38 | > _**Exercise 4.2**_ The RRRP SCARA robot of Figure 4.12 is shown in its zero position. Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {0}, and the screw axes $$\mathscr{B}_{i}$$ in {b}. For $$l_{0} = l_{1} = l_{2} = 1$$ and the joint variable values $$\theta = (0, \frac{\pi}{2}, -\frac{\pi}{2},1)$$, use both the **FKinSpace** and the **FKinBody** functions to find the end-effector configuration $$T\in \text{SE}(3)$$. Confirm that they agree with each other.
39 |
40 |
89 |
90 | > _**Exercise 4.8**_ The spatial RRRRPR open chain of Figure 4.14 is shown in its zero position, with fixed and end-effector frames chosen as indicated. Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {0}, and the screw axes $$\mathscr{B}_{i}$$ in {b}.
91 |
92 |
93 |
94 |
95 |
96 | > _**Exercise 4.9**_ The spatial RRPPRR open chain of Figure 4.15 is shown in its zero position. Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {0}, and the screw axes $$\mathscr{B}_{i}$$ in {b}.
97 |
98 |
99 |
100 |
101 |
102 | > _**Exercise 4.10**_ The URRPR spatial open chain of Figure 4.16 is shown in its zero position. Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {0}, and the screw axes $$\mathscr{B}_{i}$$ in {b}.
103 |
104 |
105 |
106 |
107 |
108 | > _**Exercise 4.11**_ The spatial RPRRR open chain of Figure 4.17 is shown in its zero position. Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {0}, and the screw axes $$\mathscr{B}_{i}$$ in {b}.
109 |
110 |
111 |
112 |
113 |
114 | > _**Exercise 4.12**_ The RRPRRR spatial open chain of Figure 4.18 is shown in its zero position (all joints lie on the same plane). Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {0}, and the screw axes $$\mathscr{B}_{i}$$ in {b}. Setting $$\theta_{5}=\pi$$ and all other joint variables to zero, find $$T_{06}$$ and $$T_{60}$$.
115 |
116 |
178 |
179 | > _**Exercise 4.14**_ The RPH robot of Figure 4.20 is shown in its zero position. Determine the end-effector zero position configuration M, the screw axes $$\mathscr{S}_{i}$$ in {s}, and the screw axes $$\mathscr{B}_{i}$$ in {b}. Use both the _FKinSpace_ and the _FKinBody_ functions to find the end-effector configuration $$T\in \text{SE}(3)$$ when $$\theta = (\pi/2, 3, \pi)$$. Confirm that the results agree.
180 |
181 |
182 |
183 |
184 |
185 | ```
186 | %% Exercise 4.14
187 | Slist = [[0; 0; 1; 4; 0; 0], ...
188 | [0; 0; 0; 0; 1; 0], ...
189 | [0; 0;-1;-6; 0; -0.1]];
190 | M = [-1 0 0 0;
191 | 0 1 0 6;
192 | 0 0 -1 2;
193 | 0 0 0 1];
194 | thetalist = [pi/2; 3; pi];
195 | Blist = [[0; 0;-1; 2; 0; 0], ...
196 | [0; 0; 0; 0; 1; 0], ...
197 | [0; 0; 1;-6; 0; 0.1]];
198 | T_03_s = FKinSpace(M, Slist, thetalist);
199 | T_03_b = FKinBody(M, Blist, thetalist);
200 | =========================================
201 | >> T_03_s
202 |
203 | T_03_s =
204 |
205 | -0.0000 1.0000 0 -5.0000
206 | 1.0000 0.0000 0 4.0000
207 | 0 0 -1.0000 1.6858
208 | 0 0 0 1.0000
209 |
210 | >> T_03_b
211 |
212 | T_03_b =
213 |
214 | -0.0000 1.0000 0 7.0000
215 | 1.0000 0.0000 0 4.0000
216 | 0 0 -1.0000 1.6858
217 | 0 0 0 1.0000
218 | ```
219 | **Question!** Still don't know why $$T_{03,s}$$ and $$T_{03,b}$$ aren't equal.
220 |
221 | > _**Exercise 4.17**_ Figure 4.22 shows a snake robot with end-effectors at each end.
222 | Reference frames {$$b_{1}$$} and {$$b_{2}$$} are attached to the two end-effectors, as shown.
223 | > - (a) Suppose that end-effector 1 is grasping a tree (which can be thought of as “ground”) and end-effector 2 is free to move. Assume that the robot is in its zero configuration. Then $$T_{b_{1}b_{2}} \in \text{SE}(3)$$ can be expressed in the following product of exponentials form:
224 | $$T_{b_{1}b_{2}} = e^{\mathscr{S}_{1}\theta_{1}} e^{\mathscr{S}_{2}\theta_{2}} \dots e^{\mathscr{S}_{5}\theta_{5}} M$$.
225 | Find $$S_{3}$$, $$S_{5}$$, and $$M$$.
226 | > - (b) **Don't know how to do**.
227 |
228 |
21 |
22 |
23 | ### Useful Equations:
24 |
25 | ***
26 |
27 | ## Textbook Exercises Attempts
28 | > _**Exercise 9.1**_ Consider an elliptical path in the $$(x,y)$$-plane. The path starts at $$(0,0)$$ and proceeds clockwise to $$(2,1)$$, $$(4,0)$$, $$(2,-1)$$, and back to $$(0,0)$$ (Figure 9.15). Write the path as a function of $$s \in [0, 1]$$.
29 |
30 |
31 |
32 |
33 | List the table below:
34 |
35 | | $$s$$ | $$x$$ | $$y$$ |
36 | | ------- | ------ | ------------- |
37 | | 0 | 0 | 0 |
38 | | $$\frac{1}{4}$$ | $$2$$ | $$1$$ |
39 | | $$\frac{2}{4}$$ | $$4$$ | $$0$$ |
40 | | $$\frac{3}{4}$$ | $$2$$ | $$-1$$ |
41 | | $$\frac{4}{4}$$ | $$0$$ | $$0$$ |
42 |
43 | By observation, we get,
44 |
45 | $$
46 | \begin{align}
47 | \begin{split}
48 | x &= 2\left(1-\cos (2\pi s) \right) \\
49 | y &= \sin (2\pi s) \\
50 | \end{split}
51 | \end{align}
52 | $$
53 |
54 | > _**Exercise 9.2**_ A cylindrical path in $$X = (x, y, z)$$ is given by $$x = \cos(2\pi s)$$, $$y = \sin(2\pi s)$$, $$z = 2s$$, $$s \in [0,1]$$, and its time scaling is $$s(t) = \frac{1}{4} t + \frac{1}{8} t^{2}$$, $$t \in [0, 2]$$. Write done $$\dot{X}$$ and $$\ddot{X}$$.
55 |
56 | $$
57 | \begin{align}
58 | \begin{split}
59 | \dot{X} &= \frac{dX}{ds} \frac{ds}{dt} \\
60 | &= \begin{bmatrix}
61 | -2 \pi \sin (2\pi s) \\
62 | 2 \pi \cos (2\pi s) \\
63 | 2 \\
64 | \end{bmatrix} \cdot \left( \frac{1}{4} + \frac{1}{4} t \right)
65 | \end{split}
66 | \end{align}
67 | $$
68 |
69 | Similarly,
70 |
71 | $$
72 | \begin{align}
73 | \begin{split}
74 | \ddot{X} &= \frac{d^{2}X}{ds^{s}} \frac{ds}{dt} + \frac{dX}{ds} \frac{d^{2}s}{dt^{2}} \\
75 | &= \ldots \\
76 | \end{split}
77 | \end{align}
78 | $$
79 |
80 | > _**Exercise 9.3**_ Blah Blah
81 |
82 | Don't know how to do.
83 |
84 | > _**Exercise 9.4**_ Consider a straight-line path $$\theta (s) = \theta_{\text{start}} + s (\theta_{\text{end}} - \theta_{\text{start}})$$, $$s \in [0,1]$$ from $$\theta_{\text{start}} = (0, 0)$$ to $$\theta_{\text{end}} = (\pi, \pi/3)$$. The motion starts and ends at rest. The feasible joint velocities are $$\lvert \dot{\theta_{1}} \rvert$$, $$\lvert \dot{\theta_{2}}\rvert \leq 2 \, \text{rad/s} $$ and the feasible joint accelerations are $$\lvert \ddot{\theta_{1}} \rvert$$, $$\ddot{\theta_{2}} \leq 0.5 \, \text{rad/s}^{2}$$. Find the fastest motion time $$T$$ using a cubic time scaling that satisfies the joint velocity and acceleration limits.
85 |
86 | Because the two joints have the same velocity and acceleration limits, we just need to consider the largest one, which in this case is $$\theta_{\text{end,}\,1} = \pi$$. Then, $$\theta(s) = 0 + s (\pi - 0) = \pi s$$. The terminal constraints for this cubic time scaling function are:
87 |
88 | $$
89 | \begin{cases}
90 | s(0) = 0, & \text{ipso facto} \\
91 | s(T) = 1, & \text{ipso facto} \\
92 | \dot{s}(0) = 0, & \text{starts at rest} \\
93 | \dot{s}(T) = 0, & \text{ends at rest} \\
94 | \end{cases}
95 | $$
96 |
97 | Substitute the above constraints into the cubic time scaling $$s(t) = a_{0} + a_{1}t + a_{2} t^{2} + a_{3} t^{3}$$, solve
98 |
99 | $$
100 | \left( \begin{array}{cccc|c}
101 | 1 & T & T^2 & T^3 & 1 \\
102 | 1 & 0 & 0 & 0 & 0 \\
103 | 0 & 1 & 0 & 0 & 0 \\
104 | 0 & 1 & 2T & 3T^2 & 0 \\
105 | \end{array} \right)
106 | $$
107 |
108 | Thus, the coefficents are obtained as $$\left\{ a_{0}=0,\, a_{1}=0,\, a_{2} = \frac{3}{T^{2}},\, a_{3}=- \frac{2}{T^{3}} \right\}$$. Substitute the coefficients into the path function gives
109 |
110 | $$
111 | \begin{align}
112 | \begin{split}
113 | \theta &= \pi s = \frac{3\pi}{T^{2}} t^{2} - \frac{2\pi}{T^{3}} t^{3} \\
114 | \dot{\theta} &= \pi \dot{s} = \frac{6\pi}{T^{2}} t - \frac{6\pi}{T^{3}} t^{2} \\
115 | \ddot{\theta} &= \pi \ddot{s} = \frac{6\pi}{T^{2}} - \frac{12\pi}{T^{3}} t \\
116 | \end{split}
117 | \end{align}
118 | $$
119 |
120 | Now we should find the minimal motion time $$T$$ in accordance with the velocity and acceleration limits. Because we don't know which one is controlling, so we have to calculate both.
121 |
122 | The elocity limit is $$ \lvert \dot{\theta_{1}} \rvert \leq 2 $$. Since we know the maximal speed occurs at $$t=\frac{T}{2}$$, we have,
123 |
124 | $$
125 | \begin{align}
126 | \begin{split}
127 | \dot{\theta}_{max} &= \dot{\theta}\left(\frac{T}{2}\right) \\
128 | &= \frac{6\pi}{T^{2}} \frac{T}{2} - \frac{6\pi}{T^{3}} \left(\frac{T}{2}\right)^{2} \\
129 | &= \frac{3\pi}{2T} \leq 2 \\
130 | T_{\text{min, vel}} &= \frac{3\pi}{4} \approx 2.3562
131 | \end{split}
132 | \end{align}
133 | $$
134 |
135 | The cceleration limit is $$ \lvert \ddot{\theta_{1}} \rvert \leq \frac{1}{2}$$. Since we know the maximal acceleration occurs at either terminals, i.e., $$t=0$$ or $$t=T$$, we have,
136 |
137 | $$
138 | \begin{align}
139 | \begin{split}
140 | \ddot{\theta}_{max} &= \ddot{\theta}\left(0\right) \\
141 | &= \frac{6\pi}{T^{2}} \leq \frac{1}{2} \\
142 | T_{\text{min, acc}} &= 2 \sqrt{3\pi} \approx 6.1400
143 | \end{split}
144 | \end{align}
145 | $$
146 |
147 | Thus, the minimal motion time is, $$T=\max( T_{\text{min, vel}},\, T_{\text{min, acc}})=2 \sqrt{3\pi}$$.
148 |
149 | > _**Exercise 9.5**_ Find the fifth-order polynomial time scaling that satisfies $$s(T) =1$$ and $$s(0) = \dot{s}(0) = \ddot{s}(0) = \dot{s}(T) = \ddot{s}(T) = 0$$.
150 |
151 | Let the fifth-order time scaling be $$s(t) = a_{0} + a_{1} t + a_{2} t^{2} + a_{3} t^{3} + a_{4} t^{4} + a_{5} t^{5}$$. Solve
152 |
153 | $$\left( \begin{array}{cccccc|c}
154 | 1 & 0 & 0 & 0 & 0 & 0 & 0 \\
155 | 0 & 1 & 0 & 0 & 0 & 0 & 0 \\
156 | 0 & 0 & 1 & 0 & 0 & 0 & 0 \\
157 | 1 & T & T^2 & T^3 & T^4 & T^5 & 1 \\
158 | 0 & 1 & 2T & 3T^2 & 4T^3 & 5T^4 & 0 \\
159 | 0 & 0 & 1 & 6T & 12T^2 & 20T^3 & 0 \\
160 | \end{array} \right)
161 | $$
162 |
163 | The solution is $$\left\{ a_{0}=0,\, a_{1}=0,\, a_{2}=0,\, a_{3}=\frac{10}{T^3},\, a_{4}=-\frac{15}{T^4},\, a_{5}=\frac{6}{T^5} \right\}$$.
164 |
165 | > _**Exercise 9.7**_ If you want to use a polynomial time scaling for point-to-point motion with zero initial and final velocities, accelerations, and jerks, what would be the minimum order of the polynomial?
166 |
167 | 7.
168 |
169 | > _**Exercise 9.8**_ Prove that the trapezoidal time scaling, using the maximum allowable acceleration a and velocity v, minimizes the time of motion T.
170 |
171 |
172 |
173 |
174 |
175 | > _**Exercise 9.9**_ Plot by hand the acceleration profile $$\ddot{s}(t)$$ for a trapezoidal time scaling.
176 |
177 |
178 |
179 |
180 |
181 | > _**Exercise 9.10**_ If $$v$$ and $$a$$ are specified for a trapezoidal time scaling of a robot, prove that $$v^2 / a \leq 1$$ is a necessary condition for the robot to reach the maximum velocity $$v$$ during the path.
182 |
183 |
184 |
185 |
186 |
187 | > _**Exercise 9.11**_ If $$v$$ and $$T$$ are specified for a trapezoidal time scaling, prove that $$vT > 1$$ is a necessary condition for the motion to be able to complete in time T . Prove that $$vT \leq 2$$ is a necessary condition for a three-stage trapezoidal motion.
188 |
189 |
190 |
191 |
192 |
193 | > _**Exercise 9.12**_ If $$a$$ and $$T$$ are specified for a trapezoidal time scaling, prove that $$aT^{2} \geq 4$$ is a necessary condition to ensure that the motion completes in time.
194 |
195 |
196 |
197 |
198 |
199 | > _**Exercise 9.14**_ Plot by hand the acceleration profile $$\ddot{s}(t)$$ for an S-curve time scaling.
200 |
201 |
202 |
203 |
204 |
205 | > _**Exercise 9.17**_ If the S-curve achieves all seven stages and uses a jerk $$J$$, an acceleration $$a$$, and a velocity $$v$$, what is the constant-velocity coasting time $$t_v$$ in terms of $$v$$, $$a$$, $$J$$, and the total motion time $$T$$?
206 |
207 |
208 |
209 |
210 |
211 | > _**Exercise 9.20**_ By hand or by computer, plot a trapezoidal motion profile in the $$(s, \dot{s})$$-plane.
212 |
213 |
214 |
215 |
216 |
217 | > _**Exercise 9.21**_ Figure 9.16 shows three candidate motion curves in the $$(s, \dot{s})$$- plane (A, B, and C) and three candidate motion cones at $$\dot{s} = 0$$ (a, b, and c). Two of the three curves and two of the three motion cones cannot be correct for any robot dynamics. Indicate which are incorrect and explain your reasoning. Explain why the remaining curve and motion cone are possibilities.
218 |
219 |
15 |
16 | ## Useful Notes and Equations
17 | For a comprehensive and thorough summary of the theory, check MuChenSun's wonderful note [here](https://muchensun.github.io/ModernRoboticsCourseNotes/ch5.html).
18 |
19 | My own notes:
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 |
29 |
30 |
31 |
32 |
33 | ### Useful Equations:
34 |
35 | ***
36 |
37 | ## Textbook Exercises Attempts
38 | > _**Exercise 5.1**_ A wheel of unit radius is rolling to the right at a rate of 1 rad/s (see Figure 5.14; the dashed circle shows the wheel at $$t = 0$$).
39 | > - (a) Find the spatial twist $$\mathscr{V}_{s}(t)$$ as a function of $$t$$.
40 | > - (b) Find the linear velocity of the {b}-frame origin expressed in {s}-frame coordinates.
41 |
42 |
43 |
44 |
45 |
46 | > _**Exercise 5.2**_ The 3R planar open chain of Figure 5.15(a) is shown in its zero position.
47 | > - (a) Suppose that the last link must apply a wrench corresponding to a force of 5 N in the $$\hat{x}^{s}$$-direction of the {s} frame, with zero component in the $$\hat{y}_{s}$$-direction and zero moment about the $$\hat{z}_{s}$$-axis. What torques should be applied at each joint?
48 | > - (b) Suppose that the last link must apply a force of 5 N in the yˆs-direction, with zero components in the other wrench directions. What torques should be applied at each joint?
49 |
50 |
51 |
52 |
53 | N.B. in figure part (a), the robot is not in its zero position!!!!
54 |
55 | - (a) Since $$\tau = J_{s}^{T}(\theta) \cdot \mathscr{F}_{s}$$, we need to first find _SList_ and _thetalist_ and then use the function _JacobianSpace_ to find $$J_{s}^{T}(\theta)$$. $$\mathscr{F}_{s}$$ can be found by observation. The following MATLAB code is self-explanatory.
56 |
57 | ```
58 | %% Exercise 5.2
59 |
60 | % Part (a)
61 | F_s = [0; 0; 0; 5; 0; 0];
62 | Slist = [[0; 0; 1; 0; 0; 0], ...
63 | [0; 0; 1; 0;-1; 0], ...
64 | [0; 0; 1; 0;-2; 0]];
65 | thetalist =[0; pi/4; 0];
66 | J_s_T = transpose(JacobianSpace(Slist, thetalist));
67 | tau = J_s_T * F_s;
68 |
69 | % Part (b)
70 | F_s_b = [0; 0; 0; 0; 5; 0];
71 | tau_b = J_s_T * F_s_b;
72 | ===================================================
73 | >> tau_a
74 |
75 | tau_a =
76 |
77 | 0
78 | 0
79 | 3.5355
80 |
81 | >> tau_b
82 |
83 | tau_b =
84 |
85 | 0
86 | -5.0000
87 | -8.5355
88 | ```
89 |
90 | > _**Exercise 5.3**_ Answer the following questions for the 4R planar open chain of Figure 5.15(b).
91 | > - (a) For the forward kinematics of the form
92 | $$T(\theta) = e^{[\mathscr{S}_{1}]\theta_{1}} e^{[\mathscr{S}_{2}]\theta_{2}} e^{[\mathscr{S}_{3}]\theta_{3}} e^{[\mathscr{S}_{4}]\theta_{4}} M$$
93 | write down $$M \in \text{SE}(2)$$ and each $$\mathscr{S}_{i} = (\omega_{zi}, v_{xi}, v_{yi}) \in \mathbb{R}^{3}$$.
94 | > - (b) Write down the body Jacobian.
95 | > - (c) Suppose that the chain is in static equilibrium at the configuration $$\theta_{1}=\theta_{2}=0, \theta_{3}=\pi/2, \theta_{4}=-\pi/2$$ and that a force $$f = (10,10,0)$$ and a moment $$m = (0,0,10)$$ are applied to the tip (both f and m are expressed with respect to the fixed frame). What are the torques experienced at each joint?
96 | > - (d) Under the same conditions as (c), suppose that a force $$f = (-10,10,0)$$ and a moment $$m = (0,0,-10)$$, also expressed in the fixed frame, are applied to the tip. What are the torques experienced at each joint?
97 | > - (e) Find all kinematic singularities for this chain.
98 |
99 |
100 |
101 |
102 | - (a) $$M = \begin{bmatrix}
103 | 1 & 0 & L_{1} + L_{2} + L_{3} + L_{4} \\
104 | 0 & 1 & 0 \\
105 | 0 & 0 & 1 \\
106 | \end{bmatrix} \in \text{SE}(2)$$, $$\mathscr{S} = \begin{bmatrix}
107 | 1 & 1 & 1 & 1 \\
108 | 0 & 0 & 0 & 0 \\
109 | 0 & -L_{1} & -(L_{1}+L_{2}) & -(L_{1}+L_{2}+L_{3}) \\
110 | \end{bmatrix} \in \mathbb{R}^{3}$$.
111 |
112 | - (b) I am too lazy to do that by hand. One of the online quizzes of this Coursera MOOC actually gives you the answer...
113 | - Because I am using MATLAB to do this course, and MATLAB is quite impotent on symbolic manipulation, part (c) and (d) are skipped.
114 | - (e) When $$\theta_{2}=\theta_{3}=\theta_{4}=n\pi, \,\text{where}\,n\in \mathbb{Z}^{+}$$.
115 |
116 | > _**Exercise 5.7**_ The RRP robot in Figure 5.19 is shown in its zero position.
117 | > - (a) Write down the screw axes in the space frame. Evaluate the forward kinematics when $$\theta = (90^{\circ}, 90^{\circ}, 1)$$. Hand-draw or use a computer to show the arm and the end-e↵ector frame in this configuration. Obtain the space Jacobian $$J_{s}$$ for this configuration.
118 | > - (b) Write down the screw axes in the end-effector body frame. Evaluate the forward kinematics when $$\theta = (90^{\circ}, 90^{\circ}, 1)$$ and confirm that you get the same result as in part (a). Obtain the body Jacobian $$J_{b}$$ for this configuration.
119 |
120 |
121 |
122 |
123 | - See the code below:
124 |
125 | ```
126 | %% Exercise 5.7
127 |
128 | % Part (a)
129 | SList = [[0; 0; 1; 0; 0; 0], ...
130 | [1; 0; 0; 0; 2; 0], ...
131 | [0; 0; 0; 0; 1; 0]];
132 | thetalist = [pi/2; pi/2; 1];
133 | M = [-1 0 0 0;
134 | 0 0 1 3;
135 | 0 1 0 2;
136 | 0 0 0 1];
137 | % T = FKinSpace(M, Slist, thetalist)
138 | T_sb = FKinSpace(M, SList, thetalist);
139 | % Js = JacobianSpace(Slist, thetalist)
140 | J_s = JacobianSpace(SList, thetalist);
141 |
142 | =======================================
143 |
144 | >> T_sb
145 |
146 | T_sb =
147 |
148 | -0.0000 1.0000 -0.0000 -0.0000
149 | -1.0000 -0.0000 0.0000 0.0000
150 | 0 0.0000 1.0000 6.0000
151 | 0 0 0 1.0000
152 |
153 | >> J_s
154 |
155 | J_s =
156 |
157 | 0 0.0000 0
158 | 0 1.0000 0
159 | 1.0000 0 0
160 | 0 -2.0000 -0.0000
161 | 0 0.0000 0.0000
162 | 0 0 1.0000
163 |
164 | =======================================
165 |
166 | % Part (b)
167 | BList = [[0; 1; 0; 3; 0; 0], ...
168 | [-1; 0; 0; 0; 3; 0], ...
169 | [0; 0; 0; 0; 0; 1]];
170 | T_bs = FKinBody(M, BList, thetalist);
171 | J_b = JacobianBody(BList, thetalist);
172 |
173 | =======================================
174 |
175 | >> T_bs
176 |
177 | T_bs =
178 |
179 | -0.0000 1.0000 -0.0000 -0.0000
180 | -1.0000 -0.0000 0.0000 0.0000
181 | 0 0.0000 1.0000 6.0000
182 | 0 0 0 1.0000
183 |
184 | >> J_b
185 |
186 | J_b =
187 |
188 | 0 -1.0000 0
189 | 0.0000 0 0
190 | 1.0000 0 0
191 | 0.0000 0 0
192 | 0 4.0000 0
193 | 0 0 1.0000
194 | ```
195 |
196 | > _**Exercise 5.26**_ The kinematics of the 7R WAM robot are given in Section 4.1.3.
197 | > - (a) Give the numerical body Jacobian $$J_{b}$$ when all joint angles are $$\pi/2$$. Separate the Jacobian matrix into an angular-velocity portion $$J_{\omega}$$ (the joint rates act on the angular velocity) and a linear-velocity portion $$J_{v}$$ (the joint rates act on the linear velocity).
198 | > - (b) For this configuration, calculate the directions and lengths of the principal semi-axes of the three-dimensional angular-velocity manipulability ellipsoid (based on $$J_{\omega}$$) and the directions and lengths of the principal semi-axes of the three-dimensional linear-velocity manipulability ellipsoid (based on $$J_{v}$$).
199 |
15 |
16 | ## Useful Notes and Equations
17 | For a comprehensive and thorough summary of the theory, check MuChenSun's wonderful note [here](https://muchensun.github.io/ModernRoboticsCourseNotes/ch6.html).
18 |
19 | My own notes:
20 |
21 |
22 |
23 |
24 |
25 | ### Useful Equations:
26 |
27 | ***
28 |
29 | ## Textbook Exercises Attempts
30 | > _**Exercise 6.8**_ Use Newton–Raphson iterative numerical root finding to perform two steps in finding the root of
31 | >
32 | $$ g(x,y) = \begin{bmatrix}
33 | x^2 - 4\\
34 | y^2 - 9\\
35 | \end{bmatrix}
36 | $$
37 | >
38 | > when your initial guess is $$(x^{0}, y^{0})=(1,1)$$. Write the general form of the gradient (for any guess $$(x,y)$$) and compute the results of the first two iterations. You can do this by hand or write a program. Also, give all the correct roots, not just the one that would be found from your initial guess. How many are there?
39 |
40 | From $$g(x,y)$$ we know that $$x_{d} = (4,9)^{T}$$, $$f(\theta)=(x^{2}, y^{2})^{T}$$.
41 |
42 | $$ \frac{\delta f}{\delta \theta} = J(\theta) = \begin{bmatrix}
43 | 2x & 0 \\
44 | 0 & 2y \\
45 | \end{bmatrix}
46 | $$
47 |
48 | $$ J^{-1}(\theta) = \begin{bmatrix}
49 | \frac{1}{2x} & 0 \\
50 | 0 & \frac{1}{2y} \\
51 | \end{bmatrix}
52 | $$
53 |
54 | $$
55 | \begin{align}
56 | \begin{split}
57 | \theta^{1} &= \begin{bmatrix}
58 | \frac{1}{2} & 0 \\
59 | 0 & \frac{1}{2} \\
60 | \end{bmatrix} \begin{bmatrix}
61 | 4-1^{2} \\
62 | 9-1^{2} \\
63 | \end{bmatrix} + \begin{bmatrix}
64 | 1 \\
65 | 1 \\
66 | \end{bmatrix} \\
67 | &= \begin{bmatrix}
68 | 2.5 \\
69 | 5
70 | \end{bmatrix} \\
71 | \theta_{2} &= \begin{bmatrix}
72 | \frac{1}{5} & 0 \\
73 | 0 & \frac{1}{10} \\
74 | \end{bmatrix} \begin{bmatrix}
75 | 4 - 2.5^{2} \\
76 | 9 - 5^{2} \\
77 | \end{bmatrix} + \begin{bmatrix}
78 | 2.5 \\
79 | 5 \\
80 | \end{bmatrix} \\
81 | &= \begin{bmatrix}
82 | 2.05 \\
83 | 3.4 \\
84 | \end{bmatrix}
85 | \end{split}
86 | \end{align}
87 | $$
88 |
89 | There should be a total of 4 solutions, since x could be -2 or 2, and y could be -3 or 3.
90 |
91 |
92 | ***
93 |
94 |
95 |
15 |
16 | ## Useful Notes and Equations
17 | For a comprehensive and thorough summary of the theory, check MuChenSun's wonderful note [here](https://muchensun.github.io/ModernRoboticsCourseNotes/ch7.html).
18 |
19 | Since I am really not interested in closed chain robots, the content in this chapter is skipped.
20 |
21 |
15 |
16 | ## Useful Notes and Equations
17 | For a comprehensive and thorough summary of the theory, check MuChenSun's wonderful note [here](https://muchensun.github.io/ModernRoboticsCourseNotes/ch8.html).
18 |
19 | My own useless notes:
20 |
21 |
22 |
23 |
24 |
25 |
26 |
27 |
28 | ### Useful Equations:
29 |
30 | ***
31 |
32 | ## Textbook Exercises Attempts
33 | > _**Exercise 8.2**_ Consider a cast iron dumbbell consisting of a cylinder connecting two solid spheres at either end of the cylinder. The density of the dumbbell is $$7500$$ $$\text{kg}/m^{3}$$. The cylinder has a diameter of $$4$$ cm and a length of $$20$$ cm. Each sphere has a diameter of $$20$$ cm.
34 | > - (a) Find the approximate rotational inertia matrix $$\mathcal{I}_{b}$$ in a frame {b} at the center of mass with axes aligned with the principal axes of inertia of the dumbbell.
35 | > - (b) Write down the spatial inertia matrix $$\mathcal{G}_{b}$$.
36 |
37 | - (a)
38 | See the diagram below:
39 |
40 |
41 |
42 | For the cylinder, we have
43 |
44 | $$
45 | \begin{align}
46 | \begin{split}
47 | m &= \pi r^{2} h \rho \\
48 | &= \pi \cdot 0.02^2 \cdot 0.2 \cdot 7500 \\
49 | &= 1.88500 \, \text{kgm}^{2} \\
50 | I_{xx} = I_{yy} &= \frac{m\left( 3r^{2} + h^{2} \right)}{12} \\
51 | &= 6.47168e-3 \, \text{kgm}^{2} \\
52 | I_{zz} = \frac{1}{2} m r^{2} \\
53 | &= 3.77e-4 \text{kgm}^{2} \\
54 | \end{split}
55 | \end{align}
56 | $$
57 |
58 | Therefore,
59 |
60 | $$ \mathcal{I}_{\text{cylinder}} =
61 | \begin{bmatrix}
62 | 6.47168e-3 & 0 & 0 \\
63 | 0 & 6.47168e-3 & 0 \\
64 | 0 & 0 & 3.77e-4 \\
65 | \end{bmatrix}
66 | $$
67 |
68 | For one sphere, we have
69 |
70 | $$
71 | \begin{align}
72 | \begin{split}
73 | a = b = c &= r = 0.1 \text{m} \\
74 | m = \rho V &= \rho \frac{4\pi a b c}{3} \\
75 | &= 31.41590 \, \text{kgm}^{2} \\
76 | \end{split}
77 | \end{align}
78 | $$
79 |
80 | $$
81 | \begin{align}
82 | \begin{split}
83 | I_{xx} = I_{yy} = I_{zz} &= \frac{2m r^{2}}{5} \\
84 | &= 0.12566 \, \text{kgm}^{2} \\
85 | \end{split}
86 | \end{align}
87 | $$
88 |
89 | Because the inertia of the sphere is calculated in its own center frame {c}, we need to shift $$\hat{x}_{c}$$ and $$\hat{y}_{c}$$ to the center frame {b} (we don't need to touch $$\hat{z}_{c}$$ because it is aligned with $$\hat{z}_{b}$$). Using the parallel axis theorem $$I_{aa} = I_{bb} + m d ^{2}$$, we have
90 |
91 | $$
92 | \begin{align}
93 | \begin{split}
94 | I_{\text{sphere, {b}}} &= I_{\text{sphere, {c}}} + m d^{2}\\
95 | &= 0.12566 + 31.4159 \cdot 0.2 \\
96 | &= 1.38230 \, \text{kgm}^{2} \\
97 | \end{split}
98 | \end{align}
99 | $$
100 |
101 | Therefore, the two spheres have the following inertia matrix:
102 |
103 | $$ \mathcal{I}_{\text{sphere}} =
104 | \begin{bmatrix}
105 | 2.76460 & 0 & 0 \\
106 | 0 & 2.76460 & 0 \\
107 | 0 & 0 & 0.25132 \\
108 | \end{bmatrix}
109 | $$
110 |
111 | Finally, adding the two inertia matrices up gives the inertial matrix of the dumbbell:
112 |
113 | $$ \mathcal{I}_{\text{dumbbell}} =
114 | \begin{bmatrix}
115 | 2.77107 & 0 & 0 \\
116 | 0 & 2.77107 & 0 \\
117 | 0 & 0 & 0.252377 \\
118 | \end{bmatrix}
119 | $$
120 |
121 | - (b) The total mass of the dumbbell is $$\sum m = 64.7168\,\text{kg}$$. Therefore, the spatial inertia matrix is,
122 |
123 | $$ \mathcal{G}_{b} =
124 | \begin{bmatrix}
125 | \mathcal{I}_{dumbbell} & 0 \\
126 | 0 & 64.7168 \cdot \begin{bmatrix}
127 | 1 & 0 & 0 \\
128 | 0 & 1 & 0 \\
129 | 0 & 0 & 1 \\
130 | \end{bmatrix}
131 | \end{bmatrix}
132 | $$
133 |
134 | > _**Exercise 8.15**_ Dynamics of the UR5 robot.
135 | > - (b) Simulate the UR5 falling under gravity with acceleration $$g = 9.81 \text{m/s}^{2}$$ in the $$-\hat{z}_{s}$$-direction. The robot starts at its zero configuration and zero joint torques are applied. Simulate the motion for three seconds, with at least 100 integration steps per second. (Ignore the effects of friction and the geared rotors.)
136 |
137 | See MATLAB code below:
138 |
139 | ```Matlab
140 | %% Data Given in Section 4.2
141 | M01 = [1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, 0.089159; 0, 0, 0, 1];
142 | M12 = [0, 0, 1, 0.28; 0, 1, 0, 0.13585; -1, 0, 0, 0; 0, 0, 0, 1];
143 | M23 = [1, 0, 0, 0; 0, 1, 0, -0.1197; 0, 0, 1, 0.395; 0, 0, 0, 1];
144 | M34 = [0, 0, 1, 0; 0, 1, 0, 0; -1, 0, 0, 0.14225; 0, 0, 0, 1];
145 | M45 = [1, 0, 0, 0; 0, 1, 0, 0.093; 0, 0, 1, 0; 0, 0, 0, 1];
146 | M56 = [1, 0, 0, 0; 0, 1, 0, 0; 0, 0, 1, 0.09465; 0, 0, 0, 1];
147 | M67 = [1, 0, 0, 0; 0, 0, 1, 0.0823; 0, -1, 0, 0; 0, 0, 0, 1];
148 | G1 = diag([0.010267495893, 0.010267495893, 0.00666, 3.7, 3.7, 3.7]);
149 | G2 = diag([0.22689067591, 0.22689067591, 0.0151074, 8.393, 8.393, 8.393]);
150 | G3 = diag([0.049443313556, 0.049443313556, 0.004095, 2.275, 2.275, 2.275]);
151 | G4 = diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]);
152 | G5 = diag([0.111172755531, 0.111172755531, 0.21942, 1.219, 1.219, 1.219]);
153 | G6 = diag([0.0171364731454, 0.0171364731454, 0.033822, 0.1879, 0.1879, 0.1879]);
154 | Glist = cat(3, G1, G2, G3, G4, G5, G6);
155 | Mlist = cat(3, M01, M12, M23, M34, M45, M56, M67);
156 | Slist = [0, 0, 0, 0, 0, 0;
157 | 0, 1, 1, 1, 0, 1;
158 | 1, 0, 0, 0, -1, 0;
159 | 0, -0.089159, -0.089159, -0.089159, -0.10915, 0.005491;
160 | 0, 0, 0, 0, 0.81725, 0;
161 | 0, 0, 0.425, 0.81725, 0, 0.81725];
162 |
163 | %% Define gravity and joint positions
164 | g = [0; 0; -9.81]; % gravity term
165 | thetalist = [0; 0; 0; 0; 0; 0]; % home configuration
166 | dthetalist = [0; 0; 0; 0; 0; 0]; % Originally stationary
167 | dt = 0.01; % 100 steps per second
168 | simuTime = 3; % Length of simulation in seconds
169 | Ftipmat = zeros(simuT1/dt1, 6); % simuT1/dt1 gives the total steps,
170 | % since the XXmat function is defined as a Nxn matrix, in which the N
171 | % refers to steps and n refers to the joint variables
172 | taumat = zeros(simuT1/dt1, 6); % same as above
173 |
174 | intRes = 10; % Magic number for integration resolution, chosen arbitrarily.
175 |
176 | %% Calculate thetamatrix
177 | [thetamat, dthetamat] ...
178 | = ForwardDynamicsTrajectory(thetalist, dthetalist, taumat, g, ...
179 | Ftipmat, Mlist, Glist, Slist, dt, ...
180 | intRes);
181 |
182 | %% Output trajectory as .csv file
183 | writematrix(thetamat,'forSimulation.csv','Delimiter','comma')
184 | % Use the .csv file in CoppeliaSim to visualize the motion
185 | ```
186 |
187 |
188 | ***
189 |
190 |
191 |
21 |
22 |
23 | ### Useful Equations:
24 |
25 | ***
26 |
27 | ## Textbook Exercises Attempts
28 |
29 | > _**Exercise**_ 11.2 The $$2\%$$ settling time of an underdamped second-order system is approximately $$t = \frac{4}{\zeta \omega_{n}}$$, for $$e^{-\zeta \omega_{n} t}=0.02$$. What is the $$5\%$$ settling time?
30 |
31 | By definition, the percentage settling time is defined as $$N\%t = -\frac{\ln\left(N/100 \right)}{\zeta \omega_{n}}$$. Therefore, $$5\%t = -\frac{\ln\left(5/100 \right)}{t} \approx \frac{3}{\zeta \omega_{n}} $$.
32 |
33 | > _**Exercise 11.3**_ Solve for any constants and give the specific equation for an underdamped second-order system with $$\omega_n = 4$$, $$\zeta = 0.2$$, $$\theta_{e}(0) = 1$$, and $$\dot{\theta_{e}}(0) = 0$$. Calculate the damped natural frequency, approximate overshoot, and $$2\%$$ settling time. Plot the solution on a computer and measure the exact overshoot and settling time.
34 |
35 | $$
36 | \begin{align}
37 | \begin{split}
38 | \omega_d &= \omega_n \sqrt{1-\zeta^2}\\
39 | &= 4 \sqrt{1-0.2^2} \\
40 | &= 3.9192 \\
41 | \end{split}
42 | \end{align}
43 | $$
44 |
45 | $$
46 | \begin{align}
47 | \begin{split}
48 | \text{overshoot} &= \text{exp}\left( \frac{-\pi \zeta}{\sqrt{1-\zeta^2}} \right) \\
49 | &= 0.5266 \\
50 | &= 52.66\% \\
51 | \end{split}
52 | \end{align}
53 | $$
54 |
55 | $$
56 | \begin{align}
57 | \begin{split}
58 | 2\%t &= -\frac{\text{ln}(0.02)}{\zeta \omega_n} \\
59 | &= 4.8900\,\text{rad/s} \\
60 | \end{split}
61 | \end{align}
62 | $$
63 |
64 |
65 |
66 |
67 |
68 |
69 | > _**Exercise 11.4**_ Solve for any constants and give the specific equation for an underdamped second-order system with $$\omega_n = 10$$, $$\zeta = 0.1$$, $$\theta_e (0) = 0$$, and $$\dot{\theta_e} (0) = 1$$. Calculate the damped natural frequency. Plot the solution on a computer.
70 |
71 | $$\omega_d = \omega_n \sqrt{1-\zeta^2} = 9.94987\,\text{rad/s}$$
72 |
73 |
74 |
75 |
76 |
77 | > _**Exercise 11.5**_ Consider a pendulum in a gravitational field with $$g = 10 \;\text{m/s}^2$$. The pendulum consists of a $$2$$ kg mass at the end of a $$1$$ m massless rod. The pendulum joint has a viscous-friction coefficient of $$b = 0.1\,\text{Nms/rad}$$.
78 | > - (a) Write the equation of motion of the pendulum in terms of $$\theta$$, where $$\theta=0$$ corresponds to the “hanging down” configuration.
79 | > - (b) Linearize the equation of motion about the stable “hanging down” equilibrium. To do this, replace any trigonometric terms in $$\theta$$ with the linear term in the Taylor expansion. Give the effective mass and spring constants $$m$$ and $$k$$ in the linearized dynamics $$m \ddot{\theta} + b\dot{\theta}+k\theta =0$$. At the stable equilibrium, what is the damping ratio? Is the system underdamped, critically damped, or overdamped? If it is underdamped, what is the damped natural frequency? What is the time constant of convergence to the equi- librium and the $$2\%$$ settling time?
80 | > - (c) Now write the linearized equations of motion for $$\theta=0$$ at the balanced upright configuration. What is the e↵ective spring constant $$k$$?
81 | > - (d) You add a motor at the joint of the pendulum to stabilize the upright position, and you choose a P controller $$\tau = K_p \theta$$. For what values of $$K_p$$ is the upright position stable?
82 |
83 | - (a) $$\ddot{\theta} + 10 \text{sin}(\theta) = 0$$
84 |
85 |
86 |
87 |
88 | - (b) $$1\cdot \ddot{\theta} + 0 \cdot \dot{\theta} + 10\cdot \theta = 0$$
89 | By observation, we get $$m=1$$, $$b = 0$$, $$k=10$$.
90 | The natural frequency is $$\omega_n = \sqrt{\frac{k}{m}} = \sqrt{\frac{10}{1}} = \sqrt{10}$$.
91 | From $$2\zeta \omega_n = 0$$, the damping ratio is obtained as $$\zeta = 0$$. Since $$\zeta < 0$$, the system is **underdamped**.
92 | The damped natural frequency is $$\omega_d = \omega_n \sqrt{1-\zeta^2} = \sqrt{10}$$.
93 | The $$2\%$$ settling time is $$2\%t = -\frac{ln(0.02)}{\zeta \omega_n} \approx \frac{4}{0} = \infty$$.
94 |
95 | - (c) Don't know how to do.
96 | - (d) Don't know how to do.
97 |
98 | ***
99 |
100 |
24 |
25 | ### Useful Equations:
26 |
27 | * From twist to CoR:
28 |
29 | $$\text{CoR}(\mathscr{V})=\left\{ \text{sgn}(\omega_z),\; \begin{bmatrix}
30 | x_c \\
31 | y_c \\
32 | \end{bmatrix}
33 | \right\}$$
34 |
35 | $$
36 | \begin{bmatrix}
37 | x_c \\
38 | y_c \\
39 | \end{bmatrix} = \begin{bmatrix}
40 | -\frac{v_y}{\omega_z} \\
41 | \frac{v_x}{\omega_z} \\
42 | \end{bmatrix}
43 | $$
44 |
45 | ## Remark on Planar Graphical Method
46 |
47 | Because I think the graphical graphical method of wrench is not as intuitive as that of CoR's, so I want to talk about how I understand this method.
48 |
49 | I found the definition of the method in another textbook (Springer Handbook of Robotics) more helpful:
50 |
51 | > - 27.3.1 Graphical Planar Methods
52 | > Just as homogeneous twist cones for planar problems can be represented as convex signed (+ or −) CoR regions in the plane, homogeneous wrench cones for planar problems can be represented as convex signed regions in the plane. This is called moment labeling [27.14, 34]. Given a collection of lines of force in the plane (e.g., the edges of friction cones from a set of point contacts), the set of all nonnegative linear combinations of these can be represented by labeling all the points in the plane with either a ‘+’ if all resultants make nonnegative moment about that point, a ‘−’ if all make nonpositive moment about that point, a ‘±’ if all make zero moment about that point, and a blank label if there exist resultants making positive moment and resultants making negative moment about that point.
53 |
54 | Two steps:
55 | 1. When the line of action of an external wrench $$w_{ext}$$ does not pass through any grey areas (signed regions), then this external wrench can be balanced off by the frictional forces provided by the contact points. In other words, the object will remain stationary or quasistatic.
56 | 2. When the line of action of an external wrench $$w_{ext}$$ does pass through any grey areas, draw an opposing wrench of that $$w_{ext}$$, then evaluate whether this opposing wrench is _compatible_ with the existing signs or not.
57 |
58 | ### Example 1 from Springer Handbook
59 |
60 |
61 |
62 |
63 |
64 | According to step (1), we can know that the external wrench $$w_{ext}$$ will not cause the object to move because its line of action does not pass through any signed areas. Secondly, according to step (2), if we draw the opposing wrench in the opposite direction of $$w_{ext}$$, we will find that the opposing wrench will still produce a counterclockwise moment for the points inside the grey area marked '+', so this opposing wrench is compatible with the original signed area, thus the object is stationary/quasistatic.
65 |
66 | ### Example 2 from Springer Handbook
67 |
68 |
69 |
70 |
71 |
72 | For figure a), $$w_2$$ does not pass through any gray area, so by step (1) we know that it will not cause the object to move; similarly, looking at the opposing wrench of $$w_2$$ we will find that the opposing wrench will still produce counter-clockwise moments for all points inside the grey area marked '+', then by step (2) we know that it will not cause the object to move.
73 |
74 | The line of action of $$w_1$$ does pass through the gray area, so we must consider its opposing wrench. This opposing wrench separates the signed region into two halfs. For points on the LHS, the opposing wrench still produces a counter-clockweise moment, but for points on the RHS, the opposing wrench will produce negative moments. Therefore, by step (2), $$w_1$$ will cause the object to mvoe.
75 |
76 | ***
77 |
78 | ## Implementation of Planar Form Closure Test
79 | ### （by First-Order Analysis）
80 | This implementation is done in MATLAB, the algorithm is adopted from the textbook. The basic idea behind the first-order analysis using the linear solver is that, if $$\text{pos}(\cup_i \mathscr{F}_i)$$ contains the origin $$[0\; 0\; 0]^T$$, then every possible wrench $$\mathscr{F}$$ will be spanned by this positive span, i.e., the body is in form closure.
81 |
82 | ```Matlab
83 | %% This script takes in a list of support points and the contact normal in
84 | % radians measured from the positive x-axis, in the following form:
85 | % | x | y | angle of contact normal from positive x axis|
86 | % | 0 | 0 | 1.5708 |
87 | % is a support point at origin with its contact normal pointing in the
88 | % direction of positive y-axis.
89 | % If the planar body IS in form closure by first-order anaylsis, this
90 | % script returns 1; if not, returns 0.
91 | % Written by Donglin Sui (lordblackwoods@gmail.com) on 2020.03.17
92 | % For personal interests
93 |
94 |
95 | %% Use this part to test the function
96 | % Alternative way of getting input support point list:
97 | % pointList = readmatrix('Input_points.csv');
98 | %% Test 1 (see the corresponding screenshot for detail)
99 | pointList1 = [[1 0 pi/2];
100 | [3 0 pi/2];
101 | [4 4 pi ];
102 | [0 6 -1.19029]];
103 | IsClosure1 = FormClosureFirstOrder(pointList1);
104 |
105 | %% Test 2
106 | pointList2 = [[1 0 pi/2];
107 | [3 0 pi/2];
108 | [4 4 pi ];
109 | [0 6 -pi/4]];
110 | IsClosure2 = FormClosureFirstOrder(pointList2);
111 |
112 | %% Function define below
113 | function IsClosure = FormClosureFirstOrder(pointList)
114 | N = size(pointList,1);
115 | f = ones(1,N);
116 | A = -1 .* eye(N);
117 | b = -1 .* f;
118 | F = [];
119 | for i = 1:N
120 | p = [pointList(i,1) pointList(i,2)];
121 | theta = pointList(i,3);
122 | n = [cos(theta) sin(theta)];
123 | m_iz = cross([p 0],[n 0]);
124 | F = [F [m_iz(3); n(1); n(2)]];
125 | end
126 | Aeq = F;
127 | beq = [0 0 0];
128 | k = linprog(f,A,b,Aeq,beq);
129 | if isempty(k)
130 | fprintf('The body is not in form closure by first-order analysis.\n');
131 | IsClosure = false;
132 | else
133 | fprintf('The body is in form closure by first-order analysis.\n');
134 | IsClosure = true;
135 | end
136 | end
137 | ```
138 |
139 |
140 |
141 |
142 |
143 | ***
144 |
145 | ## Implementation of Assembly Stability Test
146 | ### （by First-Order Analysis）
147 | The following implementation is done in MATLAB.
148 |
149 | ```Matlab
150 | %% This script performs the evaluation of the stability of an assembly
151 | %
152 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
153 | %
154 | % The scripts takes two input, the first input is a description of the
155 | % static mass properties, massList, in the form of
156 | %
157 | % massList = [ body_ID_1, mass_1, x_cm_1, y_cm_1;
158 | % body_ID_2, mass_2, x_cm_2, y_cm_2;
159 | % ... ... ... ... ];
160 | %
161 | % the second input is a description of the contacts, contactList, in the
162 | % form of
163 | %
164 | % contactList = [ body_ID_1, body_ID_2, x_c, y_c, theta, mu;
165 | % body_ID_1, body_ID_4, x_c, y_c, theta, mu;
166 | % ... ... ... ... ... ... ];
167 | %
168 | % in which body_ID_1 and body_ID_2 are the ID of bodies:
169 | % body_ID = 0 for ground; body_ID = 1 for body 1; body_ID = 2 for body 2,
170 | % etc. x_c and y_c are the point of contact, theta is the direction of
171 | % contract normal relative to the first body, mu is the coefficient of
172 | % friction at point of contact.
173 | %
174 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
175 | %
176 | % Written by Donglin Sui (lordblackwoods@gmail.com) on 2020/03/31
177 | % For personal interests
178 | %
179 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
180 | %% Testing
181 | %% Example 1
182 | % This example was taken from fig.12.37 of the textbook, in which the
183 | % structure is not stable. The expected value of IsStable is FALSE.
184 | massList1 = [1, 2, 25, 35;
185 | 2, 5, 66, 42];
186 | contactList1 = [1, 0, 0, 0, pi/2, 0.1;
187 | 1, 2, 60, 60, pi, 0.5;
188 | 2, 0, 72, 0, pi/2, 0.5;
189 | 2, 0, 60, 0, pi/2, 0.5];
190 | fprintf('*********Example 1*********\n')
191 | IsStable1 = evaluateStability(massList1,contactList1);
192 | fprintf('*********End of Example 1*********\n')
193 |
194 | %% Example 2 (also from fig.12.37 of the textbook)
195 | % This example is stable as the mu has increased from 0.1 to 0.5 for the
196 | % first contact. The expected value of IsStable is TRUE.
197 | massList2 = [1, 2, 25, 35;
198 | 2, 10, 66, 42];
199 | contactList2 = [1, 0, 0, 0, pi/2, 0.5;
200 | 1, 2, 60, 60, pi, 0.5;
201 | 2, 0, 72, 0, pi/2, 0.5;
202 | 2, 0, 60, 0, pi/2, 0.5];
203 | fprintf('*********Example 2*********\n')
204 | IsStable2 = evaluateStability(massList2,contactList2);
205 | fprintf('*********End of Example 2*********\n')
206 |
207 | %% Example 3 (three-body arch, fig.12.27 in textbook)
208 | % assume mass = 5 for all three bodies
209 | % The first frictional coefficient that allows the assembly to remain
210 | % stable is mu = 0.428. Any value below that will result in instability.
211 | massList3 = [1, 5, 110, 70;
212 | 2, 5, 260, 130;
213 | 3, 5, 410, 70];
214 | mu3 = 0.428;
215 | contactList3 = [1, 0, 0, 0, pi/2, mu3;
216 | 1, 0, 80, 0, pi/2, mu3;
217 | 1, 2, 160, 160, -2.81993, mu3;
218 | 1, 2, 180, 100, -2.81993, mu3;
219 | 3, 2, 360, 160, -0.32166, mu3;
220 | 3, 2, 340, 100, -0.32166, mu3;
221 | 3, 0, 440, 0, pi/2, mu3;
222 | 3, 0, 520, 0, pi/2, mu3];
223 | fprintf('*********Example 3*********\n')
224 | IsStable3 = evaluateStability(massList3,contactList3);
225 | fprintf('*********End of Example 3*********\n')
226 |
227 |
228 |
229 | %% The evaluation function is defined below
230 | function IsStable = evaluateStability(massList, contactList)
231 | numBody = size(massList,1);
232 | numContact = size(contactList,1);
233 |
234 | % For each body, the wrenches have 3 dimension, namely [m fx fy],
235 | % therefore we have 3*numBody columns, and each contact is specified by
236 | % a friction cone consists of two wrench, therefore we have
237 | % 2*numContact rows
238 | F = zeros(3 * numBody, 2 * numContact);
239 |
240 | % loop through all contacts
241 | for i = 1 : numContact
242 | body_ID_1 = contactList(i,1);
243 | body_ID_2 = contactList(i,2);
244 |
245 | % contact point is augmented to spatial vector because MATLAB
246 | % built-in function can only calculate the cross product between
247 | % spatial vectors
248 | contactPoint = [contactList(i,3); ...
249 | contactList(i,4); ...
250 | 0 ];
251 |
252 | whichContact = i; % contact index
253 | theta = contactList(i,5);
254 | mu = contactList(i,6);
255 |
256 | % if Body 1 is body-on-ground
257 | if body_ID_1 ~= 0
258 | angle1 = theta - atan2(mu,1); % direction of wrench 1 of the
259 | % friction cone
260 | angle2 = theta + atan2(mu,1); % direction of wrench 2 of the
261 | % friction cone
262 |
263 | % convert angle into unit vector
264 | n1 = [cos(angle1); ...
265 | sin(angle1); ...
266 | 0 ];
267 | n2 = [cos(angle2); ...
268 | sin(angle2); ...
269 | 0 ];
270 |
271 | skew_p_n1 = cross(contactPoint, n1);
272 | skew_p_n2 = cross(contactPoint, n2);
273 |
274 | F1 = [skew_p_n1(3); ...
275 | n1(1:2,:) ];
276 | F2 = [skew_p_n2(3); ...
277 | n2(1:2,:) ];
278 |
279 | F( (body_ID_1 - 1) * 3 + 1 : (body_ID_1 * 3), ...
280 | (whichContact - 1) * 2 + 1) = F1;
281 | F( (body_ID_1 - 1) * 3 + 1 : (body_ID_1 * 3), ...
282 | (whichContact - 1) * 2 + 2) = F2;
283 | end
284 |
285 | % if Body 2 is body-on-ground
286 | if body_ID_2 ~= 0
287 | angle1 = theta - atan2(mu,1) + pi; % direction of wrench 1 of
288 | % the friction cone, + pi
289 | % is used to account for
290 | % 'equal but opposite'
291 | angle2 = theta + atan2(mu,1) + pi; % direction of wrench 2 of
292 | % the friction cone, + pi
293 | % is used to account for
294 | % 'equal but opposite'
295 |
296 | % convert angle into unit vector
297 | n1 = [cos(angle1); ...
298 | sin(angle1); ...
299 | 0 ];
300 | n2 = [cos(angle2); ...
301 | sin(angle2); ...
302 | 0 ];
303 |
304 | skew_p_n1 = cross(contactPoint, n1);
305 | skew_p_n2 = cross(contactPoint, n2);
306 |
307 | F1 = [skew_p_n1(3); ...
308 | n1(1:2,:) ];
309 | F2 = [skew_p_n2(3); ...
310 | n2(1:2,:) ];
311 |
312 | F( (body_ID_2 - 1) * 3 + 1 : (body_ID_2 * 3), ...
313 | (whichContact - 1) * 2 + 1) = F1;
314 | F( (body_ID_2 - 1) * 3 + 1 : (body_ID_2 * 3), ...
315 | (whichContact - 1) * 2 + 2) = F2;
316 | end
317 | end
318 |
319 | % loop through Fext (wrench due to gravity)
320 | Fext = [];
321 | g = 9.81;
322 | theta_g = -pi/2;
323 | for ii = 1 : numBody
324 | CM = [massList(ii,3); ...
325 | massList(ii,4); ...
326 | 0 ];
327 | m = massList(ii,2);
328 | n_g = m .* g .* [cos(theta_g); ...
329 | sin(theta_g); ...
330 | 0 ];
331 | skew_p_n_g = cross(CM,n_g);
332 | Fext = [Fext; ...
333 | [skew_p_n_g(3);...
334 | n_g(1:2,:) ]];
335 | end
336 |
337 | % linear planning coefficient
338 | f = ones(1,2 * numContact);
339 | A = -1 .* eye(2 * numContact);
340 | b = -1 .* ones(1, 2 * numContact);
341 | Aeq = F;
342 | beq = -1 .* Fext;
343 | k = linprog(f,A,b,Aeq,beq);
344 |
345 | % preparing output
346 | fprintf('==================================================\n')
347 | if isempty(k)
348 | fprintf('The assembly IS NOT stable.\n')
349 | IsStable = false;
350 | fprintf('There does not exist a k such that Fk = 0 while ki >= 1.\n')
351 | else
352 | fprintf('The assembly IS stable.\n')
353 | IsStable = true;
354 | fprintf('The positve coefficient k in Fk = 0 is:\n')
355 | disp(k)
356 | end
357 | fprintf('==================================================\n')
358 | end
359 | ```
360 |
361 | ### Testing examples
362 |
363 |
428 |
429 | > _**Exercise 12.3**_ A rigid body is contacted at $$p=(1,2,3)$$ with a contact normal into the body $$\hat{n}=(0,1,0)$$. Write the constraint on the body’s twist $$\mathscr{V}$$ due to this contact.
430 |
431 | By the impenetrability constraint, we have $$ \mathscr{F}^T ( \mathscr{V}_a - \mathscr{V}_b ) \leq 0 $$. Assume everything other than the body is stationary, $$\mathscr{V}_b = 0$$.
432 |
433 | $$
434 | \begin{align}
435 | \begin{split}
436 | \mathscr{F}^T &= \left( [p]\hat{n} \;\; \hat{n} \right)^T \\
437 | &= \left( \left( \begin{bmatrix}
438 | 0 & -3 & 2 \\
439 | 3 & 0 & -1 \\
440 | -2 & 1 & 0 \\
441 | \end{bmatrix} \begin{bmatrix}
442 | 0 \\
443 | 1 \\
444 | 0 \\
445 | \end{bmatrix} \right)^T \;\; \begin{bmatrix}
446 | 0 \\
447 | 1 \\
448 | 0 \\
449 | \end{bmatrix}^T \right) \\
450 | &= \begin{bmatrix}
451 | -3 & 0 & 1 & 0 & 1 & 0 \\
452 | \end{bmatrix} \\
453 | \end{split}
454 | \end{align}
455 | $$
456 |
457 | $$
458 | \begin{align}
459 | \begin{split}
460 | \mathscr{F}^T \mathscr{V}_A &= \begin{bmatrix}
461 | -3 & 0 & 1 & 0 & 1 & 0 \\
462 | \end{bmatrix} \begin{bmatrix}
463 | \omega_x \\
464 | \omega_y \\
465 | \omega_z \\
466 | v_x \\
467 | v_y \\
468 | v_z \\
469 | \end{bmatrix} \\
470 | &= -3 \omega_x + \omega_z + v_y \leq 0 \\
471 | \end{split}
472 | \end{align}
473 | $$
474 |
475 | > _**Exercise 12.4**_ A space frame $$\{ s \}$$ is defined at a contact between a stationary constraint and a body. The contact normal, into the body, is along the $$\hat{z}$$-axis of the $$\{ s \}$$ frame.
476 | > - (a) Write down the constraint on the body’s twist $$\mathscr{V}$$ if the contact is a frictionless point contact.
477 | > - (b) Write down the constraints on $$\mathscr{V}$$ if the contact is a point contact with friction.
478 | > - (c) Write down the constraints on $$\mathscr{V}$$ if the contact is a soft contact.
479 |
480 |
481 |
482 |
483 |
484 | > _**Exercise 12.5_** Figure 12.29 shows five stationary “fingers” contacting an object. The object is in first-order form closure and therefore force closure. If we take away one finger, the object may still be in form closure. For which subsets of four fingers is the object still in form closure? Prove your answers using graphical methods.
485 |
486 |
487 |
488 |
489 |
490 | > _**Exercise 12.6**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by finger 1. Label the feasible CoRs with their contact labels.
491 |
492 |
493 |
494 |
495 |
496 | > _**Exercise 12.7**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1 and 2. Label the feasible CoRs with their contact labels.
497 |
498 |
499 |
500 |
501 |
502 | > _**Exercise 12.8**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 2 and 3. Label the feasible CoRs with their contact labels.
503 |
504 |
505 |
506 |
507 |
508 | > _**Exercise 12.9**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1 and 5. Label the feasible CoRs with their contact labels.
509 |
510 |
511 |
512 |
513 |
514 | > _**Exercise 12.10**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1, 2, and 3.
515 |
516 |
517 |
518 |
519 |
520 | > _**Exercise 12.11**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1, 2, and 4.
521 |
522 |
523 |
524 |
525 |
526 | > _**Exercise 12.12**_ Draw the set of feasible twists as CoRs when the triangle of Figure 12.29 is contacted only by fingers 1, 3, and 5.
527 |
528 |
529 |
530 |
531 |
532 | > _**Exercise 12.13**_ Refer again to the triangle of Figure 12.29.
533 | > - (a) Draw the wrench cone from contact 5, assuming a friction angle $$\alpha = 22.5^{\circ}$$ (a friction coefficient $$\mu = 0.41$$), using moment labeling.
534 | > - (b) Add contact 2 to the moment-labeling drawing. The friction coefficient at contact 2 is $$\mu = 1$$.
535 |
536 |
537 |
538 |
539 |
540 | > _**Exercise 12.14**_ Refer again to the triangle of Figure 12.29. Draw the moment-labeling region corresponding to contact 1 with $$\mu = 1$$ and contact 4 with $$\mu = 0$$.
541 |
542 |
543 |
544 |
545 |
546 | > _**Exercise 12.15**_ The planar grasp of Figure 12.30 consists of five frictionless point contacts. The square’s size is $$4 \times 4 $$.
547 | > - (a) Show that this grasp does not yield force closure.
548 | > - (b) The grasp of part (a) can be modified to yield force closure by adding one frictionless point contact. Draw all the possible locations for this contact.
549 |
550 |
551 |
552 |
553 |
554 | > _**Exercise 12.16**_ Assume the contacts shown in Figure 12.31 are frictionless point contacts. Determine whether the grasp yields force closure. If it does not, how many additional frictionless point contacts are needed to construct a force closure grasp?
555 |
556 |
557 |
558 |
559 |
560 | > _**Exercise 12.17**_ Consider the L-shaped planar object of Figure 12.32.
561 | > - (a) Suppose that both contacts are point contacts with friction coefficient $$\mu = 1$$. Determine whether this grasp yields force closure.
562 | (b) Now suppose that point contact 1 has friction coecient μ = 1, while point contact 2 is frictionless. Determine whether this grasp yields force
563 | closure.
564 | > - (c) The vertical position of contact 1 is allowed to vary; denote its height by $$x$$. Find all positions $$x$$ such that the grasp is force closure with $$\mu = 1$$ for contact 1 and $$\mu = 0$$ for contact 2.
565 | g
566 |
567 |
568 |
569 |
570 |
571 |
572 |
573 |
574 |
575 | - (c) It seems like the top-right '++++' area cannot be eliminated in the current grasp. Therefore no position is able to generate a force-closure grasp.
576 |
577 | > _**Exercise 12.18**_ A square is restrained by three point contacts as shown in Figure 12.33: $$f_1$$ is a point contact with friction coefficient $$\mu$$, while $$f_2$$ and $$f_3$$ are frictionless point contacts. If $$c = \frac{1}{4}$$ and $$h = \frac{1}{2}$$, find the range of values of $$\mu$$ such that grasp yields force closure.
578 |
579 |
580 |
581 |
582 |
583 | > _**Exercise 12.19**_
584 | > - (a) For the planar grasp of Figure 12.34(a), assume contact C is frictionless, while the friction coecient at contacts A and B is $$\mu = 1$$. Determine whether this grasp is force closure.
585 | > - (b) For the planar grasp of Figure 12.34(b), assume contacts A and B are frictionless, while contact C has a friction cone of half-angle $$\beta$$. Find the range of values of $$\beta$$ that makes this grasp force closure.
586 |
587 |
588 |
589 |
590 |
591 |
592 |
593 |
594 |
595 | > _**Exercise 12.23**_ A frictionless finger begins pushing a box over a table (Figure 12.36). There is friction between the box and the table, as indicated in the figure. There are three possible contact modes between the box and the table: either the box slides to the right flat against the table, or it tips over at the right lower corner, or it tips over that corner while the corner also slides to the right. Which actually occurs? Assume a quasistatic force balance and answer the following questions.
596 | > - (a) For each of the three contact modes, draw the moment-labeling regions corresponding to the table’s friction cone edges active in that contact mode.
597 | > - (b) For each moment-labeling drawing, determine whether the pushing force plus the gravitational force can be quasistatically balanced by the support forces. From this, determine which contact mode actually occurs.
598 | > - (c) Graphically show a different support-friction cone for which the contact mode is different from your solution above.
599 |
600 |
601 |
602 |
603 |
604 | > _**Exercise 12.24**_ In Figure 12.37 body 1, of mass m1 with center of mass at $$(x_1,y_1)$$, leans on body 2, of mass $$m_2$$ with center of mass at $$(x_2,y_2)$$. Both are supported by a horizontal plane, and gravity acts downward. The friction coefficient at all four contacts (at $$(0, 0)$$, at $$(x_L , y)$$, at $$(x_L , 0)$$, and at $$(x_R, 0)$$) is $$\mu > 0$$. We want to know whether it is possible for the assembly to stay standing by some choice of contact forces within the friction cones. Write down the six equations of force balance for the two bodies in terms of the gravitational forces and the contact forces, and express the conditions that must be satisfied for this assembly to stay standing. How many equations and unknowns are there?
605 |
606 |
607 |
608 |
609 |
610 | > _**Bonus Exercise**_ Which of the combination of two fingers yields force closure?
611 |
612 |
623 |
624 | Source: [Springer Handbook of Robotics](https://books.google.com.hk/books?id=Xpgi5gSuBxsC&pg=PA657&lpg=PA657&dq=slipping+or+tipping+problems+using+wrenches&source=bl&ots=lVolW9m97Q&sig=ACfU3U03i05F27mvky9ecBo6aak7BNeNMQ&hl=en&sa=X&ved=2ahUKEwjj_9jstMLoAhXkyIsBHVy7BHIQ6AEwAHoECAcQAQ#v=onepage&q=slipping%20or%20tipping%20problems%20using%20wrenches&f=false)
625 |
626 | ### Example 1
627 | Now, consider a box sitting on a table with $$\mu = 1$$ and the friction cone shown below, if the contact mode is SrSr, then in which region lies the center-of-mass?
628 |
629 |
630 |
631 |
632 |
633 | Using the same principle shown above, we know that the possible regions that contain the center-of-mass in SrSr contact mode are A, B, D, and G.
634 |
635 |
636 |
637 |
638 |
639 | For BR mode, the regions are A, C, D, E, F, G.
640 |
641 |
56 |
--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
1 | CC0 1.0 Universal
2 |
3 | Statement of Purpose
4 |
5 | The laws of most jurisdictions throughout the world automatically confer
6 | exclusive Copyright and Related Rights (defined below) upon the creator and
7 | subsequent owner(s) (each and all, an "owner") of an original work of
8 | authorship and/or a database (each, a "Work").
9 |
10 | Certain owners wish to permanently relinquish those rights to a Work for the
11 | purpose of contributing to a commons of creative, cultural and scientific
12 | works ("Commons") that the public can reliably and without fear of later
13 | claims of infringement build upon, modify, incorporate in other works, reuse
14 | and redistribute as freely as possible in any form whatsoever and for any
15 | purposes, including without limitation commercial purposes. These owners may
16 | contribute to the Commons to promote the ideal of a free culture and the
17 | further production of creative, cultural and scientific works, or to gain
18 | reputation or greater distribution for their Work in part through the use and
19 | efforts of others.
20 |
21 | For these and/or other purposes and motivations, and without any expectation
22 | of additional consideration or compensation, the person associating CC0 with a
23 | Work (the "Affirmer"), to the extent that he or she is an owner of Copyright
24 | and Related Rights in the Work, voluntarily elects to apply CC0 to the Work
25 | and publicly distribute the Work under its terms, with knowledge of his or her
26 | Copyright and Related Rights in the Work and the meaning and intended legal
27 | effect of CC0 on those rights.
28 |
29 | 1. Copyright and Related Rights. A Work made available under CC0 may be
30 | protected by copyright and related or neighboring rights ("Copyright and
31 | Related Rights"). Copyright and Related Rights include, but are not limited
32 | to, the following:
33 |
34 | i. the right to reproduce, adapt, distribute, perform, display, communicate,
35 | and translate a Work;
36 |
37 | ii. moral rights retained by the original author(s) and/or performer(s);
38 |
39 | iii. publicity and privacy rights pertaining to a person's image or likeness
40 | depicted in a Work;
41 |
42 | iv. rights protecting against unfair competition in regards to a Work,
43 | subject to the limitations in paragraph 4(a), below;
44 |
45 | v. rights protecting the extraction, dissemination, use and reuse of data in
46 | a Work;
47 |
48 | vi. database rights (such as those arising under Directive 96/9/EC of the
49 | European Parliament and of the Council of 11 March 1996 on the legal
50 | protection of databases, and under any national implementation thereof,
51 | including any amended or successor version of such directive); and
52 |
53 | vii. other similar, equivalent or corresponding rights throughout the world
54 | based on applicable law or treaty, and any national implementations thereof.
55 |
56 | 2. Waiver. To the greatest extent permitted by, but not in contravention of,
57 | applicable law, Affirmer hereby overtly, fully, permanently, irrevocably and
58 | unconditionally waives, abandons, and surrenders all of Affirmer's Copyright
59 | and Related Rights and associated claims and causes of action, whether now
60 | known or unknown (including existing as well as future claims and causes of
61 | action), in the Work (i) in all territories worldwide, (ii) for the maximum
62 | duration provided by applicable law or treaty (including future time
63 | extensions), (iii) in any current or future medium and for any number of
64 | copies, and (iv) for any purpose whatsoever, including without limitation
65 | commercial, advertising or promotional purposes (the "Waiver"). Affirmer makes
66 | the Waiver for the benefit of each member of the public at large and to the
67 | detriment of Affirmer's heirs and successors, fully intending that such Waiver
68 | shall not be subject to revocation, rescission, cancellation, termination, or
69 | any other legal or equitable action to disrupt the quiet enjoyment of the Work
70 | by the public as contemplated by Affirmer's express Statement of Purpose.
71 |
72 | 3. Public License Fallback. Should any part of the Waiver for any reason be
73 | judged legally invalid or ineffective under applicable law, then the Waiver
74 | shall be preserved to the maximum extent permitted taking into account
75 | Affirmer's express Statement of Purpose. In addition, to the extent the Waiver
76 | is so judged Affirmer hereby grants to each affected person a royalty-free,
77 | non transferable, non sublicensable, non exclusive, irrevocable and
78 | unconditional license to exercise Affirmer's Copyright and Related Rights in
79 | the Work (i) in all territories worldwide, (ii) for the maximum duration
80 | provided by applicable law or treaty (including future time extensions), (iii)
81 | in any current or future medium and for any number of copies, and (iv) for any
82 | purpose whatsoever, including without limitation commercial, advertising or
83 | promotional purposes (the "License"). The License shall be deemed effective as
84 | of the date CC0 was applied by Affirmer to the Work. Should any part of the
85 | License for any reason be judged legally invalid or ineffective under
86 | applicable law, such partial invalidity or ineffectiveness shall not
87 | invalidate the remainder of the License, and in such case Affirmer hereby
88 | affirms that he or she will not (i) exercise any of his or her remaining
89 | Copyright and Related Rights in the Work or (ii) assert any associated claims
90 | and causes of action with respect to the Work, in either case contrary to
91 | Affirmer's express Statement of Purpose.
92 |
93 | 4. Limitations and Disclaimers.
94 |
95 | a. No trademark or patent rights held by Affirmer are waived, abandoned,
96 | surrendered, licensed or otherwise affected by this document.
97 |
98 | b. Affirmer offers the Work as-is and makes no representations or warranties
99 | of any kind concerning the Work, express, implied, statutory or otherwise,
100 | including without limitation warranties of title, merchantability, fitness
101 | for a particular purpose, non infringement, or the absence of latent or
102 | other defects, accuracy, or the present or absence of errors, whether or not
103 | discoverable, all to the greatest extent permissible under applicable law.
104 |
105 | c. Affirmer disclaims responsibility for clearing rights of other persons
106 | that may apply to the Work or any use thereof, including without limitation
107 | any person's Copyright and Related Rights in the Work. Further, Affirmer
108 | disclaims responsibility for obtaining any necessary consents, permissions
109 | or other rights required for any use of the Work.
110 |
111 | d. Affirmer understands and acknowledges that Creative Commons is not a
112 | party to this document and has no duty or obligation with respect to this
113 | CC0 or use of the Work.
114 |
115 | For more information, please see
116 |
117 |
--------------------------------------------------------------------------------
/README.txt:
--------------------------------------------------------------------------------
1 | This repository is for the web page "Modern_Robotics_Notes", for more information please visit the page at "https://gloogger.github.io/Modern_Robotics_Notes/".
2 |
--------------------------------------------------------------------------------
/Schlusssteinprojekt.md:
--------------------------------------------------------------------------------
1 | ---
2 | layout: post
3 | mathjax: true
4 | title: 'Schlusssteinprojekt'
5 | description: My attemp for the Schlusssteinprojekt
6 | is_project_page: false
7 | ---
8 |
9 |
10 |
11 |
12 |
13 |
14 |
15 |
16 | # Capstone Project
17 |
18 | ## Main script: capstone_main.m
19 |
20 | ```Matlab
21 | %% This script produces csv files that will guide a youbot (4 mecanum whee
22 | % -ls chassis + UR5 arm) to perform pick-drop action. The script takes
23 | % four input, namely:
24 | % 1. initial configuration of the end-effector in the form of
25 | % [chassis phi, x, y, J1, J2, J3, J4, J5, W1, W2, W3, W4]
26 | % 2. location of cube-to-pick:
27 | % [x, y, theta]
28 | % 3. goal location to drop the cube:
29 | % [x, y, theta]
30 | % 4. Error gains:
31 | % [Kp, Ki]
32 | %=========================================================================%
33 | % For more information about this assignment, please visit:
34 | % http://hades.mech.northwestern.edu/index.php/Mobile_Manipulation_Capstone
35 | %=========================================================================%
36 | % Written by Donglin Sui (lordblackwoods@gmail.com) on 2020/04/03
37 | %=========================================================================%
38 | % The Program Begins Here.
39 | % _.，------..
40 | % ／ `＂_
41 | % ノ \
42 | % ノ \
43 | % | ／\ ／\ |
44 | % | ｜
45 | % \ （_人_） /
46 | % ....—＼ ＿／———.
47 | % |" ' "_ ..,-＇ '|
48 | % ＇￣￣￣＼ ／￣￣￣＇
49 | % | |
50 | % \ |'￣＼｀ |
51 | % ＼ \ ／
52 | % ＼ \_＿／
53 | % ＼_＿、\
54 | %=========================================================================%
55 |
56 |
57 | %--------------------------------------------------------------------------
58 | % Constants
59 | %--------------------------------------------------------------------------
60 | %% Kinematic Model of the youBot
61 | % For detail information, please consult #Kinematics of the youBot section
62 | % of the page:
63 | % http://hades.mech.northwestern.edu/index.php/Mobile_Manipulation_Capstone
64 | % Fixed offset from chassis frame {b} to base frame of arm {0}: Tb0
65 | T_b0 = [1 0 0 0.1662;
66 | 0 1 0 0 ;
67 | 0 0 1 0.0026;
68 | 0 0 0 1 ;];
69 |
70 | % When arm at home config, end-effector frame {e} relative to arm base
71 | % frame {0}, i.e., the home transformation matrix M0e is
72 | M_0e = [1 0 0 0.033 ;
73 | 0 1 0 0 ;
74 | 0 0 1 0.6546;
75 | 0 0 0 1 ;];
76 |
77 | % Body screw axes list, Blist
78 | B1 = [0 0 1 0 0.033 0]';
79 | B2 = [0 -1 0 -0.5076 0 0]';
80 | B3 = [0 -1 0 -0.3526 0 0]';
81 | B4 = [0 -1 0 -0.2176 0 0]';
82 | B5 = [0 0 1 0 0 0]';
83 | BList = [B1 B2 B3 B4 B5];
84 |
85 | % Chassis model
86 | l = 0.47/2;
87 | w = 0.3/2;
88 | r = 0.0475;
89 |
90 | % create moRobo Object
91 | myRobo = youbotKUKA(l, w, r, 'KUKA youbot with UR5 end-effector', ...
92 | T_b0, M_0e, BList);
93 | fprintf('Successfully created the youbot kinematic model!\n')
94 | disp('myRobo')
95 |
96 | %--------------------------------------------------------------------------
97 | % User Input
98 | %--------------------------------------------------------------------------
99 | %% Asking for input
100 | fprintf('***=========================================================================***\n')
101 | fprintf('Please fill in the following initial variables so as to perform the simulation.\n')
102 |
103 | % initial configuration of end-effector initial_T_se
104 | [initial_T_se, initial_Config] = getInitialConfiguration(M_0e,BList,T_b0);
105 |
106 | % initial and final cube location, initial_T_sc & goal_T_sc
107 | initial_T_sc = getInitialCubeLocation();
108 |
109 | goal_T_sc = getGoalCubeLocation();
110 |
111 | % compute standoff and gripping HTM
112 | T_ce_standoff = getCEStandoffHTM();
113 | T_ce_gripping = getCEGrippingHTM();
114 |
115 | % reference initial configuration to test the feedback control
116 | % because input a matrix in Command Window is troublesome, this value is
117 | % hard-coded here
118 | ref_initial_T_se = [ 0 0 1 0;
119 | 0 1 0 0;
120 | -1 0 0 0.5;
121 | 0 0 0 1];
122 | ini_Err = HTM2pose(ref_initial_T_se) - HTM2pose(initial_T_se);
123 |
124 | % feedback gains
125 | gains = getControlGains();
126 |
127 | %% Reference end-effector configuration
128 | initial_T_standoff = initial_T_sc * T_ce_standoff;
129 | T_grasp = initial_T_sc * T_ce_gripping;
130 | goal_T_standoff = goal_T_sc * T_ce_standoff;
131 | T_release = goal_T_sc * T_ce_gripping;
132 |
133 | % Arranged in a way so that it can be looped in the 'Steps' section
134 | Configurations = {ref_initial_T_se, initial_T_standoff, T_grasp, T_grasp,...
135 | initial_T_standoff, goal_T_standoff, T_release, T_release...
136 | goal_T_standoff};
137 |
138 | %--------------------------------------------------------------------------
139 | % Generate reference trajectory
140 | %--------------------------------------------------------------------------
141 | %% Generate reference trajectory
142 | delt_t = 0.01; % 10ms interval
143 | motionTime = 13; % total motion time
144 | Tf = computeTf(motionTime, Configurations);
145 |
146 | % 8 Steps
147 | % 1. A trajectory to move the gripper from its initial configuration to
148 | % a "standoff" configuration a few cm above the block.
149 | % 2. A trajectory to move the gripper down to the grasp position.
150 | % 3. Closing of the gripper.
151 | % 4. A trajectory to move the gripper back up to the "standoff"
152 | % configuration.
153 | % 5. A trajectory to move the gripper to a "standoff" configuration
154 | % above the final configuration.
155 | % 6. A trajectory to move the gripper to the final configuration of the
156 | % object.
157 | % 7. Opening of the gripper.
158 | % 8. A trajectory to move the gripper back to the "standoff"
159 | % configuration.
160 |
161 | grpOPEN = 1;
162 | grpCLOSE = 0;
163 | methodOrder = 5; % quintic polynomials
164 | gripperStatus = 0;
165 |
166 | % trajectory in form of
167 | % [r11 r12 r13 r21 r22 r23 r31 r32 r33 Xpos Ypos Zpos gripperStatus]
168 | traj = [];
169 |
170 | for ii = 1 : 8
171 | if ii == 3
172 | % gripperStatus is a BOOL-type variable, in which
173 | % 1 = gripper closed
174 | % 0 = gripper opened
175 | gripperStatus = grpOPEN;
176 | elseif ii == 7
177 | gripperStatus = grpCLOSE;
178 | end
179 | temp_traj = myRobo.trajectoryGenerator(Configurations{ii}, ...
180 | Configurations{ii+1}, ...
181 | Tf(ii), ...
182 | delt_t, ...
183 | gripperStatus, ...
184 | methodOrder);
185 | traj = [traj; temp_traj];
186 | % the above trajectory of the end-effector is used later to guide the
187 | % motion of the KUKA youbot
188 | end
189 |
190 | %--------------------------------------------------------------------------
191 | % Feedback PI controller
192 | %--------------------------------------------------------------------------
193 | %% PI Feedback control
194 | myRobo.q = [initial_Config(1) initial_Config(2) initial_Config(3)];
195 |
196 | kp = gains(1);
197 | myRobo.Kp = kp * eye(6);
198 | ki = gains(2);
199 | myRobo.Ki = ki * eye(6);
200 |
201 | myRobo.theta = [0 0 0.2 -1.67 0]';
202 | myRobo.wheelAngle = [0 0 0 0];
203 |
204 | speedLimit = 12.3 * ones(1,9);
205 | jointLimit = [ones(1,5) * pi; ones(1,5) * (-pi)];
206 |
207 | [NHTM, NgripperStatus] = NTraj2NHTM(traj);
208 |
209 | [scene6simulation, Xerr] = myRobo.feedbackController(delt_t,NHTM,...
210 | speedLimit,jointLimit,...
211 | NgripperStatus);
212 | writematrix(scene6simulation,'scene6simulation.csv','Delimiter','comma');
213 |
214 | plot(Xerr','LineWidth',1.5);
215 | title('$X_{err}$ against time','FontSize',12,'FontWeight','bold',...
216 | 'interpreter', 'latex')
217 | xlabel('Time (10ms)','FontSize',12,'FontWeight','bold');
218 | ylabel('Error Twist','FontSize',12,'FontWeight','bold');
219 | legend('$W_x$','$W_y$','$W_z$','$V_x$','$V_y$','$V_z$',...
220 | 'interpreter','latex')
221 | grid on;
222 | writematrix(Xerr,'Xerr.csv','Delimiter','comma');
223 |
224 |
225 | %--------------------------------------------------------------------------
226 | % Functions are defined below
227 | %--------------------------------------------------------------------------
228 | %% getInitialConfiguration
229 | % ask the user to input the initial configuration of the robot in array
230 | % form and then compute based on the input the HTM of the initial
231 | % configuration of end-effector: ini_T_se
232 | % For testing, type:
233 | % [0.6, 0.0, 0.0, 0.0, -0.35, -0.698, -0.505, 0.0, 0.0, 0.0, 0.0, 0.0]
234 | function [ini_T_se, iniConfig] = getInitialConfiguration(M_0e,BList,T_b0)
235 | prompt1 = ['\n 1. Please enter the initial configuration, Tse, of the\n'...
236 | 'end-effector in the form of\n'...
237 | '[phi, x, y, J1, J2, J3, J4, J5, W1, W2, W3, W4] :\n'];
238 | iniConfig = input(prompt1);
239 |
240 | % generate transformation matrix for the end-effector from input config
241 | R_vec = [0 0 iniConfig(1)];
242 | R_so3 = VecToso3(R_vec);
243 | R = MatrixExp3(R_so3);
244 | p = [iniConfig(2) iniConfig(3) 0.0963]';
245 | T_sb = RpToTrans(R,p);
246 | T_0e = FKinBody(M_0e, BList, iniConfig(4:8)');
247 | T_be = T_b0 * T_0e;
248 | ini_T_se = T_sb * T_be;
249 | end
250 |
251 | %% getInitialCubeLocation
252 | % ask the user to input the initial location of the cube-to-pick in array
253 | % form and then compute based on the input the HTM of the cube
254 | % configuration.
255 | % For testing, type [1, 0, 0]
256 | function ini_T_sc = getInitialCubeLocation()
257 | prompt2 = ['\n 2. Please enter the initial location of the cube-to-pick in the\n'...
258 | 'form of [x, y, theta] : \n'];
259 | temp = input(prompt2);
260 | x = temp(1);
261 | y = temp(2);
262 | theta = temp(3);
263 | rot = [0 0 theta]';
264 | pos = [x y 0.025]';
265 | R_so3 = VecToso3(rot);
266 | R = MatrixExp3(R_so3);
267 | ini_T_sc = RpToTrans(R,pos);
268 | end
269 |
270 | %% getGoalCubeLocation
271 | % ask the user to input the initial location of the cube-to-drop in array
272 | % form and then compute based on the input the HTM of the cube
273 | % configuration
274 | % For testing, type [0, -1, -pi/2]
275 | function goal_T_sc = getGoalCubeLocation()
276 | prompt3 = ['\n 3. Please enter the goal location of the cube-to-drop in the\n'...
277 | 'form of [x, y, theta] : \n'];
278 | temp = input(prompt3);
279 | x = temp(1);
280 | y = temp(2);
281 | theta = temp(3);
282 | rot = [0 0 theta]';
283 | pos = [x y 0.025]';
284 | R_so3 = VecToso3(rot);
285 | R = MatrixExp3(R_so3);
286 | goal_T_sc = RpToTrans(R,pos);
287 | end
288 |
289 | %% getControlGains
290 | % ask the user to input the feedback controller's gains, namely the ki and
291 | % kp
292 | function gains = getControlGains()
293 | prompt4 = ['\n 4. Please enter the feedback controller gains in\n'...
294 | 'the form of [kp, ki] : \n'];
295 | gains = input(prompt4);
296 | end
297 |
298 | %% getCEStandoffHTM
299 | % get standoff HTM
300 | function T_ce_standoff = getCEStandoffHTM()
301 | theta = [0 pi/5+pi/2 0]';
302 | R_so3 = VecToso3(theta);
303 | R = MatrixExp3(R_so3);
304 | pos = [0 0 0.250]';
305 | T_ce_standoff = RpToTrans(R,pos);
306 | end
307 |
308 | %% getCEGrippingHTM
309 | % get gripping HTM
310 | function T_ce_gripping = getCEGrippingHTM()
311 | theta = [0 pi/5+pi/2 0]';
312 | R_so3 = VecToso3(theta);
313 | R = MatrixExp3(R_so3);
314 | pos = [0 0 0]';
315 | T_ce_gripping = RpToTrans(R,pos);
316 | end
317 |
318 | %% Convert a HTM to pose ([Rx Ry Rz x y z]')
319 | function pose = HTM2pose(HTM)
320 | [R, p] = TransToRp(HTM);
321 | R_so3 = MatrixLog3(R);
322 | rot = so3ToVec(R_so3);
323 | pose = [rot; p];
324 | end
325 |
326 | %% Convert a traj to HTM
327 | % convert a traj in form of
328 | % N * [r11 r12 r13 r21 r22 r23 r31 r32 r33 Xpos Ypos Zpos gripperStatus]
329 | % into a 3x3xN tensor containing the HTM and a 1xN array containing the
330 | % gripperStatus
331 | function [NHTM, NgripperStatus] = NTraj2NHTM(Ntraj)
332 | N = size(Ntraj,1);
333 | NHTM = zeros(4,4,N);
334 | NgripperStatus = zeros(1,N);
335 | for ii = 1 : N
336 | r1j = Ntraj(ii,1:3);
337 | r2j = Ntraj(ii,4:6);
338 | r3j = Ntraj(ii,7:9);
339 | R = [r1j; r2j; r3j];
340 | p = Ntraj(ii,10:12)';
341 | temp_HTM = RpToTrans(R,p);
342 | NHTM(:,:,ii) = temp_HTM;
343 | NgripperStatus(ii) = Ntraj(ii,13);
344 | end
345 | end
346 |
347 | %% Compute weighted time Tf using Euclidean distance
348 | function Tf = computeTf(motionTime, Configurations)
349 | % Initialize variables
350 | T_se_initial = Configurations{1};
351 | T_standoff_initial = Configurations{2};
352 | T_grasp = Configurations{3};
353 | T_standoff_final = Configurations{6};
354 | T_release = Configurations{7};
355 |
356 | % for debugging
357 | %fprintf('T_se_initial:\n')
358 | %disp(T_se_initial)
359 | %fprintf('T_standoff_initial:\n')
360 | %disp(T_standoff_initial)
361 | %fprintf('T_grasp:\n')
362 | %disp(T_grasp)
363 | %fprintf('T_standoff_final:\n')
364 | %disp(T_standoff_final)
365 | %fprintf('T_release:\n')
366 | %disp(T_release)
367 |
368 | normalized_D = [norm(T_se_initial(1:3,4)-T_standoff_initial(1:3,4)),...
369 | norm(T_standoff_initial(1:3,4)-T_grasp(1:3,4)),...
370 | 0,...
371 | norm(T_standoff_initial(1:3,4)-T_grasp(1:3,4)),...
372 | norm(T_standoff_initial(1:3,4)-T_standoff_final(1:3,4)),...
373 | norm(T_standoff_final(1:3,4)-T_release(1:3,4)),...
374 | 0,...
375 | norm(T_standoff_final(1:3,4)-T_release(1:3,4))];
376 | D_sum = sum(normalized_D);
377 |
378 | % 8 Steps
379 | % 1. A trajectory to move the gripper from its initial configuration to
380 | % a "standoff" configuration a few cm above the block.
381 | % 2. A trajectory to move the gripper down to the grasp position.
382 | % 3. Closing of the gripper.
383 | % 4. A trajectory to move the gripper back up to the "standoff"
384 | % configuration.
385 | % 5. A trajectory to move the gripper to a "standoff" configuration
386 | % above the final configuration.
387 | % 6. A trajectory to move the gripper to the final configuration of the
388 | % object.
389 | % 7. Opening of the gripper.
390 | % 8. A trajectory to move the gripper back to the "standoff"
391 | % configuration.
392 |
393 | % N.B. that each of step 3 & 7 should consist of at least 63 identical
394 | % lines of the csv file in which the gripper state matches previous
395 | % line in the csv file, so as to make sure that the gripper motion is
396 | % completed.
397 |
398 | t = zeros(1,8);
399 | gripperRunningTime = 0.63; % 630 ms, 63 of 10ms intervals
400 | for i = 1:8
401 | if (i == 3) || (i == 7)
402 | t(i) = gripperRunningTime;
403 | else
404 | t(i) = normalized_D(i) * (motionTime - 2 * gripperRunningTime) / ...
405 | D_sum;
406 | end
407 | end
408 |
409 | Tf = round(t * 10^2) / 10^2; % round into multiples of 10ms
410 | end
411 | ```
412 |
413 | ## superclass: wheeledMobileRobot.m
414 |
415 | ```Matlab
416 | %% This file defines a super class called 'wheeledMobileRobot'.
417 | % This file is part of the capstone project of the Coursera MOOC Modern
418 | % Robotics Course 6.
419 | % Written by Donglin Sui (lordblackwoods@gmail.com) on 2020/04/05
420 | % In MATLAB, the relation between a subclass and a super class is like
421 | % the relation between 'Desk' and 'Furniture', in which the later contains
422 | % the generic information of certain object, and the former contains
423 | % (usually) more specific pieces of information. For readers who are not
424 | % familiar with OOP (Object-Oriented-Programming), check this Q&A :
425 | % https://www.mathworks.com/matlabcentral/answers/89957-why-subclass-superclass-in-oop
426 | classdef wheeledMobileRobot
427 | properties
428 | % kinematic model
429 | l % length from center of car to wheel axis in x_b
430 | % direction, the half_length
431 | w % length from center of car to wheel in y_b direction,
432 | % the half_width
433 | r % radius of wheel
434 | type % robot type, in this case, the type is 'youBot'
435 |
436 | % for feedback PI control
437 | q % in the form of [chassis_phi x y]
438 | q_dot
439 | Vb
440 | u
441 |
442 | % odometry
443 | wheelAngle
444 | end
445 |
446 | properties (Constant)
447 | height = 0.0963
448 | end
449 |
450 | properties (Dependent)
451 | config
452 | H_0
453 | end
454 |
455 | methods
456 | function obj = set.u(obj,val)
457 | obj.u = val;
458 | end
459 |
460 | % The following three functions are used for the dependent
461 | % properties
462 | function temp = get.config(obj)
463 | theta = obj.q(1);
464 | R = [cos(theta) -sin(theta) 0;
465 | sin(theta) cos(theta) 0;
466 | 0 0 1];
467 | x = obj.q(2);
468 | y = obj.q(3);
469 | z = obj.height;
470 | p = [x y z]';
471 | temp = RpToTrans(R,p);
472 | end
473 |
474 | function temp = get.H_0(obj)
475 | rr = obj.r;
476 | ll = obj.l;
477 | ww = obj.w;
478 | temp = [-(ll+ww)/rr 1/rr -1/rr;
479 | (ll+ww)/rr 1/rr 1/rr;
480 | (ll+ww)/rr 1/rr -1/rr;
481 | -(ll+ww)/rr 1/rr 1/rr;];
482 | end
483 |
484 | % This function allows the creation of kinematic model of chassis
485 | function obj = wheeledMobileRobot(half_length,half_width,...
486 | radius,roboType)
487 | obj.l = half_length;
488 | obj.w = half_width;
489 | obj.r = radius;
490 | obj.type = roboType;
491 | end
492 |
493 | function obj = nextState(obj, u, delt_t, speedLimit)
494 | u(find((u>=speedLimit) == 1)) = 12.3;
495 |
496 | % Odometry
497 | % Vb = H(0)^{dagger} * delta_theta = F * delta_theta
498 | delta_theta = u';
499 | obj.Vb = pinv(obj.H_0) * delta_theta;
500 |
501 | % chassis twist
502 | omega_bz = obj.Vb(1);
503 | v_bx = obj.Vb(2);
504 | v_by = obj.Vb(3);
505 | % 6-dimensional version of the planar chassis twist
506 | vb6 = [0 0 omega_bz v_bx v_by 0]';
507 | % integrate vb6 over delt_t so as to generate the displacement
508 | % created by the wheelAngle increment
509 | vb6_integral = vb6 * delt_t;
510 | vb6_se3mat = VecTose3(vb6_integral);
511 | T_bb_dash_SE3 = MatrixExp6(vb6_se3mat);
512 | T_k = obj.config;
513 | T_k_next = T_k * T_bb_dash_SE3;
514 |
515 | [R, p] = TransToRp(T_k_next);
516 | R_so3 = MatrixLog3(R);
517 | rot = so3ToVec(R_so3);
518 | chassis_phi_next = rot(3);
519 | x_next = p(1);
520 | y_next = p(2);
521 |
522 | obj.q = [chassis_phi_next x_next y_next];
523 | obj.wheelAngle = obj.wheelAngle + u*delt_t;
524 |
525 |
526 | %T_bk_bknext = MatrixExp6(VecTose3(vb6*delt_t));
527 | %T_s_bknext = obj.config*T_bk_bknext;
528 |
529 | %obj.q = [atan2(T_s_bknext(2,1), T_s_bknext(1,1)),T_s_bknext(1,4),T_s_bknext(2,4)];
530 | %obj.wheelAngle = obj.wheelAngle + u*delt_t;
531 |
532 | end
533 |
534 | end
535 |
536 |
537 | end
538 | ```
539 |
540 | ## subclass: youbotKUKA.m
541 |
542 | ```Matlab
543 | %% Subclass of 'wheeledMobileRobot'
544 | classdef youbotKUKA < wheeledMobileRobot
545 | properties
546 | % for kinematic model
547 | M_be
548 | Blist
549 |
550 | % trajectory
551 | Trajectory
552 |
553 | % for feedback PI control
554 | theta
555 | theta_dot
556 | Kp
557 | Ki
558 |
559 | % for forward kinematic
560 | T_be
561 | T_se
562 | end
563 |
564 | properties (Dependent)
565 | Je
566 | end
567 |
568 | methods
569 | function Je = get.Je(obj)
570 | J_arm = JacobianBody(obj.Blist,obj.theta);
571 | F3 = pinv(obj.H_0, 0.0001);
572 | F6 = zeros(6,4);
573 | F6(3:5,:) = F3;
574 | T_eb = TransInv(obj.T_be);
575 | Ad_T_eb = Adjoint(T_eb);
576 | J_base = Ad_T_eb * F6;
577 | Je = [J_base J_arm];
578 | end
579 |
580 | function obj = youbotKUKA(half_length, half_width, radius,...
581 | roboType, T_b0, M_0e, BList)
582 | obj@wheeledMobileRobot(half_length, half_width, radius, ...
583 | roboType);
584 | obj.M_be = T_b0 * M_0e;
585 | obj.Blist = BList;
586 | end
587 |
588 | % This function shuffles a HTM into array form
589 | % [r11 r12 r13 r21 r22 r23 r31 r32 r33 Xpos Ypos Zpos gripperStatus]
590 | function formattedTraj = shuffleTrajectory(obj,gripperStatus)
591 | len = size(obj.Trajectory,2);
592 | formattedTraj = zeros(len,13);
593 | for ii = 1 : len
594 | temp_HTM = obj.Trajectory{ii};
595 | r1j = temp_HTM(1,1:3);
596 | r2j = temp_HTM(2,1:3);
597 | r3j = temp_HTM(3,1:3);
598 | p = temp_HTM(1:3,4)';
599 | formattedTraj(ii,:) = [r1j, r2j, r3j, p, gripperStatus];
600 | end
601 | end
602 |
603 | function traj = trajectoryGenerator(obj, Xstart, Xend, T, delt_t,...
604 | gripperStatus, methodOrder)
605 | N = round(T/delt_t + 1);
606 | obj.Trajectory = CartesianTrajectory(Xstart, Xend, T, N, ...
607 | methodOrder);
608 | % shuffle the trajectory into the output format, which is
609 | % [r11 r12 r13 r21 r22 r23 r31 r32 r33 Xpos Ypos Zpos gripperStatus]
610 | % rij = terms in the rotation section of the HTM
611 | % Xpos = x-coordinate in the p section of the Rp HTM
612 | % gripperStatus is a boolean value defined as true = closed, false = open
613 | formattedTraj = shuffleTrajectory(obj,gripperStatus);
614 | traj = round(formattedTraj, 14); % this rounding digit is
615 | % chosen arbitrarily and
616 | % can be reduced to 8
617 | end
618 |
619 | % forward kinematics
620 | function obj = FK(obj)
621 | temp = FKinBody(obj.M_be, obj.Blist, obj.theta);
622 | obj.T_be = temp;
623 | obj.T_se = obj.config * temp;
624 | end
625 |
626 | function obj = nextState(obj,config,speed,delt_t,speedLimit)
627 | obj = nextState@wheeledMobileRobot(obj,speed(1:4),delt_t,speedLimit(1:4));
628 | % using logical matrix to index which element the speed is
629 | % bigger than speedLimit
630 | speed = speed(5:9);
631 | speedLimit = speedLimit(5:9);
632 | speed(find((speed>=speedLimit)==1)) = 12.3;
633 | obj.theta = config(4:8) + speed * delt_t;
634 | obj.theta = obj.theta';
635 | end
636 |
637 | function [scene6simulation, Xerr, obj] = feedbackController(obj,...
638 | delt_t,NHTM,...
639 | speedLimit,jointLimit,...
640 | NgripperStatus)
641 | N = size(NHTM,3);
642 | scene6simulation = zeros(N,13);
643 | Xerr = zeros(6,N-1);
644 | V = zeros(6,N-1);
645 | for ii = 1 : (N-1)
646 | obj = obj.FK;
647 | T_se_temp = obj.T_se;
648 | obj.T_be = FKinBody(obj.M_be, obj.Blist, obj.theta);
649 |
650 | % initialize terms required by eqn. (13.37) of the textbook
651 | Xd = NHTM(:,:,ii);
652 | Xd_inv = TransInv(Xd);
653 | Xd_next = NHTM(:,:,ii+1);
654 | Td_next = Xd_inv * Xd_next;
655 | Vd_se3 = MatrixLog6(Td_next);
656 | Vd_se3 = Vd_se3 / delt_t;
657 | Vd = se3ToVec(Vd_se3);
658 | T_es_temp = TransInv(T_se_temp);
659 | T_err = T_es_temp * Xd;
660 | xerr_se3 = MatrixLog6(T_err);
661 | xerr = se3ToVec(xerr_se3);
662 | Xerr(:,ii) = xerr;
663 | XerrSum = sum(Xerr,2);
664 | Ad_Terr = Adjoint(T_err);
665 |
666 | % now apply eqn. (13.37)
667 | V(:,ii) = Ad_Terr * Vd + obj.Kp * xerr + obj.Ki * XerrSum ...
668 | * delt_t;
669 | % the commanded end-effector-frame twist is thus
670 | % implemented as commanded_Twist = J^{dagger}_{e} * V
671 | commanded_Twist = pinv(obj.Je, 0.0090) * V(:,ii);
672 | obj.u = commanded_Twist(1:4);
673 | obj.theta_dot = commanded_Twist(5:9);
674 | commanded_Twist = [obj.u' obj.theta_dot'];
675 | config = [obj.q obj.theta' obj.wheelAngle];
676 |
677 | % Joint limit test
678 | Je_new = jointLimitCheck(obj, jointLimit, commanded_Twist, ...
679 | config, delt_t, speedLimit);
680 | commanded_Twist = pinv(Je_new,0.0090) * V(:,ii);
681 | obj.u = commanded_Twist(1:4);
682 | obj.theta_dot = commanded_Twist(5:9);
683 | commanded_Twist = [obj.u' obj.theta_dot'];
684 |
685 | obj = obj.nextState(config, commanded_Twist, delt_t, ...
686 | speedLimit);
687 | scene6simulation(ii+1,:) = [obj.q ...
688 | obj.theta' ...
689 | obj.wheelAngle ...
690 | NgripperStatus(ii)];
691 | end
692 | end
693 |
694 | function Je = jointLimitCheck(obj, jointLimit, speed, config, ...
695 | delt_t, speedLimit)
696 | obj_test = obj.nextState(config,speed,delt_t,speedLimit);
697 | No_joint = size(jointLimit,2);
698 | Je = obj.Je;
699 | theta_test = obj_test.theta';
700 | for i = 1 : No_joint
701 | if theta_test(i) > jointLimit(1,i)||theta_test(i) < jointLimit(2,i)
702 | Je(:,i+4) = zeros(6,1);
703 | end
704 | end
705 | end
706 |
707 | end
708 |
709 | end
710 | ```
711 |
712 | ***
713 |
714 |
21 | 
25 | 
29 | 
33 | 
83 | 
106 | 
127 | 
179 | 
185 | 
191 | 
197 | 
205 | 
213 | 
221 | 
230 | 
239 | 
245 | 
251 | 
260 | .jpg)
282 | 
329 | 
373 | 
401 | 
407 |