我希望仿真一个弹性管道,一端固定,另一端受力,最后管道弯曲成圆弧。
现在用一堆胶囊来仿真:
在上面的图片中,底部是固定的,顶端会施加力,沿着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()
希望大佬能帮忙看看