Dynamc Optmzaton Assgnment 1 Sasanka Nagavall snagaval@andrew.cmu.edu 16-745 January 29, 213 Robotcs Insttute Carnege Mellon Unversty
Table of Contents 1. Problem and Approach... 1 2. Optmzaton wthout Analytcal Dervatves... 1 3. Optmzaton wth Analytcal Dervatves... 6 4. References... 12
1. Problem and Approach Consder a stuaton where a humanod robot s requred to move one of ts jonts (e.g. ts hand) to a specfc locaton wthout fallng over or movng ts feet. If the model of the robot s known (.e. the knematc chan contanng the connectvty, ntal poses, and lmts of jonts and lnks s gven), ths problem can be formulated as the followng constraned nonlnear optmzaton problem. arg mn x f x subject to c eq x c x x f : c : c eq : n n n n p q The soluton to the problem above s the value of the vector varable x that mnmzes the objectve functon. In ths assgnment, the focus wll be on formulatng the problem by specfyng the approprate objectve functon f c x and the equalty constrant functon fmncon solver [1]. ceq 2. Optmzaton wthout Analytcal Dervatves x, the nequalty constrant functon x. The problem wll then be solved for x usng Matlab s Please see the provded drc.m scrpt for a tree structure defnng the knematc chan of the humanod robot. Snce the goal s to place the specfed jont at the specfed locaton, the objectve functon may be set to the Eucldean dstance between the jont s poston and the goal poston. The Eucldean dstance s a good choce for an objectve functon because t has a well-defned mnmum value (.e. ) and the goal s clearly acheved whenever the functon s mnmzed though addtonal effort may be requred to also satsfy the constrants. Whle the goal poston P goal s fxed, the jont poston P s a functon of the robot s jont angles,,, jont and t can be 1 2 28 determned by applyng forward knematcs to the robot s knematc chan. The vector of jont angles that mnmzes the value of the objectve functon s the soluton to the problem. f,,, P,,, P 1 2 28 jont 1 2 28 Snce the robot must not move ts feet, the equalty constrant may be smlarly defned n terms of the Eucldean dstance between each foot jont s poston and ts ntal poston. Ths equalty constrant s satsfed when both of these dstances are. goal 1
c,,, 1 2 28 P P,,, lfoot _ jont 1 2 28 lfoot _ ntal,,, rfoot _ jont 1 2 28 rfoot _ ntal P P The fnal condton that the robot must not fall over can be specfed n terms of the nequalty constrant usng the robot s center of mass. The robot has a base of support that can be estmated by the poston and sze of ts feet. The base of support defnes a two dmensonal regon on the ground beneath the robot such that f the robot s projected center of mass les wthn the regon, the robot wll not fall over. If the robot s projected center of mass les outsde the regon, the robot wll fall over. By crudely estmatng ths regon wth a rectangle of gven dmensons ( C lmt ) centered on the orgn, the nequalty constrant may be specfed by the followng functon. Crobot, x 1, 2,, 28 Clmt, x Crobot, y 1, 2,, 28 Clmt, y c1, 2,, 28 Crobot, x 1, 2,, 28 Clmt, x Crobot, y 1, 2,, 28 Clmt, y Please see sasanka_objectve.m for a Matlab functon that mplements the objectve functon f,,, defned above. 1 2 28 Please see sasanka_constrants.m for a Matlab functon that mplements both the nequalty constrant c,,, and equalty constrant c,,, functons defned above. 1 2 28 eq 1 2 28 Please see q1.m for a Matlab scrpt that uses fmncon to solve ths nonlnear optmzaton problem. Note that the scrpt also specfes upper and lower bounds on the jont angles as parameters to fmncon to lmt the angles to wthn the jont lmts. These jont lmts could also easly have been specfed as addtonal nonlnear constrants, but n mplementaton, t appears to be more effcent to specfy them separately to fmncon. The fgures below show the ntal pose and a varety of dfferent poses obtaned as solutons to the nonlnear optmzaton problem when the specfed jont s the rght hand and goal poston s as ndcated on the fgure. 2
Fgure 1 - Intal Robot Pose Fgure 2 - Robot Reaches for (,,) wth Rght Hand 3
Fgure 3 - Robot Reaches for (1,,.5) wth Rght Hand Fgure 4 - Robot Reaches for (1,1,) wth Rght Hand 4
Fgure 5 - Robot Reaches for (-.5,-.5,1) Fgure 6 - Robot Reaches for (,,1.5) wth Rght Hand 5
Please see Appendx A1 to Appendx A5 for the output correspondng to the above fgures. Of the postons tested, the only poston that the robot could not reach wth ts rght hand was (1,1,) whch s a pont on the ground one meter n front and one meter to the left of the robot. In ths stuaton, the solver found a local mnmum for the optmzaton functon that satsfed the constrants, but ths mnmum s far from the desred value of. 3. Optmzaton wth Analytcal Dervatves The optmzaton method descrbed above can be extended to nclude the computaton of exact gradents through the use of analytcal dervatves. Theoretcally, ths should result n faster convergence on the soluton to the optmzaton problem because there s no error n the computed gradent of the objectve functon or the constrant functons. These exact gradents can be provded to fmncon as extra output arguments n the objectve and constrant functons and by settng the GradObj and GradConstr optons to on [1]. The gradents of the objectve functon and constrant functons may be obtaned through applcaton of the chan rule. f,,, P,,, P c eq 1 2 28 jont 1 2 28 T 1, 2,, 28,,, P f Pjont Pgoal Pjont,,, P,,,,,, 1 2 28 jont 1 2 28 goal 1 2 28 1 2 28 c eq,,, 1 2 28 P P,,, goal lfoot _ jont 1 2 28 lfoot _ ntal,,, P P rfoot _ jont 1 2 28 rfoot _ ntal T Plfoot _ jont 1, 2,, 28 Plfoot _ ntal Plfoot _ jont Plfoot _ jont 1, 2,, 28 P lfoot _ ntal 1, 2,, 28 Prfoot _ jont 1 2 28 Prfoot _ ntal Prfoot _ jont Prfoot _ jont 1 2 28 rfoot _ ntal T,,,,,, P 1, 2,, 28 6
c,,, 1 2 28 c,,, 1 2 28 C,,, C,,, C,,, C,,, Crobot, x 1, 2,, 28 C robot, y 1, 2,, 28 C robot, x 1, 2,, 28 Crobot, y 1, 2,, 28 C robot, x 1 2 28 lmt, x C robot, y 1 2 28 lmt, y C robot, x 1 2 28 lmt, x C robot, y 1 2 28 lmt, y It s clear that n order to compute the dervatves of the objectve and constrant functons, one must frst compute the dervatves of the poston of every jont and the dervatves of the centerof-mass of the robot. In a robot wth 28 jonts and lnks, ths appears to be an enormous amount of computaton. However, the computaton may be sgnfcantly smplfed by consderng the followng. Gven a knematc chan wth n jonts and lnks, jont offsets d1, d2,, d n, lnk center-of-mass offsets 1, 2,, n compute the jont postons P, lnk orentatons followng recursve update rule. t t t and a vector of jont angles 1, 2,, n, t s possble to K, and lnk center-of-masses Q va the P P K d parent ( ) parent ( ) K K R parent () Q P K t The recursve update rule gven above captures the forward knematcs of the knematc chan and only requres that the root jont s poston P and the root lnk s orentaton K be specfed. By the takng the dervatves of the equatons above, a smlar update rule may be specfed to propagate the dervatves of the jont postons, lnk orentatons and lnk center-of-masses. Assume that the jonts and lnks are numbered such that parent (). 7
P P K j j j K j K j parent ( ) parent ( ) K parent () j j parent () Q P K t j j j R j d j j R j Please see drc_forward_knematcs2.m for a modfed verson of the forward knematcs functon that also computes the gradents usng the update rules above. The dervatve of the robot s center-of-mass s also easly computed once the dervatves of every lnk s center-of-mass have been computed. mq C m C j Q m Please see sasanka_objectve2.m for a Matlab functon that mplements the objectve functon f f 1, 2,, 28 and ts gradent.,,, 1 2 28 Please see sasanka_constrants2.m for a Matlab functon that mplements both the nequalty constrant c1, 2,, 28 and equalty constrant ceq 1, 2,, 28 functons and ther gradents c c eq and. A sngle modfcaton was made to the equatons above n,,,,,, 1 2 28 1 2 28 mplementaton and that was to modfy the equalty constrants to avod dvson, whch was causng a dvson-by-zero error when the jont angles dd not cause any movement n the feet of the robot. m j 8
Fgure 7 - Intal Robot Pose Fgure 8 - Robot Reaches for (,,) wth Rght Hand - Analytcal Gradent 9
Fgure 9 - Robot Reaches for (1,,.5) wth Rght Hand - Analytcal Gradent Fgure 1 - Robot Reaches for (1,1,) wth Rght Hand - Analytcal Gradent 1
Fgure 11 - Robot Reaches for (-.5,.5,1) wth Rght Hand - Analytcal Gradent Fgure 12 - Robot Reaches for (,,1.5) wth Rght Hand - Analytcal Gradent 11
Please see Appendx B1 to Appendx B5 for the output correspondng to the above fgures. The results appear to place the robot n more convoluted poses than those obtaned wthout the analytcal gradents. In most cases, the fnal poston of the jont was closer to the goal poston than wthout analytcal gradents. 4. References [1] MathWorks, "Fnd mnmum of constraned nonlnear multvarable functon - MATLAB," 212. [Onlne]. Avalable: http://www.mathworks.com/help/optm/ug/fmncon.html. [Accessed 26 January 213]. 12
Appendx A1 Robot Reaches for (,,) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans): -.1553 -.436.436.545 -.698.698.218 2.45-1.6342-1.75.524 -.2174 -.47.495.1755 -.32 1.14.3389 -.619.619 1.2211-1.2 1.28 -.3967 -.798.798.2493 -.619 1.1345.1236-1.14.32.243 -.495.47-1.658-1.745.524.1496 2.45.26 -.698.698 -.14 -.436.436 -.145-1.9635 1.9635 -.222-1.3963 1.7453 1.7129 3.1416 1.2118 2.3562.16-1.571 1.571.5732 -.436 1.571 1.2532-1.9635 1.9635 -.1953-1.7453 1.3963 1.549 3.1416 -.4241-2.3562.125-1.571 1.571 -.5725-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29: 1.e-8 * -.2253 -.1794 -.6632 Center of mass of robot (xyz, meters):.1.35.6647 Dsplacement of statonary jonts (meters): 3.6379e-8 Dstance from goal poston (meters): 7.231e-9 13
Appendx A2 Robot Reaches for (1,,.5) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans): -.1371 -.436.436 -.884 -.698.698.2751 2.45-1.4185-1.75.524.186 -.47.495.423 -.32 1.14.123 -.619.619.847-1.2 1.28 -.686 -.798.798.2459 -.619 1.1345 -.66-1.14.32.3866 -.495.47-1.4297-1.745.524.4217 2.45.348 -.698.698 -.32 -.436.436 -.2615-1.9635 1.9635 -.5679-1.3963 1.7453 1.818 3.1416.748 2.3562.293-1.571 1.571.4363 -.436 1.571 -.4118-1.9635 1.9635 -.6149-1.7453 1.3963 1.638 3.1416 -.2724-2.3562.59-1.571 1.571 -.7594-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29: 1. -..5 Center of mass of robot (xyz, meters):.144.269.7378 Dsplacement of statonary jonts (meters): 1.4334e-8 Dstance from goal poston (meters): 3.257e-7 14
Appendx A3 Robot Reaches for (1,1,) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans): -.831 -.436.436 -.398 -.698.698.6782 2.45-1.4945-1.75.524.4479 -.47.495 -.185 -.32 1.14.3886 -.619.619 1.1321-1.2 1.28 -.722 -.798.798.2599 -.619 1.1345 -.124-1.14.32.327 -.495.47-1.427-1.745.524.695 2.45.534 -.698.698 -.175 -.436.436 -.5246-1.9635 1.9635 -.7265-1.3963 1.7453 1.835 3.1416.5634 2.3562.128-1.571 1.571.3745 -.436 1.571 -.4251-1.9635 1.9635 -.7481-1.7453 1.3963 1.437 3.1416 -.1794-2.3562.249-1.571 1.571 -.873-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29:.6884.6355.382 Center of mass of robot (xyz, meters):.1461.1258.7125 Dsplacement of statonary jonts (meters): 1.9854e-8 Dstance from goal poston (meters):.571 15
Appendx A4 Robot Reaches for (-.5,-.5,1) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans):.851 -.436.436 -.421 -.698.698.8644 2.45 -.4951-1.75.524 -.167 -.47.495.4623 -.32 1.14 -.166 -.619.619.385-1.2 1.28.166 -.798.798.2628 -.619 1.1345 -.3744-1.14.32.171 -.495.47 -.6625-1.745.524.8753 2.45 -.6 -.698.698.6 -.436.436 -.664-1.9635 1.9635.1567-1.3963 1.7453 1.5776 3.1416 1.1467 2.3562.7-1.571 1.571.5613 -.436 1.571 -.5167-1.9635 1.9635.3749-1.7453 1.3963 1.8985 3.1416-1.487-2.3562.16-1.571 1.571 -.5739-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29: -.5 -.5 1. Center of mass of robot (xyz, meters): -. -..936 Dsplacement of statonary jonts (meters): 3.2679e-8 Dstance from goal poston (meters): 3.126e-9 16
Appendx A5 Robot Reaches for (,,1.5) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans): -.86 -.436.436 -.4594 -.698.698 1.3397 2.45 -.6616-1.75.524.826 -.47.495.2841 -.32 1.14.142 -.619.619.4792-1.2 1.28 -.4128 -.798.798.2652 -.619 1.1345 -.4188-1.14.32.815 -.495.47 -.6835-1.745.524 1.343 2.45 -.39 -.698.698. -.436.436 -.625-1.9635 1.9635.859-1.3963 1.7453 1.635 3.1416 1.258 2.3562.32-1.571 1.571.5639 -.436 1.571.6768-1.9635 1.9635-1.3779-1.7453 1.3963.7764 3.1416-1.8811-2.3562.176-1.571 1.571 -.575-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29:. -. 1.5 Center of mass of robot (xyz, meters): -.479.19.8738 Dsplacement of statonary jonts (meters): 6.177e-9 Dstance from goal poston (meters): 5.1267e-8 17
Appendx B1 Robot Reaches for (,,) wth Rght Hand Solver stopped prematurely. fmncon stopped because t exceeded the teraton lmt, optons.maxiter = 1 (the default value). Jont angles, lower lmts, upper lmts (radans): -.828 -.436.436 -.5568 -.698.698 2.745 2.45-1.691-1.75.524 -.369 -.47.495 -.263 -.32 1.14.3899 -.619.619 1.2172-1.2 1.28.7282 -.798.798.2411 -.619 1.1345 -.93-1.14.32 -.4489 -.495.47-1.6921-1.745.524 2.3192 2.45.5 -.698.698.1 -.436.436 -.357-1.9635 1.9635.1668-1.3963 1.7453 1.548 3.1416 1.1769 2.3562 -.1-1.571 1.571.5453 -.436 1.571 1.1525-1.9635 1.9635.8176-1.7453 1.3963 1.7354 3.1416 -.2918-2.3562.2-1.571 1.571 -.5449-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.485 -.888.89 Jont 29: 1.e-5 *.318 -.785.1123 Center of mass of robot (xyz, meters):.187 -.56.533 Dsplacement of statonary jonts (meters): 2.645e-4 Dstance from goal poston (meters): 1.467e-6 18
Appendx B2 Robot Reaches for (1,,.5) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans): -.1372 -.436.436 -.914 -.698.698.3117 2.45-1.4673-1.75.524.2838 -.47.495.37 -.32 1.14.37 -.619.619.837-1.2 1.28 -.628 -.798.798.252 -.619 1.1345.244-1.14.32.2233 -.495.47-1.4771-1.745.524.4329 2.45.451 -.698.698 -.1 -.436.436 -.3125-1.9635 1.9635 -.4198-1.3963 1.7453 1.7599 3.1416.7771 2.3562.238-1.571 1.571.4663 -.436 1.571 -.4495-1.9635 1.9635 -.68-1.7453 1.3963 1.51 3.1416 -.2871-2.3562 -.24-1.571 1.571 -.6929-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.485 -.89.89 Jont 29: 1. -..5 Center of mass of robot (xyz, meters):.1413.366.7219 Dsplacement of statonary jonts (meters): 2.1883e-4 Dstance from goal poston (meters): 7.5372e-6 19
Appendx B3 Robot Reaches for (1,1,) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans): -.747 -.436.436 -.6968 -.698.698 1.3189 2.45-1.7473-1.75.524.4913 -.47.495 -.3157 -.32 1.14.233 -.619.619 1.1382-1.2 1.28 -.7857 -.798.798 -.4597 -.619 1.1345 -.3438-1.14.32.466 -.495.47-1.5976-1.745.524 1.3492 2.45.4418 -.698.698 -.2598 -.436.436 -.5993-1.9635 1.9635 -.975-1.3963 1.7453 1.7355 3.1416.1869 2.3562.178-1.571 1.571 -.141 -.436 1.571 -.3924-1.9635 1.9635 -.7351-1.7453 1.3963 1.5113 3.1416 -.714-2.3562.882-1.571 1.571-1.4929-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.486 -.892.81 Jont 29:.777.67.2523 Center of mass of robot (xyz, meters):.157.138.6185 Dsplacement of statonary jonts (meters): 2.9485e-4 Dstance from goal poston (meters):.579 2
Appendx B4 Robot Reaches for (-.5,-.5,1) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans):.66 -.436.436 -.3899 -.698.698.8888 2.45 -.565-1.75.524 -.874 -.47.495.4528 -.32 1.14 -.898 -.619.619.255-1.2 1.28.115 -.798.798.2628 -.619 1.1345 -.3811-1.14.32.275 -.495.47 -.665-1.745.524.965 2.45 -.6 -.698.698.3 -.436.436 -.93-1.9635 1.9635.1717-1.3963 1.7453 1.5729 3.1416 1.1645 2.3562.1-1.571 1.571.5654 -.436 1.571 -.5676-1.9635 1.9635.416-1.7453 1.3963 1.9839 3.1416-1.1862-2.3562 -.1-1.571 1.571 -.576-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29: -.5 -.5 1. Center of mass of robot (xyz, meters): -.356 -.41.9392 Dsplacement of statonary jonts (meters): 3.84e-5 Dstance from goal poston (meters): 2.9162e-1 21
Appendx B5 Robot Reaches for (,,1.5) wth Rght Hand Local mnmum possble. Constrants satsfed. fmncon stopped because the sze of the current step s less than the default value of the step sze tolerance and constrants are satsfed to wthn the default value of the constrant tolerance. <stoppng crtera detals> Jont angles, lower lmts, upper lmts (radans):.671 -.436.436 -.3298 -.698.698.9892 2.45-1.1542-1.75.524 -.251 -.47.495.27 -.32 1.14 -.2332 -.619.619.867-1.2 1.28 -.327 -.798.798.2598 -.619 1.1345 -.941-1.14.32.2271 -.495.47-1.374-1.745.524.748 2.45.12 -.698.698 -.14 -.436.436 -.3252-1.9635 1.9635 -.181-1.3963 1.7453 1.715 3.1416 1.2536 2.3562.133-1.571 1.571.5748 -.436 1.571-1.6894-1.9635 1.9635.3756-1.7453 1.3963 2.6991 3.1416-1.3436-2.3562 -.155-1.571 1.571 -.5687-1.571.436 Fnal postons of statonary jonts and goal jont (xyz, meters): -.484.89.811 -.484 -.89.811 Jont 29: -. -. 1.5 Center of mass of robot (xyz, meters):.71.621.8334 Dsplacement of statonary jonts (meters): 2.117e-5 Dstance from goal poston (meters): 4.5383e-11 22