bullet:施加力后物体无法稳定

我希望仿真一个弹性管道,一端固定,另一端受力,最后管道弯曲成圆弧。

现在用一堆胶囊来仿真:

在上面的图片中,底部是固定的,顶端会施加力,沿着x/y方向。

现在的问题是,如果是施加x或者y,一个方向的力的时候,效果还不错。但是,如果x/y同时施加力,管道就无法稳定,会一直扭来扭曲。还有可能扭成下面的麻花状:

具体代码为:

import pybullet as p

physicsClient = p.connect(p.GUI)
p.setGravity(0,0,0)

plane_shape = p.createCollisionShape(p.GEOM_PLANE)
plane_body = p.createMultiBody(baseCollisionShapeIndex=plane_shape)

radius = 0.1
height = 0.1
d = height/2+radius
N = 10
cylinders = []
for i in range(N):
    cylinder = p.createCollisionShape(p.GEOM_CAPSULE, radius=radius, height=height)
    body = p.createMultiBody(baseMass=1, baseCollisionShapeIndex=cylinder,
                             # baseInertialFramePosition=[0, 0, 0],
                              basePosition=[0, 0, d*2*i])

    cylinders.append(cylinder)

for i in range(1, N):
    d = height/2+radius
    joint = p.createConstraint(
        cylinders[i-1],
        -1,
        cylinders[i],
        -1,
        p.JOINT_POINT2POINT,
        [0, 0, 1],
        [0, 0, d],
        [0, 0, -d]
    )

joint = p.createConstraint(
    plane_body, -1, cylinders[0], -1, p.JOINT_FIXED, [0, 0, 1], [0, 0, d], [0, 0, -d]
)

xId = p.addUserDebugParameter('x-force', -10, 10, 0)
yId = p.addUserDebugParameter('y-force', -10, 10, 0)

import time
p.setRealTimeSimulation(0)
while p.isConnected():
    xforce = p.readUserDebugParameter(xId)
    yforce = p.readUserDebugParameter(yId)
    p.applyExternalForce(cylinders[-1], -1, forceObj=[xforce, 0, 0], posObj=[0, 0, height], flags=p.LINK_FRAME)
    p.applyExternalForce(cylinders[-1], -1, forceObj=[0, yforce, 0], posObj=[0, 0, height], flags=p.LINK_FRAME)

    p.stepSimulation()
    time.sleep(1. / 240.)

p.disconnect()

希望大佬能帮忙看看