Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add yapf style and apply yapf to format all Python files

Browse Source 
This recreates pull request #2192
This commit is contained in:
Erwin Coumans
2019-04-27 07:31:15 -07:00
parent c591735042
commit ef9570c315
347 changed files with 70304 additions and 22752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

185
examples/pybullet/unittests/unittests.py
View File

@@ -4,113 +4,120 @@ 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_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_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_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()
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 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 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
def testJacobian(self):
import pybullet as p
clid = p.connect(p.SHARED_MEMORY)
if (clid<0):
p.connect(p.DIRECT)
clid = p.connect(p.SHARED_MEMORY)
if (clid < 0):
p.connect(p.DIRECT)
time_step = 0.001
gravity_constant = -9.81
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)
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
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()
# 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)
# 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.
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)
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()
assert(allclose(dot(jac_t, mvel), link_vt))
assert(allclose(dot(jac_r, mvel), link_vr))
p.disconnect()
if __name__ == '__main__':
unittest.main()
unittest.main()