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

expose local inertia diagonal in C-API, PyBullet, through the 'getDynamicsInfo'

Browse Source 
render the inertia boxes in examples/pybullet/examples/quadruped.py and examples/pybullet/examples/reset_dynamic_info.py
fix an issue where the original margin (0.04) was used to compute the inertia, instead of latest margin
This commit is contained in:
erwincoumans
2017-12-20 14:54:32 -08:00
parent ecba966890
commit eb35cba740
7 changed files with 150 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
examples/pybullet/examples/reset_dynamic_info.py
View File

@@ -4,20 +4,63 @@ import math
p.connect(p.GUI)
planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
p.loadURDF(fileName="cube.urdf",basePosition=[0,0,2])
cubeId = p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
#p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=0.1)
p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=100.0)
p.changeDynamics(bodyUniqueId=2,linkIndex=-1,mass=1.0)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
def drawInertiaBox(parentUid, parentLinkIndex):
mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
Ixx = inertia[0]
Iyy = inertia[1]
Izz = inertia[2]
boxScaleX = 0.5*math.sqrt(6*(Izz + Iyy - Ixx) / mass);
boxScaleY = 0.5*math.sqrt(6*(Izz + Ixx - Iyy) / mass);
boxScaleZ = 0.5*math.sqrt(6*(Ixx + Iyy - Izz) / mass);
halfExtents = [boxScaleX,boxScaleY,boxScaleZ]
pts = [[halfExtents[0],halfExtents[1],halfExtents[2]],
[-halfExtents[0],halfExtents[1],halfExtents[2]],
[halfExtents[0],-halfExtents[1],halfExtents[2]],
[-halfExtents[0],-halfExtents[1],halfExtents[2]],
[halfExtents[0],halfExtents[1],-halfExtents[2]],
[-halfExtents[0],halfExtents[1],-halfExtents[2]],
[halfExtents[0],-halfExtents[1],-halfExtents[2]],
[-halfExtents[0],-halfExtents[1],-halfExtents[2]]]
color=[1,0,0]
p.addUserDebugLine(pts[0],pts[1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[1],pts[3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[3],pts[2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[2],pts[0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[0],pts[4],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[1],pts[5],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[2],pts[6],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[3],pts[7],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+0],pts[4+1],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+1],pts[4+3],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+3],pts[4+2],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
p.addUserDebugLine(pts[4+2],pts[4+0],color,1, parentObjectUniqueId=parentUid, parentLinkIndex = parentLinkIndex)
drawInertiaBox(cubeId,-1)
t=0
while 1:
t=t+1
if t > 400:
p.changeDynamics(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
mass1,frictionCoeff1=p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
mass2,frictionCoeff2=p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
print mass1,frictionCoeff1
print mass2,frictionCoeff2
time.sleep(.01)
mass1,frictionCoeff1 =p.getDynamicsInfo(bodyUniqueId=planeId,linkIndex=-1)
mass2,frictionCoeff2 =p.getDynamicsInfo(bodyUniqueId=cubeId,linkIndex=-1)
print (mass1,frictionCoeff1)
print (mass2,frictionCoeff2)
time.sleep(1./240.)
p.stepSimulation()