home *** CD-ROM | disk | FTP | other *** search
/ Between Heaven & Hell 2 / BetweenHeavenHell.cdr / 500 / 470 / rccl016 < prev    next >
Text File  |  1987-03-02  |  53KB  |  1,825 lines
  1. .ND
  2. .EQ
  3. delim $$
  4. .EN
  5. .nr H1 4
  6. .NH 1
  7. Task Description
  8. .PP
  9. Describing a task consist of specifying positions to
  10. be reached in space and motions to these positions.
  11. RCCL implements structured positions descriptions, and asynchronous
  12. motion requests.
  13. .NH 2
  14. Position Equations
  15. .PP
  16. Position equations do not necessitate the use of absolute
  17. reference coordinates.
  18. Position equations are one representation of the more general concept
  19. of transformation graphs.
  20. The position relationships of the frames $F sub {i~i=1,n}$
  21. can be expressed
  22. in terms of transformations products.
  23. Let a transformation $T sub i$
  24. describe the position of the frame $F sub i+1$ relative
  25. to the frame $F sub i$ with
  26. $T sub n$ describing the transformation from frame $F sub n$ to $F sub 1$,
  27. we have :
  28. .EQ
  29.         T sub 1~T sub 2~...~T sub n~=~Idendity
  30. .EN
  31. A closed path of transformations from frame $F sub 1$
  32. to frame $F sub 1$, via the frames $F sub {i~i=2,n}$
  33. describes the position of $F sub 1$ with respect to
  34. itself : the identity transform.
  35. The situation is depicted by a directed closed graph :
  36. .br
  37. .cs R 23
  38. .DS L
  39.                 T1      T2
  40.         ---> F1 ---> F2 ---> F3 ....... Fn
  41.        |                                  |
  42.         ----------------------------------
  43.                 Tn
  44. .DE
  45. .cs R
  46. where the vertices are frames and the arcs transforms.
  47. .PP
  48. Given a set of frames, containing two frames $A$ and $B$,
  49. we can certainly find more than
  50. one path connecting $A$ to $B$.
  51. Let the frames on one path be called $F sub {i~i=1,n}$, and
  52. the the frames on the other path be called $G sub {i~i=1,m}$, we obtain :
  53. .br
  54. .cs R 23
  55. .DS L
  56.         T0      T1      T2                 Tn
  57.         ---> F1 ---> F2 ---> F3 ....... Fn --->
  58.       A                                         B
  59.         ---> G1 ---> G2 ---> G3 ....... Gm --->
  60.         R0      R1      R2                 Rm
  61. .DE
  62. .cs R
  63. The corresponding transformation equation is :
  64. .EQ
  65. T sub 0~T sub 1~T sub 2~...~T sub n~=~R sub 0~R sub 2~...~R sub m
  66. .EN
  67. Closed transformation graphs can be expressed in terms of a set
  68. of transformation equations.
  69. Transformation graphs can be generalized, but we will restrict them
  70. to the form above.
  71. .PP
  72. RCCL uses transformations equations in order
  73. to describe the positions the manipulator has to reach.
  74. We will first introduce the dedicated transform $T6$.
  75. We are dealing with manipulators having six links and six joints,
  76. labeled from 1 to 6.
  77. The base of the manipulator is labeled link 0.
  78. Each of the manipulator links is assigned a frame $A sub i$
  79. describing its position with respect to the previous one as a function of
  80. the joint variable.
  81. The position of link 1 is described with respect to the base.
  82. The transformation product :
  83. .EQ
  84. ~T6~=~A sub 1~...~A sub 6
  85. .EN
  86. describes the position of the last link with respect to the base.
  87. Note that for the manipulators we are dealing with,
  88. it is convenient to assign the last three frames
  89. at the intersection of the three last joint axes.
  90. Therefore, $T6$ does not take into account the end effector description.
  91. .KS
  92. By convention the following transform decompositions are given :
  93. .EQ
  94. ~T1~=~A1
  95. .EN
  96. .EQ
  97. ~T2~=~A1~A2
  98. .EN
  99. .EQ
  100. ~T3~=~A1~A2~A3
  101. .EN
  102. .EQ
  103. ~T4~=~A1~A2~A3~A4
  104. .EN
  105. .EQ
  106. ~T5~=~A1~A2~A3~A4~A5
  107. .EN
  108. .EQ
  109. ~T6~=~A1~A2~A3~A4~A5~A6
  110. .EN
  111. .EQ
  112. ~U6~=~A6
  113. .EN
  114. .EQ
  115. ~U5~=~A5~A6
  116. .EN
  117. .EQ
  118. ~U4~=~A4~A5~A6
  119. .EN
  120. .EQ
  121. ~U3~=~A3~A4~A5~A6
  122. .EN
  123. .EQ
  124. ~U2~=~A2~A3~A4~A5~A6
  125. .EN
  126. .EQ
  127. ~U1~=~A1~A2~A3~A4~A5~A6
  128. .EN
  129. .EQ
  130. ~T6~=~T5~U6~=~T4~U5~=~T3~U4~=~T2~U3~=T1~U2
  131. .EN
  132. .KE
  133. .PP
  134. Let us set up a position equation that
  135. structurely describes the situation when the manipulator is to grasp an object
  136. lying on a table.
  137. We first need to assign frames to each of the elements involved :
  138. .KS
  139. .IP -
  140. A frame is assigned to the shoulder of the manipulator : $S$.
  141. .IP -
  142. A frame is located at the last link of the manipulator : $M$.
  143. .IP -
  144. A tool is attached to the link 6, the frame $T$ is assigned
  145. to the working end of tool.
  146. .IP -
  147. A frame $W$ describes the position of the working table.
  148. .IP -
  149. The position of an object lying on the table is described by $O$.
  150. .IP -
  151. A grasp position is described by the frame $G$.
  152. .KE
  153. .PP
  154. Suppose that the manipulator is moving such as to grasp the object,
  155. the corresponding graph is :
  156. .br
  157. .cs R 23
  158. .DS L
  159.          ------ M ------ T ------
  160.         |                        |
  161.         S                        G
  162.         |                        |
  163.          ------ W ------ O ------
  164. .DE
  165. .cs R
  166. In order to turn this graph into a transform equation, we first
  167. need to orient the arcs and label them with transforms.
  168. The choice is arbitrary but a convenient possibility is :
  169. .br
  170. .cs R 23
  171. .DS L
  172.         T6        TOOL      DRIVE
  173.          -----> M -----> T <-----
  174.         |                        |
  175.         S                        G
  176.         |                        |
  177.          <----- W -----> O ----->
  178.         BASE       OBJ      GRASP
  179. .DE
  180. .cs R
  181. where $TOOL$, $BASE$, $GRASP$, $OBJ$ are predetermined transforms.
  182. The transform $T6$ is to be changed such that the transform $DRIVE$
  183. comes to identity for the manipulator to reach the desired position,
  184. or in other words, such as the frames $T$ and $G$ become identical.
  185. The way the $DRIVE$ transform changes (and therefore $T6$)
  186. as a function of time
  187. determines the way the frame $M$ and $T$ move with respect to $S$,
  188. $W$, $O$, or $G$
  189. (Note that no absolute coordinate system is involved and we could
  190. say that $S$, $W$, $O$, and $G$ move with respect to $M$ and $T$).
  191. .PP
  192. The equivalent equation of the position can be written :
  193. .EQ
  194. BASE~T6~TOOL~=~OBJ~GRASP~DRIVE
  195. .EN
  196. The equation of the final desired position can be written :
  197. .EQ
  198. BASE~T6~TOOL~=~OBJ~GRASP
  199. .EN
  200. Transformations equations can be rewritten, solved for any of the terms,
  201. or replaced by equivalent ones.
  202. For example, we have :
  203. .EQ
  204. BASE~T6~=~OBJ~GRASP~TOOL sup -1
  205. .EN
  206. .EQ
  207. T6~=~BASE sup -1~OBJ~GRASP~TOOL sup -1
  208. .EN
  209. .EQ
  210. T6~=~COORD~POS~TOOL sup -1~~~~if~ COORD~=~BASE sup -1~,POS~=~OBJ~GRASP
  211. .EN
  212. Etc....
  213. .PP
  214. The RCCL function
  215. .B makeposition
  216. permits the user to set up such a position equation.
  217. The
  218. .I setpoint
  219. process will automatically compute the terms of
  220. the $DRIVE$ transform such that the resulting motion possess certain
  221. properties.
  222. The
  223. .B makeposition
  224. function expects a variable number of arguments.
  225. They represent the left hand side of the equation, the right hand side,
  226. and a transform that will tell which frame is to be
  227. considered as the
  228. .B tool
  229. frame, the frame $T$ in the example above.
  230. Assume that the transforms, $BASE$, $TOOL$, $OBJ$, and $GRASP$
  231. have been created via RCCL calls, and that we have the respective
  232. transforms pointer available : $base$, $tool$, $obj$, and $grasp$.
  233. The `C' definition of the function
  234. .B makeposition
  235. is :
  236. .br
  237. .cs R 23
  238. .DS L
  239.         POS_PTR makeposition(n, lhs [, lhs] ..., EQ, rhs, [, rhs] ..., TL, tl)
  240.         char *n;
  241.         TRSF_PTR lhs ..., rhs ..., tl;
  242. .DE
  243. .cs R
  244. and the call corresponding to the above example is :
  245. .br
  246. .cs R 23
  247. .DS L
  248.         POS_PTR p;      /* a position pointer */
  249.  
  250.         p = makeposition("P", base, t6, tool, EQ, obj, grasp, TL, tool);
  251. .DE
  252. .cs R
  253. The names `EQ' and `TL' are predefined constants.
  254. The function
  255. .B makeposition
  256. returns a pointer to a ring data structure implementing the
  257. transform graph.
  258. The first argument is a string, the name of the position.
  259. The transform pointer
  260. .B t6,
  261. is built in RCCL, and predeclared in
  262. .B rccl.h.
  263. As the position data structure is built,
  264. .B makeposition
  265. calls the function
  266. .B optimize
  267. in order to premultiply all possible pairs of constant
  268. transformation (declared as
  269. .B const
  270. ), in order to decrease the run time computing load.
  271. The function
  272. .B optimize
  273. will internally replace the user specified position equation
  274. by an equivalent canonical form :
  275. .EQ
  276. T6~=~COORD~POS~TOOL
  277. .EN
  278. The terms $COORD$ or $TOOL$ of this canonical form can be missing.
  279. The calls :
  280. .br
  281. .cs R 23
  282. .DS L
  283.         makeposition("P1", t6, EQ, h, TL, t6);
  284.         makeposition("P2", t6, t, EQ, h, TL, t);
  285.         makeposition("P3", t6, t, EQ, h, g, TL, t6);
  286. .DE
  287. .cs R
  288. lead to the following canonical equations :
  289. .KS
  290. .EQ L
  291. T6~=~POS~~~~COORD~=~None~,~~POS~=~H~,~~TOOL~=~None
  292. .EN
  293. .EQ L
  294. T6~=~POS~TOOL~~~~COORD~=~None,~~POS~=~H~,~~TOOL~=~T sup -1
  295. .EN
  296. .EQ L
  297. T6~=~COORD~POS~~~~COORD~=~H~G~,~~POS~=~T~sup -1~,~TOOL~=~None
  298. .EN
  299. .KE
  300. .PP
  301. There is an arbitrary number of
  302. argument transform pointers for
  303. .B makeposition.
  304. The only restriction is that the left hand side of the equation
  305. must contain the predeclared pointer
  306. .B t6
  307. and the right hand side must contain at least one transform.
  308. The transforms can arbitrary belong to one of the following categories :
  309.  
  310. .IP "const : "
  311. A transform of this type will be considered as constant through out
  312. the life of the corresponding position equation.
  313. Its value must not be changed, as the system can decided to
  314. premultiply it with another transform such as it may not appear in the
  315. internal equation used for the trajectory calculation.
  316. This is the default type of the functions of the style
  317. gentr_... and it is the type that one should use when possible.
  318.  
  319. .IP "varb : "
  320. A transform of this type will not be premultiplied by the optimization
  321. function and its value will be used directly during the trajectory calculation.
  322. One sometimes need to change the value of a transform after the
  323. equation has been set up.
  324. If the change occurs while the equation is evaluated, the
  325. change will instantaneously be reflected in the manipulator's trajectory.
  326. This can cause jerky motions if the change is large and it should
  327. be carefully used.
  328. The function
  329. .B update
  330. described later on,
  331. knows when to change the value of the transform when it is safe.
  332.  
  333. .IP "hold : "
  334. A transform of this type is not directly used in the position equations,
  335. but a copy of it.
  336. We will see that
  337. .B move
  338. requests are asynchronously issued and that a number of them
  339. can ve specified ahead of time.
  340. A
  341. .B hold
  342. transform belongs to the subsequent motion request and its value
  343. is taken into account only when the corresponding
  344. motion is actually performed.
  345.  
  346. .IP
  347. The last category is the class of the functionally defined transforms.
  348. These transforms are attached to a function belonging to the user's
  349. manipulator program.
  350. The function is expected to compute the values of the transform
  351. as the corresponding motion is performed.
  352. The function is executed at interrupt level and therefore, is expected
  353. to have a reasonable execution time.
  354. As described in [6], these functions
  355. .B cannot
  356. perform
  357. .B any
  358. type of
  359. system calls, (prints, reads, etc...).
  360. If the function computes the values of the transform as a function
  361. of external data, one can obtain tracking.
  362. If the values are computed as a function of time, one obtains
  363. functionally defined trajectories.
  364. We shall call such a function a background function.
  365.  
  366. .PP
  367. The type of a transform is indicated in the `fn' field of
  368. the transform structure, a few examples :
  369. .KS
  370. .br
  371. .cs R 23
  372. .DS L
  373.         int myfunction();
  374.  
  375.         t1 = gentr_trsl("T1", 0., 0., 0.);
  376.  
  377.         t2 = newtrans("T2", const);
  378.  
  379.         t3 = newtrans("T3", varb);
  380.  
  381.         t4 = gentr_trsl("T4", 0., 0., 0., );
  382.         t4->fn = hold;
  383.  
  384.         t5 = gentr_rot("T5", 0., 0., 0., zunit, 90.);
  385.         t5->fn = myfunction;
  386.  
  387.         t6 = newtrans("T6", myfunction);
  388.         Rot(t6, zunit, 90.);
  389. .DE
  390. .cs R
  391. .IP t1
  392. is a regular
  393. .B const
  394. transform initialized to the unit value.
  395. .IP t2
  396. is a regular
  397. .B const
  398. transform initialized to the unit value.
  399. .IP t3
  400. is a transform initialized as a unit transform of type
  401. .B varb.
  402. .IP t4
  403. is first created as
  404. .B const
  405. but is turned into a
  406. .B hold
  407. transform.
  408. .IP t5
  409. is first created as
  410. .B const
  411. but is turned into a functionally defined transform attached to
  412. the function `myfunction' and is initialized as a rotation of
  413. 90 degrees around the Z axis.
  414. .IP t6
  415. is created as functionally defined, and then initialized  as a rotation of
  416. 90 degrees around the Z axis.
  417. .KE
  418. .PP
  419. The
  420. .B makeposition
  421. function implements a restricted case of transformations graphs.
  422. This limitation may be removed one day.
  423. When multibranch transform graphs are required, the user
  424. must implement it in terms of the basic graph described above
  425. and a combination of other RCCL function like
  426. .B
  427. Rot, Trslm, Trmult, Invert,
  428. .R
  429. and so on.
  430. The
  431. .B varb
  432. and
  433. .B hold
  434. features are then very important.
  435. .PP
  436. It is now time to introduce the position structure as described
  437. in the file
  438. .B rccl.h.
  439. .br
  440. .cs R 23
  441. .DS L
  442. typedef struct posit {
  443.                 char *name;
  444.                 int code;
  445.                 real scal;
  446.                 event end;
  447. } POS, *POS_PTR;
  448. .DE
  449. .cs R
  450. The first entry in the structure is the name of the position.
  451. The same remarks can be made as for the transforms structures.
  452. The name is not directly relevant from the robot control point of view,
  453. but may help in debugging.
  454. The second entry `code' is a termination code for the corresponding
  455. motion.
  456. Internal code values known by RCCL are currently :
  457. .br
  458. .cs R 23
  459. .DS L
  460. #define OK      -1
  461. #define LIMIT   -2
  462. #define ONF     -3
  463. #define OND     -4
  464. .DE
  465. .cs R
  466. After the position has been reached, the code is set
  467. by the system to the value `OK' if the motion has not be interrupted
  468. by some condition.
  469. The value `LIMIT' means that the motion caused some joints
  470. to dangerously approach their physical stops.
  471. RCCL automatically issue a stop to the current position.
  472. It is then possible to recover from this error condition as explained
  473. later.
  474. The code `ONF' means that a prespecified force condition
  475. occurred, and the code `OND' that prespecified differential
  476. motion condition occurred.
  477. The next entry is a floating point number `scal'.
  478. The value of the field `scal' varies from 0 to 1 as
  479. the motion is performed, and is useful for generating
  480. functionally defined motions or to trigger some action
  481. at a given point of a trajectory.
  482. The entry `end' is classified as an
  483. .B event.
  484. It allows the user to synchronize the program flow with the execution
  485. of trajectories.
  486. The use and the function of the fields `code', `scal', and `end'
  487. will be explained in more detail as we go on.
  488. .PP
  489. When a position is no longer needed, memory space can be retuned to
  490. the memory pool by :
  491. .br
  492. .cs R 23
  493. .DS L
  494.         freepos(p)
  495.         POS_PTR p;
  496. .DE
  497. .cs R
  498. Care must be taken so that the corresponding data in no longer in use.
  499. Transforms involved in the corresponding equation are not freed,
  500. an must be individually freed using
  501. .B freetrans.
  502. .NH 2
  503. Motion Description
  504. .PP
  505. RCCL implements two basic types of motions known as
  506. .I joint
  507. mode and
  508. .I Cartesian
  509. mode.
  510. The first one consist of solving for $T6$ the position equation
  511. of the goal position at the beginning of the motion
  512. and obtaining for it the corresponding set of joint values.
  513. The trajectory is then generated by linear interpolation
  514. of the joint values from the current position to the
  515. goal position.
  516. This type of motion should be used for large motions
  517. as it requires the minimum joint motions
  518. and less computations.
  519. High velocities can be obtained, however trajectories
  520. are not always easily predictable.
  521. The
  522. .I
  523. Cartesian mode
  524. .R
  525. makes use the $DRIVE$ transform to produce straight line
  526. trajectories for the
  527. .I tool
  528. frame.
  529. The transform equation is evaluated for $T6$ each
  530. sample time interval and the set of joint values obtained.
  531. This type of motion permits us to obtain well predictable trajectories.
  532. If the position equation contains functionally defined
  533. transforms, the associated functions are also evaluated at
  534. sample time intervals.
  535. The values resulting from these evaluations will directly
  536. influence the arm trajectory.
  537. In that case the structure of the position equation must be
  538. carefully considered.
  539. .NH 3
  540. The Basic Move Statement
  541. .PP
  542. The basic function definition is :
  543. .br
  544. .cs R 23
  545. .DS L
  546.         move(p)
  547.         POS_PTR p;
  548. .DE
  549. .cs R
  550. The call :
  551. .br
  552. .cs R 23
  553. .DS L
  554.         move(pos);
  555. .DE
  556. .cs R
  557. where `pos' is a pointer to a position equation returned by
  558. .B makeposition.
  559. instructs the system to move the arm
  560. toward the described position such as the equation becomes verified.
  561. When the arm is moving from position to position, transitions
  562. occur between each path segment.
  563. It is important to smooth out the velocity discontinuities that
  564. would be caused by an abrupt change of direction and velocity
  565. from one path segment to another.
  566. There are a number of options and parameters that can be set globally
  567. or for each path segment.
  568. .NH 3
  569. Setting Options and Parameters
  570. .PP
  571. The first group of parameters remains set until set to another value.
  572. The following calls cause a setting of the parameter starting
  573. at the next
  574. .B move
  575. request :
  576. .br
  577. .cs R 23
  578. .DS L
  579.         setvel(tv, rv)
  580.         int tv, rv;
  581.  
  582.         setmod(m)
  583.         int m;
  584.  
  585.         setconf(c)
  586.         char *c;
  587.  
  588.         sample(s)
  589.         int s;
  590. .DE
  591. .cs R
  592. .PP
  593. The
  594. .B setvel
  595. call takes two integer arguments.
  596. The first is the desired translational velocity of the
  597. .I tool
  598. frame in millimeters per second, the second one is
  599. the rotational velocity in degrees per second.
  600. The system will calculate the path segment durations to
  601. obtain the desired velocities.
  602. Since rotational and translational velocities lead to different
  603. durations, the system will pick which ever is the longest.
  604. One can give the priority to one or another by specifying very
  605. different values.
  606. For example, suppose that a motion involves a 30 millimeters
  607. translation and a 30 degrees rotation, the call
  608. .br
  609. .cs R 23
  610. .DS L
  611.         setvel(30, 300);
  612. .DE
  613. .cs R
  614. will result in a 1 second motion due to the translation, and not
  615. an unreasonable 1/10 of a second motion to perform the rotation.
  616. The function
  617. .B setmod
  618. defines the mode,
  619. .I Cartesian
  620. or
  621. .I joint,
  622. for the subsequent motions.
  623. The argument must be the character `c' for
  624. .I Cartesian
  625. mode, and `j' for
  626. .I joint
  627. mode.
  628. For example :
  629. .br
  630. .cs R 23
  631. .DS L
  632.         POS_PTR p, p1, p2;
  633.         int i, m;
  634.  
  635.         p1 = makeposition(...);
  636.         p2 = makeposition(...);
  637.  
  638.         for (move(p2), i = 0; i < 10; ++i) {
  639.                 if (i % 2 != 0) {
  640.                         m = 'c';
  641.                         p = p1;
  642.  
  643.                 } else {
  644.                         m = 'j';
  645.                         p = p2;
  646.                 }
  647.                 setmod(m);
  648.                 move(p);
  649.         }
  650. .DE
  651. .cs R
  652. will cause the arm to move from p2 to p1 (i odd) in
  653. .I Cartesian
  654. mode and from p1 to p2 (i even) in
  655. .I Joint
  656. mode.
  657. For C experts a more concise version could be :
  658. .br
  659. .cs R 23
  660. .DS L
  661.         for (move(p2), i = 10; i--;) {
  662.                 setmod((m = i % 2) ? 'c' : 'j');
  663.                 move(m ? p1 : p2);
  664.         }
  665. .DE
  666. .cs R
  667. In
  668. .I joint
  669. mode the segment durations are computed based on the distances between
  670. the frame $T6$ at each end of the segments, since this type of
  671. motion is joint oriented.
  672. The function
  673. .B setconf
  674. permits us to obtain an arm configuration change during the subsequent
  675. motion.
  676. This motion
  677. .B has
  678. to be performed in
  679. .I joint
  680. mode, since a configuration change always involves a degenerate arm
  681. configuration unreachable in
  682. .I Cartesian
  683. mode.
  684. Once the configuration change is obtained, the motions can again
  685. be performed in
  686. .I Cartesian
  687. mode.
  688. For the PUMA arm, the configurations can be : shoulder righty - lefty (r/l);
  689. elbow down - up (d/u); wrist flip - noflip (f/n).
  690. The argument is a string specifying the configuration change.
  691. For example, if the arm is lefty, up, noflip (``lun''),
  692. in order to obtain a wrist configuration change to flip, the arm
  693. remaining lefty and up (``luf''), we code :
  694. .br
  695. .cs R 23
  696. .DS L
  697.         /* the arm is currently "lun" */
  698.  
  699.         setmod('j');    /* go in joint mode if it was'nt */
  700.         setconf("f");   /* specify flip */
  701.         move(new);      /* go "luf" */
  702. .DE
  703. .cs R
  704. Note that several letters can be specified in the string argument.
  705. A program with many
  706. configuration changes is safely terminated by :
  707. .br
  708. .cs R 23
  709. .DS L
  710.         setconf("lun");
  711.         move(park);
  712. .DE
  713. .cs R
  714. .PP
  715. The function
  716. .B sample
  717. allows us to change the sampling period of the whole system.
  718. Currently valid sample rates are: 14, 28, 56, and 112 milliseconds.
  719. However the function rounds down the specified value as :
  720. sample(15) leads to 14, sample(30) leads to 28, etc.
  721. The default value is 28 milliseconds which is a good compromise
  722. for most applications. The 14 millisecond rate is helpful for tracking
  723. applications, but it is good practice to reset the rate to 28 or 56 when
  724. not needed.
  725. .PP
  726. The next group of functions cause the parameter to be taken into
  727. account for the subsequent
  728. .I move
  729. request
  730. only.
  731. .br
  732. .cs R 23
  733. .DS L
  734.         setime(ta, ts)
  735.         int ta, ts;
  736.  
  737.         evalfn(fn)
  738.         int (* fn)();
  739.  
  740.         distance(s, v[, v] ...)
  741.         char *s, real v;
  742. .DE
  743. .cs R
  744. A call to
  745. .B setime
  746. allows us to force the motion to be performed within a given period of time.
  747. The first argument specifies the duration of the transition time at the end
  748. of the segment, and the second argument the duration of the segment itself.
  749. Times are specifies in milliseconds.
  750. Besides the cases when motion duration is the primary factor,
  751. this call serves two purposes.
  752. At the present time no call has been implemented to
  753. force a rate at the joint level.
  754. The consequence is that the system is unable
  755. to compute the correct segment duration
  756. to perform a configuration change, since the same position can sometimes
  757. be reached in different configurations.
  758. A duration calculation based on distances is in this case meaningless.
  759. Therefore, the user must explicitly specify the motion duration.
  760. For this reason a macro has been included in
  761. .B rccl.h :
  762. .br
  763. .cs R 23
  764. .DS L
  765. #define moveconf(p, ta, ts, cf)  \\\\
  766.                 {setconf(cf); setmod('j'); setime(ta, ts); move(p);}
  767. .DE
  768. .cs R
  769. The example above can be more conveniently coded as :
  770. .br
  771. .cs R 23
  772. .DS L
  773.         moveconf(new, 100, 700, "f");
  774. .DE
  775. .cs R
  776. The configuration change will be performed in 7/10 of a seconds with
  777. a 1/10 of a second transition time.
  778. The function
  779. .B setime
  780. is also very useful for functionally defined motions.
  781. When a position equation includes functionally defined transforms,
  782. there are situations when the system cannot compute the correct segment
  783. duration based on the distance of the goal position because it
  784. can depend on arbitrary factors.
  785. Likewise a macro has been added :
  786. .br
  787. .cs R 23
  788. .DS L
  789. #define movecart(p, ta, ts)     {setmod('c'); setime(ta, ts); move(p);}
  790. .DE
  791. .cs R
  792. The code would be :
  793. .br
  794. .cs R 23
  795. .DS L
  796.         movecart(spiral, 300, 2000);
  797. .DE
  798. .cs R
  799. perform a spiraling motion during 2 seconds with a 3/10 of a second
  800. escape transition.
  801. In the cases when the segment duration calculation is left to the system
  802. but the acceleration time still needs to be explicitly specified, the
  803. call :
  804. .br
  805. .cs R 23
  806. .DS L
  807.         setime(200, 0);
  808. .DE
  809. .cs R
  810. forces the acceleration time to be 2/10 of a seconds but the segment duration,
  811. being left unspecified, will be computed by the system.
  812. On the other hand, it is sometimes necessary to specify a zero acceleration
  813. time, meaning that no transition is desired.
  814. This is useful for some slow motions terminated on condition and when the
  815. reaction time is of primary importance.
  816. The acceleration time can be specified as zero :
  817. .br
  818. .cs R 23
  819. .DS L
  820.         setime(0, 1000);
  821. .DE
  822. .cs R
  823. .PP
  824. The function
  825. .B evalfn
  826. cause the function argument to be evaluated during the
  827. execution of the corresponding motion.
  828. One thus can code any needed synchronous processing.
  829. The first application is to perform some monitoring of external
  830. condition in order to interrupt a motion.
  831. For example, a flag called
  832. .B nextmove
  833. , causes at any moment the current path segment to terminate and
  834. the manipulator to transition to the next.
  835. Other applications can be to trigger some action at a precise
  836. point of a trajectory.
  837. For this the field `scal' of a position structure can be used.
  838. The user's  function given as argument is executed at sample time
  839. and therefore bears the same restrictions as the background functions of
  840. functionally defined transforms : short processing, no read, prints and so on.
  841. This type of function will also be called a background function.
  842. .PP
  843. The function
  844. .B distance
  845. specifies a distance modifier for the goal positions.
  846. Modifications are expressed in the
  847. .I tool
  848. frame.
  849. The first argument is a string specifying the directions.
  850. Each direction is expressed with two letters.
  851. The first letter can be either `d' or `r', standing for `distance'
  852. and `rotation'.
  853. The second letter can be either `x', `y', or `z', standing for the
  854. principal axes of the tool frame.
  855. A valid directions specification is :
  856. .br
  857. .cs R 23
  858. .DS L
  859. "dx rz" :  translation along X, rotation around X
  860. .DE
  861. .cs R
  862. The remaining arguments are the magnitudes of the modifications.
  863. For example :
  864. .br
  865. .cs R 23
  866. .DS L
  867.         distance("dz", -30.);
  868.         move(p);
  869.         move(p);
  870. .DE
  871. .cs R
  872. implements a `approach' style motion in the `Z' direction of the tool.
  873. Modifications are obtained by internally multiplying the $POS$ part
  874. of the canonical transformation equation by a modification transform.
  875. Any combination of directions can be specified, however magnitudes
  876. should remain small for rotations.
  877. The function distance is also very usefull for motions terminated on condition
  878. to purposely specify an overshooting motion.
  879. .PP
  880. When a stop is needed the call :
  881. .br
  882. .cs R 23
  883. .DS L
  884.         stop(n);
  885. .DE
  886. .cs R
  887. where `n' is a duration in milliseconds repeats the last move statement
  888. with all it's attributes, except the time attributes.
  889. For example :
  890. .br
  891. .cs R 23
  892. .DS L
  893.         evalfn(myfunction);
  894.         distance("dx ry", 10., 3.);
  895.         move(p);
  896.         evalfn(myfunction);
  897.         distance("dx ry", 10., 3.);
  898.         setime(200, 2000);
  899.         move(p);
  900. .DE
  901. .cs R
  902. is more conveniently written as :
  903. .br
  904. .cs R 23
  905. .DS L
  906.         evalfn(myfunction);
  907.         distance("dx ry", 10., 3.);
  908.         move(p);
  909.         stop(2000);
  910. .DE
  911. .cs R
  912. .NH 2
  913. Synchronization
  914. .PP
  915. A more delicate programming of the time aspect is
  916. the price paid for the gain in flexibility obtained by the
  917. motion queue feature.
  918. Synchronization is basically achieved by `suspending' the
  919. execution of the user's program while motion requests are performed.
  920. This is can be done in two ways or by a combinations of both.
  921. The program execution is suspended when spending time
  922. performing computations and input output.
  923. Suppose a program that interacts with a user or with some
  924. long response sensor, we obtain the following pattern :
  925. .br
  926. .cs R 23
  927. .DS L
  928.         TRSF_PTR z, e , b;
  929.         POS_PTR  p;
  930.         double iz;
  931.  
  932.         z = gentr_trsl("Z",  0.,  0., 864.);
  933.         e = gentr_trsl("E" , 0. , 0. , 170.);
  934.         b = gentr_rot("B", 600. , 128., 800., yunit, 180.);
  935.         b->fn = hold;
  936.  
  937.         p = makeposition("P" , z, t6, e, EQ, b, TL, e);
  938.  
  939.         for (; ; ) {
  940.                 printf("enter Z increment ");
  941.                 scanf("%f", &iz);
  942.                 b->p.z += iz;
  943.                 move(p);
  944.         }
  945. .DE
  946. .cs R
  947. Each time the user enters data via `scanf', the value of
  948. the $B$ transform is changed, since its type is
  949. .I hold
  950. the new value is entered in the motion queue as well as the motion
  951. request itself and the next loop immediately prompts the user
  952. for a new data entry.
  953. If the user enters data quicker than the manipulator
  954. can move to the goal positions,
  955. she or he will be able to enter several requests ahead.
  956. If the user stops entering data, the requests will eventually be served,
  957. the manipulator brought to rest and the program
  958. execution suspended at the `scanf' instruction.
  959. If the data is provided by some external device, say another computer,
  960. a file ,or a sensing device the program will look like :
  961. .br
  962. .cs R 23
  963. .DS L
  964.         for (; ; ) {
  965.                 gettr(b, device);
  966.                 move(p);
  967.         }
  968. .DE
  969. .cs R
  970. where `gettr' returns a new transform value.
  971. The data is obtained asynchronously with respect to the motions,
  972. consequently two situations can occur.
  973. Either the external device is faster and the queue will fill up,
  974. either the arm is faster and it will wait for new data.
  975. In both cases we obtain an optimal utilization of resources.
  976. The only problem is to prevent the queue from becoming saturated.
  977. The external variable
  978. .B requestnb
  979. is maintained by RCCL as the number of non served motion requests.
  980. We can now introduce the primitive
  981. .B waitas
  982. (a macro) that takes as argument a predicate :
  983. .br
  984. .cs R 23
  985. .DS L
  986.         waitas(pred)
  987. .DE
  988. .cs R
  989. The macro
  990. .B waitas
  991. suspends the programs execution until the predicate becomes true.
  992. The final version of the loop is :
  993. .br
  994. .cs R 23
  995. .DS L
  996.         for (; ; ) {
  997.                 gettr(b, device);
  998.                 waitas(nbrequest < MAX)
  999.                 move(p);
  1000.         }
  1001. .DE
  1002. .cs R
  1003. .PP
  1004. The primitive
  1005. .B waitfor
  1006. (a macro) suspends program execution until occurrence of an
  1007. .I event.
  1008. We have seen that the `end'
  1009. .B event
  1010. is associated with each position record.
  1011. One application is to obtain a gripper opening and closing
  1012. at given moments.
  1013. The pattern of code :
  1014. .br
  1015. .cs R 23
  1016. .DS L
  1017.         distance("dz", -30.);
  1018.         move(p);
  1019.         move(p);
  1020.         distance("dz", -30.);
  1021.         move(p);
  1022. .DE
  1023. .cs R
  1024. implements a possible position `approach, reach, and depart' sequence.
  1025. To obtain a synchronized gripper opening and closing, the pattern is :
  1026. .br
  1027. .cs R 23
  1028. .DS L
  1029.         distance("dz", -30.);
  1030.         move(p);
  1031.         move(p);
  1032.         distance("dz", -30.);
  1033.         move(p);
  1034.  
  1035.         waitfor(p->end)
  1036.         OPEN;
  1037.         waitfor(p->end)
  1038.         CLOSE;
  1039. .DE
  1040. .cs R
  1041. The program is first suspended until, the `approach' position is
  1042. reached, opens the gripper, waits for the position to be reached,
  1043. and closes the gripper.
  1044. One other application of
  1045. .B waitfor
  1046. is to obtain a suspension of the program until all the requests
  1047. are served.
  1048. For example suppose that a function allocates positions
  1049. and transforms that have to be freed of upon termination of the
  1050. function, we must make sure that all the requests are executed before
  1051. doing so :
  1052. .br
  1053. .cs R 23
  1054. .DS L
  1055. dothat()
  1056. {
  1057.         TRSF_PTR t1 = gentr_ ...
  1058.         TRSF_PTR t2 = gentr_ ...
  1059.  
  1060.         POS_PTR p1 = makeposition( ...
  1061.         POS_PTR p2 = makeposition( ...
  1062.  
  1063.         move(p1);
  1064.         move(p2);
  1065.         ...
  1066.  
  1067.         move(there);
  1068.         waitfor(completed);
  1069.         freetrans(t1);
  1070.         freetrans(t2);
  1071.         ...
  1072.         freepos(p1);
  1073.         freepos(p2);
  1074.         ...
  1075.         return;
  1076. }
  1077. .DE
  1078. .cs R
  1079. The following program makes use of the
  1080. .B there
  1081. position that always reflects the position that the manipulator is
  1082. occupying at the end of the previous path segment.
  1083. Thus, when the `waitfor' statement is issued, we are sure that
  1084. all the previous requests are served and the corresponding
  1085. positions are no longer in use.
  1086. Note that the statement :
  1087. .br
  1088. .cs R 23
  1089. .DS L
  1090.         waitas(requestnb == 0)
  1091. .DE
  1092. .cs R
  1093. would do equally well.
  1094. .PP
  1095. Another way to obtain synchronous actions is to trigger them
  1096. from a motion associated background function.
  1097. For example a gripper opening can be specified at a given point
  1098. of a trajectory.
  1099. We will make use of the external variable
  1100. .B goalpos,
  1101. which is an ordinary position pointer updated by the
  1102. system such as to point at the position equation currently being evaluated.
  1103. It can be used in the main program to decide
  1104. which position equation is currently being evaluated.
  1105. The background functions can also use the pointer
  1106. .B goalpos
  1107. to access the fields of a position structure, and the use of
  1108. several global position pointers can be avoided.
  1109. These function can then be written independently of a given motion
  1110. statement.
  1111. .br
  1112. .cs R 23
  1113. .DS L
  1114. pumatask()
  1115. {
  1116.         int openfn();
  1117.         ...
  1118.  
  1119.  
  1120.         evalfn(openfn);
  1121.         move(p);
  1122.  
  1123.         ...
  1124. }
  1125.  
  1126. openfn()
  1127. {
  1128.         if (goalpos->scal > .5) {
  1129.                 OPEN;
  1130.         }
  1131. }
  1132. .DE
  1133. .cs R
  1134. or using an event :
  1135. .br
  1136. .cs R 23
  1137. .DS L
  1138. event openit = 0;       /* global variable */
  1139.  
  1140. pumatask()
  1141. {
  1142.         int openfn();
  1143.         ...
  1144.  
  1145.         evalfn(openfn);
  1146.         move(p);
  1147.         waitfor(openit)
  1148.         OPEN;
  1149.         ...
  1150. }
  1151.  
  1152. openfn()
  1153. {
  1154.         if (goalpos->scal > .5 && openit > 0) {
  1155.                 --openit;
  1156.         }
  1157. }
  1158. .DE
  1159. .cs R
  1160. Yet another possibility would be :
  1161. .br
  1162. .cs R 23
  1163. .DS L
  1164. pumatask()
  1165. {
  1166.         ...
  1167.  
  1168.         move(p);
  1169.         waitas(goalpos == p && p->scal > .5)
  1170.         OPEN;
  1171.         ...
  1172. }
  1173. .DE
  1174. .cs R
  1175. .PP
  1176. According to the situation, different combinations of these
  1177. techniques can be used.
  1178. Libraries of customized functions or macros could be written to
  1179. best suit the requirements.
  1180. .PP
  1181. The `wait' style primitives have the property of
  1182. suspending the program execution until occurrence of an event or condition.
  1183. One must be aware that the following code pattern :
  1184. .br
  1185. .cs R 23
  1186. .DS L
  1187.         move(p0);
  1188.         waitfor(p0->end);
  1189.         move(p1);
  1190.         waitfor(p1->end);
  1191. or
  1192.         move(p0);
  1193.         waitfor(completed);
  1194.         move(p1);
  1195.         waitfor(completed);
  1196. .DE
  1197. .cs R
  1198. causes each position to be evaluated twice, since a new request
  1199. is entered into the queue only when the previous is completed.
  1200. At that time, the system finds the queue empty and reissues a move to
  1201. the last position.
  1202. .PP
  1203. In more detail we show the body of the macros
  1204. .B waitas
  1205. and
  1206. .B waitfor
  1207.  :
  1208. .br
  1209. .cs R 23
  1210. .DS L
  1211. #define waitas(predicate)    {while(!(predicate)) suspendfg();}
  1212.  
  1213. #define waitfor(event)       {++(event); while(event > 0) suspendfg();}
  1214. .DE
  1215. .cs R
  1216. The function
  1217. .B suspendfg
  1218. merely suspends the foreground program
  1219. or user's process during a short period of time (currently .1 second).
  1220. The `setpoint' process, running in background at high priority maintains
  1221. the
  1222. .I events
  1223. associated with positions and the
  1224. .I event
  1225. .B completed.
  1226. The code in the
  1227. .I setpoint
  1228. process looks like :
  1229. .br
  1230. .cs R 23
  1231. .DS L
  1232.         newm = dequeue(&mqueue);        /* try to get a new request     */
  1233.         if (newm == NULL) {             /* then none                    */
  1234.                 --completed;            /* signal queue is empty        */
  1235.         }
  1236. .DE
  1237. .cs R
  1238. .PP
  1239. Consider the following situation :
  1240. .br
  1241. .cs R 23
  1242. .DS L
  1243.         move(p0);
  1244.         move(p1);
  1245.  
  1246.         /* do a lot of computations and/or io */
  1247.         ...
  1248.  
  1249.         waitfor(p0->end);
  1250.  
  1251. .DE
  1252. .cs R
  1253. If the sequence of code between the move requests and the wait statement
  1254. takes more time to execute than the motion to be performed,
  1255. the task will not hang at the
  1256. level of the wait statement.
  1257. .PP
  1258. One additonal point has to be considered, suppose we have
  1259. the following situation :
  1260. .br
  1261. .cs R 23
  1262. .DS L
  1263.         move(p0);
  1264.         distance(...);
  1265.         move(p0);
  1266.         distance(...);
  1267.         move(p0);
  1268.         move(p0);
  1269.  
  1270.         waitfor(p0->end)
  1271.         /* do something */
  1272.         waitfor(p0->end)
  1273.         /* do another thing */
  1274. .DE
  1275. .cs R
  1276. In this sequence of code the `p0->end'
  1277. .I event
  1278. will occur four times, but will be waited for only two times.
  1279. If the sequence of code is in a loop, an unmatching number of
  1280. moves and corresponding waits will shift the synchronization
  1281. each loop by the difference.
  1282. One way to get around that is to reset the event count prior to issuing the
  1283. .I move
  1284. requests :
  1285. .br
  1286. .cs R 23
  1287. .DS L
  1288.         for (...) {
  1289.                 p0->end = 0;
  1290.                 move(p0);
  1291.                 distance(...);
  1292.                 move(p0);
  1293.                 distance(...);
  1294.                 move(p0);
  1295.                 move(p0);
  1296.  
  1297.                 waitfor(p0->end)
  1298.                 /* do something */
  1299.                 waitfor(p0->end)
  1300.                 /* do another thing */
  1301.         }
  1302. .DE
  1303. .cs R
  1304. .NH 2
  1305. Functionally Defined Motions
  1306. .PP
  1307. One of the principal features of RCCL is the provision for
  1308. functionally defined motions.
  1309. They are approached in very general terms.
  1310. Except the $POS$ transform of the canonical equation,
  1311. any of the transforms of a position equation can be functionally defined.
  1312. We will look in this section at transforms as functions of time.
  1313. Let us examine again the typical transformation graph of a
  1314. .I Cartesian
  1315. motion :
  1316. .br
  1317. .cs R 23
  1318. .DS L
  1319.         T6        TOOL      DRIVE
  1320.          -----> M -----> T <-----
  1321.         |                        |
  1322.         S                        G
  1323.         |                        |
  1324.          <----- W -----> O ----->
  1325.         BASE       OBJ      GRASP
  1326. .DE
  1327. .cs R
  1328. We notice that the transforms $T6$ and $DRIVE$ are themselves function
  1329. of time.
  1330. The system internally computes the values of the $DRIVE$ transform
  1331. such that the frame immediately on its left, $T$,  moves along straight
  1332. lines and rotates around fixed axes with respect to $S$, $W$, $O$, or $G$.
  1333. One might notice that the combination $T~ TOOL$ can be
  1334. considered as a single transform function of time such that :
  1335. .EQ
  1336. T6~TOOL~DRIVE sup -1 = CONSTANT
  1337. .EN
  1338. The $DRIVE$ transform can be broken down into a translational part and
  1339. a rotational part :
  1340. .EQ
  1341. DRIVE~=~D sub t~D sub r
  1342. .EN
  1343. The $D sub t$ transform determines the path of the
  1344. .I
  1345. center of rotation,
  1346. .R
  1347. while $D sub r$ determines the rotation itself, which is a motion that
  1348. one can easily visualize.
  1349. The decomposition
  1350. .EQ
  1351. DRIVE~=~D sub r~D
  1352. .EN
  1353. is also possible but the second transformation cannot be a pure translation
  1354. in the general case.
  1355. It is also more difficult to visualize, because
  1356. any change in the rotation part will also cause a change in the
  1357. final translational position.
  1358. Actually, this situation occurs for the transformation product $T6~TOOL$,
  1359. which
  1360. behaves symmetricly with respect to $DRIVE$.
  1361. This effect can be observed when
  1362. a manipulator equipped with a tool performs a pure rotation
  1363. around the tip of the tool :
  1364. the manipulator must perform translations and rotations
  1365. whereas pure translations
  1366. only require translations.
  1367. This must be kept in mind when introducing functionally defined transforms
  1368. in the position equations.
  1369. It is important to carefully determine the placement of the
  1370. center of rotation when laying
  1371. out a functionally defined position equation.
  1372. .PP
  1373. For simplification we shall assume that the goal position has been reached
  1374. so that the $DRIVE$ transform is reduced to unity.
  1375. We shall also only keep two frames, the world frame $W$,
  1376. and the tool frame $T$.
  1377. Let $T(t)$ and $R(t)$ two transforms, pure translation and pure rotation
  1378. function of time.
  1379. The four graphs leading to pure translations and pure
  1380. rotations in
  1381. .I world
  1382. or base coordinates and then in
  1383. .I tool
  1384. coordinates are :
  1385. .IP
  1386. Pure translation in
  1387. .B world
  1388. coordinates :
  1389. .br
  1390. .cs R 23
  1391. .DS L
  1392.          T6      TOOL
  1393.          -----> ----->
  1394.         |             |
  1395.         W             T
  1396.         |             |
  1397.          <----- ----->
  1398.           BASE   T(t)
  1399. .DE
  1400. .cs R
  1401. .IP
  1402. Pure translation in
  1403. .B tool
  1404. coordinates :
  1405. .br
  1406. .cs R 23
  1407. .DS L
  1408.         T6        TOOL
  1409.          -----> ------>
  1410.         |              |
  1411.         W              T
  1412.         |              |
  1413.          <----- <-----
  1414.         BASE      T(t)
  1415. .DE
  1416. .cs R
  1417. .PP
  1418. As mentioned before, pure translations lead to pure translations,
  1419. no matter what frame we are working in.
  1420. The difference between these two cases is that if $T(t)$ changes along
  1421. a principal direction, the frame $T$ will also change along a principal
  1422. direction in
  1423. .I world
  1424. coordinates in the first case, and in
  1425. .I tool
  1426. coordinates in the second case.
  1427. .IP
  1428. Pure rotation in
  1429. .I world
  1430. coordinates.
  1431. .br
  1432. .cs R 23
  1433. .DS L
  1434.          T6      TOOL
  1435.          -----> ----->
  1436.         |             |
  1437.         W             T
  1438.         |             |
  1439.          <----- ----->
  1440.           BASE   R(t)
  1441. .DE
  1442. .cs R
  1443. .IP
  1444. Pure rotation in
  1445. .B tool
  1446. coordinates :
  1447. .br
  1448. .cs R 23
  1449. .DS L
  1450.         T6        TOOL
  1451.          -----> ------>
  1452.         |              |
  1453.         W              T
  1454.         |              |
  1455.          <----- <-----
  1456.         BASE      R(t)
  1457. .DE
  1458. .cs R
  1459. .PP
  1460. The first case will cause the center of rotation to be fixed with respect
  1461. to $W$ (move with respect to $T$).
  1462. The second case will cause the center of rotation to move with respect
  1463. to $W$ (be fixed with respect to $W$).
  1464. We leave to reader the writing of the corresponding equations.
  1465. Armed with this conceptual tool we can introduce actual
  1466. examples.
  1467. .bp
  1468. .PP
  1469. .B
  1470. 1)
  1471. .R
  1472. The first example defines two locations that
  1473. differ by position and orientation.
  1474. The two positions are described with respect to a moving frame in
  1475. .I world
  1476. coordinates.
  1477. A loop causes a motion back and forth from one position to the other.
  1478. The final motion translates along the Y axis.
  1479. .br
  1480. .cs R 23
  1481. .DS L
  1482.      1  #include "rccl.h"
  1483.      2
  1484.      3  pumatask()
  1485.      4  {
  1486.      5          TRSF_PTR z, e , b, pa1, pa2, conv;
  1487.      6          POS_PTR  p0, pt1, pt2;
  1488.      7          int convfn();
  1489.      8          int i;
  1490.      9
  1491.     10          conv = newtrans("CONV",convfn);
  1492.     11          z = gentr_trsl("Z",  0.,  0., 864.);
  1493.     12          e = gentr_trsl("E" , 0. , 0. , 170.);
  1494.     13          b = gentr_rot("B", 600. , -500., 600., yunit, 180.);
  1495.     14          pa1 = gentr_eul("PA1"  , 30., 0., 50., 0., 20., 0.);
  1496.     15          pa2 = gentr_eul("PA2"  , -30., 0., 50., 0., -20., 0.);
  1497.     16
  1498.     17          p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e);
  1499.     18          pt1 = makeposition("PT1", z, t6, e, EQ, conv, b, pa1, TL, e);
  1500.     19          pt2 = makeposition("PT2", z, t6, e, EQ, conv, b, pa2, TL, e);
  1501.     20
  1502.     21          setvel(300, 50);
  1503.     22          setmod('c');
  1504.     23          setime(300, 0);
  1505.     24          move(p0);
  1506.     25          for (i = 0; i < 4; ++i) {
  1507.     26                  movecart(pt1, 100, 1000);
  1508.     27                  movecart(pt2, 100, 1000);
  1509.     28          }
  1510.     29          setmod('j');
  1511.     30          move(park);
  1512.     31  }
  1513.     32
  1514.     33  convfn(t)
  1515.     34  TRSF_PTR t;
  1516.     35  {
  1517.     36          t->p.y += 3.;
  1518.     37  }
  1519. .DE
  1520. .cs R
  1521. Line 1 includes the necessary RCCL declarations.
  1522. Line 3 deserves a comment : when using the puma manipulator,
  1523. the RCCL library calls the function `pumatask' as the task
  1524. to be executed.
  1525. Before calling the `pumatask' function, the system perform some
  1526. initializations.
  1527. When the function returns, as you might expect, the system
  1528. performs a `waitfor(completed)' before concluding and exiting.
  1529. Line 5 and 6, allocates transform and position pointers as
  1530. needed by the task.
  1531. Line 7 declares the name `convfn' as a pointer to a function that
  1532. describes the moving coordinate frame, and line 8 allocates a counter
  1533. variable.
  1534. Line 10, allocates a functionally defined transform attached to `convfn'.
  1535. Lines 11 through 15, allocate and initialize transforms as described earlier.
  1536. The $Z$ transform sets a frame at the base of the manipulator.
  1537. The $E$ and $B$ transforms are the tool transform
  1538. and a location with respect
  1539. to the simulated conveyor.
  1540. Note that the $B$ transform contains a 180 degree rotation around the
  1541. Y axis such as the Z direction of frame described by $B$ points
  1542. downward (relatively to $CONV$ and $Z$).
  1543. The transforms $PA1$ and $PA2$ define two locations with respect to
  1544. the frame described by $B$.
  1545. .PP
  1546. Lines 17, 18, and 19 set up the position equations as described earlier.
  1547. .PP
  1548. Line 21 sets the velocity to 300 millimeters per seconds and 50 degrees
  1549. per second and the motion mode is set to
  1550. .I Cartesian
  1551. mode on line 22.
  1552. The call to
  1553. .B setime
  1554. on line 23, containing a null segment time, and specifies a 3/10 of a second
  1555. acceleration time when reaching P0 to allow for a sufficiently long
  1556. transition time because the next motion occurs with respect to
  1557. a moving frame (the system has no means to now how fast it is going to move).
  1558. The `for' loop, lines 25 to 28, causes
  1559. eight move requests to be entered in the queue.
  1560. The eight motions are performed in 1 second each with a 1/10 of a
  1561. second transition time
  1562. as specified by the macro
  1563. .B movecart.
  1564. Line 29 sets the mode to
  1565. .I joint
  1566. because the arm is to perform a large motion and the path the
  1567. .I tool
  1568. frame is going to follow is of no concern.
  1569. Line 30 is the last motion request to the `park' position.
  1570. .PP
  1571. The function `convfn', lines 33 to 37, starts being evaluated when
  1572. the first motion to ``PT1'' begins and during the seven subsequent motions.
  1573. The background function attached to the transform is
  1574. called by the system with
  1575. one argument pointer, a pointer to the transform it is attached to.
  1576. This permits us to write functions independently from the actual transform
  1577. they are attached to.
  1578. Since
  1579. .B newtrans
  1580. created the transform $CONV$ as a unit transform, the value of the $p sub y$
  1581. element of the position vector increases
  1582. from 0 to approximatively
  1583. 286 millimeters(
  1584. it is increased by 3 millimeters each 28 milliseconds for 8 seconds).
  1585. At the time the manipulator reaches P0, the $CONV$ transform is
  1586. equal to the unity transform.
  1587. The first time the manipulator moves to PT1, the motion is the
  1588. result of a combination of the
  1589. .I Cartesian
  1590. motion from P0 toward PT1 and the motion due to the moving coordinate frame.
  1591. .PP
  1592. This example introduce the first method for generating functionally defined
  1593. motion by a periodic increment of a static variable (here a transform element).
  1594. .bp
  1595. .PP
  1596. .B
  1597. 2)
  1598. .R
  1599. In this second example we will suppose that the manipulator is
  1600. mounted on a revolving base or that the manipulator is working with respect
  1601. to a circular conveyor whose rotation axis is collinear with the first joint.
  1602. We have introduced minor differences in order to point out some other
  1603. aspects.
  1604. .br
  1605. .cs R 23
  1606. .DS L
  1607.      1  #include "rccl.h"
  1608.      2
  1609.      3  static int t0;
  1610.      4
  1611.      5  pumatask()
  1612.      6  {
  1613.      7          TRSF_PTR z, e , b, pa1, pa2, pivot;
  1614.      8          POS_PTR  p0, pt1, pt2;
  1615.      9          int pivotfn();
  1616.     10          int i;
  1617.     11
  1618.     12          pivot = newtrans("PIVOT", pivotfn);
  1619.     13          z = gentr_trsl("Z",  0.,  0., 864.);
  1620.     14          e = gentr_trsl("E" , 0. , 0. , 170.);
  1621.     15          b = gentr_rot("B1", 600. , -300., 700., yunit, 180.);
  1622.     16          pa1 = gentr_eul("PA1"  , 30., 0., 50., -10., 10., 0.);
  1623.     17          pa2 = gentr_eul("PA2"  , -30., 0., 50., 10., -10., 0.);
  1624.     18
  1625.     19          p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e);
  1626.     20          pt1 = makeposition("PT1", pivot, z, t6, e, EQ, b, pa1, TL, e);
  1627.     21          pt2 = makeposition("PT2", pivot, z, t6, e, EQ, b, pa2, TL, e);
  1628.     22
  1629.     23          setvel(300, 20);
  1630.     24          setmod('c');
  1631.     25          setime(200, 0);
  1632.     26          move(p0);
  1633.     27          waitfor(completed);
  1634.     28          t0 = rtime;
  1635.     29          for (i = 0; i < 6; ++i) {
  1636.     30                  move(pt1);
  1637.     31                  move(pt2);
  1638.     32          }
  1639.     33          setvel(400, 100);
  1640.     34          setmod('j');
  1641.     35          move(park);
  1642.     36  }
  1643.     37
  1644.     38  pivotfn(t)
  1645.     39  TRSF_PTR t;
  1646.     40  {
  1647.     41          Rot(t, zunit, (t0 - rtime) * .010);
  1648.     42  }
  1649. .DE
  1650. .cs R
  1651. .PP
  1652. The lines 1 to 26 are basically the same ones
  1653. and do not deserve further comments.
  1654. .PP
  1655. In this example, the moving coordinate frame will explicitly
  1656. be written as a function of time.
  1657. It makes use of the external variable
  1658. .B rtime
  1659. updated by the system each sample interval.
  1660. The variable
  1661. .B rtime
  1662. reflects the time elapsed since the beginning of the task in milliseconds.
  1663. Although this variable may be reset by the user to any value,
  1664. we have chosen to record in `t0' the time when the functionally defined
  1665. motion begins.
  1666. Although the position of the moving coordinate frame is periodic,
  1667. it is necessary to set the beginning of the motion at a given instant
  1668. in order to keep the resulting task execution within the work range of the
  1669. manipulator.
  1670. The macro
  1671. .B waitfor
  1672. suspends execution until all the preceding motions are executed and
  1673. the initial time is recorded at line 28.
  1674. In actual implementations, the use of an
  1675. .I event
  1676. would permit us to synchronize the task execution with arbitrary motions
  1677. of, say, the conveyor.
  1678. The segment times in the `for' loop, lines 29 to 32, are left unspecified
  1679. and will be computed as to obtain an angular velocity of 20 degrees per
  1680. second (line 23).
  1681. .PP
  1682. It is important to notice the placement of the functionally defined
  1683. transform in position equations so as to produce the desired effect.
  1684. .PP
  1685. The functionally defined frame is merely described as a negative rotation
  1686. around the Z axis of 10 degrees per second.
  1687. .bp
  1688. .PP
  1689. .B
  1690. 3)
  1691. .R
  1692. In this third example the positions $PA1$ and $PA2$ are now
  1693. described with respect to rotating table off the axis of the manipulator's
  1694. first joint.
  1695. This will cause the end effector to rotate such as to maintain
  1696. a constant orientation with respect to the table.
  1697. .br
  1698. .cs R 23
  1699. .DS L
  1700.      1  #include "rccl.h"
  1701.      2
  1702.      3  static int t0;
  1703.      4
  1704.      5  pumatask()
  1705.      6  {
  1706.      7          TRSF_PTR z, e , b, pa1, pa2, table;
  1707.      8          POS_PTR  p0, pt1, pt2;
  1708.      9          int tablefn();
  1709.     10          int i;
  1710.     11
  1711.     12          table = newtrans("TABLE", tablefn);
  1712.     13          z = gentr_trsl("Z",  0.,  0., 864.);
  1713.     14          e = gentr_trsl("E" , 0. , 0. , 170.);
  1714.     15          b = gentr_rot("B", 600. ,  300., 700., yunit, 180.);
  1715.     16          pa1 = gentr_rpy("PA1"  , 0., 0., 0., 0., 0., 10.);
  1716.     17          pa2 = gentr_rpy("PA2"  , 0., 0., 0., 0., 0., -10.);
  1717.     18
  1718.     19          p0 = makeposition("P0" , z, t6, e, EQ, b, TL, e);
  1719.     20          pt1 = makeposition("PT1", z, t6, e, EQ, b, table, pa1, TL, e);
  1720.     21          pt2 = makeposition("PT2", z, t6, e, EQ, b, table, pa2, TL, e);
  1721.     22
  1722.     23          setvel(300, 20);
  1723.     24          setmod('c');
  1724.     25          setime(200, 0);
  1725.     26          move(p0);
  1726.     27          waitfor(completed);
  1727.     28          t0 = rtime;
  1728.     29          for (i = 0; i < 6; ++i) {
  1729.     30                  move(pt1);
  1730.     31                  move(pt2);
  1731.     32          }
  1732.     33          setvel(400, 100);
  1733.     34          setmod('j');
  1734.     35          move(park);
  1735.     36  }
  1736.     37
  1737.     38  tablefn(t)
  1738.     39  TRSF_PTR t;
  1739.     40  {
  1740.     41          real rps = .03;
  1741.     42          real alpha = rps * (t0 - rtime) * .001;
  1742.     43
  1743.     44          t->p.x = 100. * cos(alpha * pit2_m);
  1744.     45          t->p.y = 100. * sin(alpha * pit2_m);
  1745.     46          Rot(t, zunit, alpha * 360.);
  1746.     47  }
  1747. .DE
  1748. .cs R
  1749. The positions equations set apart, this program is quite similar to the
  1750. previous one.
  1751. The main difference lies in the way those equations are set up in order
  1752. to obtain the desired effect.
  1753. The functionally described transformation is made up from a translation
  1754. part and a rotation part.
  1755. The variable `rps' describes a rotational velocity of
  1756. 3/100 of a rotation per second.
  1757. The variable
  1758. .B pit2_m
  1759. belong to the set of math constant entry points.
  1760. .bp
  1761. .PP
  1762. .B
  1763. 4)
  1764. .R
  1765. The last example describes a task that causes the manipulator end effector
  1766. to follow a circular path while always being perpendicular to its
  1767. trajectory.
  1768. This achieved by setting up a position equation to obtain a
  1769. remote center of rotation.
  1770. .br
  1771. .cs R 23
  1772. .DS L
  1773.      1  #include "rccl.h"
  1774.      2
  1775.      3
  1776.      4  pumatask()
  1777.      5  {
  1778.      6          TRSF_PTR z, e , b, perp0, perp, roty;
  1779.      7          POS_PTR  p0, pt;
  1780.      8          int perpfn();
  1781.      9
  1782.     10          z = gentr_trsl("Z",  0.,  0., 864.);
  1783.     11          e = gentr_trsl("E" , 0. , 0. , 170.);
  1784.     12          b = gentr_trsl("B", 500. ,  300., 600.);
  1785.     13          roty = gentr_rot("ROTY", 0., 0., 0., yunit, 180.);
  1786.     14          perp0 = gentr_rot("PERP0", 0., 0., 300., xunit, 0.);
  1787.     15          perp = newtrans("PERP", perpfn);
  1788.     16
  1789.     17    p0 = makeposition("P0", z, t6, e, perp0, EQ, b, roty, TL, e);
  1790.     18    pt = makeposition("PT", z, t6, e, perp0, EQ, b, perp, roty, TL, e);
  1791.     19
  1792.     20          setvel(400, 100);
  1793.     21          setmod('c');
  1794.     22          setime(300, 0);
  1795.     23          move(p0);
  1796.     24          setime(200, 4000);
  1797.     25          move(pt);
  1798.     26          setmod('j');
  1799.     27          move(park);
  1800.     28  }
  1801.     29
  1802.     30  perpfn(t)
  1803.     31  TRSF_PTR t;
  1804.     32  {
  1805.     33          real rpm = .20;
  1806.     34
  1807.     35          Rot(t, xunit, rpm * goalpos->scal * 360.);
  1808.     36  }
  1809. .DE
  1810. .cs R
  1811. .PP
  1812. In this example, the functional motion
  1813. parameter is the `scal' position structure
  1814. entry.
  1815. When the background function is evaluated, the global
  1816. .B goalpos
  1817. pointer
  1818. is equal to `pt'.
  1819. The variable `rpm' stands for rotations per motion.
  1820. .IP
  1821. We have introduce some examples
  1822. for coding functionally described trajectories.
  1823. The lay out of the programs, especially the position equation specifications
  1824. are certainly not unique, and a lot of room is left to imagination.
  1825.