Optimization failure for a rolling disk using `ContactImplicitTrajectoryOptimization`

[zamzami]

Hi everyone, I'm trying to find an optimized trajectory (state, control and contact) for a simple actuated 2D rolling disk on a FlatTerrain. The disk is dropped from a certain height and should roll z meters to arrive at xf So I Just modified the testNewTrajOpt.m for the KneedCompassGait by changing the URDF file and providing adequate traj_init & BoundingBoxConstraint for x0 and xf while removing the periodic constraint.

The problem is that optimization terminates with the flag info=41 which according to SNOPT manual means current point cannot be improved,SNOPT was unable to improve on a non-optimal point the given result is physically unrealistic.

I have managed to add infeasible_constraint_name to the ContactImplicitTrajectoryOptimization.m class while naming the dynamic constraints and the NonlinearComplementarityConstraint.m. Then I found out that optimization violates almost all the dynamic constraints on qdot as well as the contact NCP constraints.

Is there a remedy for such a problem? is there a way to enforce the dynamic constraints? is there a way to add the dynamics to the objective funnction? Do I need to hard code the dynamics gradient of the plant?

PS: If I run setCheckGrad I get maximum gradient error is in row 132 for x[74], with error 72738.446987 user gradient 0.000000, numerical gradient 72738.446987

I appreciate your effort I understand that you are extremely busy for the DRC, all best wishes.

[mposa]

Hi Zamzami, SNOPT requires accurate analytic gradients to function well and the algorithm ContactImplicitTrajectoryOptimization makes use of collision detection code. For the examples provided, we've limited ourselves to point contacts. Drake handles more complex contacts, such as with a disc, by the third-party library Bullet. Unfortunately, we've found that Bullet's algorithms do not necessarily provide an output that is always nicely differentiable.

Natively supporting more collision types is something that we'd like to do, but isn't yet high priority. I did, however, take a look at the code and have a suggestion that might work.

1) Use a sphere geometry for your 2D disc 2) In RigidBodySphere.m, change line 98 from pts = []; to pts = getPoints(obj) - [0;0;obj.radius] 3) (May not be necessary) When constructing the Manipulator object from URDF, add the option option.use_bullet = false

This will enable non-zero radius spheres to collide against flat terrain objects. From the top of my head, I think everything else may go smoothly.

[zamzami]

Hi Michael,

Thank you for the suggestion, it has partly solved the problem. I Implemented it by adding an option options.replace_cylinders_with_spheres similar to the one actually implemnted replace_cylinders_with_capsules. Now I have a non-empty cached_terrain_contact_points_struct which provides better gradient.

So the result is that I recieve now ìnfo=13 which signifies that nonlinear infeasibilities minimized due to unsatisfied constraints. Perhaps this could be due to the ill-posed problem or conflicting constraints. Optimisation Results: Without Height Constraint: The disc violates the non-penetration constraint and and few dynamic constraints (unrealistic bouncing) check out the video

With Height Constraint: If I add a LinearConstraint on the z state with lb=ub=radius I will get a more visually appealling behaviour check out the video with a lot more constraints infeasibilities regarding dynamics of the height state.

The height constraint is unnatural and I hope not to use it as it can no longer be valid for more complicated scenarios with rough terrain. My Question Is there a way to enforce the dynamic constraints? Can there be a prioritization of constraints? Have you tried playing with the parameter Elastic weight associated with elastic programming (SNOPT's approach for handling constraints infeasibilities)

Thank you

[zamzami]

So I thought that I should come back and explain how I partially solved the issue before closing it.

The solution, suggested earlier, of modelling the collision geometry of a planar rolling disc/sphere as a fixed point on the circumference w.r.t its center to avoid bullet's collision detection proved to be not adequate. So what actually happens in the simulation is that the rolling disk/sphere actually contact the ground at that point then the point of contact acts as a center of rotation for the body till it penetrate the ground (Visually, as the rest of the body is not defined in the collision geometry)then it jumps to the next point and so on as shown in the video.

My approach is a multi-contact per body (disk, sphere) leveraged with bullet's Collision Detection (CD). So basically what i did is that I implemented a loop (using ruby's each iterator) in the rsdf file to generate a urdf of the disk/sphere withnspheres distributed along the circumference that act as the collision geometry as shown in the figure. Obviously, the major advantage here is that it is applicable on uneven terrain as well. Bullet's CD behaves well with non-zero radius spheres as discussed in issues #790 & #181

So the result is actually quite promising; the optimisation ended sucessfully with zero infeasible constraints. The computation time is better than that for the single point collision geometry for moderate tolerance levels (MajorOptimalityTolerance , MajorFeasibilityTolerance, MinorFeasibilityTolerance =1e-6) despite the fact that the lambda vector is now augmented by n*N instead of 1*N . The result is shown in this video.

On a side note, I guess that the downside of SNOPT's SQP solver is that it will always prioritize linear constraints (such as initial and final/goal state) over nonlinear contraints including the dynamics. This happens when SNOPT encounter infeasible constraints, it will start by resolving the linear constraints then it enters into the elastic programming mode where it uses Elastic weight to relax the nonlinear constraints (The Dynamics).