mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-07-13 15:44:36 +00:00
update bullet so it actually works
Moved the addSourceDirectory for physics/Bullet into the Engine/Source/CMakeLists.txt file that way it can actually appear where we expect it to in the solution explorer.
This commit is contained in:
marauder2k7
parent
c7be48130a
commit
13fa178cf6
5986 changed files with 1811270 additions and 453803 deletions
|
|
@ -0,0 +1,137 @@
|
|||
import unittest
|
||||
import pybullet as p
|
||||
import math, time
|
||||
import difflib, sys
|
||||
|
||||
from utils import allclose, dot
|
||||
|
||||
|
||||
class TestPybulletSaveRestoreMethods(unittest.TestCase):
|
||||
|
||||
def setupWorld(self):
|
||||
numObjects = 50
|
||||
|
||||
maximalCoordinates = False
|
||||
|
||||
p.resetSimulation()
|
||||
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
|
||||
p.loadURDF("planeMesh.urdf", useMaximalCoordinates=maximalCoordinates)
|
||||
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf", [0, 0, 10],
|
||||
useMaximalCoordinates=maximalCoordinates)
|
||||
for i in range(p.getNumJoints(kukaId)):
|
||||
p.setJointMotorControl2(kukaId, i, p.POSITION_CONTROL, force=0)
|
||||
for i in range(numObjects):
|
||||
cube = p.loadURDF("cube_small.urdf", [0, i * 0.02, (i + 1) * 0.2])
|
||||
#p.changeDynamics(cube,-1,mass=100)
|
||||
p.stepSimulation()
|
||||
p.setGravity(0, 0, -10)
|
||||
|
||||
def dumpStateToFile(self, file):
|
||||
for i in range(p.getNumBodies()):
|
||||
pos, orn = p.getBasePositionAndOrientation(i)
|
||||
linVel, angVel = p.getBaseVelocity(i)
|
||||
txtPos = "pos=" + str(pos) + "\n"
|
||||
txtOrn = "orn=" + str(orn) + "\n"
|
||||
txtLinVel = "linVel" + str(linVel) + "\n"
|
||||
txtAngVel = "angVel" + str(angVel) + "\n"
|
||||
file.write(txtPos)
|
||||
file.write(txtOrn)
|
||||
file.write(txtLinVel)
|
||||
file.write(txtAngVel)
|
||||
|
||||
def compareFiles(self, file1, file2):
|
||||
diff = difflib.unified_diff(
|
||||
file1.readlines(),
|
||||
file2.readlines(),
|
||||
fromfile='saveFile.txt',
|
||||
tofile='restoreFile.txt',
|
||||
)
|
||||
numDifferences = 0
|
||||
for line in diff:
|
||||
numDifferences = numDifferences + 1
|
||||
sys.stdout.write(line)
|
||||
self.assertEqual(numDifferences, 0)
|
||||
|
||||
def testSaveRestoreState(self):
|
||||
numSteps = 500
|
||||
numSteps2 = 30
|
||||
|
||||
verbose = 0
|
||||
self.setupWorld()
|
||||
|
||||
for i in range(numSteps):
|
||||
p.stepSimulation()
|
||||
p.saveBullet("state.bullet")
|
||||
if verbose:
|
||||
p.setInternalSimFlags(1)
|
||||
p.stepSimulation()
|
||||
if verbose:
|
||||
p.setInternalSimFlags(0)
|
||||
print("contact points=")
|
||||
for q in p.getContactPoints():
|
||||
print(q)
|
||||
|
||||
for i in range(numSteps2):
|
||||
p.stepSimulation()
|
||||
|
||||
file = open("saveFile.txt", "w")
|
||||
self.dumpStateToFile(file)
|
||||
file.close()
|
||||
|
||||
#################################
|
||||
self.setupWorld()
|
||||
|
||||
#both restore from file or from in-memory state should work
|
||||
p.restoreState(fileName="state.bullet")
|
||||
stateId = p.saveState()
|
||||
|
||||
if verbose:
|
||||
p.setInternalSimFlags(1)
|
||||
p.stepSimulation()
|
||||
if verbose:
|
||||
p.setInternalSimFlags(0)
|
||||
print("contact points restored=")
|
||||
for q in p.getContactPoints():
|
||||
print(q)
|
||||
for i in range(numSteps2):
|
||||
p.stepSimulation()
|
||||
|
||||
file = open("restoreFile.txt", "w")
|
||||
self.dumpStateToFile(file)
|
||||
file.close()
|
||||
|
||||
p.restoreState(stateId)
|
||||
if verbose:
|
||||
p.setInternalSimFlags(1)
|
||||
p.stepSimulation()
|
||||
if verbose:
|
||||
p.setInternalSimFlags(0)
|
||||
print("contact points restored=")
|
||||
for q in p.getContactPoints():
|
||||
print(q)
|
||||
for i in range(numSteps2):
|
||||
p.stepSimulation()
|
||||
|
||||
file = open("restoreFile2.txt", "w")
|
||||
self.dumpStateToFile(file)
|
||||
file.close()
|
||||
|
||||
file1 = open("saveFile.txt", "r")
|
||||
file2 = open("restoreFile.txt", "r")
|
||||
self.compareFiles(file1, file2)
|
||||
file1.close()
|
||||
file2.close()
|
||||
|
||||
file1 = open("saveFile.txt", "r")
|
||||
file2 = open("restoreFile2.txt", "r")
|
||||
self.compareFiles(file1, file2)
|
||||
file1.close()
|
||||
file2.close()
|
||||
|
||||
|
||||
#while (p.getConnectionInfo()["isConnected"]):
|
||||
# time.sleep(1)
|
||||
|
||||
if __name__ == '__main__':
|
||||
p.connect(p.DIRECT)
|
||||
unittest.main()
|
||||
123
Engine/lib/bullet/examples/pybullet/unittests/unittests.py
Normal file
123
Engine/lib/bullet/examples/pybullet/unittests/unittests.py
Normal file
|
|
@ -0,0 +1,123 @@
|
|||
import unittest
|
||||
import pybullet
|
||||
import time
|
||||
|
||||
from utils import allclose, dot
|
||||
|
||||
|
||||
class TestPybulletMethods(unittest.TestCase):
|
||||
|
||||
def test_import(self):
|
||||
import pybullet as p
|
||||
self.assertGreater(p.getAPIVersion(), 201700000)
|
||||
|
||||
def test_connect_direct(self):
|
||||
import pybullet as p
|
||||
cid = p.connect(p.DIRECT)
|
||||
self.assertEqual(cid, 0)
|
||||
p.disconnect()
|
||||
|
||||
def test_loadurdf(self):
|
||||
import pybullet as p
|
||||
p.connect(p.DIRECT)
|
||||
ob = p.loadURDF("r2d2.urdf")
|
||||
self.assertEqual(ob, 0)
|
||||
p.disconnect()
|
||||
|
||||
def test_rolling_friction(self):
|
||||
import pybullet as p
|
||||
p.connect(p.DIRECT)
|
||||
p.loadURDF("plane.urdf")
|
||||
sphere = p.loadURDF("sphere2.urdf", [0, 0, 1])
|
||||
p.resetBaseVelocity(sphere, linearVelocity=[1, 0, 0])
|
||||
p.changeDynamics(sphere, -1, linearDamping=0, angularDamping=0)
|
||||
#p.changeDynamics(sphere,-1,rollingFriction=0)
|
||||
p.setGravity(0, 0, -10)
|
||||
for i in range(1000):
|
||||
p.stepSimulation()
|
||||
vel = p.getBaseVelocity(sphere)
|
||||
self.assertLess(vel[0][0], 1e-10)
|
||||
self.assertLess(vel[0][1], 1e-10)
|
||||
self.assertLess(vel[0][2], 1e-10)
|
||||
self.assertLess(vel[1][0], 1e-10)
|
||||
self.assertLess(vel[1][1], 1e-10)
|
||||
self.assertLess(vel[1][2], 1e-10)
|
||||
p.disconnect()
|
||||
|
||||
|
||||
class TestPybulletJacobian(unittest.TestCase):
|
||||
|
||||
def getMotorJointStates(self, robot):
|
||||
import pybullet as p
|
||||
joint_states = p.getJointStates(robot, range(p.getNumJoints(robot)))
|
||||
joint_infos = [p.getJointInfo(robot, i) for i in range(p.getNumJoints(robot))]
|
||||
joint_states = [j for j, i in zip(joint_states, joint_infos) if i[3] > -1]
|
||||
joint_positions = [state[0] for state in joint_states]
|
||||
joint_velocities = [state[1] for state in joint_states]
|
||||
joint_torques = [state[3] for state in joint_states]
|
||||
return joint_positions, joint_velocities, joint_torques
|
||||
|
||||
def setJointPosition(self, robot, position, kp=1.0, kv=0.3):
|
||||
import pybullet as p
|
||||
num_joints = p.getNumJoints(robot)
|
||||
zero_vec = [0.0] * num_joints
|
||||
if len(position) == num_joints:
|
||||
p.setJointMotorControlArray(robot,
|
||||
range(num_joints),
|
||||
p.POSITION_CONTROL,
|
||||
targetPositions=position,
|
||||
targetVelocities=zero_vec,
|
||||
positionGains=[kp] * num_joints,
|
||||
velocityGains=[kv] * num_joints)
|
||||
|
||||
def testJacobian(self):
|
||||
import pybullet as p
|
||||
|
||||
clid = p.connect(p.SHARED_MEMORY)
|
||||
if (clid < 0):
|
||||
p.connect(p.DIRECT)
|
||||
|
||||
time_step = 0.001
|
||||
gravity_constant = -9.81
|
||||
|
||||
urdfs = [
|
||||
"TwoJointRobot_w_fixedJoints.urdf", "TwoJointRobot_w_fixedJoints.urdf",
|
||||
"kuka_iiwa/model.urdf", "kuka_lwr/kuka.urdf"
|
||||
]
|
||||
for urdf in urdfs:
|
||||
p.resetSimulation()
|
||||
p.setTimeStep(time_step)
|
||||
p.setGravity(0.0, 0.0, gravity_constant)
|
||||
|
||||
robotId = p.loadURDF(urdf, useFixedBase=True)
|
||||
p.resetBasePositionAndOrientation(robotId, [0, 0, 0], [0, 0, 0, 1])
|
||||
numJoints = p.getNumJoints(robotId)
|
||||
endEffectorIndex = numJoints - 1
|
||||
|
||||
# Set a joint target for the position control and step the sim.
|
||||
self.setJointPosition(robotId, [0.1 * (i % 3) for i in range(numJoints)])
|
||||
p.stepSimulation()
|
||||
|
||||
# Get the joint and link state directly from Bullet.
|
||||
mpos, mvel, mtorq = self.getMotorJointStates(robotId)
|
||||
|
||||
result = p.getLinkState(robotId,
|
||||
endEffectorIndex,
|
||||
computeLinkVelocity=1,
|
||||
computeForwardKinematics=1)
|
||||
link_trn, link_rot, com_trn, com_rot, frame_pos, frame_rot, link_vt, link_vr = result
|
||||
# Get the Jacobians for the CoM of the end-effector link.
|
||||
# Note that in this example com_rot = identity, and we would need to use com_rot.T * com_trn.
|
||||
# The localPosition is always defined in terms of the link frame coordinates.
|
||||
|
||||
zero_vec = [0.0] * len(mpos)
|
||||
jac_t, jac_r = p.calculateJacobian(robotId, endEffectorIndex, com_trn, mpos, zero_vec,
|
||||
zero_vec)
|
||||
|
||||
assert (allclose(dot(jac_t, mvel), link_vt))
|
||||
assert (allclose(dot(jac_r, mvel), link_vr))
|
||||
p.disconnect()
|
||||
|
||||
|
||||
if __name__ == '__main__':
|
||||
unittest.main()
|
||||
288
Engine/lib/bullet/examples/pybullet/unittests/userDataTest.py
Normal file
288
Engine/lib/bullet/examples/pybullet/unittests/userDataTest.py
Normal file
|
|
@ -0,0 +1,288 @@
|
|||
import unittest
|
||||
import pybullet
|
||||
|
||||
from pybullet_utils import bullet_client
|
||||
|
||||
PLANE_PATH = "plane.urdf"
|
||||
ROBOT_PATH = "r2d2.urdf"
|
||||
OBJECT_WITH_USER_DATA_PATH = "user_data_test_object.urdf"
|
||||
|
||||
|
||||
class TestUserDataMethods(unittest.TestCase):
|
||||
|
||||
@classmethod
|
||||
def setUpClass(cls):
|
||||
TestUserDataMethods.server = bullet_client.BulletClient(
|
||||
connection_mode=pybullet.SHARED_MEMORY_SERVER)
|
||||
|
||||
@classmethod
|
||||
def tearDownClass(cls):
|
||||
del TestUserDataMethods.server
|
||||
|
||||
def setUp(self):
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
def tearDown(self):
|
||||
self.client.resetSimulation()
|
||||
del self.client
|
||||
|
||||
def testLoadingUserDataFromURDF(self):
|
||||
body_id = self.client.loadURDF(OBJECT_WITH_USER_DATA_PATH)
|
||||
self.client.syncUserData(body_id)
|
||||
num_user_data = self.client.getNumUserData(body_id)
|
||||
|
||||
self.assertEqual(num_user_data, 7)
|
||||
expected_user_data_infos = {
|
||||
0 : (b"userDataInRobot", 0, -1, -1),
|
||||
1:( b"userDataInBaseLink", 0, -1, -1),
|
||||
2:( b"secondBulletTagInBaseLink1", 0, -1, -1),
|
||||
3:( b"secondBulletTagInBaseLink2", 0, -1, -1),
|
||||
4:( b"userDataInVisualShape", 0, -1, 0),
|
||||
5:( b"userDataInChildLink", 0, 0, -1),
|
||||
6:( b"userDataInVisualShape2", 0, 0, 0),
|
||||
}
|
||||
for user_data_index in range(num_user_data):
|
||||
info = self.client.getUserDataInfo(body_id, user_data_index)
|
||||
self.assertEqual(info[1:], expected_user_data_infos[info[0]])
|
||||
user_data_val = self.client.getUserData(info[0])
|
||||
self.assertIn(
|
||||
b"Expected identifier: linkIndex: %i, visualShapeIndex: %i" %
|
||||
(info[3], info[4]), user_data_val)
|
||||
|
||||
def testAddUserData(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
|
||||
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
|
||||
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
|
||||
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
|
||||
|
||||
# Retrieve user data and make sure it's correct.
|
||||
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
|
||||
self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
|
||||
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
|
||||
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
|
||||
|
||||
# Disconnect/reconnect and make sure that the user data is synced back.
|
||||
del self.client
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
|
||||
self.assertEqual(b"MyValue2", self.client.getUserData(uid2))
|
||||
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
|
||||
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
|
||||
|
||||
self.client.resetSimulation()
|
||||
self.assertEqual(None, self.client.getUserData(uid1))
|
||||
self.assertEqual(None, self.client.getUserData(uid2))
|
||||
self.assertEqual(None, self.client.getUserData(uid3))
|
||||
self.assertEqual(None, self.client.getUserData(uid4))
|
||||
|
||||
def testGetNumUserData(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
|
||||
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
|
||||
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
|
||||
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
|
||||
|
||||
self.assertEqual(4, self.client.getNumUserData(plane_id))
|
||||
|
||||
del self.client
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
self.assertEqual(4, self.client.getNumUserData(plane_id))
|
||||
|
||||
def testReplaceUserData(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid = self.client.addUserData(plane_id, "MyKey", "MyValue")
|
||||
|
||||
self.assertEqual(b"MyValue", self.client.getUserData(uid))
|
||||
|
||||
new_uid = self.client.addUserData(plane_id, "MyKey", "MyNewValue")
|
||||
self.assertEqual(uid, new_uid)
|
||||
self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
|
||||
|
||||
del self.client
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
self.assertEqual(b"MyNewValue", self.client.getUserData(uid))
|
||||
|
||||
def testGetUserDataId(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
|
||||
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
|
||||
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
|
||||
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
|
||||
|
||||
self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
|
||||
self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
|
||||
self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
|
||||
self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
|
||||
|
||||
del self.client
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
self.assertEqual(uid1, self.client.getUserDataId(plane_id, "MyKey1"))
|
||||
self.assertEqual(uid2, self.client.getUserDataId(plane_id, "MyKey2"))
|
||||
self.assertEqual(uid3, self.client.getUserDataId(plane_id, "MyKey3"))
|
||||
self.assertEqual(uid4, self.client.getUserDataId(plane_id, "MyKey4"))
|
||||
|
||||
def testRemoveUserData(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
|
||||
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
|
||||
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
|
||||
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
|
||||
|
||||
self.client.removeUserData(uid2)
|
||||
|
||||
self.assertEqual(3, self.client.getNumUserData(plane_id))
|
||||
self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
|
||||
self.assertEqual(None, self.client.getUserData(uid2))
|
||||
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
|
||||
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
|
||||
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
|
||||
|
||||
del self.client
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
self.assertEqual(3, self.client.getNumUserData(plane_id))
|
||||
self.assertEqual(-1, self.client.getUserDataId(plane_id, "MyKey2"))
|
||||
self.assertEqual(None, self.client.getUserData(uid2))
|
||||
self.assertEqual(b"MyValue1", self.client.getUserData(uid1))
|
||||
self.assertEqual(b"MyValue3", self.client.getUserData(uid3))
|
||||
self.assertEqual(b"MyValue4", self.client.getUserData(uid4))
|
||||
|
||||
def testIterateAllUserData(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
|
||||
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
|
||||
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
|
||||
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
|
||||
|
||||
entries = set()
|
||||
for i in range(self.client.getNumUserData(plane_id)):
|
||||
userDataId, key, bodyId, linkIndex, visualShapeIndex = self.client.getUserDataInfo(
|
||||
plane_id, i)
|
||||
value = self.client.getUserData(userDataId)
|
||||
entries.add((userDataId, key, value, bodyId, linkIndex, visualShapeIndex))
|
||||
|
||||
self.assertTrue((uid1, b"MyKey1", b"MyValue1", plane_id, -1, -1) in entries)
|
||||
self.assertTrue((uid2, b"MyKey2", b"MyValue2", plane_id, -1, -1) in entries)
|
||||
self.assertTrue((uid3, b"MyKey3", b"MyValue3", plane_id, -1, -1) in entries)
|
||||
self.assertTrue((uid4, b"MyKey4", b"MyValue4", plane_id, -1, -1) in entries)
|
||||
self.assertEqual(4, len(entries))
|
||||
|
||||
def testRemoveBody(self):
|
||||
plane_id = self.client.loadURDF(PLANE_PATH)
|
||||
uid1 = self.client.addUserData(plane_id, "MyKey1", "MyValue1")
|
||||
uid2 = self.client.addUserData(plane_id, "MyKey2", "MyValue2")
|
||||
uid3 = self.client.addUserData(plane_id, "MyKey3", "MyValue3")
|
||||
uid4 = self.client.addUserData(plane_id, "MyKey4", "MyValue4")
|
||||
|
||||
self.client.removeBody(plane_id)
|
||||
self.assertEqual(None, self.client.getUserData(uid1))
|
||||
self.assertEqual(None, self.client.getUserData(uid2))
|
||||
self.assertEqual(None, self.client.getUserData(uid3))
|
||||
self.assertEqual(None, self.client.getUserData(uid4))
|
||||
|
||||
del self.client
|
||||
self.client = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
self.assertEqual(None, self.client.getUserData(uid1))
|
||||
self.assertEqual(None, self.client.getUserData(uid2))
|
||||
self.assertEqual(None, self.client.getUserData(uid3))
|
||||
self.assertEqual(None, self.client.getUserData(uid4))
|
||||
|
||||
def testMultipleBodies(self):
|
||||
plane1 = self.client.loadURDF(PLANE_PATH)
|
||||
plane2 = self.client.loadURDF(PLANE_PATH)
|
||||
|
||||
uid1 = self.client.addUserData(plane1, "MyKey1", "This is plane 1 - 1")
|
||||
uid2 = self.client.addUserData(plane1, "MyKey2", "This is plane 1 - 2")
|
||||
|
||||
uid3 = self.client.addUserData(plane2, "MyKey1", "This is plane 2 - 1")
|
||||
uid4 = self.client.addUserData(plane2, "MyKey2", "This is plane 2 - 2")
|
||||
uid5 = self.client.addUserData(plane2, "MyKey3", "This is plane 2 - 3")
|
||||
|
||||
self.assertEqual(b"This is plane 1 - 1",
|
||||
self.client.getUserData(self.client.getUserDataId(plane1, "MyKey1")))
|
||||
self.assertEqual(b"This is plane 1 - 2",
|
||||
self.client.getUserData(self.client.getUserDataId(plane1, "MyKey2")))
|
||||
|
||||
self.assertEqual(b"This is plane 2 - 1",
|
||||
self.client.getUserData(self.client.getUserDataId(plane2, "MyKey1")))
|
||||
self.assertEqual(b"This is plane 2 - 2",
|
||||
self.client.getUserData(self.client.getUserDataId(plane2, "MyKey2")))
|
||||
self.assertEqual(b"This is plane 2 - 3",
|
||||
self.client.getUserData(self.client.getUserDataId(plane2, "MyKey3")))
|
||||
|
||||
def testMultipleLinks(self):
|
||||
body_id = self.client.loadURDF(ROBOT_PATH)
|
||||
num_links = self.client.getNumJoints(body_id)
|
||||
|
||||
self.assertTrue(num_links > 1)
|
||||
|
||||
for link_index in range(num_links):
|
||||
uid1 = self.client.addUserData(body_id, "MyKey1", "Value1 for link %s" % link_index,
|
||||
link_index)
|
||||
uid2 = self.client.addUserData(body_id, "MyKey2", "Value2 for link %s" % link_index,
|
||||
link_index)
|
||||
|
||||
for link_index in range(num_links):
|
||||
uid1 = self.client.getUserDataId(body_id, "MyKey1", link_index)
|
||||
uid2 = self.client.getUserDataId(body_id, "MyKey2", link_index)
|
||||
self.assertEqual(("Value1 for link %s" % link_index).encode(), self.client.getUserData(uid1))
|
||||
self.assertEqual(("Value2 for link %s" % link_index).encode(), self.client.getUserData(uid2))
|
||||
|
||||
def testMultipleClients(self):
|
||||
client1 = self.client
|
||||
client2 = bullet_client.BulletClient(pybullet.SHARED_MEMORY)
|
||||
|
||||
plane_id = client1.loadURDF(PLANE_PATH)
|
||||
client2.syncBodyInfo()
|
||||
|
||||
# Add user data on client 1, check on client 1
|
||||
uid = client1.addUserData(plane_id, "MyKey", "MyValue")
|
||||
self.assertEqual(None, client2.getUserData(uid))
|
||||
client2.syncUserData()
|
||||
self.assertEqual(b"MyValue", client2.getUserData(uid))
|
||||
|
||||
# Overwrite the value on client 2, check on client 1
|
||||
client2.addUserData(plane_id, "MyKey", "MyNewValue")
|
||||
self.assertEqual(b"MyValue", client1.getUserData(uid))
|
||||
client1.syncUserData()
|
||||
self.assertEqual(b"MyNewValue", client1.getUserData(uid))
|
||||
|
||||
# Remove user data on client 1, check on client 2
|
||||
client1.removeUserData(uid)
|
||||
self.assertEqual(b"MyNewValue", client2.getUserData(uid))
|
||||
client2.syncUserData()
|
||||
self.assertEqual(None, client2.getUserData(uid))
|
||||
|
||||
del client2
|
||||
|
||||
def testUserDataOnVisualShapes(self):
|
||||
body_id = self.client.loadURDF(ROBOT_PATH)
|
||||
num_links = self.client.getNumJoints(body_id)
|
||||
visual_shapes = self.client.getVisualShapeData(body_id)
|
||||
|
||||
self.assertTrue(num_links > 0)
|
||||
self.assertTrue(len(visual_shapes) > 0)
|
||||
|
||||
user_data_entries = set()
|
||||
for link_index in range(-1, num_links):
|
||||
num_shapes = sum([1 for shape in visual_shapes if shape[1] == link_index])
|
||||
for shape_index in range(num_shapes):
|
||||
key = "MyKey"
|
||||
value = "MyValue %s, %s" % (link_index, shape_index)
|
||||
uid = self.client.addUserData(body_id, key, value, link_index, shape_index)
|
||||
user_data_entries.add((uid, key, value.encode(), body_id, link_index, shape_index))
|
||||
|
||||
self.assertEqual(len(visual_shapes), self.client.getNumUserData(body_id))
|
||||
for uid, key, value, body_id, link_index, shape_index in user_data_entries:
|
||||
self.assertEqual(value, self.client.getUserData(uid))
|
||||
self.assertEqual(uid, self.client.getUserDataId(body_id, key, link_index, shape_index))
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
unittest.main()
|
||||
8
Engine/lib/bullet/examples/pybullet/unittests/utils.py
Normal file
8
Engine/lib/bullet/examples/pybullet/unittests/utils.py
Normal file
|
|
@ -0,0 +1,8 @@
|
|||
def dot(A, b):
|
||||
"""Dot product between a 2D matrix and a 1D vector"""
|
||||
return [sum([aij * bj for aij, bj in zip(ai, b)]) for ai in A]
|
||||
|
||||
|
||||
def allclose(a, b, tol=1e-7):
|
||||
"""Are all elements of a vector close to one another"""
|
||||
return all([abs(ai - bi) < tol for ai, bi in zip(a, b)])
|
||||
Loading…
Add table
Add a link
Reference in a new issue