How to understand apply_central_force

zhang-qiang-github · April 6, 2023, 7:22am

I want to stack some capsules to simulate a tube. It looks like:

Then, I apply the force to the top capsule to make this tube bending, and I expect these capsules bending to arc.

In order to achieve my purpose, I calculate the coordinate x/y/z for the top capsule, and calculate the force:

    x = node_paths[-1].getMat()
    m = np.array(x)
    x = [m[0][0], m[1][0], m[2][0]]
    y = [m[0][1], m[1][1], m[2][1]]
    z = [m[0][2], m[1][2], m[2][2]]

    f = xforce*np.array(x) + yforce*np.array(y)
    last.node().set_active(True)
    last.node().apply_central_force(force)

When press the keyboard d, the xforce would increase one: xforce = xforce + 1. Finally, the result is:

And it do not change even I press d.

The complete code to reproduce my experiment is:

import time

import direct.directbase.DirectStart
from panda3d.core import Vec3, TransformState, Point3, BitMask32, LPoint3, LVector3, Material
from panda3d.bullet import BulletWorld, BulletDebugNode, BulletCylinderShape, BulletCapsuleShape
from panda3d.bullet import BulletPlaneShape, BulletSphericalConstraint, BulletConeTwistConstraint
from panda3d.bullet import BulletRigidBodyNode, BulletTriangleMesh, BulletTriangleMeshShape
from panda3d.bullet import BulletBoxShape
from direct.showbase.InputStateGlobal import inputState
import numpy as np

base.cam.setPos(0, -10, 0)
base.cam.lookAt(0, 0, 0)

debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(True)
debugNode.showNormals(False)
debugNP = render.attachNewNode(debugNode)
debugNP.show()

# World
world = BulletWorld()
world.setGravity(Vec3(0, 0, 0))
world.setDebugNode(debugNP.node())

# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
node = BulletRigidBodyNode('Ground')
node.addShape(shape)
nodePath = render.attachNewNode(node)
nodePath.setPos(0, 0, 0)
world.attachRigidBody(node)

inputState.watchWithModifiers('x_add', 'd')
inputState.watchWithModifiers('x_minu', 'a')
inputState.watchWithModifiers('y_add', 'w')
inputState.watchWithModifiers('y_minu', 's')
# inputState.watchWithModifiers('turnLeft', 'q')
# inputState.watchWithModifiers('turnRight', 'e')
inputState.watchWithModifiers('up', 'u')

num_segment = 10
segment_length = 1
segment_radius = 1


shape = BulletCapsuleShape(radius=segment_radius, height=segment_length)
previous_node = None
node_paths = []

# pos = [75.81992094545193, 314.34202388786053, 9.5]
for i in range(num_segment):
    node = BulletRigidBodyNode(f'segment_{i}')
    node.addShape(shape)
    node.setMass(1.0)
    node.set_linear_damping(0.1)
    node.set_angular_damping(0.1)

    if i == 0:
        # node.set_static(True)
        node_path = render.attachNewNode(node)
        node_path.setPos(0, 0, segment_length / 2 + segment_radius)

        constraint = BulletSphericalConstraint(node, Point3(0, 0, -segment_length / 2 - segment_radius))
        world.attach_constraint(constraint)


    else:
        # # 将前一个节点连接到当前节点
        constraint = BulletConeTwistConstraint(previous_node, node,
                                               TransformState.make_pos(Vec3(0, 0, segment_length / 2 + segment_radius)),
                                               TransformState.make_pos(
                                                   Vec3(0, 0, -segment_length / 2 - segment_radius)))
        world.attach_constraint(constraint)

        node_path = render.attachNewNode(node)
        node_path.setPos(previous_node_path.get_pos() + Vec3(0, 0, segment_length + 2 * segment_radius))

    previous_node = node
    previous_node_path = node_path
    world.attachRigidBody(node)
    node_paths.append(node_path)

uselessConstrain = []

global xforce, yforce
xforce = 0
yforce = 0
def process_input():
    global xforce, yforce

    if inputState.isSet('y_add'):
        yforce += 1
        time.sleep(0.2)
    if inputState.isSet('y_minu'):
        yforce -= 1
        time.sleep(0.2)
    if inputState.isSet('x_minu'):
        xforce += 1
        time.sleep(0.2)
    if inputState.isSet('x_add'):
        xforce -= 1
        time.sleep(0.2)

    x = node_paths[-1].getMat()
    m = np.array(x)
    x = [m[0][0], m[1][0], m[2][0]]
    y = [m[0][1], m[1][1], m[2][1]]
    z = [m[0][2], m[1][2], m[2][2]]

    f = xforce*np.array(x) + yforce*np.array(y)
    print(xforce, yforce)

    force = LVector3(f[0], f[1], f[2])

    last = node_paths[-1]
    last.node().set_active(True)
    last.node().apply_central_force(force)


# Update
def update(task):
    dt = globalClock.getDt()
    process_input()
    world.doPhysics(dt)
    return task.cont


taskMgr.add(update, 'update')
base.run()

Any suggestion is appreciated~~~~

Thaumaturge · April 6, 2023, 8:04am

I don’t have numpy installed, so I removed that with a quick hack, and… when I press “d” I see the column of capsules curve over. Pressing other buttons causes them to curl around. Is that what should happen?

See the screenshots below:

If that is the intended behaviour, then it looks like the problem that you’re seeing comes from the way that you’re handling the force–it looks like you’re creating some sort of matrix, and then extracting a vector from that?

What I did was rather simpler: I constructed the force directly from the “xforce” and “yforce” values, presuming the “xforce” to be the x-component of the force and “yforce” to be the y-component. Like so:

force = LVector3(xforce, yforce, 0)

In the example-code that you gave, that line replaces everything from the last “time.sleep(0.2)” (i.e. starting with “x = node_paths[-1].getMat()”), up to (and including) the line “force = LVector3(f[0], f[1], f[2])”.

The “process_input” method thus ends up looking like this:

def process_input():
    global xforce, yforce

    if inputState.isSet('y_add'):
        yforce += 1
        time.sleep(0.2)
    if inputState.isSet('y_minu'):
        yforce -= 1
        time.sleep(0.2)
    if inputState.isSet('x_minu'):
        xforce += 1
        time.sleep(0.2)
    if inputState.isSet('x_add'):
        xforce -= 1
        time.sleep(0.2)

    force = LVector3(xforce, yforce, 0)

    last = node_paths[-1]
    last.node().set_active(True)
    last.node().apply_central_force(force)

zhang-qiang-github · April 6, 2023, 9:26am

Thanks very much for your reply.

I have tried force = LVector3(xforce, yforce, 0) before. The reason why I chaneg the force to current version is: I hope the capsules would turn to be a circle. In this way, I hope the force would be vertical to the capsule direction. For example:

The black line represent the capsules, and the red arrow is the force. I want the force would always be vertical to the direction of capsule. In this way, the force would finally make the capsules turn to be a circle. Is it possible?

Or how can I make the capsules turn to be circle by external force?

Thaumaturge · April 6, 2023, 9:43am

Ah, I see!

Well, you could perhaps take the component vectors of the capsule’s orientation (its “forward”, “right”, and “up” vectors) and use those as the basis of your force.

Specifically–if I’m reading your diagram correctly; adjust as called for if not–you might have the x- and y- forces be applied to the “right” and “forward” vectors.

And since these vectors represent the capsule’s orientation, they effectively “rotate” with the capsule, allowing the sort of capsule-relative force that you seem to be describing.

Something like this:

# First, get the orientation quaternion relative to
# the base "render" node--that is, relative to the "world".
# (By default, orientation is returned relative
#  to the given node's parent, which may not be
#  what we want here.)
orientationQuaternion = node_paths[-1].getQuat(render)

# (Don't worry if you don't know quaternions--that's not
#  really important for this process.)

# Now, get the component vectors from that quaternion
forwardVector = orientationQuaternion.getForward()
rightVector = orientationQuaternion.getRight()
# You probably don't need this next one, but I'll include
# it to illustrate that it's present
#upVector = orientationQuaternion.getUp()

# Next, apply the x-force and y-force to the relevant
#  vectors; again, adjust which vector is used for
#  which force if the below doesn't match your intentions
xVector = rightVector * xforce
yVector = forwardVector * yforce

# And finally, construct the force to be applied to the node
force = xVector + yVector

zhang-qiang-github · April 6, 2023, 10:02am

I have tried the forward/right method, but the capsules become weird twisty, for example:

And it need much time (>5 minutes in 11th Core i7 2.8GHz) to achieve static balance.

How to make it not twisty? And how to make it balance faster?

Thaumaturge · April 6, 2023, 10:29am

Those I’m not sure about–I’ll leave those questions for others better-versed in Bullet’s physics!

zhang-qiang-github · April 6, 2023, 10:39am

Thanks for your kindly reply.

This really make me confuzed~~~~

Simulan · April 6, 2023, 1:19pm

Hi there, while I do not have a full solution, I did do a quick rewrite which may get you a bit closer to understanding the dynamics here.

Quick summary:

I used a BulletHingeConstraint instead of a BulletConeTwistConstraint
I anchored the starting segment to a fixed world position
I applied the force using a relative vector with a simplified force:

force = LVector3(50, 0, 0)
force = render.get_relative_vector(node_paths[-1], force)

I removed the ground collision mesh

You had a series of capsules linked by BulletConeTwistConstraint instead of a hinge constraint which have a complex force applied to them without a relative force application. This encourages a highly chaotic simulation, which logically does twist around as it self-interacts around sphere-like hinges. Additionally once the assemblage hits the ground, this imparts yet another force, which makes the dynamics even more chaotic looking.

My version of the code:

import time

import direct.directbase.DirectStart
from panda3d.core import Vec3, TransformState, Point3, BitMask32, LPoint3, LVector3, Material
from panda3d.bullet import BulletWorld, BulletDebugNode, BulletCylinderShape, BulletCapsuleShape, BulletBoxShape
from panda3d.bullet import BulletPlaneShape, BulletSphericalConstraint, BulletConeTwistConstraint, BulletHingeConstraint
from panda3d.bullet import BulletRigidBodyNode, BulletTriangleMesh, BulletTriangleMeshShape
from panda3d.bullet import BulletBoxShape
from direct.showbase.InputStateGlobal import inputState
import numpy as np

base.cam.setPos(0, -60, 3)
base.cam.lookAt(0, 0, 0)

debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(True)
debugNode.showBoundingBoxes(True)
debugNode.showNormals(False)
debugNP = render.attachNewNode(debugNode)
debugNP.show()

# World
world = BulletWorld()
world.setGravity(Vec3(0, 0, 0))
world.setDebugNode(debugNP.node())

# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
node = BulletRigidBodyNode('Ground')
node.addShape(shape)
nodePath = render.attachNewNode(node)
nodePath.setPos(0, 0, 0)
# world.attachRigidBody(node)

inputState.watchWithModifiers('x_add', 'd')
inputState.watchWithModifiers('x_minu', 'a')
inputState.watchWithModifiers('y_add', 'w')
inputState.watchWithModifiers('y_minu', 's')
# inputState.watchWithModifiers('turnLeft', 'q')
# inputState.watchWithModifiers('turnRight', 'e')
inputState.watchWithModifiers('up', 'u')

num_segment = 10
segment_length = 1
segment_radius = 1


# shape = BulletCapsuleShape(radius=segment_radius, height=segment_length)
shape = BulletBoxShape(1)
previous_node = None
node_paths = []

# pos = [75.81992094545193, 314.34202388786053, 9.5]
for i in range(num_segment):
    node = BulletRigidBodyNode(f'segment_{i}')
    node.addShape(shape)
    node.setMass(1.0)
    # node.set_linear_damping(0.1)
    # node.set_angular_damping(0.1)

    if i == 0:
        # node.set_static(True)
        node_path = render.attachNewNode(node)
        node_path.setPos(0, 0, segment_length / 2 + segment_radius)

        # constraint = BulletSphericalConstraint(node, Point3(0, 0, -segment_length / 2 - segment_radius))
        # print(base.render.find(f'segment_{i}'))
        constraint = BulletHingeConstraint(node, Point3(0, 0, -segment_length / 2 - segment_radius), Vec3(0,0,1))
        world.attach_constraint(constraint)


    else:
        # # 将前一个节点连接到当前节点
        constraint = BulletHingeConstraint(previous_node, node,TransformState.make_pos(Vec3(0, 0, segment_length / 2 + segment_radius)),TransformState.make_pos(Vec3(0, 0, -segment_length / 2 - segment_radius)))
        world.attach_constraint(constraint)

        node_path = render.attachNewNode(node)
        node_path.setPos(previous_node_path.get_pos() + Vec3(0, 0, segment_length + 2 * segment_radius))

    previous_node = node
    previous_node_path = node_path
    world.attachRigidBody(node)
    node_paths.append(node_path)

uselessConstrain = []

print(base.render.find_all_matches('**/segment_*'))

global xforce, yforce
xforce = 0
yforce = 0
def process_input():
    global xforce, yforce

    if inputState.isSet('y_add'):
        yforce += 1
        time.sleep(0.2)
    if inputState.isSet('y_minu'):
        yforce -= 1
        time.sleep(0.2)
    if inputState.isSet('x_minu'):
        xforce += 1
        time.sleep(0.2)
    if inputState.isSet('x_add'):
        xforce -= 1
        time.sleep(0.2)

    # force = LVector3(xforce, yforce, 0)
    force = LVector3(50, 0, 0)
    force = render.get_relative_vector(node_paths[-1], force)

    last = node_paths[-1]
    last.node().set_active(True)
    last.node().apply_central_force(force)


# Update
def update(task):
    dt = globalClock.getDt()
    process_input()
    world.doPhysics(dt)
    return task.cont


taskMgr.add(update, 'update')
base.run()

zhang-qiang-github · April 6, 2023, 1:59pm

@Simulan Thanks for your solution.

When I set force = LVector3(50, 50, 0), I expect the tube would be static in the first quadrant, like the black line in the following figure:

However, the fact is that the tube can not be static for 5 minutes (I stop it after 5 minutes), and it turn around in the 2/3/4 quadrant.

The force force = LVector3(50, 50, 0) should make the tube bend in the first quadrant, but I don’t know why it would go to 2/3/4 quadrant.

How to make it balance between the applied force and the resistance of constraint?

serega-kkz · April 6, 2023, 7:40pm

Your problem is that you expect things that shouldn’t be. In reality, physics does not behave like this. In order to bend something into an arc in reality, for example, a ruler, you need to take at least two hands and make an effort in a mirror relative to two points.

zhang-qiang-github · April 7, 2023, 1:24am

For a ruler, if I fix one endpoint and apply a force to the other endpoint, it should bend into an arc. In this simulation, there are actually two hands. One hand is used to fix one endpoint, and second hand is used to apply force. Thus, it should bend into an arc.

Currently, the capsules has been bend into an arc, but it is hard to ahieve the balance between applied force and the resistance of constraint. Actually, when the force is LVector3(50, 0, 0), it can achieve balance in a short time. But if the force is LVector3(50, 50, 0), it can not achieve force balance. That’s make me confused.

serega-kkz · April 7, 2023, 4:54am

I think you didn’t take into account the rules of working with vectors.

from direct.showbase.ShowBase import ShowBase
from panda3d.core import LVector3

class MyApp(ShowBase):

    def __init__(self):
        ShowBase.__init__(self)

        # Your representation is not correct.
        print(LVector3(50, 0, 0).length())
        print(LVector3(50, 50, 0).length())

        # If you want to set the direction, then first you need to normalize the vector and then multiply it by the force you need. 
        # Find the difference.
        vec1 = LVector3(1, 0, 0)
        vec1.normalize()

        vec2 = LVector3(1, 1, 0)
        vec2.normalize()

        print((vec1*50).length())
        print((vec2*50).length())

app = MyApp()
app.run()

serega-kkz · April 7, 2023, 4:56am

You can take a real ruler and find that the shape is closer to a parabola than to an arc.

zhang-qiang-github · April 7, 2023, 6:39am

I don’t need a “strict” arc, and parabola is acceptable.

But, the current problem is that it is hard to achieve the force balance between the applied force with the constraint resistance.

zhang-qiang-github · April 7, 2023, 6:50am

I don’t need a “strict” arc, and parabola is acceptable.

The vec2 multiply a constant value would result in LVector3(50, 50, 0). Actually, (30, 30, 0) would also cause the tube not balance.

The current problem is that it is hard to achieve the force balance between the applied force with the constraint resistance.

Thaumaturge · April 7, 2023, 8:15am

You could perhaps try looking at setting the… I think that it’s the “angular damping” value. That–if I recall correctly–may reduce the degree to which the simulation swings around.

zhang-qiang-github · April 7, 2023, 9:41am

The damping value would reduce the degree of swing around, but the swing around still happen.

I wonder why the swing around always hanppen? Why the balance between applied force and constrain resistance can not be achieved?

Thaumaturge · April 7, 2023, 9:43am

It may simply be that there are a lot of forces at play in the simulation, enough that it becomes (seemingly) chaotic.

After all, the force that you apply is, I daresay, not the only one in effect: there are presumably forces acting between the capsules (applied by the constraints) that keep them connected and within their limits. Those may well be flowing back and forth, affecting the resultant movement.

[edit]
Actually, trying out Simulan’s version of the code, I realise that there’s also the element of “springiness” (“restitution force”, I suppose it would be called?): the constraints attempt to bring the simulation back to its “rest” state, presumably adding forces to the simulation in order to do so. This, too, complicates the simulation, I daresay.

[edit 2]
You can see this in action, I believe, if you modify Simulan’s code such that the force is applied only once. That is, just move the call to “process_input()” out of the “update” method and to the line just above “taskMgr.add(update, 'update')” Like so:

# Update
def update(task):
    dt = globalClock.getDt()
    world.doPhysics(dt)
    return task.cont

process_input()
taskMgr.add(update, 'update')
base.run()

If you do this, you should see the objects bend as the force takes hold–and then sway back and forth as the constraints attempt to bring them back into line.

[edit 3]
Although you may have to increase the intensity of the force in order to see this properly–after all, it’s being applied just once, rather than continuously.

zhang-qiang-github · April 8, 2023, 8:27am

Yes, that’s why I add the continuously force, because I hope the continuously force would keep balance with the constraint resistance.

And I also agree that the applied force, the constraint resistance may make the simulation chaotic.

However, the force (50, 0, 0) would give a good result, but force (50, 50, 0) give a chaotic result. It is really strange.

So headache.

serega-kkz · April 8, 2023, 9:24pm

I don’t see any headache. It’s not entirely clear what you mean by the balance. You have several ways to elegantly interfere with the behavior of physical objects. For example, you can calm the rocking in this way. However, you cannot influence the physical simulation itself. For example, the rattling can be corrected visually already on the stick model, simply discarding the details of the fine movement for the bones. Unless, of course, you want to visualize it that way.

from panda3d.core import loadPrcFileData
loadPrcFileData("", "bullet-filter-algorithm groups-mask")

import direct.directbase.DirectStart
from panda3d.core import Vec3, TransformState, Point3, BitMask32, LPoint3, LVector3, Material
from panda3d.bullet import BulletWorld, BulletDebugNode, BulletCylinderShape
from panda3d.bullet import BulletPlaneShape, BulletSphericalConstraint, BulletConeTwistConstraint
from panda3d.bullet import BulletRigidBodyNode, BulletTriangleMesh, BulletTriangleMeshShape
from panda3d.bullet import BulletBoxShape
from direct.showbase.InputStateGlobal import inputState

base.cam.setPos(0, -10, 0)
base.cam.lookAt(0, 0, 0)

debugNode = BulletDebugNode('Debug')
debugNode.showWireframe(True)
debugNode.showConstraints(False)
debugNode.showBoundingBoxes(False)
debugNode.showNormals(False)
debugNP = render.attachNewNode(debugNode)
debugNP.show()

# World
world = BulletWorld()
world.setGravity(Vec3(0, 0, 0))
world.setDebugNode(debugNP.node())
world.setGroupCollisionFlag(0, 0, False)

# Plane
shape = BulletPlaneShape(Vec3(0, 0, 1), 0)
node = BulletRigidBodyNode('Ground')
node.addShape(shape)
nodePath = render.attachNewNode(node)
nodePath.setPos(0, 0, 0)
world.attachRigidBody(node)

inputState.watchWithModifiers('x_add', 'd')
inputState.watchWithModifiers('x_minu', 'a')

inputState.watchWithModifiers('y_add', 'w')
inputState.watchWithModifiers('y_minu', 's')

num_segment = 5 # 5段圆柱
segment_length = 5
segment_radius = 1

shape = BulletCylinderShape(radius=segment_radius, height=segment_length)
previous_node = None
node_paths = []
for i in range(num_segment):
    node = BulletRigidBodyNode(f'segment_{i}')
    node.addShape(shape)
    node.setMass(1)
    node.setIntoCollideMask(BitMask32.bit(0))
    #node.set_angular_factor(False)

    if i == 0:
        # node.set_static(True)
        node_path = render.attachNewNode(node)
        node_path.setPos(0, 0, segment_length/2)
        node_path.setTag("root", "")

        constraint = BulletSphericalConstraint(node, Point3(0, 0, -segment_length / 2))
        world.attach_constraint(constraint)

    else:
        # # 将前一个节点连接到当前节点
        constraint = BulletConeTwistConstraint(previous_node, node, TransformState.make_pos(Vec3(0, 0, segment_length/2)),
                                               TransformState.make_pos(Vec3(0, 0, -segment_length/2)))
        constraint.setLimit(3, 3, 0, 0, relaxation = 2)
        
        #constraint.set_damping(100)

        world.attach_constraint(constraint)

        node_path = render.attachNewNode(node)
        node_path.setPos(previous_node_path.get_pos() + Vec3(0, 0, segment_length))
        

    previous_node = node
    previous_node_path = node_path
    world.attachRigidBody(node)
    node_paths.append(node_path)

def process_input(dt):

    force = LVector3(0, 0, 0)

    if inputState.isSet('x_minu'): force.x = -1.0
    if inputState.isSet('x_add'): force.x = 1.0
    
    if inputState.isSet('y_minu'): force.y = -1.0
    if inputState.isSet('y_add'): force.y = 1.0

    force.normalize()

    force*=50

    if force != LVector3(0, 0, 0):
        for i in node_paths:
            i.node().setActive(True)
            i.node().setAngularDamping(0)
            if i.node().hasTag("root"):
                i.node().setAngularFactor(False)

        i.node().applyCentralForce(force)

    else:
        for i in node_paths:
            i.node().setAngularDamping(1)

def update(task):
    dt = globalClock.getDt()
    process_input(dt)
    world.doPhysics(dt)

    return task.cont

taskMgr.add(update, 'update')
base.run()