summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--data/mjcf/inverted_pendulum.xml2
-rw-r--r--examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp44
-rw-r--r--examples/Importers/ImportURDFDemo/UrdfParser.h3
-rw-r--r--examples/SharedMemory/SharedMemoryPublic.h2
-rw-r--r--examples/SharedMemory/TinyRendererVisualShapeConverter.cpp27
-rw-r--r--examples/pybullet/pybullet.c14
-rw-r--r--src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp50
7 files changed, 110 insertions, 32 deletions
diff --git a/data/mjcf/inverted_pendulum.xml b/data/mjcf/inverted_pendulum.xml
index 401503893..f8fbc1aad 100644
--- a/data/mjcf/inverted_pendulum.xml
+++ b/data/mjcf/inverted_pendulum.xml
@@ -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>
diff --git a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
index 4041feb01..4d225c05b 100644
--- a/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+++ b/examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
@@ -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);
}
}
}
diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h
index 2973440af..64088a1d9 100644
--- a/examples/Importers/ImportURDFDemo/UrdfParser.h
+++ b/examples/Importers/ImportURDFDemo/UrdfParser.h
@@ -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;
diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h
index 071d371eb..40e60c3bb 100644
--- a/examples/SharedMemory/SharedMemoryPublic.h
+++ b/examples/SharedMemory/SharedMemoryPublic.h
@@ -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];
};
diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
index 13d616be6..5ae682fc2 100644
--- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
@@ -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];
diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c
index 87706aad8..06c76c8fd 100644
--- a/examples/pybullet/pybullet.c
+++ b/examples/pybullet/pybullet.c
@@ -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);
}
diff --git a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
index bc23fdb98..b54bd4893 100644
--- a/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
+++ b/src/BulletCollision/CollisionDispatch/btConvexConvexAlgorithm.cpp
@@ -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
View Lorry Controller Status