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

Merge pull request #932 from erwincoumans/master

Browse Source 
minor fixes, report visual frame instead of inertial frame in visualShapeData
This commit is contained in:
erwincoumans
2017-01-25 11:37:23 -08:00
committed by GitHub
parent a9be975601 980908bdee
commit a230c941f9
7 changed files with 110 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/mjcf/inverted_pendulum.xml
View File

@@ -2,7 +2,7 @@
<compiler inertiafromgeom="true"/>
<default>
<joint armature="0" damping="1" limited="true"/>
<geom contype="0" friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
<geom friction="1 0.1 0.1" rgba="0.7 0.7 0 1"/>
<tendon/>
<motor ctrlrange="-3 3"/>
</default>

44
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
View File

@@ -296,7 +296,8 @@ struct BulletMJCFImporterInternalData
// modelPtr->m_rootLinks.push_back(linkPtr);
parseGeom(rootxml,modelIndex, linkIndex,logger);
btVector3 inertialShift(0,0,0);
parseGeom(rootxml,modelIndex, linkIndex,logger,inertialShift);
initTreeAndRoot(*modelPtr,logger);
handled = true;
@@ -469,7 +470,7 @@ struct BulletMJCFImporterInternalData
*/
return false;
}
bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger)
bool parseGeom(TiXmlElement* link_xml, int modelIndex, int linkIndex, MJCFErrorLogger* logger, btVector3& inertialShift)
{
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0)
@@ -599,6 +600,7 @@ struct BulletMJCFImporterInternalData
geom.m_hasFromTo = true;
std::string fromto = fromtoStr;
parseVector6(geom.m_capsuleFrom,geom.m_capsuleTo,fromto,logger);
inertialShift=0.5*(geom.m_capsuleFrom+geom.m_capsuleTo);
handledGeomType = true;
} else
{
@@ -807,7 +809,9 @@ struct BulletMJCFImporterInternalData
const char* bodyName = link_xml->Attribute("name");
int orgChildLinkIndex = createBody(modelIndex,bodyName);
btTransform localInertialFrame;
localInertialFrame.setIdentity();
// int curChildLinkIndex = orgChildLinkIndex;
std::string bodyN;
@@ -830,8 +834,8 @@ struct BulletMJCFImporterInternalData
bool massDefined = false;
btVector3 inertialPos(0,0,0);
btQuaternion inertialOrn(0,0,0,1);
btScalar mass = 0.f;
btVector3 localInertiaDiag(0,0,0);
// int thisLinkIndex = -2;
@@ -839,7 +843,7 @@ struct BulletMJCFImporterInternalData
btTransform jointTrans;
jointTrans.setIdentity();
bool skipFixedJoint = false;
for (TiXmlElement* xml = link_xml->FirstChildElement() ; xml ; xml = xml->NextSiblingElement())
{
bool handled = false;
@@ -852,7 +856,22 @@ struct BulletMJCFImporterInternalData
if (p)
{
std::string posStr = p;
parseVector3(inertialPos,posStr,logger);
btVector3 inertialPos(0,0,0);
if (parseVector3(inertialPos,posStr,logger))
{
localInertialFrame.setOrigin(inertialPos);
}
}
const char* o = xml->Attribute("quat");
{
std::string ornStr = o;
btQuaternion orn(0,0,0,1);
btVector4 o4;
if (parseVector4(o4,ornStr))
{
orn.setValue(o4[1],o4[2],o4[3],o4[0]);
localInertialFrame.setRotation(orn);
}
}
const char* m = xml->Attribute("mass");
if (m)
@@ -915,7 +934,12 @@ struct BulletMJCFImporterInternalData
}
if (n == "geom")
{
parseGeom(xml,modelIndex, orgChildLinkIndex , logger);
btVector3 inertialShift(0,0,0);
parseGeom(xml,modelIndex, orgChildLinkIndex , logger,inertialShift);
if (!massDefined)
{
localInertialFrame.setOrigin(inertialShift);
}
handled = true;
}
@@ -968,7 +992,7 @@ struct BulletMJCFImporterInternalData
double volume = computeVolume(linkPtr,logger);
mass = density * volume;
}
linkPtr->m_inertia.m_linkLocalFrame.setIdentity();// = jointTrans.inverse();
linkPtr->m_inertia.m_linkLocalFrame = localInertialFrame;// = jointTrans.inverse();
linkPtr->m_inertia.m_mass = mass;
return true;
}
@@ -1631,7 +1655,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
if (childShape)
{
m_data->m_allocatedCollisionShapes.push_back(childShape);
compound->addChildShape(col->m_linkLocalFrame,childShape);
compound->addChildShape(localInertiaFrame.inverse()*col->m_linkLocalFrame,childShape);
}
}
}

3
examples/Importers/ImportURDFDemo/UrdfParser.h
View File

@@ -26,11 +26,14 @@ struct UrdfMaterial
struct UrdfInertia
{
btTransform m_linkLocalFrame;
bool m_hasLinkLocalFrame;
double m_mass;
double m_ixx,m_ixy,m_ixz,m_iyy,m_iyz,m_izz;
UrdfInertia()
{
m_hasLinkLocalFrame = false;
m_linkLocalFrame.setIdentity();
m_mass = 0.f;
m_ixx=m_ixy=m_ixz=m_iyy=m_iyz=m_izz=0.f;

2
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -329,7 +329,7 @@ struct b3VisualShapeData
int m_visualGeometryType;//box primitive, sphere primitive, triangle mesh
double m_dimensions[3];//meaning depends on m_visualGeometryType
char m_meshAssetFileName[VISUAL_SHAPE_MAX_PATH_LEN];
double m_localInertiaFrame[7];//pos[3], orn[4]
double m_localVisualFrame[7];//pos[3], orn[4]
//todo: add more data if necessary (material color etc, although material can be in asset file .obj file)
double m_rgbaColor[4];
};

27
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -289,7 +289,14 @@ void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPref
visualShapeOut.m_dimensions[0] = visual->m_geometry.m_meshScale[0];
visualShapeOut.m_dimensions[1] = visual->m_geometry.m_meshScale[1];
visualShapeOut.m_dimensions[2] = visual->m_geometry.m_meshScale[2];
visualShapeOut.m_localVisualFrame[0] = visual->m_linkLocalFrame.getOrigin()[0];
visualShapeOut.m_localVisualFrame[1] = visual->m_linkLocalFrame.getOrigin()[1];
visualShapeOut.m_localVisualFrame[2] = visual->m_linkLocalFrame.getOrigin()[2];
visualShapeOut.m_localVisualFrame[3] = visual->m_linkLocalFrame.getRotation()[0];
visualShapeOut.m_localVisualFrame[4] = visual->m_linkLocalFrame.getRotation()[1];
visualShapeOut.m_localVisualFrame[5] = visual->m_linkLocalFrame.getRotation()[2];
visualShapeOut.m_localVisualFrame[6] = visual->m_linkLocalFrame.getRotation()[3];
int sl = strlen(fullPath);
if (sl < (VISUAL_SHAPE_MAX_PATH_LEN-1))
{
@@ -550,7 +557,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
const UrdfLink* link = *linkPtr;
for (int v = 0; v < link->m_visualArray.size();v++)
for (int v1 = 0; v1 < link->m_visualArray.size();v1++)
{
btAlignedObjectArray<MyTexture2> textures;
btAlignedObjectArray<GLInstanceVertex> vertices;
@@ -558,7 +565,7 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
btTransform startTrans; startTrans.setIdentity();
//int graphicsIndex = -1;
const UrdfVisual& vis = link->m_visualArray[v];
const UrdfVisual& vis = link->m_visualArray[v1];
btTransform childTrans = vis.m_linkLocalFrame;
btHashString matName(vis.m_materialName.c_str());
UrdfMaterial *const * matPtr = model.m_materials[matName];
@@ -586,13 +593,13 @@ void TinyRendererVisualShapeConverter::convertVisualShapes(int linkIndex, const
b3VisualShapeData visualShape;
visualShape.m_objectUniqueId = bodyUniqueId;
visualShape.m_linkIndex = linkIndex;
visualShape.m_localInertiaFrame[0] = localInertiaFrame.getOrigin()[0];
visualShape.m_localInertiaFrame[1] = localInertiaFrame.getOrigin()[1];
visualShape.m_localInertiaFrame[2] = localInertiaFrame.getOrigin()[2];
visualShape.m_localInertiaFrame[3] = localInertiaFrame.getRotation()[0];
visualShape.m_localInertiaFrame[4] = localInertiaFrame.getRotation()[1];
visualShape.m_localInertiaFrame[5] = localInertiaFrame.getRotation()[2];
visualShape.m_localInertiaFrame[6] = localInertiaFrame.getRotation()[3];
visualShape.m_localVisualFrame[0] = vis.m_linkLocalFrame.getOrigin()[0];
visualShape.m_localVisualFrame[1] = vis.m_linkLocalFrame.getOrigin()[1];
visualShape.m_localVisualFrame[2] = vis.m_linkLocalFrame.getOrigin()[2];
visualShape.m_localVisualFrame[3] = vis.m_linkLocalFrame.getRotation()[0];
visualShape.m_localVisualFrame[4] = vis.m_linkLocalFrame.getRotation()[1];
visualShape.m_localVisualFrame[5] = vis.m_linkLocalFrame.getRotation()[2];
visualShape.m_localVisualFrame[6] = vis.m_linkLocalFrame.getRotation()[3];
visualShape.m_rgbaColor[0] = rgbaColor[0];
visualShape.m_rgbaColor[1] = rgbaColor[1];
visualShape.m_rgbaColor[2] = rgbaColor[2];

14
examples/pybullet/pybullet.c
View File

@@ -2931,24 +2931,24 @@ static PyObject* pybullet_getVisualShapeData(PyObject* self, PyObject* args, PyO
{
PyObject* vec = PyTuple_New(3);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[0]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[0]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[1]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[1]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[2]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[2]);
PyTuple_SetItem(vec, 2, item);
PyTuple_SetItem(visualShapeObList, 5, vec);
}
{
PyObject* vec = PyTuple_New(4);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[3]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[3]);
PyTuple_SetItem(vec, 0, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[4]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[4]);
PyTuple_SetItem(vec, 1, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[5]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[5]);
PyTuple_SetItem(vec, 2, item);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localInertiaFrame[6]);
item = PyInt_FromLong(visualShapeInfo.m_visualShapeData[i].m_localVisualFrame[6]);
PyTuple_SetItem(vec, 3, item);
PyTuple_SetItem(visualShapeObList, 6, vec);
}

50
src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
View File

@@ -16,7 +16,7 @@ subject to the following restrictions:
///Specialized capsule-capsule collision algorithm has been added for Bullet 2.75 release to increase ragdoll performance
///If you experience problems with capsule-capsule collision, try to define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER and report it in the Bullet forums
///with reproduction case
//define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
//#define BT_DISABLE_CAPSULE_CAPSULE_COLLIDER 1
//#define ZERO_MARGIN
#include "btConvexConvexAlgorithm.h"
@@ -310,10 +310,10 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
#ifndef BT_DISABLE_CAPSULE_CAPSULE_COLLIDER
if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
{
//m_manifoldPtr->clearManifold();
btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
// btVector3 localScalingA = capsuleA->getLocalScaling();
// btVector3 localScalingB = capsuleB->getLocalScaling();
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
@@ -329,6 +329,50 @@ void btConvexConvexAlgorithm ::processCollision (const btCollisionObjectWrapper*
resultOut->refreshContactPoints();
return;
}
if ((min0->getShapeType() == CAPSULE_SHAPE_PROXYTYPE) && (min1->getShapeType() == SPHERE_SHAPE_PROXYTYPE))
{
//m_manifoldPtr->clearManifold();
btCapsuleShape* capsuleA = (btCapsuleShape*) min0;
btSphereShape* capsuleB = (btSphereShape*) min1;
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,capsuleA->getHalfHeight(),capsuleA->getRadius(),
0.,capsuleB->getRadius(),capsuleA->getUpAxis(),1,
body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
if (dist<threshold)
{
btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
}
resultOut->refreshContactPoints();
return;
}
if ((min0->getShapeType() == SPHERE_SHAPE_PROXYTYPE) && (min1->getShapeType() == CAPSULE_SHAPE_PROXYTYPE))
{
//m_manifoldPtr->clearManifold();
btSphereShape* capsuleA = (btSphereShape*) min0;
btCapsuleShape* capsuleB = (btCapsuleShape*) min1;
btScalar threshold = m_manifoldPtr->getContactBreakingThreshold();
btScalar dist = capsuleCapsuleDistance(normalOnB, pointOnBWorld,0.,capsuleA->getRadius(),
capsuleB->getHalfHeight(),capsuleB->getRadius(),1,capsuleB->getUpAxis(),
body0Wrap->getWorldTransform(),body1Wrap->getWorldTransform(),threshold);
if (dist<threshold)
{
btAssert(normalOnB.length2()>=(SIMD_EPSILON*SIMD_EPSILON));
resultOut->addContactPoint(normalOnB,pointOnBWorld,dist);
}
resultOut->refreshContactPoints();
return;
}
#endif //BT_DISABLE_CAPSULE_CAPSULE_COLLIDER