teword better trajectory

oofrivia commented 3 years ago

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 commented 3 years ago

@rcywongaa again in case you didn't get noticed

rcywongaa commented 3 years ago

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 commented 3 years ago

@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 commented 3 years ago

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

Use a simplified model. I think the problem may be easier to solve.
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.
Add a better initial guess for all the variables.
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 commented 3 years ago

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

oofrivia commented 3 years ago

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

rcywongaa commented 3 years ago

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).

Use a simplified model. I think the problem may be easier to solve.

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.

Add a better initial guess for all the variables.

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

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)
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 commented 3 years ago

@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~