new feature : convex hull constraint

[PedroDesRobots]

Hi,

I want to implement a convex hull constraint (inequality constraint) in TSID.

I got some code that works well in Inverse Kinematics.

So first, let me introduce the goal of this constraint. The idea is to perform a cartesian constraint on x and y plan directly on the Center of Mass based on the support polygons of the current feet state.

It defines a set of contact point which define the support polygon, and then it calculates the different line coefficient () with CoM like reference frame.
With these coefficients, it's easy to compute something in IK like :

For example with 4 points for the support polygon, I obtained : with A [4x2] ( and coef), b=[4x1] ( coef) and J_com is [2xDoF] (x and y components).

So now, I would like to extend this in Inverse Dynamics. From my guess, I just used the same equation but expressed with the acceleration :

So far, I tried to implement that, but it stills not working efficiently. I mean the robot stills go out of its convex hull. Could you tell me if this current formulation is OK or completely wrong/miss something?

[andreadelprete]

Hi @PedroDesRobots

the typical reasoning goes like this. You have a 2d polygon defined by a set of inequality constraints $A x \leq b$. You would like a specific quantity of the robot configuration (e.g., the CoM $c$) to remain inside this polygon. To achieve this you have to constrain its acceleration $\ddot{c}$. This problem resembles a lot the one of joint position bounds, which I've treated in [1]. Transforming the position bounds into velocity bounds (as you did) is rather simple. The problem is when you have to translate them to acceleration bounds, because there you need to account for viability, i.e. the capability of the system to satisfy the constraints in the future.

However, before I go into further details, let me ask you a question: why do you wanna do this? What's the goal? Because if the goal if balancing, then you shouldn't constrain the CoM, but the capture point, as in [2]. The situation here might be a bit different to deal with because the capture point depends also on the CoM velocity.

[1] https://andreadelprete.github.io/publication/2017_ral_delprete/ [2] Ramos, O. E., Mansard, N., & Soueres, P. (2014). Whole-body Motion Integrating the Capture Point in the Operational Space Inverse Dynamics Control. In IEEE-RAS International Conference on Humanoid Robots (Humanoids).

[PedroDesRobots]

Hi @andreadelprete

Thank you for your quick reply and your resources.

So first of all, I would like to teleoperate a humanoid robot in a quasi-static position, which means no walking. The controller doesn't know in advance, which targets the user will provide to the robot. The user is giving torso reference, but if he goes out of the support polygon (infeasible reference for the robot), the robot shouldn't go out of its support polygon.

I would like mainly that the QP controller constraint the CoM position. But the capture point could be another constraint to add for avoiding to apply too fast motion on the robot also.

Is it more clear concerning my goal?

I don't know if it's better to examine on the Capture Point side or keep considering about cartesian position.

[andreadelprete]

I think capture point is better, and it should be also easier because since it depends also on the CoM velocity it can be "immediately" changed via the accelerations. Take a thorough look at the paper by Ramos, it should contain everything you need to know. Let me know if you need clarifications.

[PedroDesRobots]

Thank you for you feedback, I have taken a deep look at the paper by Ramos. I implemented the capture point constraint :

with

At the moment, it seems to work but in fact, when the robot CP reaches the limit, the robot stills to move its arms.. then fall when it starts to collide itself. From my guess, 2 inequality constraints (x and y axes) for 30 joints are not enough.

Let me know what you think and if I have done something wrong.

[andreadelprete]

@PedroDesRobots your math seems correct. The problem is probably more subtle. The whole capture-point theory is based on the LIPM assumptions, among which there's the zero angular momentum one, which never holds exactly in practice. This introduces errors, and it's not the only thing that does so. For instance, Ramos did not account for potential violations of the constraints in between time steps: constraints are only satisfied at the end of the current time step.

In practice, to account for all of these small errors, you should be using a smaller support polygon for constraining the CP with respect to the one that you use for constraining you CoP. Do you see what I mean? In this way, the CoP can always move beyond the CP to push it a bit back towards the inside. I think this is the least you should do to make things work. If I remember correctly this is also what I did a few years ago when I had implemented this in the preliminary Python version of TSID pinocchio_inv_dyn. Here was the implementation of the CP inequalities.

[PedroDesRobots]

In practice, to account for all of these small errors, you should be using a smaller support polygon for constraining the CP with respect to the one that you use for constraining you CoP. Do you see what I mean?

Not really, I understand that I can apply a Safety Margin on the support polygon to reduce the size.

If I remember correctly this is also what I did a few years ago when I had implemented this in the preliminary Python version of TSID pinocchio_inv_dyn. Here was the implementation of the CP inequalities.

Thanks for sharing the python code. I have taken a look at the code. It seems that you are not using the drift term in the computation. When you are calling B_sp et b_sp, are you using the line coefficient or directly the x y coordinate of the polygon's point? I saw different methods like compute_convex_hull and compute_support_polygon and seem to compute line coefficients.

In my case and are dim [2x1] expressed in the world frame. Should I replace by the line coefficient ?

[andreadelprete]

In practice, to account for all of these small errors, you should be using a smaller support polygon for constraining the CP with respect to the one that you use for constraining you CoP. Do you see what I mean?

Not really, I understand that I can apply a Safety Margin on the support polygon to reduce the size.

The key idea of the capturability theory is that the CoP is bounded inside the support polygon, you assume the Linear Inverted Pendulum Model (LIPM) dynamics for the CoM, and with a bit of math you can show that as long as the capture point (CP) remains inside the support polygon, then you are capturable (i.e. you can avoid falling). My suggestion is the following: to account for inaccuracies you should apply a safety margin on the support polygon that you use to constrain the CP. In this way, the CoP will be allowed to be in a larger support polygon (the original one), which should allow it to recover the capture point in case it goes out of the restricted polygon.

I have taken a look at the code. It seems that you are not using the drift term in the computation.

You're right. Probably I was just lazy! :)

When you are calling B_sp et b_sp, are you using the line coefficient or directly the x y coordinate of the polygon's point? I saw different methods like compute_convex_hull and compute_support_polygon and seem to compute line coefficients.

I don't know exactly what you mean with "line coefficient" and "polygon's point". B_sp and b_sp are the matrix and the vector defining the support polygon as the set of points x such that B_sp * x + b_sp >= 0.

[PedroDesRobots]

Thank you for the clarification on the capturability theory. I have applied a huge margin : 5cm on each side of the support polygon. The stabilization is better when I increase this value but constraint to much the CoM at the end.. I need to find a fair trade-off between stabilization and mobility.

I don't know exactly what you mean with "line coefficient" and "polygon's point". B_sp and b_sp are the matrix and the vector defining the support polygon as the set of points x such that B_sp * x + b_sp >= 0.

How did you define/calculate those set of points in B_sp and b_sp ? from my guess, we can compute line coefficients which returns the implicit form of the line passing between two points : ax + by + c = 0

p0 = [x1, y1] and p1 = [x2, y2]
a = y1 - y2
b = x2 - x1
c = -b*y1 -a*x1

[andreadelprete]

5 cm seems too much to me. It should work even with a smaller margin, such as 1 cm. You should also have a task to regulate the angular momentum to zero. If you don't, try to add that.

[olivier-stasse]

You might be interested in knowing that @NoelieRamuzat is currently implementing a centroidal dynamic controller which is a linear+angular momentum derivative controller.

[PedroDesRobots]

@andreadelprete, Thank you a lot for your feedback. I have added a task to regulate the angular momentum to zero, it aids a lot to perform the constraint on the capture point! In my case, the x-axis (front/back) with a 1cm margin works well but on the y-axis (lateral) 3cm is the minimum. Are you interested to add the capture point task in TSID? If yes, I can do a PR.

@olivier-stasse, I will be interested in the centroidal dynamic controller. Are you implementing this in TSID?

[andreadelprete]

Yes please, do a PR! Thanks!

[olivier-stasse]

@andreadelprete Yes Noelie is currently doing it.

[andreadelprete]

Good to know. @olivier-stasse What would be the difference with respect to having a Com task + an angular momentum task?

[olivier-stasse]

None you are right. But I think we might have an interesting example.

[andreadelprete]

This has been implemented with the PR of @PedroDesRobots so we can close this issue