Comments (16)
None you are right. But I think we might have an interesting example.
from tsid.
the typical reasoning goes like this. You have a 2d polygon defined by a set of inequality constraints
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).
from tsid.
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.
from tsid.
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.
from tsid.
Thank you for you feedback, I have taken a deep look at the paper by Ramos. I implemented the capture point constraint :
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.
from tsid.
@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.
from tsid.
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 ?
from tsid.
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
etb_sp
, are you using the line coefficient or directly the x y coordinate of the polygon's point? I saw different methods likecompute_convex_hull
andcompute_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
.
from tsid.
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
from tsid.
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.
from tsid.
You might be interested in knowing that @NoelieRamuzat is currently implementing a centroidal dynamic controller which is a linear+angular momentum derivative controller.
from tsid.
@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?
from tsid.
Yes please, do a PR! Thanks!
from tsid.
@andreadelprete Yes Noelie is currently doing it.
from tsid.
Good to know. @olivier-stasse What would be the difference with respect to having a Com task + an angular momentum task?
from tsid.
This has been implemented with the PR of @PedroDesRobots so we can close this issue
from tsid.
Related Issues (20)
- contact-6D: unclear - could be pulling force ? HOT 6
- ForceTask with moving object HOT 11
- GitHub Action HOT 3
- Cannot surpress SolverHQuadProg output HOT 3
- TaskJointPosVelAccBounds not exposed to the python API HOT 3
- Adding equality constraints between joints HOT 9
- Test optional dependencies during CI HOT 1
- conda install broken ? HOT 4
- undefined symbol error in libtsid_pywrap.so HOT 2
- SE3 Inequality Task or Constraint HOT 1
- Large time steps HOT 4
- github action on OSX conda Release failing on osqp solver HOT 2
- TSID Performance Issue, slow HQP solves HOT 9
- Does it contain c++ examples? HOT 4
- auto formating precommit hook HOT 1
- display error
- Plans for Pinocchio3.x integration? HOT 3
- build issue with catkin build ROS1 HOT 6
- Python Binding for TaskActuationEquality HOT 4
- Compatibility Issue with Latest Pinocchio Version and TSID HOT 1
Recommend Projects
-
React
A declarative, efficient, and flexible JavaScript library for building user interfaces.
-
Vue.js
🖖 Vue.js is a progressive, incrementally-adoptable JavaScript framework for building UI on the web.
-
Typescript
TypeScript is a superset of JavaScript that compiles to clean JavaScript output.
-
TensorFlow
An Open Source Machine Learning Framework for Everyone
-
Django
The Web framework for perfectionists with deadlines.
-
Laravel
A PHP framework for web artisans
-
D3
Bring data to life with SVG, Canvas and HTML. 📊📈🎉
-
Recommend Topics
-
javascript
JavaScript (JS) is a lightweight interpreted programming language with first-class functions.
-
web
Some thing interesting about web. New door for the world.
-
server
A server is a program made to process requests and deliver data to clients.
-
Machine learning
Machine learning is a way of modeling and interpreting data that allows a piece of software to respond intelligently.
-
Visualization
Some thing interesting about visualization, use data art
-
Game
Some thing interesting about game, make everyone happy.
Recommend Org
-
Facebook
We are working to build community through open source technology. NB: members must have two-factor auth.
-
Microsoft
Open source projects and samples from Microsoft.
-
Google
Google ❤️ Open Source for everyone.
-
Alibaba
Alibaba Open Source for everyone
-
D3
Data-Driven Documents codes.
-
Tencent
China tencent open source team.
from tsid.