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

Merge pull request #1761 from erwincoumans/master

Browse Source 
bump up pybullet version
This commit is contained in:
erwincoumans
2018-06-16 10:47:28 -07:00
committed by GitHub
parent d660042d76 b94df12081
commit ec37495142
34 changed files with 3681 additions and 85 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build_visual_studio_vr_pybullet_double.bat
View File

@@ -18,7 +18,7 @@ del tmp1234.txt
cd build3
premake4 --double --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
premake4 --double --enable_multithreading --midi --enable_static_vr_plugin --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010
#premake4 --serial --audio --double --midi --enable_openvr --enable_pybullet --python_include_dir="%myvar%/include" --python_lib_dir="%myvar%/libs" --targetdir="../bin" vs2010

12
data/quadruped/vision/chassis.mtl Normal file
View File

@@ -0,0 +1,12 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None.002
Ns 0.000000
Ka 0.000000 0.000000 0.000000
Kd 0.800000 0.800000 0.800000
Ks 0.800000 0.800000 0.800000
Ke 0.000000 0.000000 0.000000
Ni 1.000000
d 1.000000
illum 2

32
data/quadruped/vision/chassis.obj Normal file
View File

@@ -0,0 +1,32 @@
# Blender v2.78 (sub 0) OBJ File: ''
# www.blender.org
mtllib chassis.mtl
o Cube.002
v -0.150000 -0.065000 0.043500
v -0.150000 0.065000 -0.043500
v -0.150000 -0.065000 -0.043500
v -0.150000 0.065000 0.043500
v 0.150000 0.065000 -0.043500
v 0.150000 0.065000 0.043500
v 0.150000 -0.065000 -0.043500
v 0.150000 -0.065000 0.043500
vn -1.0000 -0.0000 0.0000
vn -0.0000 1.0000 0.0000
vn 1.0000 0.0000 0.0000
vn 0.0000 -1.0000 0.0000
vn -0.0000 0.0000 -1.0000
vn -0.0000 0.0000 1.0000
usemtl None.002
s 1
f 1//1 2//1 3//1
f 4//2 5//2 2//2
f 6//3 7//3 5//3
f 8//4 3//4 7//4
f 5//5 3//5 2//5
f 4//6 8//6 6//6
f 1//1 4//1 2//1
f 4//2 6//2 5//2
f 6//3 8//3 7//3
f 8//4 1//4 3//4
f 5//5 7//5 3//5
f 4//6 1//6 8//6

10
data/quadruped/vision/chassis2.mtl Normal file
View File

@@ -0,0 +1,10 @@
# Blender MTL File: 'None'
# Material Count: 1
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

1906
data/quadruped/vision/chassis2.obj Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
data/quadruped/vision/checker_blue.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 6.2 KiB

BIN
data/quadruped/vision/t-motor.jpg Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 153 KiB

BIN
data/quadruped/vision/tmotor.blend Normal file
View File

Binary file not shown.

19
data/quadruped/vision/tmotor3.mtl Normal file
View File

@@ -0,0 +1,19 @@
# Blender MTL File: 'tmotor.blend'
# Material Count: 2
newmtl None
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2
map_Kd t-motor.jpg
newmtl None_NONE
Ns 0
Ka 0.000000 0.000000 0.000000
Kd 0.8 0.8 0.8
Ks 0.8 0.8 0.8
d 1
illum 2

325
data/quadruped/vision/tmotor3.obj Normal file
View File

@@ -0,0 +1,325 @@
# Blender v2.78 (sub 0) OBJ File: 'tmotor.blend'
# www.blender.org
mtllib tmotor3.mtl
o Cylinder
v 0.000000 0.043000 -0.006500
v 0.000000 0.043000 0.006500
v 0.008389 0.042174 -0.006500
v 0.008389 0.042174 0.006500
v 0.016455 0.039727 -0.006500
v 0.016455 0.039727 0.006500
v 0.023890 0.035753 -0.006500
v 0.023890 0.035753 0.006500
v 0.030406 0.030406 -0.006500
v 0.030406 0.030406 0.006500
v 0.035753 0.023890 -0.006500
v 0.035753 0.023890 0.006500
v 0.039727 0.016455 -0.006500
v 0.039727 0.016455 0.006500
v 0.042174 0.008389 -0.006500
v 0.042174 0.008389 0.006500
v 0.043000 0.000000 -0.006500
v 0.043000 0.000000 0.006500
v 0.042174 -0.008389 -0.006500
v 0.042174 -0.008389 0.006500
v 0.039727 -0.016455 -0.006500
v 0.039727 -0.016455 0.006500
v 0.035753 -0.023890 -0.006500
v 0.035753 -0.023890 0.006500
v 0.030406 -0.030406 -0.006500
v 0.030406 -0.030406 0.006500
v 0.023890 -0.035753 -0.006500
v 0.023890 -0.035753 0.006500
v 0.016455 -0.039727 -0.006500
v 0.016455 -0.039727 0.006500
v 0.008389 -0.042174 -0.006500
v 0.008389 -0.042174 0.006500
v -0.000000 -0.043000 -0.006500
v -0.000000 -0.043000 0.006500
v -0.008389 -0.042174 -0.006500
v -0.008389 -0.042174 0.006500
v -0.016455 -0.039727 -0.006500
v -0.016455 -0.039727 0.006500
v -0.023890 -0.035753 -0.006500
v -0.023890 -0.035753 0.006500
v -0.030406 -0.030406 -0.006500
v -0.030406 -0.030406 0.006500
v -0.035753 -0.023889 -0.006500
v -0.035753 -0.023889 0.006500
v -0.039727 -0.016455 -0.006500
v -0.039727 -0.016455 0.006500
v -0.042174 -0.008389 -0.006500
v -0.042174 -0.008389 0.006500
v -0.043000 0.000000 -0.006500
v -0.043000 0.000000 0.006500
v -0.042174 0.008389 -0.006500
v -0.042174 0.008389 0.006500
v -0.039727 0.016455 -0.006500
v -0.039727 0.016455 0.006500
v -0.035753 0.023890 -0.006500
v -0.035753 0.023890 0.006500
v -0.030406 0.030406 -0.006500
v -0.030406 0.030406 0.006500
v -0.023889 0.035753 -0.006500
v -0.023889 0.035753 0.006500
v -0.016455 0.039727 -0.006500
v -0.016455 0.039727 0.006500
v -0.008389 0.042174 -0.006500
v -0.008389 0.042174 0.006500
vt 0.6520 0.1657
vt 0.8624 0.3762
vt 0.6520 0.8843
vt 0.5790 0.9064
vt 0.3543 0.8843
vt 0.5031 0.9139
vt 0.4273 0.9064
vt 0.2871 0.8484
vt 0.2281 0.8000
vt 0.1798 0.7411
vt 0.1438 0.6738
vt 0.1217 0.6009
vt 0.1142 0.5250
vt 0.1217 0.4491
vt 0.1438 0.3762
vt 0.1798 0.3089
vt 0.2281 0.2500
vt 0.2871 0.2016
vt 0.3543 0.1657
vt 0.4273 0.1436
vt 0.5031 0.1361
vt 0.5790 0.1436
vt 0.7192 0.2016
vt 0.7781 0.2500
vt 0.8265 0.3089
vt 0.8846 0.4491
vt 0.8920 0.5250
vt 0.8846 0.6009
vt 0.8624 0.6738
vt 0.8265 0.7411
vt 0.7781 0.8000
vt 0.7192 0.8484
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vt 0.0000 0.0000
vn 0.0000 0.0000 1.0000
vn 0.0980 0.9952 0.0000
vn 0.2903 0.9569 0.0000
vn 0.4714 0.8819 0.0000
vn 0.6344 0.7730 0.0000
vn 0.7730 0.6344 0.0000
vn 0.8819 0.4714 0.0000
vn 0.9569 0.2903 0.0000
vn 0.9952 0.0980 0.0000
vn 0.9952 -0.0980 0.0000
vn 0.9569 -0.2903 0.0000
vn 0.8819 -0.4714 0.0000
vn 0.7730 -0.6344 0.0000
vn 0.6344 -0.7730 0.0000
vn 0.4714 -0.8819 0.0000
vn 0.2903 -0.9569 0.0000
vn 0.0980 -0.9952 0.0000
vn -0.0980 -0.9952 0.0000
vn -0.2903 -0.9569 0.0000
vn -0.4714 -0.8819 0.0000
vn -0.6344 -0.7730 0.0000
vn -0.7730 -0.6344 0.0000
vn -0.8819 -0.4714 0.0000
vn -0.9569 -0.2903 0.0000
vn -0.9952 -0.0980 0.0000
vn -0.9952 0.0980 0.0000
vn -0.9569 0.2903 0.0000
vn -0.8819 0.4714 0.0000
vn -0.7730 0.6344 0.0000
vn -0.6344 0.7730 0.0000
vn -0.4714 0.8819 0.0000
vn -0.2903 0.9569 0.0000
vn -0.0980 0.9952 0.0000
vn 0.0000 0.0000 -1.0000
usemtl None
s off
f 30/1/1 22/2/1 6/3/1
f 6/3/1 4/4/1 62/5/1
f 2/6/1 64/7/1 62/5/1
f 62/5/1 60/8/1 58/9/1
f 58/9/1 56/10/1 54/11/1
f 54/11/1 52/12/1 50/13/1
f 50/13/1 48/14/1 54/11/1
f 46/15/1 44/16/1 42/17/1
f 42/17/1 40/18/1 38/19/1
f 38/19/1 36/20/1 34/21/1
f 34/21/1 32/22/1 38/19/1
f 30/1/1 28/23/1 26/24/1
f 26/24/1 24/25/1 22/2/1
f 22/2/1 20/26/1 18/27/1
f 18/27/1 16/28/1 22/2/1
f 14/29/1 12/30/1 10/31/1
f 10/31/1 8/32/1 6/3/1
f 4/4/1 2/6/1 62/5/1
f 62/5/1 58/9/1 6/3/1
f 54/11/1 48/14/1 46/15/1
f 46/15/1 42/17/1 54/11/1
f 38/19/1 32/22/1 30/1/1
f 30/1/1 26/24/1 22/2/1
f 22/2/1 16/28/1 14/29/1
f 14/29/1 10/31/1 22/2/1
f 6/3/1 58/9/1 54/11/1
f 54/11/1 42/17/1 38/19/1
f 38/19/1 30/1/1 6/3/1
f 22/2/1 10/31/1 6/3/1
f 6/3/1 54/11/1 38/19/1
usemtl None
f 2/33/2 3/34/2 1/35/2
f 4/36/3 5/37/3 3/34/3
f 6/38/4 7/39/4 5/37/4
f 8/40/5 9/41/5 7/39/5
f 10/42/6 11/43/6 9/41/6
f 12/44/7 13/45/7 11/43/7
f 14/46/8 15/47/8 13/45/8
f 16/48/9 17/49/9 15/47/9
f 18/50/10 19/51/10 17/49/10
f 20/52/11 21/53/11 19/51/11
f 22/54/12 23/55/12 21/53/12
f 24/56/13 25/57/13 23/55/13
f 26/58/14 27/59/14 25/57/14
f 28/60/15 29/61/15 27/59/15
f 30/62/16 31/63/16 29/61/16
f 32/64/17 33/65/17 31/63/17
f 34/66/18 35/67/18 33/65/18
f 36/68/19 37/69/19 35/67/19
f 38/70/20 39/71/20 37/69/20
f 40/72/21 41/73/21 39/71/21
f 42/74/22 43/75/22 41/73/22
f 44/76/23 45/77/23 43/75/23
f 46/78/24 47/79/24 45/77/24
f 48/80/25 49/81/25 47/79/25
f 50/82/26 51/83/26 49/81/26
f 52/84/27 53/85/27 51/83/27
f 54/86/28 55/87/28 53/85/28
f 56/88/29 57/89/29 55/87/29
f 58/90/30 59/91/30 57/89/30
f 60/92/31 61/93/31 59/91/31
f 62/94/32 63/95/32 61/93/32
f 64/96/33 1/35/33 63/95/33
f 31/63/34 55/87/34 63/95/34
f 2/33/2 4/36/2 3/34/2
f 4/36/3 6/38/3 5/37/3
f 6/38/4 8/40/4 7/39/4
f 8/40/5 10/42/5 9/41/5
f 10/42/6 12/44/6 11/43/6
f 12/44/7 14/46/7 13/45/7
f 14/46/8 16/48/8 15/47/8
f 16/48/9 18/50/9 17/49/9
f 18/50/10 20/52/10 19/51/10
f 20/52/11 22/54/11 21/53/11
f 22/54/12 24/56/12 23/55/12
f 24/56/13 26/58/13 25/57/13
f 26/58/14 28/60/14 27/59/14
f 28/60/15 30/62/15 29/61/15
f 30/62/16 32/64/16 31/63/16
f 32/64/17 34/66/17 33/65/17
f 34/66/18 36/68/18 35/67/18
f 36/68/19 38/70/19 37/69/19
f 38/70/20 40/72/20 39/71/20
f 40/72/21 42/74/21 41/73/21
f 42/74/22 44/76/22 43/75/22
f 44/76/23 46/78/23 45/77/23
f 46/78/24 48/80/24 47/79/24
f 48/80/25 50/82/25 49/81/25
f 50/82/26 52/84/26 51/83/26
f 52/84/27 54/86/27 53/85/27
f 54/86/28 56/88/28 55/87/28
f 56/88/29 58/90/29 57/89/29
f 58/90/30 60/92/30 59/91/30
f 60/92/31 62/94/31 61/93/31
f 62/94/32 64/96/32 63/95/32
f 64/96/33 2/33/33 1/35/33
f 63/95/34 1/35/34 3/34/34
f 3/34/34 5/37/34 7/39/34
f 7/39/34 9/41/34 15/47/34
f 11/43/34 13/45/34 15/47/34
f 15/47/34 17/49/34 19/51/34
f 19/51/34 21/53/34 23/55/34
f 23/55/34 25/57/34 31/63/34
f 27/59/34 29/61/34 31/63/34
f 31/63/34 33/65/34 35/67/34
f 35/67/34 37/69/34 31/63/34
f 39/71/34 41/73/34 47/79/34
f 43/75/34 45/77/34 47/79/34
f 47/79/34 49/81/34 51/83/34
f 51/83/34 53/85/34 55/87/34
f 55/87/34 57/89/34 63/95/34
f 59/91/34 61/93/34 63/95/34
f 63/95/34 3/34/34 15/47/34
f 9/41/34 11/43/34 15/47/34
f 15/47/34 19/51/34 31/63/34
f 25/57/34 27/59/34 31/63/34
f 31/63/34 37/69/34 39/71/34
f 41/73/34 43/75/34 47/79/34
f 47/79/34 51/83/34 55/87/34
f 57/89/34 59/91/34 63/95/34
f 3/34/34 7/39/34 15/47/34
f 19/51/34 23/55/34 31/63/34
f 31/63/34 39/71/34 47/79/34
f 47/79/34 55/87/34 31/63/34
f 63/95/34 15/47/34 31/63/34

220
data/quadruped/vision/vision.py Normal file
View File

@@ -0,0 +1,220 @@
import pybullet as p
import time
import math
pi = 3.14159264
limitVal = 2*pi
legpos = 3./4.*pi
legposS = 0
legposSright = 0#-0.3
legposSleft = 0#0.3
defaultERP=0.4
maxMotorForce = 5000
maxGearForce = 10000
jointFriction = 0.1
p.connect(p.GUI)
amplitudeId= p.addUserDebugParameter("amplitude",0,3.14,0.5)
timeScaleId = p.addUserDebugParameter("timeScale",0,10,1)
jointTypeNames={}
jointTypeNames[p.JOINT_REVOLUTE]="JOINT_REVOLUTE"
jointTypeNames[p.JOINT_FIXED]="JOINT_FIXED"
p.setPhysicsEngineParameter(numSolverIterations=100)
p.loadURDF("plane_transparent.urdf", useMaximalCoordinates=True)
#disable rendering during creation.
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.configureDebugVisualizer(p.COV_ENABLE_PLANAR_REFLECTION,1)
jointNamesToIndex={}
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
vision = p.loadURDF("vision60.urdf",[0,0,0.4],useFixedBase=False)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
for j in range(p.getNumJoints(vision)):
jointInfo = p.getJointInfo(vision,j)
jointInfoName = jointInfo[1].decode("utf-8")
print("joint ",j," = ",jointInfoName, "type=",jointTypeNames[jointInfo[2]])
jointNamesToIndex[jointInfoName ]=j
#print("jointNamesToIndex[..]=",jointNamesToIndex[jointInfoName])
p.setJointMotorControl2(vision,j,p.VELOCITY_CONTROL,targetVelocity=0, force=jointFriction)
chassis_right_center = jointNamesToIndex['chassis_right_center']
motor_front_rightR_joint = jointNamesToIndex['motor_front_rightR_joint']
motor_front_rightS_joint = jointNamesToIndex['motor_front_rightS_joint']
hip_front_rightR_joint = jointNamesToIndex['hip_front_rightR_joint']
knee_front_rightR_joint = jointNamesToIndex['knee_front_rightR_joint']
motor_front_rightL_joint = jointNamesToIndex['motor_front_rightL_joint']
motor_back_rightR_joint = jointNamesToIndex['motor_back_rightR_joint']
motor_back_rightS_joint = jointNamesToIndex['motor_back_rightS_joint']
hip_back_rightR_joint = jointNamesToIndex['hip_back_rightR_joint']
knee_back_rightR_joint = jointNamesToIndex['knee_back_rightR_joint']
motor_back_rightL_joint = jointNamesToIndex['motor_back_rightL_joint']
chassis_left_center = jointNamesToIndex['chassis_left_center']
motor_front_leftL_joint = jointNamesToIndex['motor_front_leftL_joint']
motor_front_leftS_joint = jointNamesToIndex['motor_front_leftS_joint']
hip_front_leftL_joint = jointNamesToIndex['hip_front_leftL_joint']
knee_front_leftL_joint = jointNamesToIndex['knee_front_leftL_joint']
motor_front_leftR_joint = jointNamesToIndex['motor_front_leftR_joint']
motor_back_leftL_joint = jointNamesToIndex['motor_back_leftL_joint']
hip_back_leftL_joint = jointNamesToIndex['hip_back_leftL_joint']
knee_back_leftL_joint = jointNamesToIndex['knee_back_leftL_joint']
motor_back_leftR_joint = jointNamesToIndex['motor_back_leftR_joint']
motor_back_leftS_joint = jointNamesToIndex['motor_back_leftS_joint']
motA_rf_Id= p.addUserDebugParameter("motA_rf",-limitVal,limitVal,legpos)
motB_rf_Id= p.addUserDebugParameter("motB_rf",-limitVal,limitVal,legpos)
motC_rf_Id= p.addUserDebugParameter("motC_rf",-limitVal,limitVal,legposSright)
erp_rf_Id= p.addUserDebugParameter("erp_rf",0,1,defaultERP)
relPosTarget_rf_Id= p.addUserDebugParameter("relPosTarget_rf",-limitVal,limitVal,0)
motA_lf_Id= p.addUserDebugParameter("motA_lf",-limitVal,limitVal,-legpos)
motB_lf_Id= p.addUserDebugParameter("motB_lf",-limitVal,limitVal,-legpos)
motC_lf_Id= p.addUserDebugParameter("motC_lf",-limitVal,limitVal,legposSleft)
erp_lf_Id= p.addUserDebugParameter("erp_lf",0,1,defaultERP)
relPosTarget_lf_Id= p.addUserDebugParameter("relPosTarget_lf",-limitVal,limitVal,0)
motA_rb_Id= p.addUserDebugParameter("motA_rb",-limitVal,limitVal,legpos)
motB_rb_Id= p.addUserDebugParameter("motB_rb",-limitVal,limitVal,legpos)
motC_rb_Id= p.addUserDebugParameter("motC_rb",-limitVal,limitVal,legposSright)
erp_rb_Id= p.addUserDebugParameter("erp_rb",0,1,defaultERP)
relPosTarget_rb_Id= p.addUserDebugParameter("relPosTarget_rb",-limitVal,limitVal,0)
motA_lb_Id= p.addUserDebugParameter("motA_lb",-limitVal,limitVal,-legpos)
motB_lb_Id= p.addUserDebugParameter("motB_lb",-limitVal,limitVal,-legpos)
motC_lb_Id= p.addUserDebugParameter("motC_lb",-limitVal,limitVal,legposSleft)
erp_lb_Id= p.addUserDebugParameter("erp_lb",0,1,defaultERP)
relPosTarget_lb_Id= p.addUserDebugParameter("relPosTarget_lb",-limitVal,limitVal,0)
camTargetPos=[0.25,0.62,-0.15]
camDist = 2
camYaw = -2
camPitch=-16
p.resetDebugVisualizerCamera(camDist, camYaw, camPitch, camTargetPos)
c_rf = p.createConstraint(vision,knee_front_rightR_joint,vision,motor_front_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,maxForce=maxGearForce)
c_lf = p.createConstraint(vision,knee_front_leftL_joint,vision,motor_front_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,maxForce=maxGearForce)
c_rb = p.createConstraint(vision,knee_back_rightR_joint,vision,motor_back_rightL_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,maxForce=maxGearForce)
c_lb = p.createConstraint(vision,knee_back_leftL_joint,vision,motor_back_leftR_joint,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,maxForce=maxGearForce)
p.setRealTimeSimulation(1)
for i in range (1):
#while (1):
motA_rf = p.readUserDebugParameter(motA_rf_Id)
motB_rf = p.readUserDebugParameter(motB_rf_Id)
motC_rf = p.readUserDebugParameter(motC_rf_Id)
erp_rf = p.readUserDebugParameter(erp_rf_Id)
relPosTarget_rf = p.readUserDebugParameter(relPosTarget_rf_Id)
#motC_rf
p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce)
p.changeConstraint(c_rf,gearRatio=-1, gearAuxLink = motor_front_rightR_joint,erp=erp_rf, relativePositionTarget=relPosTarget_rf,maxForce=maxGearForce)
motA_lf = p.readUserDebugParameter(motA_lf_Id)
motB_lf = p.readUserDebugParameter(motB_lf_Id)
motC_lf = p.readUserDebugParameter(motC_lf_Id)
erp_lf = p.readUserDebugParameter(erp_lf_Id)
relPosTarget_lf = p.readUserDebugParameter(relPosTarget_lf_Id)
p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce)
p.changeConstraint(c_lf,gearRatio=-1, gearAuxLink = motor_front_leftL_joint,erp=erp_lf, relativePositionTarget=relPosTarget_lf,maxForce=maxGearForce)
motA_rb = p.readUserDebugParameter(motA_rb_Id)
motB_rb = p.readUserDebugParameter(motB_rb_Id)
motC_rb = p.readUserDebugParameter(motC_rb_Id)
erp_rb = p.readUserDebugParameter(erp_rb_Id)
relPosTarget_rb = p.readUserDebugParameter(relPosTarget_rb_Id)
p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce)
p.changeConstraint(c_rb,gearRatio=-1, gearAuxLink = motor_back_rightR_joint,erp=erp_rb, relativePositionTarget=relPosTarget_rb,maxForce=maxGearForce)
motA_lb = p.readUserDebugParameter(motA_lb_Id)
motB_lb = p.readUserDebugParameter(motB_lb_Id)
motC_lb = p.readUserDebugParameter(motC_lb_Id)
erp_lb = p.readUserDebugParameter(erp_lb_Id)
relPosTarget_lb = p.readUserDebugParameter(relPosTarget_lb_Id)
p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)
p.changeConstraint(c_lb,gearRatio=-1, gearAuxLink = motor_back_leftL_joint,erp=erp_lb, relativePositionTarget=relPosTarget_lb,maxForce=maxGearForce)
p.setGravity(0,0,-10)
time.sleep(1./240.)
t = 0
prevTime = time.time()
while (1):
timeScale = p.readUserDebugParameter(timeScaleId)
amplitude = p.readUserDebugParameter(amplitudeId)
newTime = time.time()
dt = (newTime-prevTime)*timeScale
t = t+dt
prevTime = newTime
amp=amplitude
motA_rf = math.sin(t)*amp+legpos
motA_rb = math.sin(t)*amp+legpos
motA_lf = -(math.sin(t)*amp+legpos)
motA_lb = -(math.sin(t)*amp+legpos)
motB_rf = math.sin(t)*amp+legpos
motB_rb = math.sin(t)*amp+legpos
motB_lf = -(math.sin(t)*amp+legpos)
motB_lb = -(math.sin(t)*amp+legpos)
p.setJointMotorControl2(vision,motor_front_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_front_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lf,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_rightR_joint,p.POSITION_CONTROL,targetPosition=motA_rb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_rightL_joint,p.POSITION_CONTROL,targetPosition=motB_rb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_rightS_joint,p.POSITION_CONTROL,targetPosition=motC_rb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_leftL_joint,p.POSITION_CONTROL,targetPosition=motA_lb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_leftR_joint,p.POSITION_CONTROL,targetPosition=motB_lb,force=maxMotorForce)
p.setJointMotorControl2(vision,motor_back_leftS_joint,p.POSITION_CONTROL,targetPosition=motC_lb,force=maxMotorForce)
p.setGravity(0,0,-10)
time.sleep(1./240.)

696
data/quadruped/vision/vision60.urdf Normal file
View File

@@ -0,0 +1,696 @@
<?xml version="1.0"?>
<robot name="quadruped">
<link name="base_chassis_link">
<visual>
<geometry>
<mesh filename="chassis.obj" scale="1 1 1"/>
</geometry>
<material name="black">
<color rgba=".3 .3 .3 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".3 .13 .087"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz=".05 0 0"/>
<mass value="3.353"/>
<inertia ixx=".006837" ixy=".0" ixz=".0" iyy=".027262" iyz=".0" izz=".029870"/>
</inertial>
</link>
<link name="chassis_right">
<visual>
<geometry>
<mesh filename="chassis2.obj" scale="1 1 1"/>
</geometry>
<material name="yellow">
<color rgba=".7 .7 .3 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="chassis2.obj" scale="1 1 1"/>
</geometry>
</collision>
<inertial>
<mass value="1.32"/>
<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
</inertial>
</link>
<joint name="chassis_right_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_right"/>
<origin rpy="0 0 0" xyz="0 -.1265 -.0185"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightS_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightS_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_front_rightS_link"/>
<origin rpy="0 1.57075 0" xyz=".1375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightS_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightS_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightS_link"/>
<origin rpy="0 1.57075 0" xyz="-.1375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="chassis_left">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="chassis2.obj" scale="1 1 1"/>
</geometry>
<material name="yellow"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="chassis2.obj" scale="1 1 1"/>
</geometry>
</collision>
<inertial>
<mass value="1.32"/>
<inertia ixx=".004147" ixy=".0" ixz=".0" iyy=".040913" iyz=".0" izz=".041840"/>
</inertial>
</link>
<joint name="chassis_left_center" type="fixed">
<axis xyz="0 0 1"/>
<parent link="base_chassis_link"/>
<child link="chassis_left"/>
<origin rpy="0 0 0" xyz="0 .1265 -.0185"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftS_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftS_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_front_leftS_link"/>
<origin rpy="0 1.57075 0" xyz=".1375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftS_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".05" radius=".01"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftS_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftS_link"/>
<origin rpy="0 1.57075 0" xyz="-.1375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightR_link">
<visual>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 0"/>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightS_link"/>
<child link="motor_front_rightR_link"/>
<origin rpy="1.57075 -1.57075 0" xyz="0 0 0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightS_link"/>
<child link="motor_front_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="0 0.01 .1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftS_link"/>
<child link="motor_front_leftL_link"/>
<origin rpy="-1.57075 1.57075 0" xyz="0 0 0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_front_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_front_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftS_link"/>
<child link="motor_front_leftR_link"/>
<origin rpy="1.57075 0 0" xyz="0 0 0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightS_link"/>
<child link="motor_back_rightR_link"/>
<origin rpy="1.57075 -1.57075 0" xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_rightL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_rightL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_right"/>
<child link="motor_back_rightL_link"/>
<origin rpy="1.57075 0 3.141592" xyz="-.2375 .0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftL_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftL_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftS_link"/>
<child link="motor_back_leftL_link"/>
<origin rpy="-1.57075 1.57075 0" xyz="0 0 -0.1"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="motor_back_leftR_link">
<visual>
<geometry>
<mesh filename="tmotor3.obj" scale="1 1 1"/>
</geometry>
<material name="white">
<color rgba="1 1 1 1"/>
</material>
</visual>
<collision>
<geometry>
<cylinder length=".021" radius=".0425"/>
</geometry>
</collision>
<inertial>
<mass value=".072"/>
<inertia ixx=".00011397" ixy=".0" ixz=".0" iyy=".00011397" iyz=".0" izz=".00021765"/>
</inertial>
</link>
<joint name="motor_back_leftR_joint" type="continuous">
<axis xyz="0 0 1"/>
<parent link="chassis_left"/>
<child link="motor_back_leftR_link"/>
<origin rpy="1.57075 0 0" xyz="-.2375 -.0275 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_rightR_link">
<visual>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_rightR_link"/>
<child link="upper_leg_front_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 0.095 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_rightR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_front_rightR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_rightR_link"/>
<child link="lower_leg_front_rightR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .096"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_front_leftL_link">
<visual>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_front_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_front_leftL_link"/>
<child link="upper_leg_front_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .095 .0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_front_leftL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_front_leftL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_front_leftL_link"/>
<child link="lower_leg_front_leftL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .096"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_rightR_link">
<visual>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_rightR_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_rightR_link"/>
<child link="upper_leg_back_rightR_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .095 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_rightR_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_back_rightR_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_rightR_link"/>
<child link="lower_leg_back_rightR_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .096"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="upper_leg_back_leftL_link">
<visual>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
<material name="grey">
<color rgba=".65 .65 .75 1"/>
</material>
</visual>
<collision>
<geometry>
<box size=".039 .008 .199"/>
</geometry>
</collision>
<inertial>
<mass value=".034"/>
<inertia ixx=".000038770" ixy=".0" ixz=".0" iyy=".000042198" iyz=".0" izz=".0000036030"/>
</inertial>
</link>
<joint name="hip_back_leftL_joint" type="fixed">
<axis xyz="0 0 1"/>
<parent link="motor_back_leftL_link"/>
<child link="upper_leg_back_leftL_link"/>
<origin rpy="-1.57075 0 0" xyz="0 .095 0"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
<link name="lower_leg_back_leftL_link">
<contact>
<friction_anchor/>
<stiffness value="3000.0"/>
<damping value="100.0"/>
<spinning_friction value=".3"/>
<lateral_friction value="1"/>
</contact>
<visual>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<geometry>
<box size=".017 .009 .200"/>
</geometry>
</collision>
<inertial>
<origin rpy="0 0 0" xyz="0 0 .120"/>
<mass value=".086"/>
<inertia ixx=".00032459" ixy=".0" ixz=".0" iyy=".00032637" iyz=".0" izz=".000002178"/>
</inertial>
</link>
<joint name="knee_back_leftL_joint" type="continuous">
<axis xyz="0 1 0"/>
<parent link="upper_leg_back_leftL_link"/>
<child link="lower_leg_back_leftL_link"/>
<origin rpy="0 0 0" xyz="0 .0085 .096"/>
<limit effort="100" velocity="100"/>
<joint_properties damping=".0" friction=".0"/>
</joint>
</robot>

7
examples/RobotSimulator/premake4.lua
View File

@@ -275,8 +275,13 @@ if not _OPTIONS["no-enet"] then
if os.is("MacOSX") then
links{"Cocoa.framework"}
end
if os.is("Linux") then initX11()
links {"pthread"}
end
files {
"HelloBulletRobotics.cpp"
}

9
examples/SharedMemory/PhysicsClient.h
View File

@@ -43,8 +43,9 @@ public:
virtual void setSharedMemoryKey(int key) = 0;
virtual void uploadBulletFileToSharedMemory(const char* data, int len) = 0;
virtual char* getSharedMemoryStreamBuffer() = 0;
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays) = 0;
virtual int getNumDebugLines() const = 0;
virtual const float* getDebugLinesFrom() const = 0;
@@ -78,6 +79,10 @@ public:
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const = 0;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const = 0;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const = 0;
virtual void pushProfileTiming(const char* timingName)=0;
virtual void popProfileTiming()=0;
};

80
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2443,8 +2443,16 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemo
return 0;
}
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linkIndex = -1;
command->m_changeDynamicsInfoArgs.m_activationState = activationState;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE;
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info)
@@ -2728,16 +2736,13 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS;
command->m_requestRaycastIntersections.m_numRays = 1;
command->m_requestRaycastIntersections.m_numRays = 0;
command->m_requestRaycastIntersections.m_numThreads = 1;
b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer();
rayDataStream[0].m_rayFromPosition[0] = rayFromWorldX;
rayDataStream[0].m_rayFromPosition[1] = rayFromWorldY;
rayDataStream[0].m_rayFromPosition[2] = rayFromWorldZ;
rayDataStream[0].m_rayToPosition[0] = rayToWorldX;
rayDataStream[0].m_rayToPosition[1] = rayToWorldY;
rayDataStream[0].m_rayToPosition[2] = rayToWorldZ;
double rayFrom[3] = {rayFromWorldX,rayFromWorldY,rayFromWorldZ};
double rayTo[3] = {rayToWorldX,rayToWorldY,rayToWorldZ};
cl->uploadRaysToSharedMemory(*command, rayFrom, rayTo, 1);
return (b3SharedMemoryCommandHandle)command;
}
@@ -2762,31 +2767,37 @@ B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle comm
command->m_requestRaycastIntersections.m_numThreads = numThreads;
}
B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3])
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
b3Assert(command->m_client)
PhysicsClient *cl = command->m_client;
b3Assert(cl);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{
int numRays = command->m_requestRaycastIntersections.m_numRays;
if (numRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
b3RayData* rayDataStream = (b3RayData *)cl->getSharedMemoryStreamBuffer();
rayDataStream[numRays].m_rayFromPosition[0] = rayFromWorld[0];
rayDataStream[numRays].m_rayFromPosition[1] = rayFromWorld[1];
rayDataStream[numRays].m_rayFromPosition[2] = rayFromWorld[2];
rayDataStream[numRays].m_rayToPosition[0] = rayToWorld[0];
rayDataStream[numRays].m_rayToPosition[1] = rayToWorld[1];
rayDataStream[numRays].m_rayToPosition[2] = rayToWorld[2];
command->m_requestRaycastIntersections.m_numRays++;
}
cl->uploadRaysToSharedMemory(*command, rayFromWorld, rayToWorld, 1);
}
}
B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS);
b3Assert(numRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
if (command->m_type == CMD_REQUEST_RAY_CAST_INTERSECTIONS)
{
cl->uploadRaysToSharedMemory(*command, rayFromWorldArray, rayToWorldArray, numRays);
}
}
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo)
{
@@ -4430,6 +4441,21 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsCl
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API void b3PushProfileTiming(b3PhysicsClientHandle physClient, const char* timingName)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
cl->pushProfileTiming(timingName);
}
B3_SHARED_API void b3PopProfileTiming(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
cl->popProfileTiming();
}
B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;

7
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -142,6 +142,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetContactStiffnessAndDamping(b3SharedMemo
B3_SHARED_API int b3ChangeDynamicsInfoSetFrictionAnchor(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, int frictionAnchor);
B3_SHARED_API int b3ChangeDynamicsInfoSetCcdSweptSphereRadius(b3SharedMemoryCommandHandle commandHandle,int bodyUniqueId,int linkIndex, double ccdSweptSphereRadius);
B3_SHARED_API int b3ChangeDynamicsInfoSetContactProcessingThreshold(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double contactProcessingThreshold);
B3_SHARED_API int b3ChangeDynamicsInfoSetActivationState(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int activationState);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyUniqueId, int parentJointIndex, int childBodyUniqueId, int childJointIndex, struct b3JointInfo* info);
@@ -543,7 +544,8 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastCommandInit(b3PhysicsCl
B3_SHARED_API b3SharedMemoryCommandHandle b3CreateRaycastBatchCommandInit(b3PhysicsClientHandle physClient);
// Sets the number of threads to use to compute the ray intersections for the batch. Specify 0 to let Bullet decide, 1 (default) for single core execution, 2 or more to select the number of threads to use.
B3_SHARED_API void b3RaycastBatchSetNumThreads(b3SharedMemoryCommandHandle commandHandle, int numThreads);
B3_SHARED_API void b3RaycastBatchAddRay(b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[/*3*/], const double rayToWorld[/*3*/]);
B3_SHARED_API void b3RaycastBatchAddRay(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double rayFromWorld[3], const double rayToWorld[3]);
B3_SHARED_API void b3RaycastBatchAddRays(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, const double* rayFromWorld, const double* rayToWorld, int numRays);
B3_SHARED_API void b3GetRaycastInformation(b3PhysicsClientHandle physClient, struct b3RaycastInformation* raycastInfo);
@@ -597,6 +599,9 @@ B3_SHARED_API int b3StateLoggingStop(b3SharedMemoryCommandHandle commandHandle,
B3_SHARED_API b3SharedMemoryCommandHandle b3ProfileTimingCommandInit(b3PhysicsClientHandle physClient, const char* name);
B3_SHARED_API void b3SetProfileTimingDuractionInMicroSeconds(b3SharedMemoryCommandHandle commandHandle, int duration);
B3_SHARED_API void b3PushProfileTiming(b3PhysicsClientHandle physClient, const char* timingName);
B3_SHARED_API void b3PopProfileTiming(b3PhysicsClientHandle physClient);
B3_SHARED_API void b3SetTimeOut(b3PhysicsClientHandle physClient, double timeOutInSeconds);
B3_SHARED_API double b3GetTimeOut(b3PhysicsClientHandle physClient);

80
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -12,7 +12,7 @@
#include "SharedMemoryBlock.h"
#include "BodyJointInfoUtility.h"
#include "SharedMemoryUserData.h"
#include "LinearMath/btQuickprof.h"
struct UserDataCache
@@ -41,11 +41,16 @@ struct BodyJointInfoCache
}
};
struct PhysicsClientSharedMemoryInternalData {
SharedMemoryInterface* m_sharedMemory;
bool m_ownsSharedMemory;
SharedMemoryBlock* m_testBlock1;
btAlignedObjectArray<CProfileSample* > m_profileTimings;
btHashMap<btHashString, std::string*> m_profileTimingStringArray;
btHashMap<btHashInt,BodyJointInfoCache*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
@@ -216,6 +221,16 @@ PhysicsClientSharedMemory::~PhysicsClientSharedMemory() {
}
resetData();
for (int i=0;i<m_data->m_profileTimingStringArray.size();i++)
{
std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i);
if (str)
{
delete *str;
}
}
m_data->m_profileTimingStringArray.clear();
if (m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
@@ -234,6 +249,8 @@ void PhysicsClientSharedMemory::removeCachedBody(int bodyUniqueId)
}
void PhysicsClientSharedMemory::resetData()
{
m_data->m_debugLinesFrom.clear();
m_data->m_debugLinesTo.clear();
m_data->m_debugLinesColor.clear();
@@ -950,6 +967,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
case CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED:
{
B3_PROFILE("m_raycastHits");
if (m_data->m_verboseOutput)
{
b3Printf("Raycast completed");
@@ -1665,7 +1683,6 @@ bool PhysicsClientSharedMemory::canSubmitCommand() const {
struct SharedMemoryCommand* PhysicsClientSharedMemory::getAvailableSharedMemoryCommand() {
static int sequence = 0;
m_data->m_testBlock1->m_clientCommands[0].m_sequenceNumber = sequence++;
m_data->m_testBlock1->m_clientCommands[0].m_client = this;
return &m_data->m_testBlock1->m_clientCommands[0];
}
@@ -1685,9 +1702,6 @@ bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& c
return false;
}
char* PhysicsClientSharedMemory::getSharedMemoryStreamBuffer() {
return m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
}
void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) {
btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
@@ -1703,6 +1717,31 @@ void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data,
}
}
void PhysicsClientSharedMemory::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{
int curNumRays = command.m_requestRaycastIntersections.m_numRays;
int newNumRays = curNumRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
for (int i=0;i<numRays;i++)
{
b3RayData* rayDataStream = (b3RayData *)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2];
rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2];
command.m_requestRaycastIntersections.m_numRays++;
}
}
}
void PhysicsClientSharedMemory::getCachedCameraImage(struct b3CameraImageData* cameraData)
{
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
@@ -1882,3 +1921,34 @@ void PhysicsClientSharedMemory::getUserDataInfo(int bodyUniqueId, int linkIndex,
SharedMemoryUserData *userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
}
void PhysicsClientSharedMemory::pushProfileTiming(const char* timingName)
{
std::string** strPtr = m_data->m_profileTimingStringArray[timingName];
std::string* str = 0;
if (strPtr)
{
str = *strPtr;
} else
{
str = new std::string(timingName);
m_data->m_profileTimingStringArray.insert(timingName,str);
}
m_data->m_profileTimings.push_back(new CProfileSample(str->c_str()));
}
void PhysicsClientSharedMemory::popProfileTiming()
{
if (m_data->m_profileTimings.size())
{
CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size()-1];
m_data->m_profileTimings.pop_back();
delete sample;
}
}

5
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@@ -54,7 +54,8 @@ public:
virtual void setSharedMemoryKey(int key);
virtual void uploadBulletFileToSharedMemory(const char* data, int len);
virtual char* getSharedMemoryStreamBuffer();
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
@@ -89,6 +90,8 @@ public:
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif // BT_PHYSICS_CLIENT_API_H

80
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -14,12 +14,15 @@
#include <string>
#include "SharedMemoryUserData.h"
#include "LinearMath/btQuickprof.h"
struct UserDataCache {
btHashMap<btHashInt, SharedMemoryUserData> m_userDataMap;
btHashMap<btHashString, int> m_keyToUserDataIdMap;
~UserDataCache() {
~UserDataCache()
{
}
};
@@ -58,6 +61,9 @@ struct PhysicsDirectInternalData
btHashMap<btHashInt,BodyJointInfoCache2*> m_bodyJointMap;
btHashMap<btHashInt,b3UserConstraint> m_userConstraintInfoMap;
btAlignedObjectArray<CProfileSample* > m_profileTimings;
btHashMap<btHashString, std::string*> m_profileTimingStringArray;
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
btAlignedObjectArray<double> m_cachedMassMatrix;
int m_cachedCameraPixelsWidth;
@@ -106,11 +112,21 @@ PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk, bool pas
m_data = new PhysicsDirectInternalData;
m_data->m_commandProcessor = physSdk;
m_data->m_ownsCommandProcessor = passSdkOwnership;
m_data->m_command.m_client = this;
}
PhysicsDirect::~PhysicsDirect()
{
for (int i=0;i<m_data->m_profileTimingStringArray.size();i++)
{
std::string** str = m_data->m_profileTimingStringArray.getAtIndex(i);
if (str)
{
delete *str;
}
}
m_data->m_profileTimingStringArray.clear();
if (m_data->m_commandProcessor->isConnected())
{
m_data->m_commandProcessor->disconnect();
@@ -1318,16 +1334,11 @@ bool PhysicsDirect::getJointInfo(int bodyIndex, int jointIndex, struct b3JointIn
return false;
}
///todo: move this out of the
void PhysicsDirect::setSharedMemoryKey(int key)
{
//m_data->m_physicsServer->setSharedMemoryKey(key);
//m_data->m_physicsClient->setSharedMemoryKey(key);
}
char* PhysicsDirect::getSharedMemoryStreamBuffer() {
return m_data->m_bulletStreamDataServerToClient;
}
void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
{
@@ -1342,6 +1353,31 @@ void PhysicsDirect::uploadBulletFileToSharedMemory(const char* data, int len)
//m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
}
void PhysicsDirect::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{
int curNumRays = command.m_requestRaycastIntersections.m_numRays;
int newNumRays = curNumRays + numRays;
btAssert(newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE);
if (newNumRays<MAX_RAY_INTERSECTION_BATCH_SIZE)
{
for (int i=0;i<numRays;i++)
{
b3RayData* rayDataStream = (b3RayData *)m_data->m_bulletStreamDataServerToClient;
rayDataStream[curNumRays+i].m_rayFromPosition[0] = rayFromWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayFromPosition[1] = rayFromWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayFromPosition[2] = rayFromWorldArray[i*3+2];
rayDataStream[curNumRays+i].m_rayToPosition[0] = rayToWorldArray[i*3+0];
rayDataStream[curNumRays+i].m_rayToPosition[1] = rayToWorldArray[i*3+1];
rayDataStream[curNumRays+i].m_rayToPosition[2] = rayToWorldArray[i*3+2];
command.m_requestRaycastIntersections.m_numRays++;
}
}
}
int PhysicsDirect::getNumDebugLines() const
{
return m_data->m_debugLinesFrom.size();
@@ -1530,3 +1566,31 @@ void PhysicsDirect::getUserDataInfo(int bodyUniqueId, int linkIndex, int userDat
SharedMemoryUserData* userDataPtr = (userDataCachePtr)->m_userDataMap.getAtIndex(userDataIndex);
*keyOut = (userDataPtr)->m_key.c_str();
}
void PhysicsDirect::pushProfileTiming(const char* timingName)
{
std::string** strPtr = m_data->m_profileTimingStringArray[timingName];
std::string* str = 0;
if (strPtr)
{
str = *strPtr;
} else
{
str = new std::string(timingName);
m_data->m_profileTimingStringArray.insert(timingName,str);
}
m_data->m_profileTimings.push_back(new CProfileSample(str->c_str()));
}
void PhysicsDirect::popProfileTiming()
{
if (m_data->m_profileTimings.size())
{
CProfileSample* sample = m_data->m_profileTimings[m_data->m_profileTimings.size()-1];
m_data->m_profileTimings.pop_back();
delete sample;
}
}

6
examples/SharedMemory/PhysicsDirect.h
View File

@@ -78,7 +78,8 @@ public:
virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len);
virtual char* getSharedMemoryStreamBuffer();
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
@@ -118,6 +119,9 @@ public:
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //PHYSICS_DIRECT_H

16
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -140,15 +140,17 @@ void PhysicsLoopBack::setSharedMemoryKey(int key)
m_data->m_physicsClient->setSharedMemoryKey(key);
}
char* PhysicsLoopBack::getSharedMemoryStreamBuffer() {
return m_data->m_physicsClient->getSharedMemoryStreamBuffer();
}
void PhysicsLoopBack::uploadBulletFileToSharedMemory(const char* data, int len)
{
m_data->m_physicsClient->uploadBulletFileToSharedMemory(data,len);
}
void PhysicsLoopBack::uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays)
{
m_data->m_physicsClient->uploadRaysToSharedMemory(command, rayFromWorldArray, rayToWorldArray, numRays);
}
int PhysicsLoopBack::getNumDebugLines() const
{
return m_data->m_physicsClient->getNumDebugLines();
@@ -245,3 +247,11 @@ void PhysicsLoopBack::getUserDataInfo(int bodyUniqueId, int linkIndex, int userD
m_data->m_physicsClient->getUserDataInfo(bodyUniqueId, linkIndex, userDataIndex, keyOut, userDataIdOut);
}
void PhysicsLoopBack::pushProfileTiming(const char* timingName)
{
m_data->m_physicsClient->pushProfileTiming(timingName);
}
void PhysicsLoopBack::popProfileTiming()
{
m_data->m_physicsClient->popProfileTiming();
}

6
examples/SharedMemory/PhysicsLoopBack.h
View File

@@ -58,7 +58,8 @@ public:
virtual void setSharedMemoryKey(int key);
void uploadBulletFileToSharedMemory(const char* data, int len);
virtual char* getSharedMemoryStreamBuffer();
virtual void uploadRaysToSharedMemory(struct SharedMemoryCommand& command, const double* rayFromWorldArray, const double* rayToWorldArray, int numRays);
virtual int getNumDebugLines() const;
@@ -92,6 +93,9 @@ public:
virtual int getCachedUserDataId(int bodyUniqueId, int linkIndex, const char *key) const;
virtual int getNumUserData(int bodyUniqueId, int linkIndex) const;
virtual void getUserDataInfo(int bodyUniqueId, int linkIndex, int userDataIndex, const char **keyOut, int *userDataIdOut) const;
virtual void pushProfileTiming(const char* timingName);
virtual void popProfileTiming();
};
#endif //PHYSICS_LOOP_BACK_H

69
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1659,6 +1659,8 @@ struct PhysicsServerCommandProcessorInternalData
b3HashMap<b3HashString, char*> m_profileEvents;
b3HashMap<b3HashString, UrdfVisualShapeCache> m_cachedVUrdfisualShapes;
btITaskScheduler* m_scheduler;
PhysicsServerCommandProcessorInternalData(PhysicsCommandProcessorInterface* proc)
:m_pluginManager(proc),
m_useRealTimeSimulation(false),
@@ -1686,7 +1688,8 @@ struct PhysicsServerCommandProcessorInternalData
m_pickedBody(0),
m_pickedConstraint(0),
m_pickingMultiBodyPoint2Point(0),
m_pdControlPlugin(-1)
m_pdControlPlugin(-1),
m_scheduler(0)
{
{
@@ -1780,13 +1783,15 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor()
createEmptyDynamicsWorld();
#ifdef BT_THREADSAFE
if (btGetTaskScheduler() == 0) {
btITaskScheduler *scheduler = btCreateDefaultTaskScheduler();
if (scheduler == 0) {
scheduler = btGetSequentialTaskScheduler();
m_data->m_scheduler = btCreateDefaultTaskScheduler();
if (m_data->m_scheduler == 0) {
m_data->m_scheduler = btGetSequentialTaskScheduler();
}
btSetTaskScheduler(scheduler);
btSetTaskScheduler(m_data->m_scheduler);
}
#endif //BT_THREADSAFE
}
PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
@@ -1802,6 +1807,9 @@ PhysicsServerCommandProcessor::~PhysicsServerCommandProcessor()
char* event = *m_data->m_profileEvents.getAtIndex(i);
delete[] event;
}
if (m_data->m_scheduler)
delete m_data->m_scheduler;
delete m_data;
}
@@ -4821,10 +4829,11 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
const int numRays = clientCmd.m_requestRaycastIntersections.m_numRays;
const int numThreads = clientCmd.m_requestRaycastIntersections.m_numThreads;
b3RayData *rayInputBuffer = (b3RayData *)malloc(sizeof(b3RayData) * numRays);
memcpy(rayInputBuffer, bufferServerToClient, sizeof(b3RayData) * numRays);
BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, rayInputBuffer, (b3RayHitInfo *)bufferServerToClient, numRays);
btAlignedObjectArray<b3RayData> rays;
rays.resize(numRays);
memcpy(&rays[0],bufferServerToClient,numRays*sizeof(b3RayData));
BatchRayCaster batchRayCaster(m_data->m_dynamicsWorld, &rays[0], (b3RayHitInfo *)bufferServerToClient, numRays);
if (numThreads == 0) {
// When 0 is specified, Bullet can decide how many threads to use.
// About 16 rays per thread seems to work reasonably well.
@@ -4840,7 +4849,6 @@ bool PhysicsServerCommandProcessor::processRequestRaycastIntersectionsCommand(co
batchRayCaster.castRays(numThreads);
}
free(rayInputBuffer);
serverStatusOut.m_raycastHits.m_numRaycastHits = numRays;
serverStatusOut.m_type = CMD_REQUEST_RAY_CAST_INTERSECTIONS_COMPLETED;
return hasStatus;
@@ -6747,6 +6755,26 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
btMultiBody* mb = body->m_multiBody;
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
{
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateWakeUp)
{
mb->wakeUp();
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateSleep)
{
mb->goToSleep();
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateEnableSleeping)
{
mb->setCanSleep(true);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateDisableSleeping)
{
mb->setCanSleep(false);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
@@ -6872,6 +6900,27 @@ bool PhysicsServerCommandProcessor::processChangeDynamicsInfoCommand(const struc
{
if (body && body->m_rigidBody)
{
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE)
{
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateEnableSleeping)
{
body->m_rigidBody->forceActivationState(ACTIVE_TAG);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateDisableSleeping)
{
body->m_rigidBody->forceActivationState(DISABLE_DEACTIVATION);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateWakeUp)
{
body->m_rigidBody->forceActivationState(ACTIVE_TAG);
}
if (clientCmd.m_changeDynamicsInfoArgs.m_activationState&eActivationStateSleep)
{
body->m_rigidBody->forceActivationState(ISLAND_SLEEPING);
}
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
btScalar angDamping = body->m_rigidBody->getAngularDamping();

10
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -23,6 +23,12 @@
typedef unsigned long long int smUint64_t;
#endif
#ifdef __APPLE__
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024)
#else
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024)
#endif
#define SHARED_MEMORY_SERVER_TEST_C
#define MAX_DEGREE_OF_FREEDOM 128
#define MAX_NUM_SENSORS 256
@@ -161,6 +167,7 @@ enum EnumChangeDynamicsInfoFlags
CHANGE_DYNAMICS_INFO_SET_LOCAL_INERTIA_DIAGONAL = 1024,
CHANGE_DYNAMICS_INFO_SET_CCD_SWEPT_SPHERE_RADIUS = 2048,
CHANGE_DYNAMICS_INFO_SET_CONTACT_PROCESSING_THRESHOLD = 4096,
CHANGE_DYNAMICS_INFO_SET_ACTIVATION_STATE = 8192,
};
struct ChangeDynamicsInfoArgs
@@ -181,6 +188,7 @@ struct ChangeDynamicsInfoArgs
int m_frictionAnchor;
double m_ccdSweptSphereRadius;
double m_contactProcessingThreshold;
int m_activationState;
};
struct GetDynamicsInfoArgs
@@ -1009,7 +1017,7 @@ struct SharedMemoryCommand
int m_type;
smUint64_t m_timeStamp;
int m_sequenceNumber;
struct PhysicsClient *m_client;
//m_updateFlags is a bit fields to tell which parameters need updating
//for example m_updateFlags = SIM_PARAM_UPDATE_DELTA_TIME | SIM_PARAM_UPDATE_NUM_SOLVER_ITERATIONS;

21
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -4,8 +4,11 @@
#define SHARED_MEMORY_KEY 12347
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
//Please don't replace an existing magic number:
//instead, only ADD a new one at the top, comment-out previous one
#define SHARED_MEMORY_MAGIC_NUMBER 201806150
//#define SHARED_MEMORY_MAGIC_NUMBER 201806020
//#define SHARED_MEMORY_MAGIC_NUMBER 201801170
//#define SHARED_MEMORY_MAGIC_NUMBER 201801080
//#define SHARED_MEMORY_MAGIC_NUMBER 201801010
@@ -17,11 +20,7 @@
//#define SHARED_MEMORY_MAGIC_NUMBER 201706001
//#define SHARED_MEMORY_MAGIC_NUMBER 201703024
#ifdef __APPLE__
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (512*1024)
#else
#define SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE (8*1024*1024)
#endif
enum EnumSharedMemoryClientCommand
{
@@ -319,6 +318,15 @@ struct b3BodyInfo
char m_bodyName[1024]; // for btRigidBody, it does not have a base, but can still have a body name from urdf
};
enum DynamicsActivationState
{
eActivationStateEnableSleeping = 1,
eActivationStateDisableSleeping = 2,
eActivationStateWakeUp = 4,
eActivationStateSleep = 8,
};
struct b3DynamicsInfo
{
double m_mass;
@@ -539,6 +547,7 @@ enum b3StateLoggingType
STATE_LOGGING_PROFILE_TIMINGS = 6,
STATE_LOGGING_ALL_COMMANDS=7,
STATE_REPLAY_ALL_COMMANDS=8,
STATE_LOGGING_CUSTOM_TIMER=9,
};
@@ -573,7 +582,7 @@ typedef union {
struct b3RayData a;
struct b3RayHitInfo b;
} RAY_DATA_UNION;
#define MAX_RAY_INTERSECTION_BATCH_SIZE SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE / sizeof( RAY_DATA_UNION )
#define MAX_RAY_INTERSECTION_BATCH_SIZE 16*1024
#define MAX_RAY_HITS MAX_RAY_INTERSECTION_BATCH_SIZE
#define VISUAL_SHAPE_MAX_PATH_LEN 1024

63
examples/pybullet/examples/batchRayCast.py Normal file
View File

@@ -0,0 +1,63 @@
import pybullet as p
import time
import math
useGui = True
if (useGui):
p.connect(p.GUI)
else:
p.connect(p.DIRECT)
#p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.loadURDF("samurai.urdf")
p.loadURDF("r2d2.urdf",[3,3,1])
rayFrom=[]
rayTo=[]
numRays = 1024
rayLen = 13
for i in range (numRays):
rayFrom.append([0,0,1])
rayTo.append([rayLen*math.sin(2.*math.pi*float(i)/numRays), rayLen*math.cos(2.*math.pi*float(i)/numRays),1])
if (not useGui):
timingLog = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS,"rayCastBench.json")
numSteps = 10
if (useGui):
numSteps = 327680
for i in range (numSteps):
p.stepSimulation()
for j in range (8):
results = p.rayTestBatch(rayFrom,rayTo,j+1)
for i in range (10):
p.removeAllUserDebugItems()
rayHitColor = [1,0,0]
rayMissColor = [0,1,0]
if (useGui):
p.removeAllUserDebugItems()
for i in range (numRays):
hitObjectUid=results[i][0]
if (hitObjectUid<0):
p.addUserDebugLine(rayFrom[i],rayTo[i], rayMissColor)
else:
hitPosition = results[i][3]
p.addUserDebugLine(rayFrom[i],hitPosition, rayHitColor)
#time.sleep(1./240.)
if (not useGui):
p.stopStateLogging(timingLog)

4
examples/pybullet/examples/manyspheres.py
View File

@@ -16,11 +16,15 @@ gravYid = p.addUserDebugParameter("gravityY",-10,10,0)
gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10)
p.setPhysicsEngineParameter(numSolverIterations=10)
p.setPhysicsEngineParameter(contactBreakingThreshold=0.001)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
for i in range (10):
for j in range (10):
for k in range (10):
ob = p.loadURDF("sphere_1cm.urdf",[0.02*i,0.02*j,0.2+0.02*k],useMaximalCoordinates=True)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(1)
while True:
gravX = p.readUserDebugParameter(gravXid)

1
examples/pybullet/examples/saveRestoreState.py
View File

@@ -12,6 +12,7 @@ logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "saveRestoreTimings
def setupWorld():
p.resetSimulation()
p.setPhysicsEngineParameter(deterministicOverlappingPairs=1)
p.loadURDF("planeMesh.urdf")
kukaId = p.loadURDF("kuka_iiwa/model_free_base.urdf",[0,0,10])
for i in range (p.getNumJoints(kukaId)):

11
examples/pybullet/examples/sleeping.py
View File

@@ -9,12 +9,18 @@ p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,0)
p.loadURDF("plane100.urdf",flags=flags, useMaximalCoordinates=useMaximalCoordinates)
#p.loadURDF("cube_small.urdf", [0,0,0.5], flags=flags)
r2d2 = -1
for k in range (5):
for i in range (5):
r2d2=p.loadURDF("r2d2.urdf",[k*2,i*2,1], useMaximalCoordinates=useMaximalCoordinates, flags=p.URDF_ENABLE_CACHED_GRAPHICS_SHAPES+flags)
#enable sleeping: you can pass the flag during URDF loading, or do it afterwards
#p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_ENABLE_SLEEPING)
for j in range (p.getNumJoints(r2d2)):
p.setJointMotorControl2(r2d2,j,p.VELOCITY_CONTROL,targetVelocity=0)
print("r2d2=",r2d2)
p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)
timestep = 1./240.
p.setTimeStep(timestep)
@@ -23,3 +29,6 @@ p.setGravity(0,0,-10)
while p.isConnected():
p.stepSimulation()
time.sleep(timestep)
#force the object to wake up
p.changeDynamics(r2d2,-1,activationState=p.ACTIVATION_STATE_WAKE_UP)

28
examples/pybullet/pybullet.c
View File

@@ -1183,14 +1183,15 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
double ccdSweptSphereRadius=-1;
int frictionAnchor = -1;
double contactProcessingThreshold = -1;
int activationState = -1;
PyObject* localInertiaDiagonalObj=0;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold, &physicsClientId))
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "spinningFriction", "rollingFriction","restitution", "linearDamping", "angularDamping", "contactStiffness", "contactDamping", "frictionAnchor", "localInertiaDiagonal", "ccdSweptSphereRadius", "contactProcessingThreshold", "activationState", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|dddddddddiOddii", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &spinningFriction, &rollingFriction, &restitution,&linearDamping, &angularDamping, &contactStiffness, &contactDamping, &frictionAnchor, &localInertiaDiagonalObj, &ccdSweptSphereRadius, &contactProcessingThreshold,&activationState, &physicsClientId))
{
return NULL;
}
@@ -1260,6 +1261,10 @@ static PyObject* pybullet_changeDynamicsInfo(PyObject* self, PyObject* args, PyO
{
b3ChangeDynamicsInfoSetCcdSweptSphereRadius(command,bodyUniqueId,linkIndex, ccdSweptSphereRadius);
}
if (activationState >= 0)
{
b3ChangeDynamicsInfoSetActivationState(command, bodyUniqueId, activationState);
}
if (contactProcessingThreshold >= 0)
{
b3ChangeDynamicsInfoSetContactProcessingThreshold(command, bodyUniqueId, linkIndex, contactProcessingThreshold);
@@ -4701,6 +4706,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
}
commandHandle = b3CreateRaycastBatchCommandInit(sm);
b3RaycastBatchSetNumThreads(commandHandle, numThreads);
@@ -4723,7 +4729,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
} else
{
int i;
if (lenFrom > MAX_RAY_INTERSECTION_BATCH_SIZE)
{
PyErr_SetString(SpamError, "Number of rays exceed the maximum batch size.");
@@ -4731,6 +4737,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
Py_DECREF(seqRayToObj);
return NULL;
}
b3PushProfileTiming(sm, "extractPythonFromToSequenceToC");
for (i = 0; i < lenFrom; i++)
{
PyObject* rayFromObj = PySequence_GetItem(rayFromObjList,i);
@@ -4741,7 +4748,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
if ((pybullet_internalSetVectord(rayFromObj, rayFromWorld)) &&
(pybullet_internalSetVectord(rayToObj, rayToWorld)))
{
b3RaycastBatchAddRay(commandHandle, rayFromWorld, rayToWorld);
b3RaycastBatchAddRay(sm, commandHandle, rayFromWorld, rayToWorld);
} else
{
PyErr_SetString(SpamError, "Items in the from/to positions need to be an [x,y,z] list of 3 floats/doubles");
@@ -4749,11 +4756,13 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
Py_DECREF(seqRayToObj);
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
b3PopProfileTiming(sm);
return NULL;
}
Py_DECREF(rayFromObj);
Py_DECREF(rayToObj);
}
b3PopProfileTiming(sm);
}
} else
{
@@ -4776,8 +4785,9 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
struct b3RaycastInformation raycastInfo;
PyObject* rayHitsObj = 0;
int i;
b3PushProfileTiming(sm, "convertRaycastInformationToPython");
b3GetRaycastInformation(sm, &raycastInfo);
rayHitsObj = PyTuple_New(raycastInfo.m_numRayHits);
for (i = 0; i < raycastInfo.m_numRayHits; i++)
{
@@ -4816,6 +4826,7 @@ static PyObject* pybullet_rayTestBatch(PyObject* self, PyObject* args, PyObject*
}
PyTuple_SetItem(rayHitsObj, i, singleHitObj);
}
b3PopProfileTiming(sm);
return rayHitsObj;
}
@@ -9586,6 +9597,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "STATE_LOGGING_PROFILE_TIMINGS", STATE_LOGGING_PROFILE_TIMINGS);
PyModule_AddIntConstant(m, "STATE_LOGGING_ALL_COMMANDS", STATE_LOGGING_ALL_COMMANDS);
PyModule_AddIntConstant(m, "STATE_REPLAY_ALL_COMMANDS", STATE_REPLAY_ALL_COMMANDS);
PyModule_AddIntConstant(m, "STATE_LOGGING_CUSTOM_TIMER", STATE_LOGGING_CUSTOM_TIMER);
PyModule_AddIntConstant(m, "COV_ENABLE_GUI", COV_ENABLE_GUI);
PyModule_AddIntConstant(m, "COV_ENABLE_SHADOWS", COV_ENABLE_SHADOWS);
@@ -9625,7 +9637,11 @@ initpybullet(void)
PyModule_AddIntConstant(m, "URDF_ENABLE_CACHED_GRAPHICS_SHAPES", URDF_ENABLE_CACHED_GRAPHICS_SHAPES);
PyModule_AddIntConstant(m, "URDF_ENABLE_SLEEPING", URDF_ENABLE_SLEEPING);
PyModule_AddIntConstant(m, "URDF_INITIALIZE_SAT_FEATURES", URDF_INITIALIZE_SAT_FEATURES);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_ENABLE_SLEEPING", eActivationStateEnableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_DISABLE_SLEEPING", eActivationStateDisableSleeping);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_WAKE_UP", eActivationStateWakeUp);
PyModule_AddIntConstant(m, "ACTIVATION_STATE_SLEEP", eActivationStateSleep);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION", URDF_USE_SELF_COLLISION);
PyModule_AddIntConstant(m, "URDF_USE_SELF_COLLISION_EXCLUDE_PARENT", URDF_USE_SELF_COLLISION_EXCLUDE_PARENT);

2
setup.py
View File

@@ -452,7 +452,7 @@ print("-----")
setup(
name = 'pybullet',
version='2.0.5',
version='2.0.7',
description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning',
long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.',
url='https://github.com/bulletphysics/bullet3',

8
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -21,6 +21,7 @@ subject to the following restrictions:
#include "btSequentialImpulseConstraintSolver.h"
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
#include "LinearMath/btIDebugDraw.h"
#include "LinearMath/btCpuFeatureUtility.h"
@@ -780,8 +781,13 @@ int btSequentialImpulseConstraintSolver::getOrInitSolverBody(btCollisionObject&
}
else
{
bool isMultiBodyType = (body.getInternalType()&btCollisionObject::CO_FEATHERSTONE_LINK);
// Incorrectly set collision object flags can degrade performance in various ways.
btAssert( body.isStaticOrKinematicObject() );
if (!isMultiBodyType)
{
btAssert( body.isStaticOrKinematicObject() );
}
//it could be a multibody link collider
// all fixed bodies (inf mass) get mapped to a single solver id
if ( m_fixedBodyId < 0 )
{

1
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -1847,6 +1847,7 @@ void btMultiBody::fillConstraintJacobianMultiDof(int link,
void btMultiBody::wakeUp()
{
m_sleepTimer = 0;
m_awake = true;
}

30
src/LinearMath/TaskScheduler/btTaskScheduler.cpp
View File

@@ -209,13 +209,19 @@ public:
}
~JobQueue()
{
freeJobMem();
exit();
}
void exit()
{
freeJobMem();
if (m_queueLock && m_threadSupport)
{
m_threadSupport->deleteCriticalSection(m_queueLock);
m_queueLock = NULL;
m_threadSupport = 0;
}
}
}
void init(btThreadSupportInterface* threadSup, btAlignedObjectArray<JobQueue>* contextArray)
{
m_threadSupport = threadSup;
@@ -412,11 +418,13 @@ static void WorkerThreadFunc( void* userPtr )
}
}
}
// go sleep
localStorage->m_mutex.lock();
localStorage->m_status = WorkerThreadStatus::kSleeping;
localStorage->m_mutex.unlock();
{
BT_PROFILE("sleep");
// go sleep
localStorage->m_mutex.lock();
localStorage->m_status = WorkerThreadStatus::kSleeping;
localStorage->m_mutex.unlock();
}
}
@@ -446,6 +454,12 @@ public:
virtual ~btTaskSchedulerDefault()
{
waitForWorkersToSleep();
for ( int i = 0; i < m_jobQueues.size(); ++i )
{
m_jobQueues[i].exit();
}
if (m_threadSupport)
{
delete m_threadSupport;
@@ -503,7 +517,7 @@ public:
storage.m_threadId = i;
storage.m_directive = m_workerDirective;
storage.m_status = WorkerThreadStatus::kSleeping;
storage.m_cooldownTime = 1000; // 1000 microseconds, threads go to sleep after this long if they have nothing to do
storage.m_cooldownTime = 100; // 100 microseconds, threads go to sleep after this long if they have nothing to do
storage.m_clock = &m_clock;
storage.m_queue = m_perThreadJobQueues[i];
}