Move an object towards a target in PyBullet

I'm pretty new to PyBullet and physics engines in general. My first step is trying to get one object to move towards another.

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()

But this just gets R2 to wiggle a bit. How do I do this?

Solution

First of all, if you are moving a robot, you should do something a little more complicated, by providing some commands to the joints of the robot. Here is an example

Now assuming that you are moving something less complicated by applying an external force, the simplest thing you can do is multiply a factor alpha with the difference between the two positions; this would be your force.

For your example this would be:

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()