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

47
examples/pybullet/examples/quadruped.py
View File

@@ -2,6 +2,44 @@ import pybullet as p
import time
import math
def drawInertiaBox(parentUid, parentLinkIndex, color):
mass,frictionCoeff, inertia =p.getDynamicsInfo(bodyUniqueId=parentUid,linkIndex=parentLinkIndex, flags = p.DYNAMICS_INFO_REPORT_INERTIA)
if (mass>0):
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]]]
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)
toeConstraint = True
useMaximalCoordinates = False
useRealTime = 1
@@ -44,7 +82,6 @@ p.setRealTimeSimulation(0)
quadruped = p.loadURDF("quadruped/minitaur_v1.urdf",[1,-1,.3],orn,useFixedBase=False, useMaximalCoordinates=useMaximalCoordinates)
nJoints = p.getNumJoints(quadruped)
jointNameToId = {}
for i in range(nJoints):
jointInfo = p.getJointInfo(quadruped, i)
@@ -76,6 +113,14 @@ motor_back_leftL_joint = jointNameToId['motor_back_leftL_joint']
motor_back_leftL_link = jointNameToId['motor_back_leftL_link']
knee_back_leftL_link = jointNameToId['knee_back_leftL_link']
drawInertiaBox(quadruped,-1, [1,0,0])
#drawInertiaBox(quadruped,motor_front_rightR_joint, [1,0,0])
for i in range (nJoints):
drawInertiaBox(quadruped,i, [0,1,0])
#fixtorso = p.createConstraint(-1,-1,quadruped,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,0])
motordir=[-1,-1,-1,-1,1,1,1,1]