将对象移动到PyBullet中的目标

Question

总的来说，我对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稍微扭动一下。我该怎么做？

Answer 1

首先，如果你在移动一个机器人，你应该做一些更复杂的事情，向机器人的关节提供一些命令。下面是一个例子

现在假设你通过施加外力来移动一些不那么复杂的东西，你能做的最简单的事情就是将一个因子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()