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

Expose optional "globalScaling" factor to pybullet.loadURDF and pybullet.loadSDF. This will scale the visual, collision shapes and transform locations.

Browse Source 
Fix two_cubes.sdf (was lacking collision shape for second cube)
This commit is contained in:
Erwin Coumans
2017-08-14 14:59:41 -07:00
parent dfaa717fed
commit aafaa7e33e
15 changed files with 130 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -69,6 +69,12 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
{
m_pathPrefix[0] = 0;
}
void setGlobalScaling(btScalar scaling)
{
m_urdfParser.setGlobalScaling(scaling);
}
};
void BulletURDFImporter::printTree()
@@ -76,10 +82,10 @@ void BulletURDFImporter::printTree()
// btAssert(0);
}
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling)
{
m_data = new BulletURDFInternalData;
m_data->setGlobalScaling(globalScaling);
m_data->m_guiHelper = helper;
m_data->m_customVisualShapesConverter = customConverter;

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

@@ -15,10 +15,11 @@ class BulletURDFImporter : public URDFImporterInterface
public:
BulletURDFImporter(struct GUIHelperInterface* guiHelper, LinkVisualShapesConverter* customConverter);
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling);
virtual ~BulletURDFImporter();
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path

2
examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -194,7 +194,7 @@ void ImportUrdfSetup::initPhysics()
}
BulletURDFImporter u2b(m_guiHelper, 0);
BulletURDFImporter u2b(m_guiHelper, 0,1);
bool loadOk = u2b.loadURDF(m_fileName);

1
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -173,6 +173,7 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
}
btScalar tmpUrdfScaling=2;
void ConvertURDF2BulletInternal(

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

@@ -6,7 +6,8 @@
UrdfParser::UrdfParser()
:m_parseSDF(false),
m_activeSdfModel(-1)
m_activeSdfModel(-1),
m_urdfScaling(1)
{
m_urdf2Model.m_sourceFile = "IN_MEMORY_STRING"; // if loadUrdf() called later, source file name will be replaced with real
}
@@ -132,22 +133,24 @@ bool UrdfParser::parseMaterial(UrdfMaterial& material, TiXmlElement *config, Err
}
bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false)
bool UrdfParser::parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger, bool parseSDF )
{
tr.setIdentity();
btVector3 vec(0,0,0);
if (parseSDF)
{
parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
parseVector3(vec,std::string(xml->GetText()),logger);
}
else
{
const char* xyz_str = xml->Attribute("xyz");
if (xyz_str)
{
parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
parseVector3(vec,std::string(xyz_str),logger);
}
}
tr.setOrigin(vec*m_urdfScaling);
if (parseSDF)
{
@@ -344,7 +347,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
} else
{
geom.m_sphereRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
}
}
else if (type_name == "box")
@@ -359,6 +362,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
}
parseVector3(geom.m_boxSize,size->GetText(),logger);
geom.m_boxSize *= m_urdfScaling;
}
else
{
@@ -369,6 +373,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
} else
{
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
geom.m_boxSize *= m_urdfScaling;
}
}
}
@@ -382,8 +387,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
}
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "capsule")
@@ -396,8 +401,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
return false;
}
geom.m_hasFromTo = false;
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
}
else if (type_name == "mesh")
{
@@ -438,6 +443,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
}
geom.m_meshScale *= m_urdfScaling;
if (fn.empty())
{
logger->reportError("Mesh filename is empty");
@@ -1031,6 +1038,11 @@ bool UrdfParser::parseJointLimits(UrdfJoint& joint, TiXmlElement* config, ErrorL
joint.m_upperLimit = urdfLexicalCast<double>(upper_str);
}
if (joint.m_type == URDFPrismaticJoint)
{
joint.m_lowerLimit *= m_urdfScaling;
joint.m_upperLimit *= m_urdfScaling;
}
// Get joint effort limit
const char* effort_str = config->Attribute("effort");

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

@@ -251,7 +251,8 @@ protected:
bool m_parseSDF;
int m_activeSdfModel;
btScalar m_urdfScaling;
bool parseTransform(btTransform& tr, class TiXmlElement* xml, ErrorLogger* logger, bool parseSDF = false);
bool parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
@@ -277,6 +278,11 @@ public:
{
return m_parseSDF;
}
void setGlobalScaling(btScalar scaling)
{
m_urdfScaling = scaling;
}
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
bool loadSDF(const char* sdfText, ErrorLogger* logger);