question about the optimization result of rabbit example in FROST

[yuan0623]

Hi, I have one problem about the optimization results about rabbit example in FROST. In RightStance.m, I change tau = (t-p(2))/(p(1)-p(2)); into tau = (x('BasePosX')-p(2))/(p(1)-p(2)); and I also change the TimeBased phase-type into StateBased phase type.
The optimization runs just fine.
When I look into the result, I found something weird.
I assume the optimized Bezier curve coefficient is stored in params.atime of [tspan, states, inputs, params] = exportSolution(nlp, sol); Based on what I've learned, the first coefficient of Bezier curve should equal to the initial corresponding state of the optimized orbit. the coefficient of Bezier curve should equal to the last corresponding state of the optimized orbit. But it's not the case in the optimization result.
Here is the coefficient of the 4 5th-order Bezier curves (24 numbers in the param.atime, not sure what does atime mean)

• 2.71618571692464 0.515515095636679 2.71705785916497 1.02499554796246 2.86538716761488 0.468324521468795

• 2.48324948627866 1.15124079095115 2.98496637234353 0.499653975816616 2.24823964188322 0.897687575012935

• 3.03613034792144 0.646974647833675 2.42657908023512 0.0167244308523125 3.10412530275568 0.801064202668094

• 3.27017750951884 -0.147216276707749 2.74238646146068 3.79910980485377 1.35969708221527 -0.260488859460432

And there is the first column of states.x

• 0.160669780340591 0.741546376010217 0.146173027697750 2.45916544725954 0.645968711581861 2.95613077740528 0.597108636820039

As you can see, the first coefficient of each Bezier curve is not equal to x4-x7.

My question is:

1. what does the results of optimization mean?

2. where can I find the value of p in tau = (t-p(2))/(p(1)-p(2));, I suppose p is also being optimized?

3. has there any one used FROST to optimize planar multi-domain walking?

[ayonga]

There could be a few cases this result happens:

1. your tau is not equals to 0 at the beginning of the domain;
2. you did not enforce the hybrid invariance constraints, i.e., y(0) = 0 and ydot(0) =0.

[yuan0623]

Ayonga, thanks so much for your reply! I thought for an orbit planning problem, every one will assume tau starts from 0. I also though if I do the hybrid trajectory optimization, the hybrid invariance constraint is the default setting.
So my next questions are:

1. how do I enforce tau starts from 0 and ends at 1
2. how to enforce the hybrid invariance constraints????
   I didn't see it in other examples. :(

[yuan0623]

I think the hybrid invariance constraint is enforced in
r_stance.UserNlpConstraint = str2func('right_stance_constraints');
r_impact.UserNlpConstraint = str2func('right_impact_constraints');
In the right_stance_constraints, the author have domain.VirtualConstraints.time.imposeNLPConstraint(nlp, [bounds.time.kp, bounds.time.kd], [1,1], load_path); . but the results is still not correct.
I think it can be the tau is not start from 0, how do I check the range of the tau in the optimization result? Have no clue :(

[yuan0623]

@ayonga
I have just tried the default version for rabbit optimization. I did not modified anything. The phase variable is time based.
In the results, params{1,1}.ptime=[0.4956,0]., and the tspan is from 0 to 0.4956. Based on the definition of tau=(t-p(2))/(p(1)-p(2)), it should start from 0 to 1;
Then based on what you said, the problem should be the enforcement of hybrid invariance constraints, but in the code right_stance_constraints.m, it does set the constraint. Then everything thing should be good. However, the result still doesn't make sense.
The first column of x is
-0.1678 0.7400 0.1690 2.4270 0.6454 2.9454 0.5880
the coefficients of Bezier curve are :
2.4271 0.6455 2.9453 0.5880 2.5426 0.5941
2.9781 0.7263 2.6988 0.4451 2.8123 1.0771
2.7080 0.5621 2.5740 1.1960 2.8510 0.4166
2.3711 1.0778 2.9458 0.5877 2.4274 0.6458.
As you can see, the first coefficient of first Bezier curve is really close to x(4), whick make sense, however, the rest looks bad to me. Is there any bug on that existing rabbit example? I really can not figure it out myself.
Thanks you!

[ayonga]

You did not correctly reformat the Bezier coefficient, you should use reshape(params{1}.atime,4,6), which will give you
2.4269 2.5425 2.6987 2.7077 2.8508 2.9457
0.6454 0.5941 0.4452 0.5623 0.4165 0.5875
2.9453 2.9781 2.8124 2.5739 2.3709 2.4272
0.5879 0.7262 1.0771 1.1959 1.0778 0.6458
where the first column of x would be:
-0.1678
0.7400
0.1692
2.4269
0.6454
2.9453
0.5879
.

[yuan0623]

Thank you so much! That is simply awesome! I think we can close this case for now.. Orz