teword better trajectory

[oofrivia]

hello! I am here to discuss this with you again.
Do you have any advice on generating a better trajectory, that is at least good enough for a controller to apply to a robot?

I tried your Planner.py, and based on it I changed the in_stance sequence, initguess of q, and add different q_cost.
The optimizer does generate a jumping motion, but with large joint vibration, and I think it is not applicable to the controller.
I am not sure what to ask if I start a StackOverflow question, so I want to discuss it with you first.

If you are interested in the jumping motion, I am glad to push the code to github.

[oofrivia]

@rcywongaa again in case you didn't get noticed

[rcywongaa]

Sorry for the delay, I must have misconfigured my notifications to keep missing issues...

I'm also struggling to fix this issue, as mentioned in the README, the solver actually does not solve until optimum, instead terminates with the following error message

SNOPTA EXIT  80 -- insufficient storage allocated
SNOPTA INFO  83 -- not enough integer storage

I am still looking into the problem, looking into ways to increase the integer storage size.

I'd be curious of the jumping motion you managed to achieve with your modified settings, perhaps you can fork and/or give me the link to your repo so I can have a look? It would be especially interesting if your settings managed to solve the problem to optimum. You can look at the generated snopt.out and search for SNOPTA to see if snopt managed to solve to optimum.

[oofrivia]

@rcywongaa hello! I forked your repository and pushed the jump planner code here. But on mine computer the SNOPT output is:

SNOPTA EXIT 10 -- the problem appears to be infeasible
SNOPTA INFO 13 -- nonlinear infeasibilities minimized

I think the solved problem is far from optimum. We added centroidal dynamics contrains, Kinematic constraints, etc, but I think we need add more cost and set more specific cost weigh Q to guide the solver.

[oofrivia]

I have some thoughts to explore the way to solve the trajectory:

1. Use a simplified model. I think the problem may be easier to solve.
2. Take a closer look at each constraint. For example, in the jumping case, I think it is better to constrain the x and z axis angular_momentum and leave the y axis angular_momentum unconstrained.
3. Add a better initial guess for all the variables.
4. Add reference trajectory to the cost to guide the solver.

And, maybe it is time to ask Prof. Russ Tedrake for help! I am going to discuss with them at StackOverflow and see if they have any advice.

[oofrivia]

By the way, do you use Zhihu (a Chinese social media)? I find someone with the same nickname as you.

[oofrivia]

And here is the StackOverflow post
https://stackoverflow.com/questions/68698187/how-to-improve-the-trajectory-optimization-results

[rcywongaa]

SNOPTA EXIT 10 -- the problem appears to be infeasible
SNOPTA INFO 13 -- nonlinear infeasibilities minimized

From this message, it is certain that the optimization failed. Likely it is stuck at a local minimum or there are some errors in the constraints (or a combination of both and more). Since it failed to find a feasible solution, the solution you see is violating some constraints and is likely physically impossible (e.g. the strong joint vibrations you see).

1. Use a simplified model. I think the problem may be easier to solve.
2. Take a closer look at each constraint. For example, in the jumping case, I think it is better to constrain the x and z axis angular_momentum and leave the y axis angular_momentum unconstrained.
3. Add a better initial guess for all the variables.
4. Add reference trajectory to the cost to guide the solver.

All of these are good ideas.

(1) might be done by locking some joints or replacing it with a robot with fewer joints. It might be tricky though since you still want to maintain enough joints for the robot to actually be able to execute the jump.

(2) would also help and is also likely to solve the infeasibility problem. However, I have yet to find an efficient way of troubleshooting constraints. Most have been based on gut-feel, trial and error, and blind luck with frequent unfruitful results. I'd also like to learn how to do this properly... Looking at your implementation though, I have a few comments

1. Be careful when constraining the contacts. I learnt that constraining multiple contact points to contact / lift off at the exact same moment often made the problem infeasible (which kinda make sense because there will always be microsecond delays between contacts)
2. Since the solver thinks the problem is infeasible, a large cost will likely not help.

(3) As mentioned by Honkai, this can be very effective in getting the optimizer out of a local optimum. But won't work if the constraints are wrong themselves.

(4) This would be similar to (3), since you'll be guessing all variables across the trajectory (over all time t)

By the way, do you use Zhihu (a Chinese social media)? I find someone with the same nickname as you.

Nope, I don't use Zhihu

[oofrivia]

@rcywongaa sorry for the delay! Back to the trajectory optimization again. Find a post related to the not enough integer storage problem.
https://stackoverflow.com/questions/68245808/failure-of-solving-trajectory-optimization-use-snopt-in-drake-how-to-fix-it

I am still trying to adjust these constraints. So I start from just standing. The main problem I find is the angular_momentum_constraint. In the LittleDog.py, the robot has four contact points, but the Atlas has eight. So the related code in the angular_momentum_constraint should be:

Fn = np.concatenate([contact_force[i][:,n] for i in range(num_contacts)])

contact_force = contact_force.reshape(3, num_contacts, order='F')

for i in range(num_contacts):
p_WF = plant.CalcPointsPositions(context[context_index], contact_frame[i], [0,0,0], plant.world_frame())
com_q = plant.CalcCenterOfMassPositionInWorld(context[context_index])
torque += np.cross(p_WF.reshape(3) - com_q, contact_force[:,i])

The Strange thing is that when adding eight contact points, snopt will fail with Terminated after numerical difficulties. But adding four contact points the solver will success, solving with an incline standing posture. Maybe there are some flaws in the angular_momentum_constraint. For example when computing the torque, I think it should be

com_q = plant.CalcCenterOfMassPositionInWorld(context[context_index])
torque += np.cross(p_WF.reshape(3) - com_q, contact_force[:,i])

rather than

torque += np.cross(p_WF.reshape(3) - com, contact_force[:,i])

Although I replaced this, it didn't work for the Terminated after numerical difficulties.
Actually, I do not understand why the angular_momentum_constraint works in your Planner.py with only four contact points.

Hope these little details help, thinks~

[oofrivia]

@rcywongaa hello, nice to see your updates.
I am working on my jumping optimization recently, now it can solve successfully, but it still needs a lot to improve, for example, shorter timesteps and smooth contact force.
https://github.com/zisang/humanoid/blob/jump/SingleLegMultiBodyTwoContacts.py

[oofrivia]

And I think the not enough integer storage error is just because the problem dimension is too large.
Problem dimension is really important, and I find it is hard to increase the trajectory knot point, which is bothering me now.
The model I used is 9 dofs, and I set 25 knot point. At this problem scale, I also need to normalize variables, set proper initial guesses to make it work.
I can't imagine how hard SNOPT is to solve your problem with nearly thirty dofs and fifty knot point.

[oofrivia]

By the way, in my problem, normalizing the contact force variable is VERY useful, making snopt much easier to solve.

And now I'm quite sure that the angular_momentum_constraint you used in Planner.py is Inappropriate. As in the stance_schedule, both left and right leg have stance phase, but only left leg contact force is used to calculate angular momentum.

[oofrivia]

I tested fix contact force related inappropriateness, and the exit flag changed to ill-conditioned null-space basis, which is a sub flag of Terminated after numerical difficulties. These exit flags are much more common, and I think the reason may be the nonlinearity of the problem itself. Reducing the problem scale and normalizing may help a little.

[oofrivia]

I can push the test if you need.

[rcywongaa]

Hi @zisang , it's very encouraging to see you making progress! Please push the test and make a PR, I'd be very interested in the changes!

Could you elaborate a bit on normalizing the contact force variables? I'm not sure I completely understand. Do you mean adding a constraint that the contact forces must sum to something?

Indeed the angular_momentum_constraints don't seem right, thanks for pointing that out! I'll have to take a look.

[oofrivia]

@rcywongaa I created a pull request. Both the active contact number and the dimension of the GradientMatrix of ad_p_WF need to be changed. As the GradientMatrix is the jacobian matrix of the autodiff variable with respect to other variables in this constraint function.
normalizing in this usage scenario means adding a scale to make the contact force variables between 0~1, which is between 0~4*g*total_mass without scale. The larger difference in the variable scale, the more difficult it is to solve. Dai mentioned this here