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

Merge pull request #944 from erwincoumans/master

Browse Source 
[pybullet] fix type quation->quaternion, fix getVisualShapeData int->float, URDF parser accepts <xx/yy/zz> inertial data.
This commit is contained in:
erwincoumans
2017-02-03 09:24:20 -08:00
committed by GitHub
parent ab5a86576f 257b7b89aa
commit c1eabcf059
5 changed files with 64 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
data/sphere2_rolling_friction.urdf
View File

@@ -3,6 +3,7 @@
<link name="base_link"> <link name="base_link">
<contact> <contact>
<rolling_friction value="0.3"/> <rolling_friction value="0.3"/>
<spinning_friction value="0.3"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>

BIN
docs/pybullet_quickstartguide.pdf
View File

Binary file not shown.

47
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -295,8 +295,19 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
inertia.m_izz = urdfLexicalCast<double>(izz->GetText()); inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
} else } else
{ {
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements"); if (ixx && iyy && izz)
return false; {
inertia.m_ixx = urdfLexicalCast<double>(ixx->GetText());
inertia.m_ixy = 0;
inertia.m_ixz = 0;
inertia.m_iyy = urdfLexicalCast<double>(iyy->GetText());
inertia.m_iyz = 0;
inertia.m_izz = urdfLexicalCast<double>(izz->GetText());
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz child elements");
return false;
}
} }
} else } else
{ {
@@ -304,15 +315,29 @@ bool UrdfParser::parseInertia(UrdfInertia& inertia, TiXmlElement* config, ErrorL
inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") && inertia_xml->Attribute("iyy") && inertia_xml->Attribute("iyz") &&
inertia_xml->Attribute("izz"))) inertia_xml->Attribute("izz")))
{ {
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes"); if ((inertia_xml->Attribute("ixx") && inertia_xml->Attribute("iyy") &&
return false; inertia_xml->Attribute("izz")))
} {
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx")); inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy")); inertia.m_ixy = 0;
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz")); inertia.m_ixz = 0;
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy")); inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz")); inertia.m_iyz = 0;
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz")); inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
} else
{
logger->reportError("Inertial: inertia element must have ixx,ixy,ixz,iyy,iyz,izz attributes");
return false;
}
} else
{
inertia.m_ixx = urdfLexicalCast<double>(inertia_xml->Attribute("ixx"));
inertia.m_ixy = urdfLexicalCast<double>(inertia_xml->Attribute("ixy"));
inertia.m_ixz = urdfLexicalCast<double>(inertia_xml->Attribute("ixz"));
inertia.m_iyy = urdfLexicalCast<double>(inertia_xml->Attribute("iyy"));
inertia.m_iyz = urdfLexicalCast<double>(inertia_xml->Attribute("iyz"));
inertia.m_izz = urdfLexicalCast<double>(inertia_xml->Attribute("izz"));
}
} }
return true; return true;

2
examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
View File

@@ -211,7 +211,7 @@ updateVertex(
positions.push_back(in_positions[3*i.v_idx+1]); positions.push_back(in_positions[3*i.v_idx+1]);
positions.push_back(in_positions[3*i.v_idx+2]); positions.push_back(in_positions[3*i.v_idx+2]);
if (i.vn_idx >= 0) { if (i.vn_idx >= 0 && ((3*i.vn_idx+2)<in_normals.size())) {
normals.push_back(in_normals[3*i.vn_idx+0]); normals.push_back(in_normals[3*i.vn_idx+0]);
normals.push_back(in_normals[3*i.vn_idx+1]); normals.push_back(in_normals[3*i.vn_idx+1]);
normals.push_back(in_normals[3*i.vn_idx+2]); normals.push_back(in_normals[3*i.vn_idx+2]);

39
examples/pybullet/pybullet.c
View File

@@ -2668,7 +2668,7 @@ static PyObject* pybullet_rayTest(PyObject* self, PyObject* args, PyObject *keyw
return Py_None; return Py_None;
} }
static PyObject* pybullet_getMatrixFromQuaterion(PyObject* self, PyObject* args) static PyObject* pybullet_getMatrixFromQuaternion(PyObject* self, PyObject* args)
{ {
PyObject* quatObj; PyObject* quatObj;
double quat[4]; double quat[4];
@@ -2907,7 +2907,7 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes); pyResultList = PyTuple_New(visualShapeInfo.m_numVisualShapes);
for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++) for (i = 0; i < visualShapeInfo.m_numVisualShapes; i++)
{ {
PyObject* visualShapeObList = PyTuple_New(7); PyObject* visualShapeObList = PyTuple_New(8);
PyObject* item; PyObject* item;
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId); item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_objectUniqueId);
PyTuple_SetItem(visualShapeObList, 0, item); PyTuple_SetItem(visualShapeObList, 0, item);
@@ -2920,11 +2920,11 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{ {
PyObject* vec = PyTuple_New(3); PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[0]);
PyTuple_SetItem(vec, 0, item); PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[1]);
PyTuple_SetItem(vec, 1, item); PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_dimensions[2]);
PyTuple_SetItem(vec, 2, item); PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 3, vec); PyTuple_SetItem(visualShapeObList, 3, vec);
} }
@@ -2934,28 +2934,41 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{ {
PyObject* vec = PyTuple_New(3); PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]);
PyTuple_SetItem(vec, 0, item); PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]);
PyTuple_SetItem(vec, 1, item); PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]);
PyTuple_SetItem(vec, 2, item); PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 5, vec); PyTuple_SetItem(visualShapeObList, 5, vec);
} }
{ {
PyObject* vec = PyTuple_New(4); PyObject* vec = PyTuple_New(4);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]);
PyTuple_SetItem(vec, 0, item); PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]);
PyTuple_SetItem(vec, 1, item); PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]);
PyTuple_SetItem(vec, 2, item); PyTuple_SetItem(vec, 2, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]); item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]);
PyTuple_SetItem(vec, 3, item); PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(visualShapeObList, 6, vec); PyTuple_SetItem(visualShapeObList, 6, vec);
} }
{
PyObject* rgba = PyTuple_New(4);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[0]);
PyTuple_SetItem(rgba, 0, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[1]);
PyTuple_SetItem(rgba, 1, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[2]);
PyTuple_SetItem(rgba, 2, item);
item = PyFloat_FromDouble(visualShapeInfo.m_visualShapeData[i].m_rgbaColor[3]);
PyTuple_SetItem(rgba, 3, item);
PyTuple_SetItem(visualShapeObList, 7, rgba);
}
PyTuple_SetItem(pyResultList, i, visualShapeObList); PyTuple_SetItem(pyResultList, i, visualShapeObList);
} }
@@ -4905,7 +4918,7 @@ static PyMethodDef SpamMethods[] = {
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF " "Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF "
"convention"}, "convention"},
{"getMatrixFromQuaterion", pybullet_getMatrixFromQuaterion,METH_VARARGS, {"getMatrixFromQuaternion", pybullet_getMatrixFromQuaternion,METH_VARARGS,
"Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"}, "Compute the 3x3 matrix from a quaternion, as a list of 9 values (row-major)"},
{"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS, {"calculateInverseDynamics", (PyCFunction)pybullet_calculateInverseDynamics, METH_VARARGS| METH_KEYWORDS,