在 PyBullet 中将对象移向目标
Move an object towards a target in PyBullet
总的来说,我对 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 稍微摆动一下。我该怎么做?
首先,如果你正在移动机器人,你应该做一些更复杂的事情,向机器人的关节提供一些命令。 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()
总的来说,我对 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 稍微摆动一下。我该怎么做?
首先,如果你正在移动机器人,你应该做一些更复杂的事情,向机器人的关节提供一些命令。 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()