CODEWITHSUNDEEP
drake
zisangsang
zisangsang

Reputation: 81

How to improve the trajectory optimization results

I want to do trajectory optimization with the Atlas robot model in Drake. Fallow the litterdog example and this repository, I tried the jump motion optimization here. Basically, I changed the in_stance sequence, added Initial guess of q, and added a different q_cost.

But it seems hard to get the optimal solution, the SNOPT output is:

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

Here is a video to show the optimized trajectory.

I have some thoughts to explore the way to improve the solution:

  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.
  5. Add some cost to stable the flight phase of the leg.

I am not sure which one I should do first, or did I missed something important that the problem cannot be solved？
Thanks for your advice!

Thanks for your quick reply @Hongkai Dai. I checked for the matlab version and find the discussion at here and the testJump.m.

I have added an initial guess of q by using the InverseKinematics class. Will try to add contact force trajectory and CoM trajectory initial guess too.

And in the testJump.m, is this your initial guess? It is not a kinematic trajectory initial guess, right?

x_seed = zeros(cdfkp.num_vars,1);
x_seed(cdfkp.h_inds) = 0.06;
x_seed(cdfkp.q_inds(:)) = reshape(bsxfun(@times,qstar,ones(1,nT)),[],1);
x_seed(cdfkp.com_inds(:)) = reshape(bsxfun(@times,com_star,ones(1,nT)),[],1);
x_seed(cdfkp.lambda_inds{1}(:)) = reshape(bsxfun(@times,1/num_edges*ones(num_edges,4,1),ones(1,1,nT)),[],1);
x_seed(cdfkp.lambda_inds{2}(:)) = reshape(bsxfun(@times,1/num_edges*ones(num_edges,4,1),ones(1,1,nT)),[],1);

Upvotes: 0

Views: 378

Answers (1)

Hongkai Dai
Hongkai Dai

Reputation: 2766

  1. Better initial guess helps a lot. You can try to first solve a kinematic trajectory (without considering the forces), and then initialize your trajectory optimization variables with the kinematic trajectory.
  2. When the optimization is infeasible, you can check which constraints are violated at the infeasible solution. Using function result.GetInfeasibleConstraints(prog), it returns the list of violated constraints, and then you can try to relax/remove the constraints. You can call prog.RemoveConstraint() function to remove a constraint, or relaxing the constraint bounds by using constraint.UpdateLowerBound(), constraint.UpdateUpperBound().

For Atlas jumping motion, we do have a Matlab implementation. If you check out the old matlab version of Drake, then in the folder drake/matlab/solvers/trajectoryOptimization/dev/testJump.m, it implements trajectory optimization for Atlas jumping up motion, using the idea in this paper

In testJump.m, I didn't use a kinematic trajectory as initial guess. I computed a initial posture of the robot standing on the ground, with contact forces uniformly distributed on each leg.

There are other nuances in testJump.m:

  1. I scaled the decision variables for the contact forces by mass * g. So the decision variable value for the contact force is roughly in the order of 1. In the currently version of Drake, you can try SetVariableScaling() method in MathematicalProgram.
  2. In testJump.m, the orientation of the floating base was represented by the Euler angles. In the current version of Drake, the floating base orientation is represented by unit quaternions. The time integration of the quaternion is a lot trickier than Euler angles. You can try QuaternionEulerIntegrationConstraint for the unit quaternion, and the rest of the joints can use Euler/Midpoint integration as usual.

Upvotes: 2

Related Questions