Penalize base linear and angular accelerations

[awinkler]

Problem:

Currently there is no cost on the 6 splines defining the linear and angular acceleration of the base. There are constraints on the base (range-of-motion, dynamic), but if these constraints can be fulfilled and there are still variables left over for the optimizer to set, these can cause jerky, non-natural looking motions.

Possible Solution:

One way to avoid this is to not use too many polynomials to defining the motion, taking away those extra degrees-of-freedom from the solver. However, for quicker, more dynamic motions, those "extra" polynomials might be necessary simply to fulfil all the constraints. If we remove them from the start, no solution for this type of motion might be found.

So a more general approach would be to add an abundance of polynomials defining each spline, giving the optimizer a lot of freedom to shape it, but add also a cost that tries to minimize the acceleration of this spline. Implementation detail: The node variables only include position and velocity, not acceleration, so the cost has to build the full polynomial from the node values and then query the acceleration.

[underactuated]

Hi Alex,

Maybe I am not quite grasping the issue, but should not this be straightforward? You have equations of motion (Eq 2 in your paper) that express the linear and angular accelerations in terms of forces and positions, which are parametrized with polynomials in your approach. Can you just add a squared acceleration term to your non-linear optimization cost? Am I missing something?

[awinkler]

Hi,

Yes, you understand the gist of it! However, the polynomials are parameterized by position and velocity nodes, so there are not acceleration variables directly available. So the acceleration has to be calculated from two sets of nodes, e.g. x0,xd0 & x1,xd1. This makes the formulation a bit more complicated. Also for supplying the analytical derivative of the change of acceleration w.r.t. any of the nodes (x0,xd0,x1,xd1) and the duration deltaT1 requires a bit of math. The last thing you have to keep in mind is at which times you want to evaluate the acceleration and sum them up. Does it make sense to have them evenly placed on the time axis, or evaluated at the nodes. This affects how the deltaTs influence the accelerations.

But thinking this through once and writing down a generic acceleration cost for pos-vel-parameterized splines should be handy for a variety of tasks.

[underactuated]

However, the polynomials are parameterized by position and velocity nodes, so there are not acceleration variables directly available. So the acceleration has to be calculated from two sets of nodes, e.g. x0,xd0 & x1,xd1.

I still do not quite understand. If you have a dynamics model, then you know (in principle) the accelerations at any moment, as long as the positions, velocities and forces are given. Now, since you parametrize your trajectory using splines (polynomials), it will depends on all the parameters of that polynomial (so on four nodes, x0,x1,x2 and xT (plus velocities), not just two nodes, correct?). And yes, then you need to decide how to sum up over the time. You can probably also integrate it out analytically. Anyway, I may be missing details, but overall it seems doable.

So, if I understand correctly, it should not be difficult to add soft constrains to the optimization. For example, I would like to impose constraints on the robot orientation. I do not like how it does not walk facing straight, instead turning kind of sideways. It seems to be an artifact of using boxes for the kinematic constraints.

[awinkler]

So, if I understand correctly, it should not be difficult to add soft constrains to the optimization. For example, I would like to impose constraints on the robot orientation. I do not like how it does not walk facing straight, instead turning kind of sideways. It seems to be an artifact of using boxes for the kinematic constraints.

Yes absolutely, I've had soft constraints in the problem before. If you want to reuse some existing code, checkout the class soft_constraint.h and possibly combine it with base_motion_constraint.h.

I still do not quite understand. If you have a dynamics model, then you know (in principle) the accelerations at any moment, as long as the positions, velocities and forces are given.

Yes. you can also penalize base accelerations like that. However, these terms will be a lot less sparse, as the acceleration now depends on the feet and forces as well. Possibly looking only at individual spline accelerations defined by the enclosing node values is easier. Then you need no dynamic model, no knowledge of the feet positions, forces, etc. This would also allow you to penalize feet accelerations, which are not part of the dynamic model. I'm not sure which one will work best, I would just start with one.

[EricWang1hitsz]

So, if I understand correctly, it should not be difficult to add soft constrains to the optimization. For example, I would like to impose constraints on the robot orientation. I do not like how it does not walk facing straight, instead turning kind of sideways. It seems to be an artifact of using boxes for the kinematic constraints.

Yes absolutely, I've had soft constraints in the problem before. If you want to reuse some existing code, checkout the class soft_constraint.h and possibly combine it with base_motion_constraint.h.

I still do not quite understand. If you have a dynamics model, then you know (in principle) the accelerations at any moment, as long as the positions, velocities and forces are given.

Yes. you can also penalize base accelerations like that. However, these terms will be a lot less sparse, as the acceleration now depends on the feet and forces as well. Possibly looking only at individual spline accelerations defined by the enclosing node values is easier. Then you need no dynamic model, no knowledge of the feet positions, forces, etc. This would also allow you to penalize feet accelerations, which are not part of the dynamic model. I'm not sure which one will work best, I would just start with one.

Hi, I want to know how to add the soft constraint to the optimization problem?

[EricWang1hitsz]

@awinkler Yes! That's a good idea, help me understand them. Thanks very much! Have a good day

[junhyeokahn]

Any code implementation regarding this? @awinkler, rather than analytically computing acceleration at each node, can I just penalize the velocity difference between two consecutive nodes? And I guess this is equivalent to penalizing the integral of acceleration over the spline between two nodes.

[cyoahs]

Any code implementation regarding this? @awinkler, rather than analytically computing acceleration at each node, can I just penalize the velocity difference between two consecutive nodes? And I guess this is equivalent to penalizing the integral of acceleration over the spline between two nodes.

I think the answer is, yes and no. Penalizing the integral of acceleration is equivalent to the velocity difference, but the effective value is the absolute difference or the integral of absolute acceleration, rather than the origin values. A simple example can be (v_1-v_0) + (v_2-v_1) + ... + (v_n-v_{n-1}) = v_n-v_1, the entire process is neglected in this summation.

~~However, anthoer concern for analytical acceleration is that for two adjoin polynomial, only the zero and first order of derivatives are smooth. The acceleration can be different at t=t_1- and t=t_1+, for example.~~

Sorry for the wrong opinion above.