在 PyBullet 中将对象移向目标

Posted 2023-02-15

【中文标题】在 PyBullet 中将对象移向目标

【英文标题】：Move an object towards a target in PyBullet

【发布时间】：2018-09-13 07:35:39

【问题描述】：

总的来说，我对 PyBullet 和物理引擎还是很陌生。我的第一步是尝试让一个对象向另一个对象移动。

import pybullet as p
import time
import pybullet_data

DURATION = 10000

physicsClient = p.connect(p.GUI)#or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath()) #optionally
print("data path: %s " % pybullet_data.getDataPath())
p.setGravity(0,0,-10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0,0,1]
cubeStartOrientation = p.getQuaternionFromEuler([0,0,0])
boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation)
gemId = p.loadURDF("duck_vhacd.urdf", [2,2,1],  p.getQuaternionFromEuler([0,0,0]) )
for i in range (DURATION):
    p.stepSimulation()
    time.sleep(1./240.)
    gemPos, gemOrn = p.getBasePositionAndOrientation(gemId)
    cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId)
    oid, lk, frac, pos, norm = p.rayTest(cubePos, gemPos)[0]
    #rt = p.rayTest(cubePos, gemPos)
    #print("rayTest: %s" % rt[0][1])
    print("rayTest: Norm: ")
    print(norm)
    p.applyExternalForce(objectUniqueId=boxId, linkIndex=-1, forceObj=pos
        ,posObj=gemPos, flags=p.WORLD_FRAME)
print(cubePos,cubeOrn)
p.disconnect()

但这只会让 R2 稍微摆动一下。我该怎么做？

【参考方案1】：

首先，如果你在移动一个机器人，你应该做一些更复杂的事情，通过向机器人的关节提供一些命令。 Here is an example

现在假设您通过施加外力来移动不太复杂的东西，您可以做的最简单的事情就是将系数alpha 与两个位置之间的差异相乘；这将是你的力量。

对于您的示例，这将是：

import numpy as np
import pybullet as p
import time
import pybullet_data

DURATION = 10000
ALPHA = 300

physicsClient = p.connect(p.GUI)  # or p.DIRECT for non-graphical version
p.setAdditionalSearchPath(pybullet_data.getDataPath())  # optionally
print("data path: %s " % pybullet_data.getDataPath())
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
cubeStartPos = [0, 0, 1]
cubeStartOrientation = p.getQuaternionFromEuler([0, 0, 0])
boxId = p.loadURDF("r2d2.urdf", cubeStartPos, cubeStartOrientation)
gemId = p.loadURDF("duck_vhacd.urdf", [
                   2, 2, 1],  p.getQuaternionFromEuler([0, 0, 0]))
for i in range(DURATION):
    p.stepSimulation()
    time.sleep(1./240.)
    gemPos, gemOrn = p.getBasePositionAndOrientation(gemId)
    boxPos, boxOrn = p.getBasePositionAndOrientation(boxId)

    force = ALPHA * (np.array(gemPos) - np.array(boxPos))
    p.applyExternalForce(objectUniqueId=boxId, linkIndex=-1,
                         forceObj=force, posObj=boxPos, flags=p.WORLD_FRAME)

    print('Applied force magnitude = '.format(force))
    print('Applied force vector = '.format(np.linalg.norm(force)))

p.disconnect()