How to apply ground collision?

[littleggghost]

Hi, @jmirabel @jcarpent
Afaik, if I want to add a ground collision, then I should add a big box which represents the ground collision box.
But the question is, how many collisionPairs should be added? The ground box will be huge and I don't know where the collision will happen. The concept of collisionPairs is introduced in 4.1) Direct dynamics.
I think the collision point should be detected automatically based on intuition, not set by hands. Because if we introduce curve faces then the contact point can't be predicted.
You can also have a look at issue: #756.
Really hope your reply!

[jcarpent]

I'm not so familiar with FCL. I think @jmirabel may help you for the first issue which is: how to get the collision point between a Mesh and a Plane with FCL. @jmirabel Can you provide some insights for this question?
I will then provide the rest of the answer later for the computation of the contact dynamics.

[jmirabel]

A collision pair is a pair of geometry indices in the geometry model. A collision checking algorithm will be run for each active specified pairs, upon collision queries.
In your case, you must add a collision pair between the world and each robot bodies.

[jmirabel]

Class GeometryModel has several geometries. It can be one for each link of your robot and some more for the environment. A collision pair is a pair of geometries, however complex these geometries are. It is not a pair of points, as you seem to be thinking. It is something like the pair ( "link 4 of the robot", "ground" ), replacing geometry names by their indices.

[jcarpent]

For 1., @nmansard did the writing. @nmansard can you provide some fix to it?
For 2., when you will apply contact dynamics, only the joint velocity or acceleration of the robot will be affected either by the impact or by the contact dynamics. In the equation, the ground variables are not involved. Which also means that the static floor won’t move. It is not the same as the hand you have done in the tutorial.

[jcarpent]

The id of a joint is unique, and independent from the number of DOF a joint.
If you do model.joints[1].id will give you the id of the joint in the model tree (here, model.joints[1].id = 1 corresponding to the first joint).

[jcarpent]

Sorry @littleggghost, but I do get the end of your previous remark. What do you mean by a free-flyer scene?

[jcarpent]

It is hard for me to help you on this point remotely. Maybe, you may provide some source code example and explain inside which problem you are currently facing.

[jcarpent]

@littleggghost Thanks for your example. I will be off these next few days. I will try to have a look at it as soon as possible in order to provide the help you need.

[jcarpent]

@littleggghost I think you may understand that the delay in our answers may vary according to our job load. And personally, I only have few times free to answer questions like yours which are a bit aside of the Pinocchio developments as you might guess.

Concerning your issue, you need to check whenever the two objects collide and then add an impactDynamics with the appropriate Jacobian (the Jacobian of the contact point in fact) and use forwardDynamics to simulate the rigid contact interaction between the two objects, assuming the floor is not moving. This is what internally all rigid body dynamics simulators do. You might also need to implement an LCP solver on top of that, but this out of the current scope of Pinocchio.
If you have some concerns about contact simulation, I encourage you to read some related works on this topic like this Wikipedia article:

• https://en.wikipedia.org/wiki/Contact_dynamics
  or this excellent textbook:
• Numerical Methods for Nonsmooth Dynamical Systems

I hope this will answer your current issue.
By the way, you might be interesting by this tutorial which is doing something very similar to your target application: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_d-practical-exercises_4-dyn.html

[gabrielebndn]

@littleggghost, other than what @jcarpent explained, I add a few comments on your code:

self.q0 = zero(self.model.nq)
# self.q0[3] = 1.0

Notice that the element w of a quaternion is at the end, in Pinocchio's convention. So, this should rather be

self.q0 = zero(self.model.nq)
self.q0[6] = 1.0

but the best is to use Pinocchio's built-in function

self.q0 = se3.neutral(model)

which directly outputs a valid "zero" configuration, no matter your model

---

q = rand(robot.model.nq)

Similar to the point above. This is not the proper way of creating a random configuration for a Pinocchio model. If your model contains non-trivial configuration types (and I see your model contains quaternions) the corresponding configuration will not be normalized. So either you normalize it a posteriori, doing

q[3:7] /= np.linalg.norm(q[3:7])

or you use Pinocchio's built-in function, which will work for any type of configuration

q = pin.randomConfiguration(model)

Notice that in order to use the latter method you need to set joint limits (including for the position of the freeflyer).

---

    b = se3.rnea(robot.model, robot.data, q, vq, aq0) # compute dynamic drift -- Coriolis, centrifugal, gravity
    M = se3.crba(robot.model, robot.data, q) # compute mass matrix M
    M1 = np.linalg.inv(M)
    aq = M1*(tq - b)

For forward dynamics, you can directly use ABA:

aq = se3.aba(robot.model, robot.data, q, vq, tq)

---

vq = se3.integrate(robot.model, vq, aq * dt)

This is a mistake. integrate only integrates the position, not the velocity. Unless your robot is a simple manipulator (which it is not), this line will result in WRONG integration or in a crash.
In order to integrate the velocity, you may simply do

vq += aq * dt

which will work for any type of configuration

[gabrielebndn]

Hi @littleggghost,
sorry I can't help you on that. I suggest you to follow the steps suggested by @jcarpent, which are quite clear, and to take a look at the example pointed out by him, which seems exhaustive, in order to check the implementation. For a simple problem such as computing the impact of a single ball, I expect this to be very simple. You can find the source code for the examples in doc/d-practical-exercises/src.
Notice you need to create the collision objects for the floor and for the ball. Currently, this can only be done through URDF.

[jcarpent]

• About collision check, should I directly calculate the distance of two objects? Or call an function?

You can use the GeometryModel and GeometryData associated to your URDF model in order to compute collision with computeCollisions.

• How to add impactDynamics and contact jacobian?

The contact Jacobian corresponds to the Jacobian of the contact points when you perform a collision detection to get the list of the current collision points.

• How to implement an LCP solver? That's really difficult for a new comer. Is there some tutorial about this?

What do you want to do exactly? If you need to do simulation, I would suggest relying on Gazebo, which is intended.
If you want to control a dynamical system making contacts, then I will need more details to provide the correct pieces of advice.

[jcarpent]

Yes, you can do that by using the impulseDynamics and the contactDynamics to compute all those quantities indeed.
Unfortunately, I don't have time to provide you a short example and the tutorial contains all the information to do it.

[jcarpent]

Sorry, it was forwardDynamics: https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/namespacepinocchio.html#a8b7d42cb4e87d9de4ae108919fa3d2cc
and for inpulseDynamics: https://github.com/stack-of-tasks/pinocchio/blob/master/src/algorithm/contact-dynamics.hpp

[jcarpent]

impulseDynamics should be used when two objects collide in order to reset the normal vertical velocity and the tangential one (depending on your contact model). Then, when the two objects are in contact, you have to simulate the dynamics of your system with forwardDynamics.

[jcarpent]

If there is not contact, then you must run aba which is the classic forwardDynamics.
At the instant of contact, you must run impulseDynamics and when the two objects remain in contact, you must run forwardDynamics which accounts for the forward Dynamics with Contact.
This will behave similarly to a minimal LCP, even if an LCP solver would be more generic.

[gabrielebndn]

And, need I implement an LCP solver in such circumstance?

For such a simple problem as a ball bouncing on the ground, I don't think it is needed.
An LCP solver would be necessary for much more complex cases such as simulating legged locomotion.
For a bouncing ball, I think all you need is a simple criterion to determine when the ball remains in contact with the ground and stops bouncing, in order to avoid having an infinite amount of bounces. You might base this on the vertical velocity of the ball (such as, if the ball is in contact with the ground and its impact velocity is below a certain threshold, then the ball stops bouncing, thus you run impulseDynamics with the restitution coefficient r_coeff=0 instead of whatever it used to be in order to get the ball to a definite halt along the vertical axis).

[gabrielebndn]

@littleggghost, I am sorry, I can't. This completely falls outside the scopes of Pinocchio API. Pinocchio provides the tools for performing dynamical computations, but in no way at all it contains any numerical solver or any full-scale simulation engine.

I hope you understand making a good, stable LCP solver is no trivial task, which would cost me a non-negligible amount of time and work for something that is in no way part of the core library (plus, the library itself is not even my main work topic, I am just here to give a hand when it's needed).

If you need to implement a full dynamic simulation, I suggest that you look for appropriate sources on the internet on how to implement it. Pinocchio will be very useful for performing appropriate related computations.

Meanwhile, I strongly suggest that you start simple and implement the bouncing-ball example first. This way you will gain more knowledge and confidence in the tools provided by Pinocchio. You may certainly use a simplified criterion for that.

Then, you may try to switch to a full-scale walk. Maybe a simple extension of the velocity criterion will be enough for your needs. This I cannot know.

Meanwhile, I would ask myself whether starting to implement a full dynamic simulator yourself is really what you need and if it is not maybe better to employ an off-the-shelf solution such as Gazebo

[nmansard]

Dear @littleggghost,

I am Nicolas Mansard, senior researcher in CNRS and one of the team leader behind the software you are apparently extensively using for your work. We are very happy that you find interest in our work. However, I believe that you are not interacting properly with the development team, and I would like you to adapt your behavior accordingly.

First, you have to understand that we are answering questions of the external users independently of our own work. It is our choice to freely publish our work, because we believe science benefit from free access to knowledge. However, our work is not to solve your own objectives or to train you to the basic knowledge that you need to solve them. Using our software requires a set of state-of-the-art knowledge, that is available in various Master class, books and web tutorial. It is your duty to make the effort to train yourself before starting using our software and asking questions about subject you do not know.

In this frame, questions like #771 are very not welcome. LCP is out of the scope of Pinocchio, it is an evidence, and the way you ask the question is evidently consuming the time of @gabrielebndn unduly (and other like @jcarpent, @jmirabel or me before that). You are neither welcome to unconveniently push for new features when they are not the natural results of the work of one of the members of our team. You can neither violently remind open issue, with a lack of elementary politeness (quoting you: "Hi, @jcarpent One month passed.").

Here is what your are welcome to ask. You can propose new features in the software, and (under the condition that somebody in the team is interesting by the new feature) receive support to implement it following our standards. You may politely ask for information about the API when it lacks of documentation ; however, you are expected to acknowledge the support when it is helping you, and a nice contribution in return is to propose a fix in the documentation to add the corresponding information on the devel branch. You are also welcome to raise bug report when they are decently documented.

I also think that, considering the extensive usage of our work you are doing, the team might be interested by you introducing yourself and presenting in which frame you are working. Among others, this would have allowed me to contact you privately and not through a public post.

Please take this post very seriously. Not complying with the basic rules of human cooperation while lead to being ban of our chat platform and being cut from any resources at all. I might also be pushed to publicly raise the issue by an issue on the Pinocchio repo advising members of the team to ignore your help request and systematically close the issues you open.

I sincerely hope that this recall will help you to find a reasonable way to collaborate together. We are happy to count you among the Pinocchio users and hope that your interest might result in relevant contributions.

Best regards,

Nicolas Mansard