AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 22:00:05 +00:00
Code Issues Releases Wiki Activity

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
erwincoumans 2018-09-29 10:35:01 -07:00
commit 254edb61cb
10 changed files with 134 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/Importers/ImportMJCFDemo/BulletMJCFImporter.h
View File

@ -34,7 +34,7 @@ public:
virtual bool loadMJCF(const char* fileName, MJCFErrorLogger* logger, bool forceFixedBase = false);
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0)
{
return false;
}

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

@ -117,7 +117,7 @@ struct BulletErrorLogger : public ErrorLogger
}
};
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase, int flags)
{
if (strlen(fileName) == 0)
return false;
@ -155,7 +155,7 @@ bool BulletURDFImporter::loadURDF(const char* fileName, bool forceFixedBase)
BulletErrorLogger loggie;
m_data->m_urdfParser.setParseSDF(false);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase);
bool result = m_data->m_urdfParser.loadUrdf(xml_string.c_str(), &loggie, forceFixedBase, (flags & CUF_PARSE_SENSORS));
return result;
}

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

@ -23,7 +23,7 @@ public:
virtual ~BulletURDFImporter();
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0);
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false);

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

@ -31,6 +31,7 @@ enum ConvertURDFFlags
CUF_ENABLE_SLEEPING = 2048,
CUF_INITIALIZE_SAT_FEATURES = 4096,
CUF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
CUF_PARSE_SENSORS = 16384,
};
struct UrdfVisualShapeCache

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

@ -12,7 +12,7 @@ class URDFImporterInterface
public:
virtual ~URDFImporterInterface() {}
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false) = 0;
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0) = 0;
virtual bool loadSDF(const char* fileName, bool forceFixedBase = false) { return false; }

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

@ -1436,6 +1436,72 @@ bool UrdfParser::parseJoint(UrdfJoint& joint, XMLElement* config, ErrorLogger* l
return true;
}
bool UrdfParser::parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, XMLElement* config, ErrorLogger* logger)
{
// Sensors are mapped to Links with a Fixed Joints connecting to the parents.
// They has no extent or mass so they will work with the existing
// model without affecting the system.
logger->reportError("Adding Sensor ");
const char* sensorName = config->Attribute("name");
if (!sensorName)
{
logger->reportError("Sensor with no name");
return false;
}
logger->reportError(sensorName);
link.m_name = sensorName;
link.m_linkTransformInWorld.setIdentity();
link.m_inertia.m_mass = 0.f;
link.m_inertia.m_linkLocalFrame.setIdentity();
link.m_inertia.m_ixx = 0.f;
link.m_inertia.m_iyy = 0.f;
link.m_inertia.m_izz = 0.f;
// Get Parent Link
XMLElement* parent_xml = config->FirstChildElement("parent");
if (parent_xml)
{
if (m_parseSDF)
{
joint.m_parentLinkName = std::string(parent_xml->GetText());
}
else
{
const char* pname = parent_xml->Attribute("link");
if (!pname)
{
logger->reportError("no parent link name specified for sensor. this might be the root?");
logger->reportError(joint.m_name.c_str());
return false;
}
else
{
joint.m_parentLinkName = std::string(pname);
}
}
}
joint.m_name = std::string(sensorName).append("_Joint");
joint.m_childLinkName = sensorName;
joint.m_type = URDFFixedJoint;
joint.m_localJointAxis.setValue(0, 0, 0);
// Get transform from Parent Link to Joint Frame
XMLElement* origin_xml = config->FirstChildElement("origin");
if (origin_xml)
{
if (!parseTransform(joint.m_parentLinkToJointTransform, origin_xml, logger))
{
logger->reportError("Malformed origin element for sensor:");
logger->reportError(joint.m_name.c_str());
return false;
}
}
return true;
}
bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
{
// every link has children links and joints, but no parents, so we create a
@ -1516,7 +1582,7 @@ bool UrdfParser::initTreeAndRoot(UrdfModel& model, ErrorLogger* logger)
return true;
}
bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors)
{
XMLDocument xml_doc;
xml_doc.Parse(urdfText);
@ -1647,6 +1713,54 @@ bool UrdfParser::loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceF
}
}
if (parseSensors)
{
// Get all Sensor Elements.
for (XMLElement* sensor_xml = robot_xml->FirstChildElement("sensor"); sensor_xml; sensor_xml = sensor_xml->NextSiblingElement("sensor"))
{
UrdfLink* sensor = new UrdfLink;
UrdfJoint* sensor_joint = new UrdfJoint;
if (parseSensor(m_urdf2Model, *sensor, *sensor_joint, sensor_xml, logger))
{
if (m_urdf2Model.m_links.find(sensor->m_name.c_str()))
{
logger->reportError("Sensor name is not unique, sensor and link names in the same model have to be unique");
logger->reportError(sensor->m_name.c_str());
delete sensor;
delete sensor_joint;
return false;
}
else if (m_urdf2Model.m_joints.find(sensor_joint->m_name.c_str()))
{
logger->reportError("Sensor Joint name is not unique, joint names in the same model have to be unique");
logger->reportError(sensor_joint->m_name.c_str());
delete sensor;
delete sensor_joint;
return false;
}
else
{
m_urdf2Model.m_links.insert(sensor->m_name.c_str(), sensor);
m_urdf2Model.m_joints.insert(sensor_joint->m_name.c_str(), sensor_joint);
}
}
else
{
logger->reportError("failed to parse sensor");
delete sensor;
delete sensor_joint;
return false;
}
}
}
if (m_urdf2Model.m_links.size() == 0)
{
logger->reportWarning("No links found in URDF file");
return false;
}
bool ok(initTreeAndRoot(m_urdf2Model, logger));
if (!ok)
{

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

@ -267,6 +267,7 @@ protected:
bool parseJointDynamics(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseJoint(UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseLink(UrdfModel& model, UrdfLink& link, tinyxml2::XMLElement* config, ErrorLogger* logger);
bool parseSensor(UrdfModel& model, UrdfLink& link, UrdfJoint& joint, tinyxml2::XMLElement* config, ErrorLogger* logger);
public:
UrdfParser();
@ -285,7 +286,13 @@ public:
m_urdfScaling = scaling;
}
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase, bool parseSensors);
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase)
{
return loadUrdf(urdfText, logger, forceFixedBase, false);
}
bool loadSDF(const char* sdfText, ErrorLogger* logger);
int getNumModels() const

2
examples/SharedMemory/PhysicsClientGRPC.cpp
View File

@ -14,7 +14,7 @@ using grpc::Channel;
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "SharedMemory/grpc/ConvertGRPCBullet.h"
using pybullet_grpc::PyBulletAPI;
using pybullet_grpc::grpc::PyBulletAPI;
static unsigned int b3DeserializeInt2(const unsigned char* input)
{

6
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1990,7 +1990,7 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
{
}
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false)
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false, int flags = 0)
{
b3Assert(0);
return false;
@ -3014,7 +3014,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
BulletURDFImporter u2b(m_data->m_guiHelper, m_data->m_pluginManager.getRenderInterface(), globalScaling, flags);
u2b.setEnableTinyRenderer(m_data->m_enableTinyRenderer);
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
bool loadOk = u2b.loadURDF(fileName, useFixedBase, flags);
if (loadOk)
{
@ -5676,7 +5676,7 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
}
}
} //fi
//break;
//break;
}
}
} //if (body && body->m_rigidBody)

1
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -820,6 +820,7 @@ enum eURDF_Flags
URDF_ENABLE_SLEEPING = 2048,
URDF_INITIALIZE_SAT_FEATURES = 4096,
URDF_USE_SELF_COLLISION_INCLUDE_PARENT = 8192,
URDF_PARSE_SENSORS = 16384,
};
enum eUrdfGeomTypes //sync with UrdfParser UrdfGeomTypes