Start of a urdfEditor.py, limited support to extract a URDF from a PyBullet body.
Use btCylinderShapeZ for URDF cylinder, instead of converting it to a btConvexHullShape. Implement PyBullet.getCollisionShapeData Extend PyBullet.getDynamicsInfo / b3GetDynamicsInfo, remove flag (don't rely on API returning a fixed number of elements in a list!) Extend PyBullet.getJointInfo: add parentIndex
This commit is contained in:
Erwin Coumans
325
examples/pybullet/examples/urdfEditor.py
Normal file
325
examples/pybullet/examples/urdfEditor.py
Normal file
@@ -0,0 +1,325 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
|
||||
p.connect(p.GUI)
|
||||
|
||||
door = p.loadURDF("door.urdf")
|
||||
|
||||
class UrdfInertial(object):
|
||||
def __init__(self):
|
||||
self.mass = 1
|
||||
self.inertia_xxyyzz=[7,8,9]
|
||||
self.origin_rpy=[1,2,3]
|
||||
self.origin_xyz=[4,5,6]
|
||||
|
||||
class UrdfContact(object):
|
||||
def __init__(self):
|
||||
self.lateral_friction = 1
|
||||
self.rolling_friction = 0
|
||||
self.spinning_friction = 0
|
||||
|
||||
class UrdfLink(object):
|
||||
def __init__(self):
|
||||
self.link_name = "dummy"
|
||||
self.urdf_inertial = UrdfInertial()
|
||||
self.urdf_visual_shapes=[]
|
||||
self.urdf_collision_shapes=[]
|
||||
|
||||
class UrdfVisual(object):
|
||||
def __init__(self):
|
||||
self.origin_rpy = [1,2,3]
|
||||
self.origin_xyz = [4,5,6]
|
||||
self.geom_type = p.GEOM_BOX
|
||||
self.geom_radius = 1
|
||||
self.geom_extents = [7,8,9]
|
||||
self.geom_length=[10]
|
||||
self.geom_meshfile = "meshfile"
|
||||
self.material_rgba = [1,0,0,1]
|
||||
self.material_name = ""
|
||||
|
||||
class UrdfCollision(object):
|
||||
def __init__(self):
|
||||
self.origin_rpy = [1,2,3]
|
||||
self.origin_xyz = [4,5,6]
|
||||
self.geom_type = p.GEOM_BOX
|
||||
self.geom_radius = 1
|
||||
self.geom_extents = [7,8,9]
|
||||
self.geom_meshfile = "meshfile"
|
||||
|
||||
class UrdfJoint(object):
|
||||
def __init__(self):
|
||||
self.joint_name = "joint_dummy"
|
||||
self.joint_type = p.JOINT_REVOLUTE
|
||||
self.joint_lower_limit = 0
|
||||
self.joint_upper_limit = -1
|
||||
self.parent_name = "parentName"
|
||||
self.child_name = "childName"
|
||||
self.joint_origin_xyz = [1,2,3]
|
||||
self.joint_axis_xyz = [1,2,3]
|
||||
|
||||
class UrdfEditor(object):
|
||||
def __init__(self):
|
||||
self.initialize()
|
||||
|
||||
def initialize(self):
|
||||
self.urdfLinks=[]
|
||||
self.urdfJoints=[]
|
||||
self.robotName = ""
|
||||
|
||||
#def addLink(...)
|
||||
|
||||
#def createMultiBody(self):
|
||||
|
||||
def convertLinkFromMultiBody(self, bodyUid, linkIndex, urdfLink):
|
||||
dyn = p.getDynamicsInfo(bodyUid,linkIndex)
|
||||
urdfLink.urdf_inertial.mass = dyn[0]
|
||||
urdfLink.urdf_inertial.inertia_xxyyzz = dyn[2]
|
||||
#todo
|
||||
urdfLink.urdf_inertial.origin_xyz = dyn[3]
|
||||
rpy = p.getEulerFromQuaternion(dyn[4])
|
||||
urdfLink.urdf_inertial.origin_rpy = rpy
|
||||
|
||||
visualShapes = p.getVisualShapeData(bodyUid)
|
||||
matIndex = 0
|
||||
for v in visualShapes:
|
||||
if (v[1]==linkIndex):
|
||||
print("visualShape base:",v)
|
||||
urdfVisual = UrdfVisual()
|
||||
urdfVisual.geom_type = v[2]
|
||||
if (v[2]==p.GEOM_BOX):
|
||||
urdfVisual.geom_extents = v[3]
|
||||
if (v[2]==p.GEOM_SPHERE):
|
||||
urdfVisual.geom_radius = v[3][0]
|
||||
if (v[2]==p.GEOM_MESH):
|
||||
urdfVisual.geom_meshfile = v[4].decode("utf-8")
|
||||
if (v[2]==p.GEOM_CYLINDER):
|
||||
urdfVisual.geom_radius=v[3][1]
|
||||
urdfVisual.geom_length=v[3][0]
|
||||
|
||||
urdfVisual.origin_xyz = v[5]
|
||||
urdfVisual.origin_rpy = p.getEulerFromQuaternion(v[6])
|
||||
urdfVisual.material_rgba = v[7]
|
||||
name = 'mat_{}_{}'.format(linkIndex,matIndex)
|
||||
urdfVisual.material_name = name
|
||||
|
||||
urdfLink.urdf_visual_shapes.append(urdfVisual)
|
||||
matIndex=matIndex+1
|
||||
|
||||
collisionShapes = p.getCollisionShapeData(bodyUid, linkIndex)
|
||||
for v in collisionShapes:
|
||||
print("collisionShape base:",v)
|
||||
urdfCollision = UrdfCollision()
|
||||
print("geom type=",v[0])
|
||||
urdfCollision.geom_type = v[2]
|
||||
if (v[2]==p.GEOM_BOX):
|
||||
urdfCollision.geom_extents = v[3]
|
||||
if (v[2]==p.GEOM_SPHERE):
|
||||
urdfCollision.geom_radius = v[3][0]
|
||||
if (v[2]==p.GEOM_MESH):
|
||||
urdfCollision.geom_meshfile = v[4].decode("utf-8")
|
||||
#localInertiaFrame*childTrans
|
||||
if (v[2]==p.GEOM_CYLINDER):
|
||||
urdfCollision.geom_radius=v[3][1]
|
||||
urdfCollision.geom_length=v[3][0]
|
||||
|
||||
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
|
||||
v[5], v[6])
|
||||
urdfCollision.origin_xyz = pos
|
||||
urdfCollision.origin_rpy = p.getEulerFromQuaternion(orn)
|
||||
urdfLink.urdf_collision_shapes.append(urdfCollision)
|
||||
|
||||
def initializeFromBulletBody(self, bodyUid):
|
||||
self.initialize()
|
||||
|
||||
#always create a base link
|
||||
baseLink = UrdfLink()
|
||||
baseLinkIndex = -1
|
||||
self.convertLinkFromMultiBody(bodyUid, baseLinkIndex, baseLink)
|
||||
baseLink.link_name = p.getBodyInfo(bodyUid)[0].decode("utf-8")
|
||||
self.urdfLinks.append(baseLink)
|
||||
|
||||
#print(visualShapes)
|
||||
#optionally create child links and joints
|
||||
for j in range(p.getNumJoints(bodyUid)):
|
||||
jointInfo = p.getJointInfo(bodyUid,j)
|
||||
urdfLink = UrdfLink()
|
||||
self.convertLinkFromMultiBody(bodyUid, j, urdfLink)
|
||||
urdfLink.link_name = jointInfo[12].decode("utf-8")
|
||||
self.urdfLinks.append(urdfLink)
|
||||
|
||||
urdfJoint = UrdfJoint()
|
||||
urdfJoint.joint_name = jointInfo[1].decode("utf-8")
|
||||
urdfJoint.joint_type = jointInfo[2]
|
||||
urdfJoint.joint_axis_xyz = jointInfo[13]
|
||||
parentIndex = jointInfo[16]
|
||||
if (parentIndex<0):
|
||||
urdfJoint.parent_name = baseLink.link_name
|
||||
else:
|
||||
parentJointInfo = p.getJointInfo(bodyUid,parentIndex)
|
||||
urdfJoint.parent_name = parentJointInfo[12].decode("utf-8")
|
||||
|
||||
urdfJoint.child_name = urdfLink.link_name
|
||||
|
||||
#todo, compensate for inertia/link frame offset
|
||||
dyn = p.getDynamicsInfo(bodyUid,parentIndex)
|
||||
parentInertiaPos = dyn[3]
|
||||
parentInertiaOrn = dyn[4]
|
||||
|
||||
pos,orn = p.multiplyTransforms(dyn[3],dyn[4],\
|
||||
jointInfo[14], jointInfo[15])
|
||||
urdfJoint.joint_origin_xyz = pos
|
||||
urdfJoint.joint_origin_rpy = p.getEulerFromQuaternion(orn)
|
||||
|
||||
|
||||
self.urdfJoints.append(urdfJoint)
|
||||
|
||||
def writeInertial(self,file,urdfInertial, precision=5):
|
||||
file.write("\t\t<inertial>\n")
|
||||
str = '\t\t\t<origin rpy=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\" xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(\
|
||||
urdfInertial.origin_rpy[0],urdfInertial.origin_rpy[1],urdfInertial.origin_rpy[2],\
|
||||
urdfInertial.origin_xyz[0],urdfInertial.origin_xyz[1],urdfInertial.origin_xyz[2], prec=precision)
|
||||
file.write(str)
|
||||
str = '\t\t\t<mass value=\"{:.{prec}f}\"/>\n'.format(urdfInertial.mass,prec=precision)
|
||||
file.write(str)
|
||||
str = '\t\t\t<inertia ixx=\"{:.{prec}f}\" ixy=\"0\" ixz=\"0\" iyy=\"{:.{prec}f}\" iyz=\"0\" izz=\"{:.{prec}f}\"/>\n'.format(\
|
||||
urdfInertial.inertia_xxyyzz[0],\
|
||||
urdfInertial.inertia_xxyyzz[1],\
|
||||
urdfInertial.inertia_xxyyzz[2],prec=precision)
|
||||
file.write(str)
|
||||
file.write("\t\t</inertial>\n")
|
||||
|
||||
def writeVisualShape(self,file,urdfVisual, precision=5):
|
||||
file.write("\t\t<visual>\n")
|
||||
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
|
||||
urdfVisual.origin_rpy[0],urdfVisual.origin_rpy[1],urdfVisual.origin_rpy[2],
|
||||
urdfVisual.origin_xyz[0],urdfVisual.origin_xyz[1],urdfVisual.origin_xyz[2], prec=precision)
|
||||
file.write(str)
|
||||
file.write("\t\t\t<geometry>\n")
|
||||
if urdfVisual.geom_type == p.GEOM_BOX:
|
||||
str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfVisual.geom_extents[0],\
|
||||
urdfVisual.geom_extents[1],urdfVisual.geom_extents[2], prec=precision)
|
||||
file.write(str)
|
||||
if urdfVisual.geom_type == p.GEOM_SPHERE:
|
||||
str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfVisual.geom_radius,\
|
||||
prec=precision)
|
||||
file.write(str)
|
||||
if urdfVisual.geom_type == p.GEOM_MESH:
|
||||
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfVisual.geom_meshfile,\
|
||||
prec=precision)
|
||||
file.write(str)
|
||||
if urdfVisual.geom_type == p.GEOM_CYLINDER:
|
||||
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
|
||||
urdfVisual.geom_length, urdfVisual.geom_radius, prec=precision)
|
||||
file.write(str)
|
||||
|
||||
file.write("\t\t\t</geometry>\n")
|
||||
str = '\t\t\t<material name=\"{}\">\n'.format(urdfVisual.material_name)
|
||||
file.write(str)
|
||||
str = '\t\t\t\t<color rgba="{:.{prec}f} {:.{prec}f} {:.{prec}f} {:.{prec}f}" />\n'.format(urdfVisual.material_rgba[0],\
|
||||
urdfVisual.material_rgba[1],urdfVisual.material_rgba[2],urdfVisual.material_rgba[3],prec=precision)
|
||||
file.write(str)
|
||||
file.write("\t\t\t</material>\n")
|
||||
file.write("\t\t</visual>\n")
|
||||
|
||||
def writeCollisionShape(self,file,urdfCollision, precision=5):
|
||||
file.write("\t\t<collision>\n")
|
||||
str = '\t\t\t<origin rpy="{:.{prec}f} {:.{prec}f} {:.{prec}f}" xyz="{:.{prec}f} {:.{prec}f} {:.{prec}f}"/>\n'.format(\
|
||||
urdfCollision.origin_rpy[0],urdfCollision.origin_rpy[1],urdfCollision.origin_rpy[2],
|
||||
urdfCollision.origin_xyz[0],urdfCollision.origin_xyz[1],urdfCollision.origin_xyz[2], prec=precision)
|
||||
file.write(str)
|
||||
file.write("\t\t\t<geometry>\n")
|
||||
if urdfCollision.geom_type == p.GEOM_BOX:
|
||||
str = '\t\t\t\t<box size=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfCollision.geom_extents[0],\
|
||||
urdfCollision.geom_extents[1],urdfCollision.geom_extents[2], prec=precision)
|
||||
file.write(str)
|
||||
if urdfCollision.geom_type == p.GEOM_SPHERE:
|
||||
str = '\t\t\t\t<sphere radius=\"{:.{prec}f}\"/>\n'.format(urdfCollision.geom_radius,\
|
||||
prec=precision)
|
||||
file.write(str)
|
||||
if urdfCollision.geom_type == p.GEOM_MESH:
|
||||
str = '\t\t\t\t<mesh filename=\"{}\"/>\n'.format(urdfCollision.geom_meshfile,\
|
||||
prec=precision)
|
||||
file.write(str)
|
||||
if urdfCollision.geom_type == p.GEOM_CYLINDER:
|
||||
str = '\t\t\t\t<cylinder length=\"{:.{prec}f}\" radius=\"{:.{prec}f}\"/>\n'.format(\
|
||||
urdfCollision.geom_length, urdfCollision.geom_radius, prec=precision)
|
||||
file.write(str)
|
||||
|
||||
file.write("\t\t\t</geometry>\n")
|
||||
file.write("\t\t</collision>\n")
|
||||
|
||||
|
||||
def writeLink(self, file, urdfLink):
|
||||
file.write("\t<link name=\"")
|
||||
file.write(urdfLink.link_name)
|
||||
file.write("\">\n")
|
||||
|
||||
self.writeInertial(file,urdfLink.urdf_inertial)
|
||||
for v in urdfLink.urdf_visual_shapes:
|
||||
self.writeVisualShape(file,v)
|
||||
for c in urdfLink.urdf_collision_shapes:
|
||||
self.writeCollisionShape(file,c)
|
||||
file.write("\t</link>\n")
|
||||
|
||||
def writeJoint(self, file, urdfJoint, precision=5):
|
||||
jointTypeStr = "invalid"
|
||||
if urdfJoint.joint_type == p.JOINT_REVOLUTE:
|
||||
if urdfJoint.joint_upper_limit < urdfJoint.joint_lower_limit:
|
||||
jointTypeStr = "continuous"
|
||||
else:
|
||||
jointTypeStr = "revolute"
|
||||
if urdfJoint.joint_type == p.JOINT_FIXED:
|
||||
jointTypeStr = "fixed"
|
||||
if urdfJoint.joint_type == p.JOINT_PRISMATIC:
|
||||
jointTypeStr = "prismatic"
|
||||
str = '\t<joint name=\"{}\" type=\"{}\">\n'.format(urdfJoint.joint_name, jointTypeStr)
|
||||
file.write(str)
|
||||
str = '\t\t<parent link=\"{}\"/>\n'.format(urdfJoint.parent_name)
|
||||
file.write(str)
|
||||
str = '\t\t<child link=\"{}\"/>\n'.format(urdfJoint.child_name)
|
||||
file.write(str)
|
||||
file.write("\t\t<dynamics damping=\"1.0\" friction=\"0.0001\"/>\n")
|
||||
str = '\t\t<origin xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_origin_xyz[0],\
|
||||
urdfJoint.joint_origin_xyz[1],urdfJoint.joint_origin_xyz[2], prec=precision)
|
||||
file.write(str)
|
||||
str = '\t\t<axis xyz=\"{:.{prec}f} {:.{prec}f} {:.{prec}f}\"/>\n'.format(urdfJoint.joint_axis_xyz[0],\
|
||||
urdfJoint.joint_axis_xyz[1],urdfJoint.joint_axis_xyz[2], prec=precision)
|
||||
file.write(str)
|
||||
file.write("\t</joint>\n")
|
||||
|
||||
def saveUrdf(self, fileName):
|
||||
file = open(fileName,"w")
|
||||
file.write("<?xml version=\"0.0\" ?>\n")
|
||||
file.write("<robot name=\"")
|
||||
file.write(self.robotName)
|
||||
file.write("\">\n")
|
||||
|
||||
for link in self.urdfLinks:
|
||||
self.writeLink(file,link)
|
||||
|
||||
for joint in self.urdfJoints:
|
||||
self.writeJoint(file,joint)
|
||||
|
||||
file.write("</robot>\n")
|
||||
file.close()
|
||||
|
||||
def __del__(self):
|
||||
pass
|
||||
|
||||
parser = UrdfEditor()
|
||||
parser.initializeFromBulletBody(door)
|
||||
parser.saveUrdf("test.urdf")
|
||||
parser=0
|
||||
|
||||
p.setRealTimeSimulation(1)
|
||||
print("numJoints:",p.getNumJoints(door))
|
||||
|
||||
print("base name:",p.getBodyInfo(door))
|
||||
|
||||
for i in range(p.getNumJoints(door)):
|
||||
print("jointInfo(",i,"):",p.getJointInfo(door,i))
|
||||
print("linkState(",i,"):",p.getLinkState(door,i))
|
||||
|
||||
while (p.getConnectionInfo()["isConnected"]):
|
||||
time.sleep(0.01)
|
||||
|
||||
@@ -913,13 +913,12 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
|
||||
{
|
||||
int bodyUniqueId = -1;
|
||||
int linkIndex = -2;
|
||||
int flags = 0;
|
||||
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "flags", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex, &flags, &physicsClientId))
|
||||
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "physicsClientId", NULL};
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &bodyUniqueId, &linkIndex, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
@@ -934,11 +933,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
|
||||
b3SharedMemoryCommandHandle cmd_handle;
|
||||
b3SharedMemoryStatusHandle status_handle;
|
||||
struct b3DynamicsInfo info;
|
||||
int numFields = 2;
|
||||
if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
|
||||
{
|
||||
numFields++;
|
||||
}
|
||||
|
||||
|
||||
if (bodyUniqueId < 0)
|
||||
{
|
||||
@@ -959,25 +954,41 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje
|
||||
return NULL;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
if (b3GetDynamicsInfo(status_handle, &info))
|
||||
{
|
||||
|
||||
int numFields = 10;
|
||||
PyObject* pyDynamicsInfo = PyTuple_New(numFields);
|
||||
PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff));
|
||||
|
||||
if (flags & eDYNAMICS_INFO_REPORT_INERTIA)
|
||||
{
|
||||
PyObject* pyInertiaDiag = PyTuple_New(3);
|
||||
PyTuple_SetItem(pyInertiaDiag,0,PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
|
||||
PyTuple_SetItem(pyInertiaDiag,1,PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
|
||||
PyTuple_SetItem(pyInertiaDiag,2,PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
|
||||
PyTuple_SetItem(pyInertiaDiag, 0, PyFloat_FromDouble(info.m_localInertialDiagonal[0]));
|
||||
PyTuple_SetItem(pyInertiaDiag, 1, PyFloat_FromDouble(info.m_localInertialDiagonal[1]));
|
||||
PyTuple_SetItem(pyInertiaDiag, 2, PyFloat_FromDouble(info.m_localInertialDiagonal[2]));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 2, pyInertiaDiag);
|
||||
}
|
||||
|
||||
{
|
||||
PyObject* pyInertiaPos= PyTuple_New(3);
|
||||
PyTuple_SetItem(pyInertiaPos, 0, PyFloat_FromDouble(info.m_localInertialFrame[0]));
|
||||
PyTuple_SetItem(pyInertiaPos, 1, PyFloat_FromDouble(info.m_localInertialFrame[1]));
|
||||
PyTuple_SetItem(pyInertiaPos, 2, PyFloat_FromDouble(info.m_localInertialFrame[2]));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 3, pyInertiaPos);
|
||||
}
|
||||
{
|
||||
PyObject* pyInertiaOrn= PyTuple_New(4);
|
||||
PyTuple_SetItem(pyInertiaOrn, 0, PyFloat_FromDouble(info.m_localInertialFrame[3]));
|
||||
PyTuple_SetItem(pyInertiaOrn, 1, PyFloat_FromDouble(info.m_localInertialFrame[4]));
|
||||
PyTuple_SetItem(pyInertiaOrn, 2, PyFloat_FromDouble(info.m_localInertialFrame[5]));
|
||||
PyTuple_SetItem(pyInertiaOrn, 3, PyFloat_FromDouble(info.m_localInertialFrame[6]));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 4, pyInertiaOrn);
|
||||
}
|
||||
PyTuple_SetItem(pyDynamicsInfo, 5, PyFloat_FromDouble(info.m_restitution));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 6, PyFloat_FromDouble(info.m_rollingFrictionCoeff));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 7, PyFloat_FromDouble(info.m_spinningFrictionCoeff));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping));
|
||||
PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness));
|
||||
return pyDynamicsInfo;
|
||||
}
|
||||
}
|
||||
@@ -3067,7 +3078,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
|
||||
|
||||
int bodyUniqueId = -1;
|
||||
int jointIndex = -1;
|
||||
int jointInfoSize = 13; // size of struct b3JointInfo
|
||||
int jointInfoSize = 17; // size of struct b3JointInfo
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
int physicsClientId = 0;
|
||||
static char* kwlist[] = {"bodyUniqueId", "jointIndex", "physicsClientId", NULL};
|
||||
@@ -3138,6 +3149,30 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject*
|
||||
PyTuple_SetItem(pyListJointInfo, 12,
|
||||
PyString_FromString("not available"));
|
||||
}
|
||||
{
|
||||
PyObject* axis = PyTuple_New(3);
|
||||
PyTuple_SetItem(axis, 0, PyFloat_FromDouble(info.m_jointAxis[0]));
|
||||
PyTuple_SetItem(axis, 1, PyFloat_FromDouble(info.m_jointAxis[1]));
|
||||
PyTuple_SetItem(axis, 2, PyFloat_FromDouble(info.m_jointAxis[2]));
|
||||
PyTuple_SetItem(pyListJointInfo, 13, axis);
|
||||
}
|
||||
{
|
||||
PyObject* pos = PyTuple_New(3);
|
||||
PyTuple_SetItem(pos, 0, PyFloat_FromDouble(info.m_parentFrame[0]));
|
||||
PyTuple_SetItem(pos, 1, PyFloat_FromDouble(info.m_parentFrame[1]));
|
||||
PyTuple_SetItem(pos, 2, PyFloat_FromDouble(info.m_parentFrame[2]));
|
||||
PyTuple_SetItem(pyListJointInfo, 14, pos);
|
||||
}
|
||||
{
|
||||
PyObject* orn = PyTuple_New(4);
|
||||
PyTuple_SetItem(orn, 0, PyFloat_FromDouble(info.m_parentFrame[3]));
|
||||
PyTuple_SetItem(orn, 1, PyFloat_FromDouble(info.m_parentFrame[4]));
|
||||
PyTuple_SetItem(orn, 2, PyFloat_FromDouble(info.m_parentFrame[5]));
|
||||
PyTuple_SetItem(orn, 3, PyFloat_FromDouble(info.m_parentFrame[6]));
|
||||
PyTuple_SetItem(pyListJointInfo, 15, orn);
|
||||
}
|
||||
PyTuple_SetItem(pyListJointInfo, 16, PyInt_FromLong(info.m_parentIndex));
|
||||
|
||||
|
||||
return pyListJointInfo;
|
||||
}
|
||||
@@ -4773,6 +4808,107 @@ static PyObject* pybullet_setDebugObjectColor(PyObject* self, PyObject* args, Py
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
static PyObject* pybullet_getCollisionShapeData(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int objectUniqueId = -1;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
struct b3CollisionShapeInformation collisionShapeInfo;
|
||||
int statusType;
|
||||
int i;
|
||||
int linkIndex;
|
||||
PyObject* pyResultList = 0;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
static char* kwlist[] = { "objectUniqueId", "linkIndex", "physicsClientId", NULL };
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|i", kwlist, &objectUniqueId, &linkIndex, &physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
{
|
||||
commandHandle = b3InitRequestCollisionShapeInformation(sm, objectUniqueId, linkIndex);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType == CMD_COLLISION_SHAPE_INFO_COMPLETED)
|
||||
{
|
||||
b3GetCollisionShapeInformation(sm, &collisionShapeInfo);
|
||||
pyResultList = PyTuple_New(collisionShapeInfo.m_numCollisionShapes);
|
||||
for (i = 0; i < collisionShapeInfo.m_numCollisionShapes; i++)
|
||||
{
|
||||
PyObject* collisionShapeObList = PyTuple_New(7);
|
||||
PyObject* item;
|
||||
|
||||
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_objectUniqueId);
|
||||
PyTuple_SetItem(collisionShapeObList, 0, item);
|
||||
|
||||
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_linkIndex);
|
||||
PyTuple_SetItem(collisionShapeObList, 1, item);
|
||||
|
||||
item = PyInt_FromLong(collisionShapeInfo.m_collisionShapeData[i].m_collisionGeometryType);
|
||||
PyTuple_SetItem(collisionShapeObList, 2, item);
|
||||
|
||||
{
|
||||
PyObject* vec = PyTuple_New(3);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[0]);
|
||||
PyTuple_SetItem(vec, 0, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[1]);
|
||||
PyTuple_SetItem(vec, 1, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_dimensions[2]);
|
||||
PyTuple_SetItem(vec, 2, item);
|
||||
PyTuple_SetItem(collisionShapeObList, 3, vec);
|
||||
}
|
||||
|
||||
item = PyString_FromString(collisionShapeInfo.m_collisionShapeData[i].m_meshAssetFileName);
|
||||
PyTuple_SetItem(collisionShapeObList, 4, item);
|
||||
|
||||
{
|
||||
PyObject* vec = PyTuple_New(3);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[0]);
|
||||
PyTuple_SetItem(vec, 0, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[1]);
|
||||
PyTuple_SetItem(vec, 1, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[2]);
|
||||
PyTuple_SetItem(vec, 2, item);
|
||||
PyTuple_SetItem(collisionShapeObList, 5, vec);
|
||||
}
|
||||
|
||||
{
|
||||
PyObject* vec = PyTuple_New(4);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[3]);
|
||||
PyTuple_SetItem(vec, 0, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[4]);
|
||||
PyTuple_SetItem(vec, 1, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[5]);
|
||||
PyTuple_SetItem(vec, 2, item);
|
||||
item = PyFloat_FromDouble(collisionShapeInfo.m_collisionShapeData[i].m_localCollisionFrame[6]);
|
||||
PyTuple_SetItem(vec, 3, item);
|
||||
PyTuple_SetItem(collisionShapeObList, 6, vec);
|
||||
}
|
||||
|
||||
|
||||
PyTuple_SetItem(pyResultList, i, collisionShapeObList);
|
||||
}
|
||||
return pyResultList;
|
||||
}
|
||||
else
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error receiving collision shape info");
|
||||
return NULL;
|
||||
}
|
||||
}
|
||||
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyObject* keywds)
|
||||
{
|
||||
int objectUniqueId = -1;
|
||||
@@ -8088,6 +8224,9 @@ static PyMethodDef SpamMethods[] = {
|
||||
{"getVisualShapeData", (PyCFunction)pybullet_getVisualShapeData, METH_VARARGS | METH_KEYWORDS,
|
||||
"Return the visual shape information for one object."},
|
||||
|
||||
{ "getCollisionShapeData", (PyCFunction)pybullet_getCollisionShapeData, METH_VARARGS | METH_KEYWORDS,
|
||||
"Return the collision shape information for one object." },
|
||||
|
||||
{"changeVisualShape", (PyCFunction)pybullet_changeVisualShape, METH_VARARGS | METH_KEYWORDS,
|
||||
"Change part of the visual shape information for one object."},
|
||||
|
||||
@@ -8306,8 +8445,7 @@ initpybullet(void)
|
||||
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
|
||||
PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read
|
||||
|
||||
PyModule_AddIntConstant(m, "DYNAMICS_INFO_REPORT_INERTIA", eDYNAMICS_INFO_REPORT_INERTIA); // report local inertia in 'getDynamicsInfo'
|
||||
|
||||
|
||||
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read
|
||||
|
||||
PyModule_AddIntConstant(m, "TORQUE_CONTROL", CONTROL_MODE_TORQUE);
|
||||
|
||||
Reference in New Issue
Block a user