The maximum number of successive solution rejections has been reached for Cassie integration

[wchengh2010]

Hi,

We are trying to integrate bipedal robot Cassie into Ocs2 based on the legged robot example of ANYmal. We have some issues with Cassie being able to stand with convergence and the solver gives the error below:

Our setup, the center of mass and the locations of four support/contact points of the feet with nominal contact forces are shown below:

The center of mass is within the support polygon while the error is given. We tried both single body centroidal dynamics and full body centroidal dynamics. We tried to tune the cost coefficients and removed some constraint conditions to relax the optimization. None of them worked.

We have been stuck here for a few weeks. Any suggestions to fix this issue from the algorithmic point of view? Thank you very much!

[rubengrandia]

Hi,

Great that hear that you are trying out the toolbox with Cassie!

A good place to start debugging is to set this flag to true: https://github.com/leggedrobotics/ocs2/blob/b037d819c9a02a674de9badb628b32646ce11d6a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info#L69 What will happen is that the linear quadratic approximation of the cost, dynamics, etc will be checked for the main assumption that are made in the Algorithm. The Q must be positive semi definite, the R must be positive definite etc. etc.

Since your error occurs in the LEVENBERG_MARQUARDT algorithm. A second suggestion is to try LINE_SEARCH instead of LEVENBERG_MARQUARDT. This is configured here: https://github.com/leggedrobotics/ocs2/blob/b037d819c9a02a674de9badb628b32646ce11d6a/ocs2_robotic_examples/ocs2_legged_robot/config/mpc/task.info#L89 The default is usually set to LINE_SEARCH, did you change this on purpose?

[wchengh2010]

Hi Ruben,

Thank you very much for your reply. It is really helpful for us to figure out what's the problem with our setup. We are now able to get Cassie stand and walk in place, see gif below. It turned out that our issue is the ZeroVelocity constraints linear independence. Although we also have four 3D contact points (the same as anyMAL), they are two pairs of which each resides on a same link. Therefore, there are dependency between the contact points' velocities, so ZeroVelocity constraints can't guarantee full row rank. We changed the codes and made sure that the added ZeroVelocity constraints had full row rank and the ddp optimization can run without problems now. We also changed back to LINE_SEARCH.

However, when we tried to command Cassie to walk with gait backwards, the following error occurred.

As you could see from the figure, the front leg tried to move backwards while the rear leg supported the base on ground. Three questions from us:

1. With regards to the reported error, any suggestions for debugging from the algebraic point of view?

2. Since Cassie has closed loop kinematics chains (we didn't add it yet), any suggestions to add these to the optimization? There are two ideas from us right now: 1) We tried to integrate the pinocchio-preview3 which includes the feature of constraint dynamics with closed loop chains, so we could add it to the centroidal dynamics directly. 2) We could add it as two soft inequality constraints. Any comments on this since you are probably much more familiar with the code base than us?

3. About the following lines of codes in the 'initializeInputCostWeight' function of the 'LeggedRobotInterface.cpp' file, it seems that R is transformed from joint space to contact space, so if I understand correctly, instead of using as the cost for joint velocity input, here (where is the contact points' linear velocities) is used as the cost. Is there a specific reason for this? Since the number of actuated joints in Cassie is different from 12, therefore, by using the following lines of codes, we will get dimension inconsistency issue for matrix multiplication. Currently, we just use the diagonal R loaded from the task.info file and as the cost term.

Thank you very much!

[rubengrandia]

Nice progress!

1. If the error shows up only when you send the first command, I would double check if your commanded trajectories are of the correct size, sign, etc. The first target trajectory is derived from the initial state in the LeggedRobotDummyNode.cpp. But when you send a new target from the commandline interface, the trajectory comes from LeggedRobotPoseCommandNode.cpp. Have a look there that you are loading and sending the correct state and input trajectories.

2. If the kinematic loops can be solved analytically, I would choose a minimal representation in your state and have a function that converts between that reduced state and the full state with kinematic loops. You can then compose this function at all places that you need the full state. For example: p_foot = kinematics(reduced2full(x)). Then automatic differentiation should take care of the rest.

Another option if you want to have all coordinates in the state and their velocities in the input vector, is the add a stateInput constraint that implements the closed kinematics chain on velocity level. This should still be full rank if you add the same number of constraints as there are additional degrees of freedom. Furthermore, you would probably have to add some feedback term in the constraint to take care of drift over time. Compared to the other approach this one might be a bit slower (higher state dimension), but easier to implement.

We have not yet looked at the closed loop feature in pinnochio, so I cannot say much about that.

Adding the constraint as a penalty might be numerically not very nice. You will have to make the penalty quite large to have sufficient accuracy and this will make your problem ill-conditioned.

3. The reason for multiplication is just to make the robot walk nicer. If the joint space cost works well for Cassie this is also fine. Still, putting a task space cost should also be possible for different amount of joints, but the hardcoded sizes should be changed.

[rubengrandia]

Closing this issue, feel free to open a new one if new problems have shown up.