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: https://github.com/stack-of-tasks/pinocchio/issues/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.

[littleggghost]

I agree with you @jcarpent .Hope @jmirabel will have the time to answer.

[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.

[littleggghost]

Thanks your reply. But the ground box is huge, and robot bodies have many curved surfaces, as you said "add a collision pair", then can one pair enough to handle this? And where should this pair locate at?

[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.

[littleggghost]

Thanks @jmirabel Here I have two questions:

1. As the issue https://github.com/stack-of-tasks/pinocchio/issues/731 mentioned, the collision model not move with the visual model. I do as the exercise tutorial do, but I failed on this.
2. How to apply the ground collision box as static that will not move after contact.

[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.

[littleggghost]

Thanks @jcarpent I found function robot.visuals.append() can stick collision model on visual model which will move together. But it seems that only work with revolute joint, not free-flyer joint. The robot.visuals.append() needs a parameter called joint_id, so it's easy to use revolute joint which has only one id, but a free-flyer joint has 7 id, and it can't be sticked to visual model in this case.

[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).

[littleggghost]

Yes, I'm wrong and you are right. Now the problem is I can't stick the collision model to visual model with free-flyer joint as I did with revolute joint.

[littleggghost]

In more detail, the hand can attach witness point to move, but not work if I use in free-flyer scene.

[jcarpent]

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

[littleggghost]

Well, the free-flyer scene, for example, like I drop a ball or a cylinder or a box from one meter height and then contact with ground box. The ball which dropped has a free-flyer joint with 6DOF.

[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.

[littleggghost]

Hi, @jcarpent @jmirabel I post my test example here. I want to use the FCL to calc the collision, so I did't add any witness point as I do at issue https://github.com/stack-of-tasks/pinocchio/issues/731. Here is the code:

from pinocchio.utils import cross, zero, rotate, eye
from display import Display
from pinocchio.utils import *
from math import pi
import time
import numpy as np
import pinocchio as se3

def inertia(m, c):
    return se3.Inertia(m, np.matrix(c, np.double).T, eye(3) * m ** 2)

def trans(x, y, z):
    return se3.SE3(eye(3), np.matrix([x, y, z]).T)

cm = 1e-2
FL = 20 * cm
L, W, H = 20 * cm, 40 * cm, 15 * cm

class Visual(object):
    def __init__(self, name, jointParent, placement):
        self.name = name                  # Name in gepetto viewer
        self.jointParent = jointParent    # ID (int) of the joint
        self.placement = placement        # placement of the body wrt joint, i.e. bodyMjoint

    def place(self, display, oMjoint):
        oMbody = oMjoint * self.placement
        display.place(self.name, oMbody, False)

class Robot(object):
    def __init__(self):
        self.viewer = Display()
        self.visuals = []
        self.model = se3.Model()
        self.createScene()
        self.data = self.model.createData()
        self.q0 = zero(self.model.nq)
        # self.q0[3] = 1.0
        self.v0 = zero(self.model.nv)
        self.collisionPairs = []

    def createScene(self):    # create a box at 0.5m height
        color = [red, green, blue, transparency] = [1, 1, 0.78, 1.0]

        joint_placement = se3.SE3(eye(3), np.matrix([0, 0, 0.5]).T)
        joint_id = self.model.addJoint(0, se3.JointModelFreeFlyer(), joint_placement, 'world/freebox_joint')
        self.model.appendBodyToJoint(joint_id, inertia(.5, [0, 0, 0]), se3.SE3.Identity())
        self.viewer.viewer.gui.addBox('world/freebox', L , W , H, color)
        pos = se3.SE3(rotate('y', pi / 2), np.matrix([0, 0, 0]).T)
        self.visuals.append(Visual('world/freebox', joint_id, pos))

    def display(self, q):
        se3.forwardKinematics(self.model, self.data, q)
        for visual in self.visuals:
            visual.place(self.viewer, self.data.oMi[visual.jointParent])
        self.viewer.viewer.gui.refresh()

robot = Robot()   #init a robot
robot.display(robot.q0)
# print(robot.model.nq)
# print(robot.model.nv)
robot.viewer.viewer.gui.addFloor('world/floor') #ground visual
robot.viewer.viewer.gui.addBox('world/groundbox', 3, 3, .01, [1, 0, 0, 1])   #ground collision box
robot.viewer.viewer.gui.setVisibility('world/groundbox', 'ON')

q = rand(robot.model.nq)
q[0] = 0
q[1] = 0
q[2] = 0.6    # start drop height = 0.6m
print(q)
vq = zero(robot.model.nv)
aq0 = zero(robot.model.nv)
tq = zero(robot.model.nv)
tfriction = zero(robot.model.nq)

dt = 0.001  # drop acc integration
time.sleep(1)
for x in range(0, 10000):
    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)
    vq = se3.integrate(robot.model, vq, aq * dt)
    q = se3.integrate(robot.model, q, vq * dt)
    robot.display(q)
    time.sleep(0.001)

You need run gepetto-gui and will see the box just drop through the ground box. So my main question is how to simulate it real. Thanks in advance!

[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.

[littleggghost]

Thanks. I really hope the box can bound. Hope for your next advice!

[littleggghost]

Hi, @jcarpent I wonder if there has been some progress recently?

[littleggghost]

Hi, @jcarpent Have you made some progress? After all, half a month has passed since the last discussion.

[littleggghost]

Hi, @jcarpent One month passed.

[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

[littleggghost]

Thanks for your instructions @gabrielebndn I will correct the wrong code. Do you have some advice about the collision simulation as I mentioned?

[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.

[littleggghost]

Hi, @jcarpent @gabrielebndn Thanks for your advises and there still has some key difficulties:

1. About collision check, should I directly calculate the distance of two objects? Or call an function?
2. How to add impactDynamics and contact jacobian?
3. How to implement an LCP solver? That's really difficult for a new comer. Is there some tutorial about this?

I have already read the exercises you mentioned and seems like has little effect to this three question.

[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.

[littleggghost]

For the third question: If I create a ground robot using pinocchio, then I don't care visual simulation, I just want the state (position, velocity and acceleration ) of all robot links and joints when interacting with ground. And I need the loop runs as fast as possible.

[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.

[littleggghost]

Could you tell me the location of API about impulseDynamics and contactDynamics?

[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

[littleggghost]

Does impulseDynamics used as a callback? Because I don't know when to call it? Or just run it in loop no matter if contact happens?

[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.

[littleggghost]

My understand: The forwardDynamics is always running in the loop, and impulseDynamics only runs when computeCollisions returns true, which might be a few microseconds. And, need I implement an LCP solver in such circumstance?

[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).

[littleggghost]

I think the LCP solver is still needed. Because my ground robot is a bipped robot, and the ball is just for a simple test.

[littleggghost]

Hi, @gabrielebndn Could you post a simple example about how to implement an LCP solver?

[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 https://github.com/stack-of-tasks/pinocchio/issues/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

[littleggghost]

Hi, @nmansard I am a nameless junior researcher. Your team made a good work. Forgive me if I bring trouble to your team. I didn't mean to. I has submitted many issues about how to use Pinocchio, and I gave my acknowledgement to whom helped me. To be honest, the documents have extremely limited effect, and tutorials are the same. So I have to ask issues. I guess many member of your team might be permanent researcher, so I can understand the delay of the answers. It's your choice to answer questions or not. But I want to communicate directly, not ambiguously. @jcarpent said he will answer

I will try to have a look at it as soon as possible in order to provide the help you need.

And after that, I concern the progress every four or five days. But with no answers. You can directly tell me a expected time like two moth, or just tell me this way has bothered you and has no time to answer. But no, I was deliberately ignored. Until I said

One month passed.

Because I intended to give up with no single reply for 24 days. Nowadays, communication is not using envelop anymore, so a clear and timely answer is the true

elementary politeness

and

basic rules of human cooperation

, no matter yes or no. Please forgive me for being blunt.

Best regards.