ComDynamicsFullKinematicsPlanner class

[ddliu365]

I am working trajectory optimization. Is there any possibility that you will add ComDynamicsFullKinematicsPlanner class which is similar in Matlab version in the repo?

[hongkai-dai]

Thanks @ddliugit for your interest. I am afraid TRI won't add ComDynamicsFullKinematicsPlanner in C++ soon, as we don't work on humanoid robot now. Does the Matlab version still work for you?

[ddliu365]

I download the version"last-sha-with-original-matlab", but I cannot find the instruction of how to install that. Do you keep the instruction about installing the old version now?

[hongkai-dai]

The current drake doesn't keep instruction about installing old version.

But in your downloaded version, if you look at the file drake-last_sha_with_original_matlab/drake/doc/from_cource.rst, it should have a section `Build the collection", which contains the instruction to build Drake for the version you downloaded. You could check other other revisions of Drake, and they all put the installation instruction in that file.

If you use Ubuntu machine, then I would recommend you follow the instruction on Build with Make.

[ddliu365]

Thank you very much @hongkai-dai. I will go through that carefully. Actually, I found this repo through your paper titled with "Whole-body Motion Planning with Centroidal Dynamics and Full Kinematics".

Did you use the function in drake to work on atlas' trajectory optimization with Matlab?

[hongkai-dai]

Thanks for your interest in our work! Yes, the Atlas results from that paper are obtained through the class CoMDynamicsFullKinematicsPlanner in drake with Matlab.

I am not sure if the code still runs. But the class ComDynamicsFullKinematicsPlanner can be used as a reference if you want to implement the function yourself.

[ddliu365]

@hongkai-dai Thanks for your reply. I found a similar little dog example which is useful for implementation, but I am working on a humanoid robot. Is there an atlas' trajectory optimization example open with Matlab? If TRI don't have plan to add trajectory optimization with c++ implementation soon, I can help to contribute that implementation if I have the old code.

[hongkai-dai]

There is an example in examples/Atlas/test/testRunningPlanner which calls CoMDynamicsFullKinematicsPlanner to plan a running gait.

[ddliu365]

@hongkai-dai Thanks for your reply! I installed the software from here. Run 'runAllTests.m' for about 70 minutes and it gets stuck at 'testRunningPlanner'.

My environment is like so:

OS:Mac Mojave 10.14. 
Matlab version: 2018b.
CPU: I7 4.0 GHz.

Did you remember how long usually it will cost to get an answer for this planner?

Another possibility I think stuck at 'testRunningPlanner' may be caused by following error.

Invalid MEX-file '/Users/dongdong/Downloads/drake-0.9.9-44f46e2f9dcd1860700b3a65681c09f0ceda1c39-mac/drake/thirdParty/path/lcppath.mexmaci64': dlopen(/Users/dongdong/Downloads/drake-0.9.9-44f46e2f9dcd1860700b3a65681c09f0ceda1c39-mac/drake/thirdParty/path/lcppath.mexmaci64, 6): Library not loaded: /usr/local/lib/x86_64/libgfortran.3.dylib
  Referenced from: /Users/dongdong/Downloads/drake-0.9.9-44f46e2f9dcd1860700b3a65681c09f0ceda1c39-mac/drake/thirdParty/path/lcppath.mexmaci64
  Reason: image not found.

Since running all test in Matlab, this error appears. Solver needs external image(or library) of SNOPT?

Another issue I found just now is that at the beginning of test SNOPT dependency is not found.

LCM: Disabling IPV6 support
LCM: TTL set to zero, traffic will not leave localhost.
Cannot find required pod snopt

Because you mentioned that this problem is solved with SNOPT library, should the stuck of solving is caused by missing SNOPT solver?

Also, because Matlab version is not maintained, I am not sure if you can remember this issue. If you can, kindly let me know.

[hongkai-dai]

I never used precompiled version of Drake.

I use Drake on Ubuntu with SNOPT. It is possible that the pre-compiled drake in MATLAB doesn't use SNOPT. It should took just a minute to finish the trajectory optimization.

The error you got Cannot find required pod snopt means that Drake uses another nonlinear solver, probably fmincon, which is very slow for this trajectory optimization problem.

The error

Invalid MEX-file '/Users/dongdong/Downloads/drake-0.9.9-44f46e2f9dcd1860700b3a65681c09f0ceda1c39-mac/drake/thirdParty/path/lcppath.mexmaci64': dlopen(/Users/dongdong/Downloads/drake-0.9.9-44f46e2f9dcd1860700b3a65681c09f0ceda1c39-mac/drake/thirdParty/path/lcppath.mexmaci64, 6): Library not loaded: /usr/local/lib/x86_64/libgfortran.3.dylib
  Referenced from: /Users/dongdong/Downloads/drake-0.9.9-44f46e2f9dcd1860700b3a65681c09f0ceda1c39-mac/drake/thirdParty/path/lcppath.mexmaci64
  Reason: image not found.

means that it cannot find the pathlcp solver. This solver is used in simulating rigid body dynamics. It is not used in trajectory optimization.

Sorry I can't offer much help to fix the SNOPT issue. I would recommend to use the implementation CoMDynamicsFullKinematicsPlanner just as a reference. You could try to download snopt from their website (some universities might have an academic versions, and I think SNOPT still provides a free version with limits on 300 decision variables and constraints). You could then add the snopt folder to your MATLAB path (make sure that you have compiled snopt and obtained a mex file for MATLAB).

[ddliu365]

@hongkai-dai Thanks for your detailed reply!

I got a copy of SNOPT 7.7 by sending a request to UCSD, but I got the same error reported in #6123 , then I just followed the procedure reported in #1488. However, I got the same error reported in #6123 again.

Error using snoptmex
Wrong number of input arguments

Error in snopt (line 250)
[x,F,inform,xmul,Fmul,xstate,Fstate,itn,mjritn] = snoptmex( solveopt, x, ...

Error in NonlinearProgram/snopt (line 1411)
        [x_free,objval,exitflag,xmul,Fmul] = snopt(x0_free, ...

Error in NonlinearProgram/solve (line 980)
            [x,objval,exitflag,infeasible_constraint_name] = snopt(obj,x0);

Error in testRunningPlanner (line 430)
  [x_sol,~,exitflag] = cdfkp.solve(x_seed);

In #6123, @GuiyangXin just switched to 7.2 version to solve that problem. Due to the license, I am not sure if I can get the old version from your group.

Is there any way to fix the problem of snopt arguments with latest version?

The line 250 in snopt.m is like so:

[x,F,inform,xmul,Fmul,xstate,Fstate,itn,mjritn] = snoptmex( solveopt, x, ...
                          xlow, xupp, xmul, xstate, ...
                          Flow, Fupp, Fmul, Fstate, ...
                          ObjAdd, ObjRow, A, iAfun, jAvar, ...
                          iGfun, jGvar, userFG, probName, ...
                          mlSTOP );

The line 1411 in NonlinearProgram.m is like so, here I just follow the way in #1488 and added [] to the empty argument:

        [x_free,objval,exitflag,xmul,Fmul] = snopt(x0_free, ...
          x_lb_free,x_ub_free,[],[],...
          lb,ub,[],[],...
          'snoptUserfun',...
          0,1,...
          Avals,iAfun,jAvar,...
          iGfun_free,jGvar_free);

The line 980 in NonlinearProgram.m is like so:

        switch lower(obj.solver)
          case 'snopt'
            [x,objval,exitflag,infeasible_constraint_name] = snopt(obj,x0);

From UCSD, I got the release version of source folder from their release website and a mex file called snoptmex.mexmaci64.

Currently, I just use the wrapper and add the snoptmex.mexmaci64 intomatlab folder and run build. Then run addpath_snopt in Matlab.

Is there any other way to install the software instead of using this wrapper? I am not clear about how drake use snopt solver.

I think the problem may be caused by the conflict between the wrapper in this repo and latest snopt mex file.

I am not clear about the last 5 variables in NonlinearProgram.m, namely, Avals,iAfun,jAvar, iGfun_free,jGvar_free);. I checked other variables which match well with the latest version after adding [] to the empty variable.

The instruction in the new snop.m is like so:

function [x,F,info,xmul,Fmul,xstate,Fstate,output] = snopt(x, xlow, xupp, xmul, xstate,...
                          Flow, Fupp, Fmul, Fstate,...
                          userfun, varargin);

for the varargin part, the comment in the snopt.m is like so:

% A              is either a struct type or a matrix (dense or sparse)
%                that defines the constant elements in the Jacobian of F.
%
%                If A is a struct, the structure is provided in
%                coordinate form with fields:
%                   A.row
%                   A.col
%                   A.val
%                If i = A.row(k) and j = A.col(k), then A.val(k) is the (i,j)-th
%                element in A.
%
% G              is a struct type or a matrix (dense or sparse) defining
%                the nonlinear elements in the Jacobian of F.
%
%                If G is a struct, the Jacobian structure is provided in
%                coordinate form with fields:
%                   G.row
%                   G.col
%                If G is a matrix, then nonzero elements in the matrix
%                denote the nonzero elements of the nonlinear elements of
%                the Jacobian.
%
%            More IMPORTANT details:
%            1) The indices (A.row,A.col) must be DISJOINT from (G.row,G.col).
%               A nonzero element in F' must be either an element of G or an
%               element of A, but not the sum of the two.
%
%            2) If the user does not wish to provide A.row, A.col, G.row,
%               G.col, then snopt() will determine them by calling snJac().
%
%               WARNING: In this case, the derivative level will be set to zero
%               if constant elements exist.  This is because the linear
%               elements have not yet been deleted from the definition of
%               userfun.  Furthermore, if G is given in vector form, the
%               ordering of G may not necessarily correspond to (iGfun,jGvar)
%               computed by snJac().

After reading the comment, I still don't know to how construct A, G argument with Avals,iAfun,jAvar, and iGfun_free,jGvar_free. Another way to solve this problem is to use the solve_snopt to replace snop directly in NonlinearProgram.m. It seems more similar to your original code.

function [x,F,inform,xmul,Fmul,xstate,Fstate,output] = solve_snopt(start,stopFun,name,userfun,x, ...
                          xlow, xupp, xmul, xstate,...
                          Flow, Fupp, Fmul, Fstate,...
                          ObjAdd, ObjRow, ...
                          A, iAfun, jAvar, iGfun, ...
                          jGvar)

What do you think about where the problem is?

[ddliu365]

I figured that out and wrote a small patch code in snopt.m to map the argument in old code to the function of latest version . It works!

However, the solution failed!

Warning:   13 nonlinear infeasibilities minimized

> In NonlinearProgram/mapSolverInfo (line 1551)
  In NonlinearProgram/snopt (line 1427)
  In NonlinearProgram/solve (line 980)
  In testRunningPlanner (line 430) 
Elapsed time is 20.314129 seconds.
Error using testRunningPlanner (line 435)
Trajectory optimization failed. exitflag = 13 

Do you have any idea why it fails?

[hongkai-dai]

Thanks for the investigation! Snopt 7.7 and snopt 7.2 have different implementations. We observed that the nonlinear optimization generates different results (solved to optimal vs local infeasibility) after we upgrade our snopt version.

You could try to tweak the initial guess of the optimization problem. Snopt is a nonlinear solver, and the nonlinear solver performance depends heavily on the initial guess.

[hongkai-dai]

BTW, if you have iAfun, jAvar, Avals, then you could set A as a struct, and

A.row = iAfun;
A.col = jAvar;
A.val = Avals;

[ddliu365]

@hongkai-dai Thank you very much!

I just tested it with struct and it runs, too.

Make up the remaining code for other people in case they need to use the old version of drake with new SNOPT.

SNOPT 7.7
Drake v0.9.10.
environment: macOS

In the NonlinearProgramming.m: insert following code after line 1409:

        A.row = iAfun;
        A.col = jAvar;
        A.val = Avals;

        G.row = iGfun_free;
        G.col = jGvar_free;

The other way is to insert the following code after line 420 in snopt.m in snopt_matlab wrapper.

elseif nargin == 17,
  ObjAdd = varargin{1};
  ObjRow = varargin{2};
  valA   = varargin{3};
  iAfun  = varargin{4};
  jAvar  = varargin{5};
  iGfun  = varargin{6};
  jGvar  = varargin{7};

Either of above works!

I just used your solution as the initial value and it works successfully! (silly method, just want to test if the program runs well)

Tweaking the initial guess seems quite difficult and needs special technique. The default option is to insert linear points between initial value and final value. Intuitive idea to me is use spline line to generate these points between start and end points.

I am not sure if I am correct.

Do you have any good idea to tune that?

[hongkai-dai]

Thanks for the fix!

Right as you said, tweaking the initial guess can be difficult. There is not a principled approach to tweak the initial guess. I mostly do it by trial and error. For this specific problem, you could do an interpolation between the initial state and the final state, and then set the contact forces to be roughly balancing the gravitational forces.

As your code runs, is it OK if we close this issue? We can re-open it if you run into more problems.

[ddliu365]

I truly appreciate your patient help! Thanks again! @hongkai-dai

[ddliu365]

I have another question, can I use ComDynamicsFullKinematicsPlanner in an implicit contact trajectory optimization problem?

I checked the testRunningPlanner, it seems a sequential contact order should be indicated before the optimization problem, namely, the contact is explicit.

However, if I want to optimize a problem such as falling, it's impossible to indicate the contact sequence explicitly. Is there a similar problem to refer in drake?

I just go through ComDynamicsFullKinematicsPlannerclass and find that testRunningPlanner didn't use the function function obj = addContactDynamicConstraints(obj,knot_idx,contact_wrench_idx,knot_lambda_idx), but it seems to implement the complementary constraints. In which case should the function be used?

[hongkai-dai]

In that folder drake/solvers/trajectoryOptimization, there are several files start with Complementarity, for example ComplementarityStaticContactConstraint.m. These classes impose the complementarity constraint so that you can do implicit trajectory optimization. In line 331 of ComDynamicsFullKinematicsPlanner.m, it adds the complementarity constraint, if you specify the complementarity_flag = True.

[ddliu365]

@hongkai-dai Thanks for your reply! For complementarity function, I ignore the abstract method in SimpleDynamicsFullKinematicsPlanner. The function addContactDynamicConstraints is actually called in its parent class. I checked through the complementarity constraints and find they are static, but if I want to use them in sliding case of friction cone, will that work? e.g. , when objects contact each other, if the tangent force between too surface is large enough to overwhelm the friction, it may slide on the surface. Would the static complementarity constraint take this situation into account?

[hongkai-dai]

Sorry for my belated reply. I haven't implemented the sliding friction version yet. You could add a complementarity constraint on the sliding friction. The idea is that the if the sliding velocity is zero, then the force is within the friction cone. If the sliding velocity is non-zero, then the tangential force should be in the opposite direction of the sliding velocity, and the friction force is on the boundary of the friction cone. You could refer to Michael's paper "A Direct Method for Trajectory Optimization of Rigid Bodies Through Contact" for a detailed formulation.

[ddliu365]

Thanks for your reply. I go through that paper and it's clear now.

I adjust the initial pose of the robot (not your solution) and it can calculate an answer now!

But when I use the controller runAtlasRunning.m to verify my solution, I found bodyMotionControlmex file is missing.

Currently I can only found the *.cpp version instead of its compiled version. I plan to recompile it and the error where I am stuck is like so:

Undefined symbols for architecture x86_64:
  "_mexCallMATLABWithObject", referenced from:
      isa(mxArray_tag const*, char const*) in drakeMexUtil.cpp.o
      mexCallMATLABsafe(int, mxArray_tag**, int, mxArray_tag**, char const*) in drakeMexUtil.cpp.o
  "_mexCallMATLABWithTrapWithObject", referenced from:
      mexCallMATLABsafe(int, mxArray_tag**, int, mxArray_tag**, char const*) in drakeMexUtil.cpp.o
  "_mexErrMsgIdAndTxt", referenced from:
      mexCallMATLABsafe(int, mxArray_tag**, int, mxArray_tag**, char const*) in drakeMexUtil.cpp.o
...

ld: symbol(s) not found for architecture x86_64
clang: error: linker command failed with exit code 1 (use -v to see invocation)

Do you know the reason?

[ddliu365]

I made a mistake that I should use the flag while generating make file.
-DCMAKE_BUILD_TYPE=Release, but other error appears still.

I fix arm, head joints, then remove their constraints(gazing constraint, etc) and only keep the leg part. It doesn't work.

I guess you add the gaze constraint because we can limit the movement of the body which can help the snopt solve the problem quickly?

For your last answer, I am not clear about balancing the gravity with contact force while setting up initial solution. Your setup in the initial seed(line 405 in testRunningPlanner.m):

lambda_seed = sqrt(2)/24;

Is this code relevant to the above idea? The number sort(2)/24 is kind of confusing.

Thanks for the fix!

Right as you said, tweaking the initial guess can be difficult. There is not a principled approach to tweak the initial guess. I mostly do it by trial and error. For this specific problem, you could do an interpolation between the initial state and the final state, and then set the contact forces to be roughly balancing the gravitational forces.

As your code runs, is it OK if we close this issue? We can re-open it if you run into more problems.

[hongkai-dai]

Sorry I don't quite understand the linking error, it complains about _mexCallMATLABWithObject, but this is a MATLAB built in function.

For lambda_seed = sqrt(2)/24, the goal is to have the contact force to balance the gravitational force in the initial guess. There are 8 contact points on two feet, each contact point has a linearized friction cone of 3 edges, each linearized friction cone has a friction coefficient of 1 (namely the angle of the friction cone is 45 degrees, and cos(45 degree) = 1 / sqrt(2)). So by setting lambda_seed = sqrt(2) / 24, the total contact force is 8 3 lambda_seed cos(45 degree) mg = mg, so as to balance the gravitational force.

To make the decision variable values on the same scale, that lambda is not the contact force, but the weight of each edge of the friction cone, normalized by mg.

I guess you add the gaze constraint because we can limit the movement of the body which can help the snopt solve the problem quickly

I added the gaze constraint since otherwise there is too much motion on the upper torso, I don't want the upper torso to shake a lot during running.

[ddliu365]

@hongkai-dai .Thanks for your detailed reply, I see.

I assume lambda is contact force after reading your paper, but in your code it is the coefficient of the linearized edge, which you use beta_ij to indicate in the paper.

I found errors while compiling it on Mac, but it seems better on Ubuntu(No error appears right now except Gurobi solver is not installed for that library). Which version did you use with Ubuntu then? I will prefer to use the same version to prevent further errors from happening.

I import a small robot model different from Atlas. I try to find the solution, but it doesn't work. The following is the result I got:

In the picture, the solver gave me two basins, which means that robot just jumps down, then jumps up, and jumps down , up again, but while running it should be one basin like the solution given by Atlas. Is there any idea to remove it by tuning the initial solution? Would adjusting the knots of touch toes and heels affect the result? Another possible reason is caused by the initial foot height?

I tried it many times and it failed. I found the final result of atlas is kind of symmetry, like a parabola. Is there any reason why this works? And how to tune it to become that?

Another confusing part is that I don't know how to define the speed and stride length of the robot.

For the initial pose, I just copy the pose of Atlas to the new robot, would that matter?

What is the max iterations did you usually set for the solver when you adjust the parameters or initial solution?

Do you refer the log of snopt solver to tune the parameter?

Sorry for so many questions. Thanks again!

[hongkai-dai]

Which version did you use with Ubuntu then?

I think I was on Ubuntu 12.04 LTS when I wrote the paper. The code should work on other versions of Ubuntu also.

In the picture, the solver gave me two basins, which means that robot just jumps down, then jumps up, and jumps down , up again, but while running it should be one basin like the solution given by Atlas. Is there any idea to remove it by tuning the initial solution? Would adjusting the knots of touch toes and heels affect the result? Another possible reason is caused by the initial foot height?

The peak between these two basins are quite low, just 1 cm above the basins, does this peak matter? I don't know your robot, but I don't see immediately why you want to remove this peak.

I tried it many times and it failed. I found the final result of atlas is kind of symmetry, like a parabola. Is there any reason why this works? And how to tune it to become that?

I don't think we explictly added any constraints to make it symmetric or parabola. The tweaking we did are mostly on the kinematic part of the robot, like the using a gaze constraint on the upper torso to prevent it from swinging.

Another confusing part is that I don't know how to define the speed and stride length of the robot.

You could define the speed as the final CoM translation divides by the total time, you could then impose a constraint on this ratio (by writing your own constraint). For the stride length, you could impose a WorldPositionConstraint on the foot contact location.

For the initial pose, I just copy the pose of Atlas to the new robot, would that matter?

That is probably not a good thing, if the two robot have different DoFs. I would solve an inverse kinematics problem to find an initial pose of your robot, that the robot is in static equilibrium.

What is the max iterations did you usually set for the solver when you adjust the parameters or initial solution?

The setup for the solver options (like iterations) are in line 419 of testRunningPlanner.m. You could see I didn't care too much about the optimality (the optimality tolerance is 5e-4).

Do you refer the log of snopt solver to tune the parameter?

Referring to the log is helpful. By looking at the merit function or "Feasible" part, I can see how close I am to the feasibility. If the solver spends many iterations without decreasing the "Feasible" tolerance, I will just use another initial seed as the starting point. Another useful debugging approach is to add a callback to your optimization, that in each iteration it plots out something (like CoM trajectory).

[ddliu365]

@hongkai-dai Thanks!

Solving inverse kinematics for static equilibrium really gave me a good result. The feasible error is reduced from 9.8e-2 to best 1.9e-3 now, but it cannot give a desired trajectory.

I recorded the result after the tolerance achieved 1.9e-3. It seems the feet just move randomly, which is different from Atlas solution.

For Atlas:

My robot foot doesn't fix on the ground during instance period like Atlas, and the toe direction is not towards forward, which makes me be curious why Atlas' feet always fixed on the ground while touching and always in the one direction. I didn't change your constraints except arm relevant constraints and gaze constraint.

This robot doesn't have arms(only 12 DOF on feet), so I reduced the half periodic constraint with arms. Would that cause the random movements of feet? I guess half periodic constraint will not affect the movement of torso, because these constraints only apply on the joints instead of the end effectors like hands.

Another problem is swing movement of torso. Because there is no arm, the gaze constraints cannot be applied. Instead of gazing constraint I just set bounding box constraint for three rotation variables of the torso, namely, q[4:6,N], would that be good?

I just try this bounding box method, and actually it seriously affects the performance of the solver, which stuck at a certain solution earlier.

Do you have any good idea to solve these problems?

I am not sure how to setup the translation of torso(base of robot, since atlas has extra DoFs in body), and you seems to setup the torso translation to make CoM be zero at x and y direction in the world coordinate system. Is there any reason for that? About the height of torso(z direction), because atlas begins at certain height above the ground, do you remember how to set it up initially?

In solving Atlas process, the com height in the fist node is always the same as the height at the last node, which is different from my robot. The height at the last node is always moving up and down.

[ddliu365]

For the rotation of foot while touching ground, I guess it is caused by missing of equal matrix for the torso in half_periodic_constraint = LinearConstraint(lb,ub,[symmetric_matrix;equal_matrix]);I deleted that before and the rotation of feet seems caused by the rotation of body.

After adding gazing constraint and making up half periodic constraint, the problem of moving feet randomly on the ground is better.

Another issue is that the solver always tries to find a solution with larger height of center of mass than initial seed, which makes the robot smooth the initial pose totally, i.e. no knee bending, both of legs straight during running.

The actual robot's com is about 0.31 before running the optimization. However, the optimizer always stays about 0.365 even at the beginning, which will make the optimizer do inverse kinematics to totally change the initial seed I guess.

The desired solution is that optimizer should find solution under initial com height while robot touches the ground.

I try to fix the initial pose q[3:end,1] to initial seed like below, then the solver must find solution under the initial seed, but it failed.

cdfkp = cdfkp.addConstraint(ConstantConstraint(q0(9:end)), ...
      cdfkp.q_inds(9:end,1));

The error is like so:

Warning:   41 current point cannot be improved

> In NonlinearProgram/mapSolverInfo (line 1558)
  In NonlinearProgram/snopt (line 1434)
  In NonlinearProgram/solve (line 980)
  In testRunningPlanner (line 372) 
Elapsed time is 7.292226 seconds.
Error using testRunningPlanner (line 377)
Trajectory optimization failed. exitflag = 41

I may miss something or happen to add some constraints which I don't notice. What do you think?

Below is initial pose and snap shots for one solution which is not complete.

[hongkai-dai]

which makes me be curious why Atlas' feet always fixed on the ground while touching and always in the one direction

There should be a kinematic constraint that the feet doesn't move on the ground. Drake's Atlas example is solved successfully, so the foot don't move. In your case, as the problem is not solved to a feasible solution yet, the output trajectory violates this "foot no move" constraint.

You could enforce the constraint "feet always in one direction", by imposing a WorldGazeDirConstraint

Instead of gazing constraint I just set bounding box constraint for three rotation variables of the torso, namely, q[4:6,N], would that be good?

That might work for your robot. From your plot, it seems that that your torso is your floating base. But q[4:6] is the Euler angles of the torso, which is not exactly the same as the gaze constraint (but might prevent the torso swinging problem). I would suggest to start with a minimal set of constraints (like the foot should touch the ground at the beginning), and only add other constraints (torso no swinging, foot in one direction, ect) when you see bad behaviors from the output trajectory, so as to present these bad behaviors by imposing constraints.

I am not sure how to setup the translation of torso(base of robot, since atlas has extra DoFs in body), and you seems to setup the torso translation to make CoM be zero at x and y direction in the world coordinate system. Is there any reason for that?

You could set a bounding box constraint on the x/y position of the torso at the beginning, and a bounding box constraint on the x/y position at the end of the trajectory. I set the constraints on the CoM x/y position, but you could also try to bound the torso position.

About the height of torso(z direction), because atlas begins at certain height above the ground, do you remember how to set it up initially?

I think I solved an IK problem to find the initial pose of Atlas. In that IK problem, I have the constraint that

1. The CoM is within the support region of the foot (Check QuasiStaticConstraint).
2. The knees are bent (we didn't know how to control Atlas with straight knees back then).

Another issue is that the solver always tries to find a solution with larger height of center of mass than initial seed, which makes the robot smooth the initial pose totally, i.e. no knee bending, both of legs straight during running.

You could add a constraint that the knee should bent during running.

[jwnimmer-tri]

@hongkai-dai Could you help us clarify the topic of this issue? What are the victory conditions necessary to close it? Is this a feature request? If so, what is the feature? Is there any chance we will work on it? I'm curious whether we should close this issue.

[hongkai-dai]

I consider this issue closed. We have CentroidalMomentumConstraint inside Drake, and Russ also has an example of little dog doing centroidal dynamics planning in underactuated repo https://github.com/RussTedrake/underactuated/blob/master/examples/littledog.ipynb