eb35cba740
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
67 lines
3.0 KiB
Python
67 lines
3.0 KiB
Python
import pybullet as p
|
|
import time
|
|
import math
|
|
|
|
p.connect(p.GUI)
|
|
planeId = p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
|
|
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=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(1./240.)
|
|
p.stepSimulation()
|