AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge pull request #1041 from olegklimov/master

Browse Source 
Random improvements (MJCF, error messages, gravity)
This commit is contained in:
erwincoumans 2017-03-29 09:12:48 -07:00 committed by GitHub
commit 0750d502a8
6 changed files with 109 additions and 89 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -79,7 +79,7 @@ static bool parseVector3(btVector3& vec3, const std::string& vector_str, MJCFErr
} }
if (rgba.size() < 3) if (rgba.size() < 3)
{ {
logger->reportWarning("Couldn't parse vector3"); logger->reportWarning( ("Couldn't parse vector3 '" + vector_str + "'").c_str() );
return false; return false;
} }
if (lastThree) { if (lastThree) {
@ -113,7 +113,8 @@ static bool parseVector6(btVector3& v0, btVector3& v1, const std::string& vector
} }
if (values.size() < 6) if (values.size() < 6)
{ {
logger->reportWarning("Couldn't parse 6 floats"); logger->reportWarning( ("Couldn't parse 6 floats '" + vector_str + "'").c_str() );
return false; return false;
} }
v0.setValue(values[0],values[1],values[2]); v0.setValue(values[0],values[1],values[2]);
@ -143,6 +144,7 @@ struct BulletMJCFImporterInternalData
//<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/> //<compiler angle="radian" meshdir="mesh/" texturedir="texture/"/>
std::string m_meshDir; std::string m_meshDir;
std::string m_textureDir; std::string m_textureDir;
std::string m_angleUnits;
int m_activeModel; int m_activeModel;
@ -213,6 +215,8 @@ struct BulletMJCFImporterInternalData
{ {
m_textureDir = textureDirStr; m_textureDir = textureDirStr;
} }
const char* angle = root_xml->Attribute("angle");
m_angleUnits = angle ? angle : "degree"; // degrees by default, http://www.mujoco.org/book/modeling.html#compiler
#if 0 #if 0
for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement()) for (TiXmlElement* child_xml = root_xml->FirstChildElement() ; child_xml ; child_xml = child_xml->NextSiblingElement())
{ {
@ -345,8 +349,7 @@ struct BulletMJCFImporterInternalData
} }
if (!handled) if (!handled)
{ {
logger->reportWarning("Unhandled root element"); logger->reportWarning( (sourceFileLocation(rootxml) + ": unhandled root element '" + n + "'").c_str() );
logger->reportWarning(n.c_str());
} }
} }
return true; return true;
@ -354,6 +357,7 @@ struct BulletMJCFImporterInternalData
bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut) bool parseJoint(TiXmlElement* link_xml, int modelIndex, int parentLinkIndex, int linkIndex, MJCFErrorLogger* logger, const btTransform& parentToLinkTrans, btTransform& jointTransOut)
{ {
bool jointHandled = false;
const char* jType = link_xml->Attribute("type"); const char* jType = link_xml->Attribute("type");
const char* limitedStr = link_xml->Attribute("limited"); const char* limitedStr = link_xml->Attribute("limited");
const char* axisStr = link_xml->Attribute("axis"); const char* axisStr = link_xml->Attribute("axis");
@ -383,83 +387,29 @@ struct BulletMJCFImporterInternalData
jointTrans.setRotation(orn); jointTrans.setRotation(orn);
} }
} }
btVector3 jointAxis(1,0,0);
btVector3 jointAxis(1,0,0);
if (axisStr) if (axisStr)
{ {
std::string ax = axisStr; std::string ax = axisStr;
parseVector3(jointAxis,ax,logger); parseVector3(jointAxis,ax,logger);
} else } else
{ {
logger->reportWarning("joint without axis attribute"); logger->reportWarning( (sourceFileLocation(link_xml) + ": joint without axis attribute").c_str() );
} }
bool isLimited = false;
double range[2] = {1,0};
double range[2] = {1,0};
std::string lim = m_defaultJointLimited; std::string lim = m_defaultJointLimited;
if (limitedStr) if (limitedStr)
{ {
lim = limitedStr; lim = limitedStr;
} }
if (lim=="true") bool isLimited = lim=="true";
{
isLimited = true;
//parse the 'range' field
btArray<std::string> pieces;
btArray<float> sizes;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, rangeStr, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
sizes.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (sizes.size()==2)
{
// TODO angle units are in "<compiler angle="degree" inertiafromgeom="true"/>
range[0] = sizes[0] * B3_PI / 180;
range[1] = sizes[1] * B3_PI / 180;
} else
{
logger->reportWarning("Expected range[2] in joint with limits");
}
}
// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
// degrees of freedom created by this joint. These are constants added to the
// diagonal of the inertia matrix in generalized coordinates. They make the
// simulation more stable, and often increase physical realism. This is because
// when a motor is attached to the system with a transmission that amplifies
// the motor force by c, the inertia of the rotor (i.e. the moving part of the
// motor) is amplified by c*c. The same holds for gears in the early stages of
// planetary gear boxes. These extra inertias often dominate the inertias of
// the robot parts that are represented explicitly in the model, and the
// armature attribute is the way to model them.
// TODO damping : real, "0" Damping applied to all degrees of
// freedom created by this joint. Unlike friction loss
// which is computed by the constraint solver, damping is
// simply a force linear in velocity. It is included in
// the passive forces. Despite this simplicity, larger
// damping values can make numerical integrators unstable,
// which is why our Euler integrator handles damping
// implicitly. See Integration in the Computation chapter.
bool jointHandled = false;
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
btTransform parentLinkToJointTransform;
parentLinkToJointTransform.setIdentity();
parentLinkToJointTransform = parentToLinkTrans*jointTrans;
jointTransOut = jointTrans;
UrdfJointTypes ejtype; UrdfJointTypes ejtype;
if (jType) if (jType)
{ {
std::string jointType = jType; std::string jointType = jType;
if (jointType == "fixed") if (jointType == "fixed")
{ {
ejtype = URDFFixedJoint; ejtype = URDFFixedJoint;
@ -483,9 +433,67 @@ struct BulletMJCFImporterInternalData
} }
} else } else
{ {
logger->reportWarning("Expected 'type' attribute for joint"); logger->reportWarning( (sourceFileLocation(link_xml) + ": expected 'type' attribute for joint").c_str() );
} }
if (isLimited)
{
//parse the 'range' field
btArray<std::string> pieces;
btArray<float> limits;
btAlignedObjectArray<std::string> strArray;
urdfIsAnyOf(" ", strArray);
urdfStringSplit(pieces, rangeStr, strArray);
for (int i = 0; i < pieces.size(); ++i)
{
if (!pieces[i].empty())
{
limits.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
}
}
if (limits.size()==2)
{
range[0] = limits[0];
range[1] = limits[1];
if (m_angleUnits=="degree" && ejtype==URDFRevoluteJoint)
{
range[0] = limits[0] * B3_PI / 180;
range[1] = limits[1] * B3_PI / 180;
}
}
else
{
logger->reportWarning( (sourceFileLocation(link_xml) + ": cannot parse 'range' attribute (units='" + m_angleUnits + "'')").c_str() );
}
}
// TODO armature : real, "0" Armature inertia (or rotor inertia) of all
// degrees of freedom created by this joint. These are constants added to the
// diagonal of the inertia matrix in generalized coordinates. They make the
// simulation more stable, and often increase physical realism. This is because
// when a motor is attached to the system with a transmission that amplifies
// the motor force by c, the inertia of the rotor (i.e. the moving part of the
// motor) is amplified by c*c. The same holds for gears in the early stages of
// planetary gear boxes. These extra inertias often dominate the inertias of
// the robot parts that are represented explicitly in the model, and the
// armature attribute is the way to model them.
// TODO damping : real, "0" Damping applied to all degrees of
// freedom created by this joint. Unlike friction loss
// which is computed by the constraint solver, damping is
// simply a force linear in velocity. It is included in
// the passive forces. Despite this simplicity, larger
// damping values can make numerical integrators unstable,
// which is why our Euler integrator handles damping
// implicitly. See Integration in the Computation chapter.
const UrdfLink* linkPtr = getLink(modelIndex,linkIndex);
btTransform parentLinkToJointTransform;
parentLinkToJointTransform.setIdentity();
parentLinkToJointTransform = parentToLinkTrans*jointTrans;
jointTransOut = jointTrans;
if (jointHandled) if (jointHandled)
{ {
UrdfJoint* jointPtr = new UrdfJoint(); UrdfJoint* jointPtr = new UrdfJoint();
@ -528,6 +536,7 @@ struct BulletMJCFImporterInternalData
UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex); UrdfLink** linkPtrPtr = m_models[modelIndex]->m_links.getAtIndex(linkIndex);
if (linkPtrPtr==0) if (linkPtrPtr==0)
{ {
// XXX: should it be assert?
logger->reportWarning("Invalide linkindex"); logger->reportWarning("Invalide linkindex");
return false; return false;
} }
@ -621,7 +630,7 @@ struct BulletMJCFImporterInternalData
geom.m_sphereRadius = urdfLexicalCast<double>(sz); geom.m_sphereRadius = urdfLexicalCast<double>(sz);
} else } else
{ {
logger->reportWarning("Expected size field (scalar) in sphere geom"); logger->reportWarning( (sourceFileLocation(link_xml) + ": no size field (scalar) in sphere geom").c_str() );
} }
handledGeomType = true; handledGeomType = true;
} }
@ -645,18 +654,18 @@ struct BulletMJCFImporterInternalData
} }
geom.m_capsuleRadius = 0; geom.m_capsuleRadius = 0;
geom.m_capsuleHalfHeight = 0.f; geom.m_capsuleHeight = 0.f;
if (sizes.size()>0) if (sizes.size()>0)
{ {
geom.m_capsuleRadius = sizes[0]; geom.m_capsuleRadius = sizes[0];
if (sizes.size()>1) if (sizes.size()>1)
{ {
geom.m_capsuleHalfHeight = sizes[1]; geom.m_capsuleHeight = 2*sizes[1];
} }
} else } else
{ {
logger->reportWarning("couldn't convert 'size' attribute of capsule geom"); logger->reportWarning( (sourceFileLocation(link_xml) + ": couldn't convert 'size' attribute of capsule geom").c_str() );
} }
const char* fromtoStr = link_xml->Attribute("fromto"); const char* fromtoStr = link_xml->Attribute("fromto");
geom.m_hasFromTo = false; geom.m_hasFromTo = false;
@ -672,7 +681,7 @@ struct BulletMJCFImporterInternalData
{ {
if (sizes.size()<2) if (sizes.size()<2)
{ {
logger->reportWarning("capsule without fromto attribute requires 2 sizes (radius and halfheight)"); logger->reportWarning( (sourceFileLocation(link_xml) + ": capsule without fromto attribute requires 2 sizes (radius and halfheight)").c_str() );
} else } else
{ {
handledGeomType = true; handledGeomType = true;
@ -733,13 +742,11 @@ struct BulletMJCFImporterInternalData
} else } else
{ {
char warn[1024]; logger->reportWarning( (sourceFileLocation(link_xml) + ": unhandled geom type '" + geomType + "'").c_str() );
sprintf(warn,"Unknown/unhandled geom type: %s", geomType.c_str());
logger->reportWarning(warn);
} }
} else } else
{ {
logger->reportWarning("geom requires type"); logger->reportWarning( (sourceFileLocation(link_xml) + ": geom requires type").c_str() );
} }
return handledGeomType; return handledGeomType;
@ -1021,10 +1028,7 @@ struct BulletMJCFImporterInternalData
} }
if (!handled) if (!handled)
{ {
char warn[1024]; logger->reportWarning( (sourceFileLocation(xml) + ": unknown field '" + n + "'").c_str() );
std::string n = xml->Value();
sprintf(warn,"Unknown/unhandled field: %s", n.c_str());
logger->reportWarning(warn);
} }
} }
@ -1677,7 +1681,7 @@ class btCompoundShape* BulletMJCFImporter::convertLinkCollisionShapes(int linkIn
} else } else
{ {
btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius, btCapsuleShapeZ* cap = new btCapsuleShapeZ(col->m_geometry.m_capsuleRadius,
2.*col->m_geometry.m_capsuleHalfHeight); col->m_geometry.m_capsuleHeight);
childShape = cap; childShape = cap;
} }
break; break;

8
examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
View File

@ -564,9 +564,9 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
case URDF_GEOM_CAPSULE: case URDF_GEOM_CAPSULE:
{ {
btScalar radius = collision->m_geometry.m_capsuleRadius; btScalar radius = collision->m_geometry.m_capsuleRadius;
btScalar height = btScalar(2.f)*collision->m_geometry.m_capsuleHalfHeight; btScalar height = collision->m_geometry.m_capsuleHeight;
btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height); btCapsuleShapeZ* capsuleShape = new btCapsuleShapeZ(radius,height);
shape = capsuleShape; shape = capsuleShape;
shape ->setMargin(gUrdfDefaultCollisionMargin); shape ->setMargin(gUrdfDefaultCollisionMargin);
break; break;
} }
@ -574,7 +574,7 @@ btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, co
case URDF_GEOM_CYLINDER: case URDF_GEOM_CYLINDER:
{ {
btScalar cylRadius = collision->m_geometry.m_capsuleRadius; btScalar cylRadius = collision->m_geometry.m_capsuleRadius;
btScalar cylLength = collision->m_geometry.m_capsuleHalfHeight; btScalar cylLength = collision->m_geometry.m_capsuleHeight;
btAlignedObjectArray<btVector3> vertices; btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float)); //int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
@ -797,7 +797,7 @@ static void convertURDFToVisualShapeInternal(const UrdfVisual* visual, const cha
{ {
btScalar cylRadius = visual->m_geometry.m_capsuleRadius; btScalar cylRadius = visual->m_geometry.m_capsuleRadius;
btScalar cylLength = visual->m_geometry.m_capsuleHalfHeight; btScalar cylLength = visual->m_geometry.m_capsuleHeight;
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.); btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
vertices.push_back(vert); vertices.push_back(vert);

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

@ -403,7 +403,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
} }
geom.m_hasFromTo = false; geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius")); geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHalfHeight = urdfLexicalCast<double>(shape->Attribute("length")); geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
} }
else if (type_name == "capsule") else if (type_name == "capsule")
@ -417,7 +417,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
} }
geom.m_hasFromTo = false; geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius")); geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHalfHeight = btScalar(0.5)*urdfLexicalCast<double>(shape->Attribute("length")); geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
} }
else if (type_name == "mesh") else if (type_name == "mesh")
{ {

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

@ -65,7 +65,7 @@ struct UrdfGeometry
btVector3 m_boxSize; btVector3 m_boxSize;
double m_capsuleRadius; double m_capsuleRadius;
double m_capsuleHalfHeight; double m_capsuleHeight;
int m_hasFromTo; int m_hasFromTo;
btVector3 m_capsuleFrom; btVector3 m_capsuleFrom;
btVector3 m_capsuleTo; btVector3 m_capsuleTo;

17
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1398,8 +1398,8 @@ bool PhysicsServerCommandProcessor::processImportedObjects(const char* fileName,
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
int rootLinkIndex = u2b.getRootLinkIndex(); //int rootLinkIndex = u2b.getRootLinkIndex();
b3Printf("urdf root link index = %d\n",rootLinkIndex); //b3Printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_data->m_guiHelper); MyMultiBodyCreator creation(m_data->m_guiHelper);
u2b.getRootTransformInWorld(rootTrans); u2b.getRootTransformInWorld(rootTrans);
@ -3812,6 +3812,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
linkIndexB = mblB->m_link; linkIndexB = mblB->m_link;
objectIndexB = mblB->m_multiBody->getUserIndex2(); objectIndexB = mblB->m_multiBody->getUserIndex2();
if (
(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_B_FILTER) &&
clientCmd.m_requestContactPointArguments.m_linkIndexBIndexFilter != linkIndexB)
{
continue;
}
} }
int objectIndexA = -1; int objectIndexA = -1;
@ -3824,8 +3830,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (mblA && mblA->m_multiBody) if (mblA && mblA->m_multiBody)
{ {
linkIndexA = mblA->m_link; linkIndexA = mblA->m_link;
objectIndexA = mblA->m_multiBody->getUserIndex2(); objectIndexA = mblA->m_multiBody->getUserIndex2();
if (
(clientCmd.m_updateFlags & CMD_REQUEST_CONTACT_POINT_HAS_LINK_INDEX_A_FILTER) &&
clientCmd.m_requestContactPointArguments.m_linkIndexAIndexFilter != linkIndexA)
{
continue;
}
} }
btAssert(bodyA || mblA); btAssert(bodyA || mblA);

7
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@ -220,7 +220,7 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
rad = visual->m_geometry.m_capsuleRadius; rad = visual->m_geometry.m_capsuleRadius;
} else { } else {
tr = visual->m_linkLocalFrame; tr = visual->m_linkLocalFrame;
len = visual->m_geometry.m_capsuleHalfHeight; len = visual->m_geometry.m_capsuleHeight;
rad = visual->m_geometry.m_capsuleRadius; rad = visual->m_geometry.m_capsuleRadius;
} }
visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0]; visualShapeOut.m_localVisualFrame[0] = tr.getOrigin()[0];
@ -414,6 +414,11 @@ void convertURDFToVisualShape(const UrdfShape* visual, const char* urdfPathPrefi
break; break;
} // case mesh } // case mesh
case URDF_GEOM_PLANE:
// TODO: plane in tiny renderer
// TODO: export visualShapeOut for external render
break;
default: default:
{ {
b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type); b3Warning("TinyRenderer: unknown visual geometry type %i\n", visual->m_geometry.m_type);