Gripping simulation for a robotic hand

[Zentetsu]

Hi,

I'm working on a project with SOFA which consists in grasping an object with a robotic hand by following specific constraints.
For this project, I'm experiencing some difficulties and would like your help.

Sliding cube

My first concern is the process of grasping a cube with two fingers of the robotic hand. The two fingers are able to move to a specific location thanks to InverseSolver and the PositionEffector.

From here, my goal is to move the thumb to the left and index finger to the right side of the cube.
However, when I try to bring a finger close to the cube, without making contact, the cube starts to slide. I checked the collision mesh of each element and the finger doesn't get close enough to the cube to move it. To simplify the case, I only used the index finger.

Initial position	Sliding cube

Finger orientation

My second concern is how to orient a finger along the z axis of the PositionEffector.
I've tried orienting it with useDirections from PositionEffector, but the finger keeps moving around each axis.
Is there any method to achieve this positioning?

Inverse Solver

My final concern is with the Inverse Solver.
I've set a limit angle for each joint to avoid unwanted movement, however, I'd like to know if it's possible to apply a ratio to balance the rotation around each joint to avoid a right angle between the metacarpal and the proximal phalanx (image: sliding cube).

Thank you in advance for your help.

PS: You can find my code here

[bakpaul]

Hello !

First of all, I wasn't able to run your simulation because I don't have the meshes. I'll try to answer you question with what I see in the scripts.

1. For your first point : There is a very simple way to know if interaction constraints are added : in the visualization panel in the GUI you can check "Detection Outputs". But from what I see, you have a quite big contactDistance in your LocalMinDistance, and because I don't know the scale of your scene, this might be the issue (same for the CollisionModels where you kept the default value), make sure those value matches your scale. Also make sure the normals of your collision models are pointing outwards.

2. Again because I cannot launch your scene, I can only give my answer based on your image. First a general advice : make sure that the targeted configuration is reachable. Given the described scenario, if you cannot rotate the wrist then you'll need to be able to rotate the finger at its basis to be able to grasp from the side using the tip (what would be done by rotating the wrist by a human). A second advice : only imposing a rotation of the end effector might be translated in a underconstrained problem. Even more when the finger is in a degenerated configuration (alignment of all the axis) : it will have an infinite number of solution with a non-empty null space, thus the random rotation of multiple axis. Again, I would need to run the scene to be able to provide a more precise answer.

3. If I understand well, what you want is to be able to weight the dofs of your robot during the solving, to favor one joint over the other ? If this is the case, then what you want to do is play on the null-space of you task (if it exists, it depends on the number of objective and dofs of your robots). This can be performed by adding dimensions to your objective functions. It can also be performed by weighting the jacobian matrix used in the QP solver. I don't think that acting directly on the jacobian matrix is currently possible in the plugin. On the other hand, the only available objective functions are effectors, you might be able to add some effector to the plugin that might be able to compute the objective you want (for instance a ration over the joints). The only difficulty is that the jacobians are then determined through the projected compliance of the models, meaning that those objective funcitons needs to be linked to the simulated objects through mapping to be able to generate those Jacobian. It is not impossible, but might require some work.

I hope that I've answered your questions !
See you !

[Zentetsu]

Hello,

Thanks for your message and sorry for my late reply.

Concerning the code, I've tweaked it a bit this week to fix some strange behavior.
I've also added the mesh of the models I'm working with.

• For the first point, I think I've figured out why the cube was sliding without being touched. The cube was not in the same node as the hand (and now the robotic arm) and was not correctly included in the calculation with the solver (as I understand it).
  However, when I try to interact with the cube with the fingertip, it starts to move in a strange direction or fly away and causes strange effects such as a rotation on each hand joint.
  Now, I'm wondering how to interact with the cube without being affected by this behavior.

• For the second point, after refactoring my code and comparing it with other examples, RestShapeSpringsForceField was part of the inability of the fingers to reach the desired position while aligned with the Z axis.
  As for this part, the other finger that doesn't have a PositionEffector moves strangely. I assume this is because the RestShapeSpringsForceField is no longer active.

• For the last point, yes, that's the objective. It's not the most important part of my current step, but I'll keep it in mind.
  One of the next ones is to optimize finger and hand placement. It may be a way to avoid diving into plugin adaptation.

Still, I don't know SOFA well enough and I'm not yet familiar with how it works.

EDIT:
I've also found that when I launch SOFA with just the arm and hand, I don't get any warnings or errors. However, when I add the cube, I get warnings and errors about the QPSolver and the cube start sliding again.
Is this related to the fact they don't share the LDLSover?

[WARNING] [QPInverseProblemImpl] QP infeasible at time = 3.29 with 12 contacts, check constraint on actuators.                          
ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility! 
[WARNING] [QPInverseProblemImpl] QP infeasible at time = 3.29, try with option HST_INDEF.
ERROR:  Premature homotopy termination because QP is infeasible
->ERROR:  Initial QP could not be solved due to infeasibility! 
[ERROR]   [QPInverseProblemImpl] QP infeasible at time = 3.29, iteration = 0, and final nWSR = 31

[bakpaul]

Hello !

he fact that the cube makes the QP infeasible means that you are overconstraining your problem. It comes back to my first point:

• Are you sure of the values used for the contactDistance ?
• Contact might be wrongly set because the normals of your elements are wrong. Are you sure that the triangles normals of your fingers and of the cube are well computed and pointing outwards ?
• Finally, a simulated object that flies away like that in dynamics might be due to a lack of mass, have you set the cube mass correctly ?

[Zentetsu]

Hello,

Thank you for your answer.

So, I checked the various points you mentioned.

• The unit I use for the scene is mm and the target point for the arm is (0, 500, 300) with a contactDistance set to 1.
• I've also checked the normals in SOFA and Blender, and they seem to be correctly oriented.
• Finally, I've set the totalMass to 500 (g) (I tried with 50 and 5 too) with the uniformMass. However, I'm not sure if other elements are taken into account when defining the mass of an object.

Also, when I run a simulation with just a cube on a flat surface I get this error:

ERROR:  Division by zero
->ERROR:  Abnormal termination due to Cholesky factorisation
  ->ERROR:  Determination of step direction failed
    ->ERROR:  Abnormal termination due to Cholesky factorisation
      ->ERROR:  Initialisation failed! QP could not be solved!
ERROR:  Maximum number of working set recalculations performed

PS: gravity and dt are set at 9810 and 0.01 respectively.

[Zentetsu]

Sorry, I should have uploaded an image of my scene earlier to illustrate the behavior I'm facing:

Collision with finger tip	Collision with robot arm

[bakpaul]

Hello,

If the collision with tobot arm is a forward simulation (meaning you can make the same displacement without using SoftRobot.Inverse), could you try to use a GenericConstraintSovler with a FreeMotionAnimationLoop instead ? To see if it comes from the QP solver or if the problem is really unsolvable

[Zentetsu]

Hello,

Thank you for your answer.

So, when I control the robot with a forward simulation, the collision seems to behave correctly.
However, with the finger, the collision is detected but doesn't affect the movement of the cube.
On the other hand, I don't really know why the cube slips when I switch to the GenericConstraintSovler.

Collision with robot arm with forward simulation	Collision with finger forward simulation

[bakpaul]

Hello ! Sorry for having never answered you...
Have you found your issue ? I though that maybe it was due to the type of Collision model used. But I don't have access to your code anymore.

We can at least se that the QP might be the issue no ?