pybullet: applyExternalForce to Soft Body not working

[rlee3359]

Hi,

I am trying to manipulate soft objects directly by applying forces. For example, I would like to manipulate a cloth by applying a force directly to one of the links of the mesh.

However, applying a force with the applyExternalForce function has no effect.
Here is a simple example, I am applying a small Z force to both a rigid and soft body cube. The soft cube does not appear to be effected. Here I am applying it to the base link but I've tried applying it to the other links also.

import pybullet as p

physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
p.setTimeStep(1e-3)
p.setRealTimeSimulation(0)
cubeId = p.loadURDF("cube.urdf",[0,0,2])
softCubeId = p.loadSoftBody("cube.obj", basePosition = [0,0,0.05], scale = 1, mass = 1)

numLinks = p.getMeshData(softCubeId)[0]
while True:
  p.applyExternalForce(cubeId,-1,[0,0,0.001],[0,0,0],p.WORLD_FRAME)
  p.applyExternalForce(softCubeId,-1,[0,0,0.001],[0,0,0],p.WORLD_FRAME)
  p.stepSimulation()

I noticed that the C++ addForce function works with soft bodies, is this functionality not yet available in pybullet?

Cheers!

[erwincoumans]

Almost all the pybullet APIs are assuming a multibody. There is some effort to add softbody/deformable, and the quickstart guide will explicitly mention if an API supports soft/deformables.

[rlee3359]

Thanks for the response! Understood.

I have also attempted this by adding an anchor to a rigid body with createSoftBodyAnchor, and then apply a force to that body. Do you know if there is a way to remove an anchor after it has been created?

[DanielTakeshi]

@rlee3359 Not sure if this would be useful but this paper seems to have used PyBullet (see Section 4): https://arxiv.org/abs/1806.07851 about deformable cloth manipulation, and talks about adding "anchors".

@erwincoumans Thank you for your work on PyBullet. I'm curious if you were aware of the paper from Matas et al, which was able to use PyBullet for cloth. I'm guessing the author had to wrestle deep inside PyBullet. (I contacted him and looked briefly at his code.)

[DanielTakeshi]

Ah! Found the code base: https://github.com/JanMatas/bullet3

It's a fork of this repo.

Edit: here's the other repo for the paper linked below: https://github.com/Zackory/bullet3 (this is more recent and may be easier to work with)

[erwincoumans]

Thanks for the links, I've been in touch with Jan Matas but didn't have the time to help out a lot.
Our focus is on locomotion at the moment, we used PyBullet in this research: https://twitter.com/erwincoumans/status/1246139922848735232

This paper also use PyBullet with deformables:
https://arxiv.org/abs/1910.04700

Xuchen Han is implementing FEM and mass-spring systems in PyBullet, next to the existing position based dynamics (PBD) and Chuyuan Fu is also contributing in this area. Let's re-open and see if we can add the feature.

[rlee3359]

@DanielTakeshi Thanks for the response! Yes, I have read that paper and that was my inspiration for exploring Bullet's softbody capabilities. I have also attempted to apply anchors and got some success, however I was unable to remove these anchors to "release" the fabric after graspsing.

I found a few of the authors pull requests, and it appears he did indeed modify the python API to add the cloth features. Digging into the repository you just linked (thanks for that!), I found at line 8292:

if (userConstraintPtr->anchoredSoftBody) {
	userConstraintPtr->anchoredSoftBody->removeAllAnchors();
}

Which doesn't appear in the original codebase, so perhaps he was able to remove softbody constraints.

[rlee3359]

@erwincoumans Thank you! I understand that softbody is not the main focus at the moment. I'm not much of C++ developer but I'd love to help contribute if I can.

Super awesome locomotion work by the way!

Regarding applying forces directly to the soft body, I've been attempting to add this feature into the python API for my research as a workaround for being unable to remove anchors. I realised the processApplyExternalForceCommand in the CommandProcessor does nothing if the body is not rigid or multibody. I've added this:

if (body && body->m_softBody)
{
  btSoftBody* sb = body->m_softBody;
  int link = clientCmd.m_externalForceArguments.m_linkIds[i];
  if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE) != 0)
  {
    btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 0],
      clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 1],
      clientCmd.m_externalForceArguments.m_forcesAndTorques[i * 3 + 2]);
    btVector3 positionLocal(
      clientCmd.m_externalForceArguments.m_positions[i * 3 + 0],
      clientCmd.m_externalForceArguments.m_positions[i * 3 + 1],
      clientCmd.m_externalForceArguments.m_positions[i * 3 + 2]);

    btVector3 forceWorld = isLinkFrame ? forceLocal : sb->getWorldTransform().getBasis()*forceLocal;
    btVector3 relPosWorld = isLinkFrame ? positionLocal : sb->getWorldTransform().getBasis()*positionLocal;
    sb->addForce(forceWorld, link);
}

It does indeed apply the forces to the correct link, but the behaviour is a little strange (oscillating movements). I realise this is probably not the ideal way to achieve this behaviour, I'd love any advice on this.

Thanks guys!

EDIT:
Just wanted to add that I'm using a temporary hack to achieve the behaviour I need, which is to clear the anchor to the rigid body with:
sbodyHandle->m_softBody->m_deformableAnchors.clear();

[erwincoumans]

I implemented the removeConstraint for soft body anchors here:
e6a0206

For example:

anchorUid = p.createSoftBodyAnchor(clothId ,3,boxId,-1, [0.5,-0.5,0])
p.removeConstraint(anchorUid )

Also, applied your patch, @rlee3359, can you modify a soft body example to show the oscillating movements? Are the forces too high? Do you apply them right before each stepSimulation command?

I tried your initial script (modified), it seems to work fine:

import pybullet as p

physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)
p.setTimeStep(1e-3)
p.setRealTimeSimulation(0)
cubeId = p.loadURDF("cube.urdf",[0,2.2,0])
p.changeDynamics(cubeId,-1, linearDamping=0, angularDamping=0)
softCubeId = p.loadSoftBody("cube.obj", basePosition = [0,0,0], scale = 1, mass = 1)

numLinks = p.getMeshData(softCubeId)[0]
numNodes=8


while True:
  p.applyExternalForce(cubeId,-1,[0,0,0.001],[0,0,0],p.LINK_FRAME)
  for i in range (numNodes):
    p.applyExternalForce(softCubeId,i,[0,0,0.001/(numNodes)],[0,0,0],p.WORLD_FRAME)  
  p.stepSimulation()

[DanielTakeshi]

Thanks @erwincoumans I did a test by running python deformable_anchor.py where I modified the script (as of commit 3fc3b08 on master) based on the following git diff command.

(py3-bullet-github) seita@penguin:~/bullet3/examples/pybullet/examples (master) $ git diff
diff --git a/examples/pybullet/examples/deformable_anchor.py b/examples/pybullet/examples/deformable_anchor.py
index fa3d75867..86ff77de2 100644
--- a/examples/pybullet/examples/deformable_anchor.py
+++ b/examples/pybullet/examples/deformable_anchor.py
@@ -23,10 +23,19 @@ p.createSoftBodyAnchor(clothId ,1,-1,-1)
 p.createSoftBodyAnchor(clothId ,3,boxId,-1, [0.5,-0.5,0])
 p.createSoftBodyAnchor(clothId ,2,boxId,-1, [-0.5,-0.5,0])
 p.setPhysicsEngineParameter(sparseSdfVoxelSize=0.25)
-p.setRealTimeSimulation(1)
-
+#p.setRealTimeSimulation(1)
 
+i = 0
 while p.isConnected():
-  p.setGravity(0,0,gravZ)
+  #p.setGravity(0,0,gravZ)
+  p.stepSimulation()
+  i += 1
   sleep(1./240.)
+  if i % 10 == 0:
+    print('sim steps: {}'.format(i))
+  if i == 100:
+    p.removeConstraint(1)
+    p.removeConstraint(2)
+    p.removeConstraint(3)
+    p.removeConstraint(4)

(There are a few things in the git diff that aren't relevant to this discussion, for example I commented out the setGravity method as I'm not sure why it's repeatedly set in the for loop.)

I am using a python 3.7 conda environment, and with installing using pip install -e. to get the bleeding edge version before it's uploaded on PyPI. Running python deformable_anchor.py, I get this GIF, which correctly shows that all four constraints are removed after 100 simulation steps, resulting in the cloth dropping from mid-air and detaching from the cube.

Pardon the extreme lag in the GIF above, I was using a laptop without much computational power.