Optimize Five-Link Robot Model
R2026bThis example shows how to optimize a five-link, two-dimensional robot model created using Robotics System Toolbox™ functions. To speed the optimization, the example uses code generation for the robot model.
Create Robot Model
Create a five-link planar robot model based on the RABBIT robot https://www.biped.solutions/rabbit. The code for creating this model is in the createBipedModel.m file, which is included in the live script for this example. You can see an image of the model at the end of this example.
The model consists of a torso, which is a line segment that can rotate relative to the hip joints of the two legs below it. The two legs are line segments representing the femurs (upper legs) and tibias (lower legs). The legs joints are the hips, which connect the femurs to the torso, and knees, which connect the femurs to the tibias. The "feet" are simply the ends of the tibias, with no joints or segments to help provide balance.
The dynamics of this robot model are incorporated into the model creation function. The creation function begins by creating a rigidBodyTree object:
robot = rigidBodyTree(DataFormat="column",MaxNumBodies=9);
robot.Gravity = [0 0 -p.g];
The code then adds the body parts, namely the joints and associated masses and inertias. The two legs are in the same plane, but the model passes them through each other as they move, as if they were separated by the width of the hip. For the details of the construction, view the createBipedModel.m file.
Given the model dynamics, the goal is twofold:
Create a plan for the robot movement that has the robot walk forward as far as possible while remaining upright.
Have the plan match the initial and final conditions, except for the forward movement, so that the robot can continue walking indefinitely.
Generate Code for Model Dynamics
During the simulated robot movements, the optimization function fmincon calls the robot dynamics multiple times. This process can be time consuming. To speed the computations, generate code for the dynamics by calling the generateBipedMex function first.
generateBipedMex
Generating MEX for bipedPointConstraints...
Code generation successful: View report
Done in 311.2 seconds.
Generating MEX for bipedImpactMap...
Code generation successful: View report
Done in 246.0 seconds.
Verifying MEX outputs...
bipedPointConstraints max errors:
dynamics: 0.00e+00
holonomic: 0.00e+00
stance foot: 0.00e+00
swing foot: 0.00e+00
Jacobian: 0.00e+00
bipedImpactMap max error: 3.77e-15
Benchmarking (1000 calls)...
Interpreted: 13.090 s (13.090 ms/call)
MEX: 0.629 s (0.629 ms/call)
Speedup: 20.8x
MEX generation complete. You can now run five_link_biped_codegen.m
This call takes just a few minutes, and saves hours of computation time.
Optimize Robot Gait
After you generate the robot dynamics code, optimize one half cycle of the robot gait. A half cycle consists of the time from when the rear leg leaves the ground and swings forward, until the time that the leg lands on the ground again. During this time, the front leg remains on the ground, and the rear leg swings forward to become the front leg. The optimization maximizes the distance that the robot travels in this half cycle, with the constraint that the front leg has exactly the same positions and velocities at the end of the half cycle as the rear leg at the beginning of the half cycle. In a full cycle, the rear leg leaves the ground, swings forward, lands on the ground, and swings back to the point where it is about to leave the ground again.
To perform this optimization, initialize the robot to a standing position with the legs swinging appropriately, calculate the half cycle dynamics, and have the fmincon solver attempt to match the initial and final positions (that is, reduce any infeasibilities to zero) while simultaneously maximizing the stride length. The dynamics, which are built into the model, are constraints that the optimizer attempts to satisfy.
To perform this optimization efficiently, first use the fmincon "interior-point" algorithm for 100 iterations. This process reduces the infeasibilities considerably, while providing for a robust initial optimization. Then, to obtain a more nearly feasible solution, switch to the "sqp" algorithm with central finite differences for 100 more iterations. The "sqp" algorithm is often more accurate than the "interior-point" algorithm once the solution is close to an optimal point, and central finite differences provide for even more accuracy. These optimizations are coded into the five_link_biped_codegen function. Limiting the optimizations to 100 iterations each ensures that the code runs reasonably quickly, but this limit can degrade the final solution quality.
At the end of the optimization, take the best feasible point found as the solution. This procedure ensures that the solution is the best found in the limited number of iterations you impose. This best feasible point is taken in the five_link_biped_codegen function.
five_link_biped_codegen
Phase 1: Finding feasible periodic walking gait (interior-point)...
First-order Norm of
Iter F-count f(x) Feasibility optimality step Wall-Clock Time
0 561 -4.920788e-01 7.987e+01 1.527e-05 6.860837 sec
1 1122 -4.398649e-01 3.652e+01 1.568e+00 2.986e+02 13.577133 sec
2 1683 -4.389378e-01 3.337e+01 1.620e+00 2.053e+01 18.196896 sec
3 2244 -4.389111e-01 3.331e+01 1.626e+00 4.323e-01 22.919297 sec
4 2805 -4.371410e-01 2.899e+01 1.681e+00 3.857e+01 28.156531 sec
5 3366 -4.371273e-01 2.896e+01 1.684e+00 4.726e-01 33.329361 sec
6 3927 -4.345337e-01 2.499e+01 1.739e+00 4.626e+01 38.241059 sec
7 4488 -4.345072e-01 2.495e+01 1.739e+00 6.431e-01 44.044319 sec
8 5049 -4.344317e-01 2.433e+01 1.739e+00 9.822e+00 51.123222 sec
9 5610 -4.309240e-01 1.629e+01 1.787e+00 9.586e+01 56.777462 sec
10 6171 -4.308109e-01 1.615e+01 1.799e+00 3.232e+00 62.330833 sec
11 6732 -4.302066e-01 1.573e+01 1.803e+00 1.169e+01 67.724671 sec
12 7293 -4.299164e-01 1.547e+01 1.830e+00 4.590e+00 72.679210 sec
13 7854 -4.247548e-01 9.341e+00 1.880e+00 9.309e+01 77.265843 sec
14 8415 -4.247877e-01 9.262e+00 1.873e+00 2.519e+00 82.141878 sec
15 8976 -4.247976e-01 6.925e+00 1.923e+00 8.700e+01 87.398505 sec
16 9537 -4.203646e-01 3.681e+00 2.018e+00 1.336e+02 92.665907 sec
17 10098 -4.201758e-01 3.437e+00 2.042e+00 1.836e+01 98.175627 sec
18 10659 -4.181388e-01 2.816e+00 2.139e+00 4.940e+01 102.979108 sec
19 11220 -4.181184e-01 2.778e+00 2.135e+00 3.056e+00 108.130110 sec
20 11781 -4.176563e-01 2.626e+00 2.161e+00 1.690e+01 113.633184 sec
21 12342 -4.176566e-01 2.625e+00 2.154e+00 1.209e-01 118.734254 sec
22 12903 -4.181384e-01 2.495e+00 2.165e+00 2.696e+01 123.748408 sec
23 13464 -4.188068e-01 2.424e+00 2.150e+00 9.914e+00 128.913411 sec
24 14025 -4.202435e-01 2.268e+00 2.089e+00 3.196e+01 133.706731 sec
25 14586 -4.204279e-01 2.182e+00 2.101e+00 1.287e+01 138.290753 sec
26 15147 -4.209577e-01 2.147e+00 2.068e+00 7.636e+00 143.700342 sec
27 15708 -4.212142e-01 2.088e+00 2.071e+00 7.341e+00 149.763171 sec
28 16269 -4.212343e-01 2.085e+00 2.067e+00 7.423e-01 156.398712 sec
29 16830 -4.214823e-01 1.958e+00 2.064e+00 2.117e+01 162.689840 sec
30 17391 -4.215221e-01 1.952e+00 2.069e+00 1.174e+00 168.114271 sec
First-order Norm of
Iter F-count f(x) Feasibility optimality step Wall-Clock Time
31 17952 -4.207789e-01 1.793e+00 2.100e+00 5.766e+01 173.850088 sec
32 18513 -4.208541e-01 1.784e+00 2.113e+00 3.365e+00 178.594303 sec
33 19074 -4.208686e-01 1.782e+00 2.108e+00 5.650e-01 183.293369 sec
34 19635 -4.222623e-01 1.632e+00 2.054e+00 3.626e+01 187.747980 sec
35 20196 -4.222657e-01 1.630e+00 2.049e+00 9.050e-01 192.322255 sec
36 20757 -4.220028e-01 1.595e+00 2.072e+00 1.361e+01 197.443799 sec
37 21318 -4.219924e-01 1.593e+00 2.060e+00 6.467e-01 203.497149 sec
38 21879 -4.218921e-01 1.554e+00 2.068e+00 1.428e+01 209.001400 sec
39 22440 -4.219015e-01 1.545e+00 2.071e+00 3.212e+00 214.699344 sec
40 23001 -4.225034e-01 1.490e+00 2.044e+00 4.629e+01 220.666200 sec
41 23562 -4.227181e-01 1.411e+00 2.034e+00 3.603e+01 225.986030 sec
42 24123 -4.215328e-01 1.391e+00 2.094e+00 7.761e+01 231.653276 sec
43 24684 -4.220099e-01 1.264e+00 2.102e+00 4.088e+01 236.952663 sec
44 25245 -4.219134e-01 1.256e+00 2.098e+00 7.158e+00 242.072022 sec
45 25806 -4.214480e-01 1.234e+00 2.115e+00 3.887e+01 246.716904 sec
46 26367 -4.214391e-01 1.233e+00 2.122e+00 1.121e+00 251.408197 sec
47 26928 -4.214349e-01 1.232e+00 2.115e+00 4.024e-01 256.041674 sec
48 27489 -4.208812e-01 1.231e+00 2.141e+00 3.430e+01 260.927169 sec
49 28050 -4.208711e-01 1.230e+00 2.144e+00 6.329e-01 265.601604 sec
50 28611 -4.197991e-01 1.161e+00 2.192e+00 5.383e+01 270.158743 sec
51 29172 -4.197800e-01 1.159e+00 2.185e+00 1.437e+00 274.680907 sec
52 29733 -4.197762e-01 1.158e+00 2.192e+00 3.862e-01 279.286519 sec
53 30294 -4.185255e-01 1.101e+00 2.246e+00 6.720e+01 283.845444 sec
54 30855 -4.185147e-01 1.100e+00 2.251e+00 6.640e-01 289.132735 sec
55 31416 -4.180990e-01 1.005e+00 2.270e+00 6.004e+01 293.623447 sec
56 31977 -4.180912e-01 1.003e+00 2.255e+00 6.481e-01 298.396806 sec
57 32538 -4.178944e-01 9.558e-01 2.265e+00 3.010e+01 302.812017 sec
58 33099 -4.178431e-01 9.519e-01 2.263e+00 2.867e+00 307.184458 sec
59 33660 -4.178748e-01 9.415e-01 2.266e+00 1.328e+01 311.790375 sec
60 34221 -4.179756e-01 8.866e-01 2.255e+00 5.433e+01 316.377151 sec
First-order Norm of
Iter F-count f(x) Feasibility optimality step Wall-Clock Time
61 34782 -4.179655e-01 8.852e-01 2.250e+00 1.252e+00 320.566639 sec
62 35343 -4.179640e-01 8.842e-01 2.261e+00 8.269e-01 324.795379 sec
63 35904 -4.178574e-01 8.684e-01 2.260e+00 8.274e+00 328.908204 sec
64 36465 -4.176263e-01 8.608e-01 2.264e+00 3.729e+01 332.879259 sec
65 37026 -4.174634e-01 8.318e-01 2.269e+00 2.167e+01 337.030270 sec
66 37587 -4.169646e-01 8.286e-01 2.302e+00 3.836e+01 341.175302 sec
67 38148 -4.169608e-01 8.281e-01 2.289e+00 5.341e-01 345.361342 sec
68 38709 -4.167636e-01 7.980e-01 2.293e+00 2.537e+01 350.065524 sec
69 39270 -4.167390e-01 7.930e-01 2.297e+00 3.836e+00 355.099702 sec
70 39831 -4.165837e-01 7.611e-01 2.309e+00 1.986e+01 359.642548 sec
71 40392 -4.165780e-01 7.593e-01 2.306e+00 1.045e+00 364.199576 sec
72 40953 -4.164088e-01 7.479e-01 2.312e+00 2.886e+01 368.498135 sec
73 41514 -4.164055e-01 7.473e-01 2.316e+00 6.745e-01 373.142352 sec
74 42075 -4.164032e-01 7.471e-01 2.318e+00 1.751e-01 377.434054 sec
75 42636 -4.176143e-01 2.972e-01 9.085e-02 9.450e+00 382.029737 sec
76 43197 -4.176142e-01 2.972e-01 9.086e-02 4.482e-02 386.666378 sec
77 43758 -4.175760e-01 2.896e-01 1.207e-01 2.080e+01 391.894365 sec
78 44320 -4.175377e-01 2.834e-01 1.982e-01 1.666e+01 396.619158 sec
79 44881 -4.175343e-01 2.828e-01 2.080e-01 2.068e+00 401.471921 sec
80 45442 -4.175272e-01 2.818e-01 2.159e-01 5.109e+00 405.703029 sec
81 46003 -4.175269e-01 2.818e-01 2.157e-01 2.011e-01 410.213666 sec
82 46564 -4.175259e-01 2.814e-01 2.220e-01 1.641e+00 416.166064 sec
83 47125 -4.175020e-01 2.779e-01 2.621e-01 1.921e+01 420.918501 sec
84 47686 -4.175020e-01 2.779e-01 2.621e-01 4.872e-02 425.539415 sec
85 48247 -4.174371e-01 2.660e-01 4.236e-01 4.615e+01 429.928430 sec
86 48808 -4.174366e-01 2.659e-01 4.222e-01 4.190e-01 434.311830 sec
87 49369 -4.174336e-01 2.646e-01 4.392e-01 5.627e+00 439.225944 sec
88 49930 -4.174336e-01 2.646e-01 4.338e-01 1.780e-01 445.371212 sec
89 50493 -4.174064e-01 2.618e-01 4.772e-01 1.534e+01 450.155256 sec
90 51054 -4.174002e-01 2.606e-01 4.888e-01 5.049e+00 454.630312 sec
First-order Norm of
Iter F-count f(x) Feasibility optimality step Wall-Clock Time
91 51615 -4.174000e-01 2.606e-01 4.908e-01 2.226e-01 459.145445 sec
92 52176 -4.173843e-01 2.565e-01 5.135e-01 2.562e+01 463.482302 sec
93 52737 -4.173802e-01 2.557e-01 5.188e-01 8.166e+00 468.515003 sec
94 53298 -4.173794e-01 2.555e-01 5.241e-01 2.297e+00 473.559581 sec
95 53859 -4.173725e-01 2.544e-01 5.402e-01 9.797e+00 478.314736 sec
96 54420 -4.173725e-01 2.544e-01 5.347e-01 3.173e-02 483.220716 sec
97 54981 -4.173724e-01 2.544e-01 5.423e-01 2.188e-01 487.627148 sec
98 55542 -4.173688e-01 2.533e-01 5.489e-01 1.070e+01 492.265192 sec
99 56103 -4.173550e-01 2.517e-01 5.562e-01 4.054e+01 496.649227 sec
100 56664 -4.173547e-01 2.516e-01 5.610e-01 3.948e-01 501.254397 sec
Solver stopped prematurely.
fmincon stopped because it exceeded the iteration limit,
options.MaxIterations = 1.000000e+02.
Elapsed time is 501.273191 seconds.
Phase 2: Refining with SQP (central differences)...
Iter Func-count Fval Feasibility Step Length Norm of First-order Wall-Clock Time
step optimality
0 1121 -4.173547e-01 2.516e-01 1.000e+00 0.000e+00 1.000e+00 8.260563 sec
1 2242 -4.172438e-01 4.345e-01 1.000e+00 3.604e+01 1.092e+06 16.167734 sec
2 3363 -4.175504e-01 9.347e-02 1.000e+00 1.601e+01 7.049e+05 24.344169 sec
3 4484 -4.165002e-01 1.250e-02 1.000e+00 3.736e+00 3.439e+04 34.446336 sec
4 5605 -4.161709e-01 3.930e-04 1.000e+00 1.210e+00 3.572e+04 43.012066 sec
5 6726 -4.161538e-01 1.808e-06 1.000e+00 4.708e-02 6.118e+02 51.035595 sec
6 7847 -4.161565e-01 7.656e-09 1.000e+00 1.687e-03 8.723e+00 59.840162 sec
7 8968 -4.161667e-01 5.292e-08 1.000e+00 6.854e-03 8.723e+00 67.953638 sec
8 10089 -4.162174e-01 1.279e-06 1.000e+00 3.350e-02 8.725e+00 76.325299 sec
9 11210 -4.164680e-01 3.137e-05 1.000e+00 1.665e-01 8.730e+00 84.353692 sec
10 12331 -4.176265e-01 6.775e-04 1.000e+00 7.675e-01 8.759e+00 92.968797 sec
11 13452 -4.183070e-01 2.263e-04 1.000e+00 4.460e-01 8.783e+00 100.834763 sec
12 14573 -4.183100e-01 3.139e-07 1.000e+00 1.985e-02 8.786e+00 108.304693 sec
13 15694 -4.183162e-01 3.996e-08 1.000e+00 4.812e-03 8.786e+00 117.531052 sec
14 16815 -4.183472e-01 1.019e-06 1.000e+00 2.426e-02 8.783e+00 125.353272 sec
15 17936 -4.185025e-01 2.571e-05 1.000e+00 1.217e-01 8.766e+00 133.224863 sec
16 19057 -4.192709e-01 6.307e-04 1.000e+00 6.017e-01 8.684e+00 141.195177 sec
17 20178 -4.211673e-01 3.826e-03 1.000e+00 1.479e+00 8.481e+00 149.490289 sec
18 21299 -4.231606e-01 5.429e-03 1.000e+00 1.774e+00 8.174e+00 158.030884 sec
19 22420 -4.243557e-01 2.094e-03 1.000e+00 1.118e+00 7.969e+00 166.272562 sec
20 23541 -4.269814e-01 1.043e-02 1.000e+00 2.502e+00 7.512e+00 175.218990 sec
21 24662 -4.280772e-01 1.985e-03 1.000e+00 1.145e+00 7.964e+00 183.654793 sec
22 25783 -4.302618e-01 7.690e-03 1.000e+00 2.183e+00 9.132e+00 192.933418 sec
23 26904 -4.316770e-01 3.121e-03 1.000e+00 1.426e+00 9.858e+00 202.252079 sec
24 28025 -4.318731e-01 2.301e-05 1.000e+00 1.411e-01 9.843e+00 211.426197 sec
25 29146 -4.322261e-01 6.727e-05 1.000e+00 2.485e-01 9.769e+00 222.243835 sec
26 30267 -4.339875e-01 1.645e-03 1.000e+00 1.224e+00 9.443e+00 232.428845 sec
27 31388 -4.353336e-01 1.098e-03 1.000e+00 9.525e-01 9.175e+00 241.513719 sec
28 32509 -4.354026e-01 7.915e-06 1.000e+00 7.354e-02 9.136e+00 251.707794 sec
29 33630 -4.354403e-01 1.391e-06 1.000e+00 2.887e-02 9.133e+00 261.936065 sec
Iter Func-count Fval Feasibility Step Length Norm of First-order Wall-Clock Time
step optimality
30 34751 -4.356304e-01 3.500e-05 1.000e+00 1.453e-01 9.115e+00 273.023163 sec
31 35872 -4.366065e-01 9.210e-04 1.000e+00 7.466e-01 9.023e+00 281.939109 sec
32 36993 -4.393060e-01 7.167e-03 1.000e+00 2.099e+00 9.234e+00 291.035233 sec
33 38114 -4.398945e-01 3.679e-04 1.000e+00 5.895e-01 9.409e+00 299.649127 sec
34 39235 -4.399314e-01 5.320e-06 1.000e+00 6.203e-02 9.396e+00 309.706707 sec
35 40356 -4.400506e-01 3.948e-05 1.000e+00 1.657e-01 9.364e+00 319.261694 sec
36 41477 -4.400547e-01 1.701e-08 1.000e+00 7.484e-03 9.365e+00 327.874336 sec
37 42598 -4.400723e-01 3.648e-07 1.000e+00 2.984e-02 9.368e+00 336.919554 sec
38 43719 -4.401023e-01 8.612e-07 1.000e+00 4.918e-02 9.373e+00 347.216289 sec
39 44840 -4.401066e-01 2.553e-08 1.000e+00 6.616e-03 9.374e+00 354.858225 sec
40 45961 -4.401278e-01 5.883e-07 1.000e+00 3.123e-02 9.377e+00 363.080897 sec
41 47082 -4.401305e-01 9.810e-08 1.000e+00 1.367e-02 9.377e+00 372.077047 sec
42 48203 -4.401422e-01 2.168e-06 1.000e+00 6.479e-02 9.380e+00 380.944657 sec
43 49325 -4.402005e-01 3.479e-06 1.000e+00 3.135e-01 9.391e+00 390.219735 sec
44 50447 -4.405110e-01 1.756e-04 1.000e+00 1.672e+00 9.453e+00 398.584982 sec
45 51569 -4.412485e-01 4.681e-04 1.000e+00 3.938e+00 9.593e+00 408.421007 sec
46 52691 -4.416130e-01 4.115e-04 1.000e+00 1.944e+00 7.333e+00 418.845568 sec
47 53814 -4.421556e-01 6.241e-04 7.000e-01 2.903e+00 9.461e+00 427.486490 sec
48 54936 -4.425523e-01 1.841e-04 1.000e+00 1.996e+00 1.093e+01 436.704741 sec
49 56059 -4.428445e-01 2.657e-04 7.000e-01 1.489e+00 1.204e+01 446.245227 sec
50 57181 -4.439766e-01 3.116e-04 1.000e+00 5.755e+00 1.644e+01 455.749870 sec
51 58302 -4.439815e-01 3.229e-08 1.000e+00 1.241e-02 1.612e+01 464.043495 sec
52 59423 -4.439864e-01 7.698e-08 1.000e+00 1.586e-02 1.611e+01 471.876546 sec
53 60544 -4.440105e-01 1.970e-06 1.000e+00 7.996e-02 1.519e+01 479.968781 sec
54 61665 -4.441292e-01 4.740e-05 1.000e+00 3.921e-01 1.514e+01 488.005796 sec
55 62787 -4.447341e-01 9.586e-05 1.000e+00 2.003e+00 1.484e+01 497.295159 sec
56 63909 -4.473365e-01 1.691e-03 1.000e+00 8.604e+00 5.348e+01 506.106147 sec
57 65031 -4.497773e-01 2.203e-03 1.000e+00 7.037e+00 4.857e+01 515.317927 sec
58 66155 -4.520053e-01 2.399e-03 4.900e-01 6.049e+00 4.870e+01 523.812165 sec
59 67277 -4.542807e-01 4.156e-04 1.000e+00 6.219e+00 5.196e+01 531.996046 sec
Iter Func-count Fval Feasibility Step Length Norm of First-order Wall-Clock Time
step optimality
60 68399 -4.548998e-01 1.185e-04 1.000e+00 2.295e+00 2.918e+01 541.952949 sec
61 69524 -4.555093e-01 2.264e-04 3.430e-01 2.192e+00 3.164e+01 550.519851 sec
62 70646 -4.583755e-01 1.383e-03 1.000e+00 1.001e+01 5.305e+01 559.497745 sec
63 71767 -4.591604e-01 1.033e-03 1.000e+00 2.252e+00 5.986e+01 568.410389 sec
64 72889 -4.621612e-01 1.434e-03 1.000e+00 8.660e+00 5.090e+01 576.544223 sec
65 74015 -4.636953e-01 2.130e-03 2.401e-01 4.483e+00 2.057e+02 584.331859 sec
66 75138 -4.667082e-01 4.185e-03 7.000e-01 9.250e+00 1.030e+02 591.489364 sec
67 76260 -4.684931e-01 1.336e-03 1.000e+00 5.208e+00 7.774e+01 598.942845 sec
68 77384 -4.705185e-01 1.005e-03 4.900e-01 5.868e+00 6.597e+01 606.620088 sec
69 78508 -4.730614e-01 1.840e-03 4.900e-01 7.433e+00 1.747e+02 614.899953 sec
70 79630 -4.779040e-01 5.828e-03 1.000e+00 1.403e+01 9.856e+01 622.570509 sec
71 80752 -4.795499e-01 8.360e-04 1.000e+00 4.783e+00 8.434e+01 629.781870 sec
72 81875 -4.819072e-01 2.504e-03 7.000e-01 6.744e+00 7.835e+01 637.765368 sec
73 82998 -4.858611e-01 4.179e-03 7.000e-01 1.122e+01 1.048e+02 645.803066 sec
74 84120 -4.883798e-01 2.701e-03 1.000e+00 7.131e+00 7.615e+01 654.012242 sec
75 85243 -4.907070e-01 1.518e-03 7.000e-01 6.472e+00 5.452e+01 663.171470 sec
76 86365 -4.929942e-01 1.979e-03 1.000e+00 6.309e+00 4.650e+01 670.801131 sec
77 87486 -4.930289e-01 2.835e-06 1.000e+00 9.003e-02 1.728e+01 677.962332 sec
78 88607 -4.930829e-01 9.417e-06 1.000e+00 1.271e-01 1.728e+01 685.111381 sec
79 89728 -4.933485e-01 2.327e-04 1.000e+00 6.281e-01 1.730e+01 692.992461 sec
80 90850 -4.946022e-01 3.638e-04 1.000e+00 2.929e+00 1.737e+01 700.988805 sec
81 91972 -4.963536e-01 5.883e-04 1.000e+00 4.025e+00 1.745e+01 708.688686 sec
82 93093 -4.964846e-01 6.834e-05 1.000e+00 3.242e-01 1.746e+01 716.256501 sec
83 94214 -4.965962e-01 5.114e-05 1.000e+00 3.020e-01 1.747e+01 723.933288 sec
84 95336 -4.971074e-01 3.125e-05 1.000e+00 1.403e+00 1.753e+01 731.973240 sec
85 96458 -4.993980e-01 1.865e-03 1.000e+00 6.252e+00 1.773e+01 739.824890 sec
86 97581 -5.009750e-01 2.302e-03 7.000e-01 4.323e+00 1.854e+01 747.767033 sec
87 98702 -5.017184e-01 2.221e-03 1.000e+00 2.017e+00 1.298e+01 757.878403 sec
88 99823 -5.017573e-01 3.459e-06 1.000e+00 1.119e-01 1.291e+01 766.245886 sec
89 100944 -5.019033e-01 9.055e-05 1.000e+00 4.325e-01 1.266e+01 775.363973 sec
Iter Func-count Fval Feasibility Step Length Norm of First-order Wall-Clock Time
step optimality
90 102066 -5.025955e-01 1.222e-04 1.000e+00 2.039e+00 1.204e+01 783.763020 sec
91 103188 -5.044447e-01 2.409e-03 1.000e+00 5.444e+00 1.240e+01 792.571120 sec
92 104309 -5.044563e-01 8.537e-07 1.000e+00 3.367e-02 5.418e+00 802.986984 sec
93 105430 -5.044622e-01 4.078e-08 1.000e+00 1.386e-02 5.269e+00 813.434888 sec
94 106551 -5.044921e-01 9.137e-07 1.000e+00 6.564e-02 4.599e+00 822.205574 sec
95 107672 -5.046393e-01 2.238e-05 1.000e+00 3.268e-01 3.110e+00 830.745287 sec
96 108793 -5.053628e-01 5.386e-04 1.000e+00 1.611e+00 3.106e+00 839.133529 sec
97 109915 -5.078857e-01 2.220e-03 1.000e+00 5.769e+00 2.147e+02 848.479667 sec
98 111037 -5.103121e-01 9.176e-04 1.000e+00 5.653e+00 9.101e+01 858.445732 sec
99 112160 -5.130784e-01 4.364e-03 7.000e-01 6.709e+00 1.004e+02 867.653555 sec
100 113281 -5.152263e-01 5.448e-03 1.000e+00 5.209e+00 1.095e+02 876.010588 sec
Feasible point with lower objective function value found, but optimality criteria not satisfied. See output.bestfeasible..
Solver stopped prematurely.
fmincon stopped because it exceeded the iteration limit,
options.MaxIterations = 1.000000e+02.
Elapsed time is 876.064320 seconds.
Max equality constraint violation: 9.14e-07
Max inequality constraint violation: -1.48e-10
Step length: 0.5045 m
Step time: 0.6171 s
Average speed: 0.8175 m/s
The resulting infeasibilities in the robot movement are close to 0, which means that the initial and final conditions match except for movement in the forward direction. Therefore, the robot can continue to walk forward indefinitely, as shown in five cycles of the robot gait.
animate_biped(sol,p)
Hip height range: [0.759, 0.800] m Swing foot height range: [0.000, 0.104] m Step length: 0.504 m
See Also
fmincon | rigidBody (Robotics System Toolbox) | rigidBodyJoint (Robotics System Toolbox) | rigidBodyTree (Robotics System Toolbox) | setFixedTransform (Robotics System Toolbox)
Topics
- Mobile Robot Modeling (Robotics System Toolbox)
