Expose optional "globalScaling" factor to pybullet.loadURDF and pybullet.loadSDF. This will scale the visual, collision shapes and transform locations.
Fix two_cubes.sdf (was lacking collision shape for second cube)
This commit is contained in:
@@ -142,7 +142,7 @@
|
|||||||
</link>
|
</link>
|
||||||
</model>
|
</model>
|
||||||
<model name='unit_box_0_clone'>
|
<model name='unit_box_0_clone'>
|
||||||
<pose frame=''>0.105158 -4.55002 0.499995 -2.89297 -0.988287 -3.14159</pose>
|
<pose frame=''>0.105158 -4.55002 1.499995 -2.89297 -0.988287 -3.14159</pose>
|
||||||
<link name='unit_box_0::link'>
|
<link name='unit_box_0::link'>
|
||||||
<inertial>
|
<inertial>
|
||||||
<mass>1</mass>
|
<mass>1</mass>
|
||||||
@@ -155,7 +155,26 @@
|
|||||||
<izz>0.166667</izz>
|
<izz>0.166667</izz>
|
||||||
</inertia>
|
</inertia>
|
||||||
</inertial>
|
</inertial>
|
||||||
|
<collision name='collision1'>
|
||||||
|
<geometry>
|
||||||
|
<box>
|
||||||
|
<size>1 1 1</size>
|
||||||
|
</box>
|
||||||
|
</geometry>
|
||||||
|
<max_contacts>10</max_contacts>
|
||||||
|
<surface>
|
||||||
|
<contact>
|
||||||
|
<ode/>
|
||||||
|
</contact>
|
||||||
|
<bounce/>
|
||||||
|
<friction>
|
||||||
|
<torsional>
|
||||||
|
<ode/>
|
||||||
|
</torsional>
|
||||||
|
<ode/>
|
||||||
|
</friction>
|
||||||
|
</surface>
|
||||||
|
</collision>
|
||||||
<visual name='visual'>
|
<visual name='visual'>
|
||||||
<geometry>
|
<geometry>
|
||||||
<mesh>
|
<mesh>
|
||||||
|
|||||||
@@ -187,7 +187,7 @@ void ImportSDFSetup::initPhysics()
|
|||||||
m_dynamicsWorld->setGravity(gravity);
|
m_dynamicsWorld->setGravity(gravity);
|
||||||
|
|
||||||
|
|
||||||
BulletURDFImporter u2b(m_guiHelper, 0);
|
BulletURDFImporter u2b(m_guiHelper, 0,1);
|
||||||
|
|
||||||
bool loadOk = u2b.loadSDF(m_fileName);
|
bool loadOk = u2b.loadSDF(m_fileName);
|
||||||
|
|
||||||
|
|||||||
@@ -69,6 +69,12 @@ ATTRIBUTE_ALIGNED16(struct) BulletURDFInternalData
|
|||||||
{
|
{
|
||||||
m_pathPrefix[0] = 0;
|
m_pathPrefix[0] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
void setGlobalScaling(btScalar scaling)
|
||||||
|
{
|
||||||
|
m_urdfParser.setGlobalScaling(scaling);
|
||||||
|
}
|
||||||
|
|
||||||
};
|
};
|
||||||
|
|
||||||
void BulletURDFImporter::printTree()
|
void BulletURDFImporter::printTree()
|
||||||
@@ -76,10 +82,10 @@ void BulletURDFImporter::printTree()
|
|||||||
// btAssert(0);
|
// btAssert(0);
|
||||||
}
|
}
|
||||||
|
|
||||||
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter)
|
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling)
|
||||||
{
|
{
|
||||||
m_data = new BulletURDFInternalData;
|
m_data = new BulletURDFInternalData;
|
||||||
|
m_data->setGlobalScaling(globalScaling);
|
||||||
m_data->m_guiHelper = helper;
|
m_data->m_guiHelper = helper;
|
||||||
m_data->m_customVisualShapesConverter = customConverter;
|
m_data->m_customVisualShapesConverter = customConverter;
|
||||||
|
|
||||||
|
|||||||
@@ -15,10 +15,11 @@ class BulletURDFImporter : public URDFImporterInterface
|
|||||||
|
|
||||||
public:
|
public:
|
||||||
|
|
||||||
BulletURDFImporter(struct GUIHelperInterface* guiHelper, LinkVisualShapesConverter* customConverter);
|
BulletURDFImporter::BulletURDFImporter(struct GUIHelperInterface* helper, LinkVisualShapesConverter* customConverter, btScalar globalScaling);
|
||||||
|
|
||||||
virtual ~BulletURDFImporter();
|
virtual ~BulletURDFImporter();
|
||||||
|
|
||||||
|
|
||||||
virtual bool loadURDF(const char* fileName, bool forceFixedBase = false);
|
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
|
//warning: some quick test to load SDF: we 'activate' a model, so we can re-use URDF code path
|
||||||
|
|||||||
@@ -194,7 +194,7 @@ void ImportUrdfSetup::initPhysics()
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
BulletURDFImporter u2b(m_guiHelper, 0);
|
BulletURDFImporter u2b(m_guiHelper, 0,1);
|
||||||
|
|
||||||
|
|
||||||
bool loadOk = u2b.loadURDF(m_fileName);
|
bool loadOk = u2b.loadURDF(m_fileName);
|
||||||
|
|||||||
@@ -173,6 +173,7 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
|
|||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
btScalar tmpUrdfScaling=2;
|
||||||
|
|
||||||
|
|
||||||
void ConvertURDF2BulletInternal(
|
void ConvertURDF2BulletInternal(
|
||||||
|
|||||||
@@ -6,7 +6,8 @@
|
|||||||
|
|
||||||
UrdfParser::UrdfParser()
|
UrdfParser::UrdfParser()
|
||||||
:m_parseSDF(false),
|
: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
|
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();
|
tr.setIdentity();
|
||||||
|
|
||||||
|
btVector3 vec(0,0,0);
|
||||||
if (parseSDF)
|
if (parseSDF)
|
||||||
{
|
{
|
||||||
parseVector3(tr.getOrigin(),std::string(xml->GetText()),logger);
|
parseVector3(vec,std::string(xml->GetText()),logger);
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
const char* xyz_str = xml->Attribute("xyz");
|
const char* xyz_str = xml->Attribute("xyz");
|
||||||
if (xyz_str)
|
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)
|
if (parseSDF)
|
||||||
{
|
{
|
||||||
@@ -344,7 +347,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
|||||||
return false;
|
return false;
|
||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
geom.m_sphereRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
geom.m_sphereRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
else if (type_name == "box")
|
else if (type_name == "box")
|
||||||
@@ -359,6 +362,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
parseVector3(geom.m_boxSize,size->GetText(),logger);
|
parseVector3(geom.m_boxSize,size->GetText(),logger);
|
||||||
|
geom.m_boxSize *= m_urdfScaling;
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
@@ -369,6 +373,7 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
|||||||
} else
|
} else
|
||||||
{
|
{
|
||||||
parseVector3(geom.m_boxSize,shape->Attribute("size"),logger);
|
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;
|
return false;
|
||||||
}
|
}
|
||||||
geom.m_hasFromTo = false;
|
geom.m_hasFromTo = false;
|
||||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||||
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
|
||||||
|
|
||||||
}
|
}
|
||||||
else if (type_name == "capsule")
|
else if (type_name == "capsule")
|
||||||
@@ -396,8 +401,8 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
geom.m_hasFromTo = false;
|
geom.m_hasFromTo = false;
|
||||||
geom.m_capsuleRadius = urdfLexicalCast<double>(shape->Attribute("radius"));
|
geom.m_capsuleRadius = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("radius"));
|
||||||
geom.m_capsuleHeight = urdfLexicalCast<double>(shape->Attribute("length"));
|
geom.m_capsuleHeight = m_urdfScaling * urdfLexicalCast<double>(shape->Attribute("length"));
|
||||||
}
|
}
|
||||||
else if (type_name == "mesh")
|
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())
|
if (fn.empty())
|
||||||
{
|
{
|
||||||
logger->reportError("Mesh filename is 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);
|
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
|
// Get joint effort limit
|
||||||
const char* effort_str = config->Attribute("effort");
|
const char* effort_str = config->Attribute("effort");
|
||||||
|
|||||||
@@ -251,7 +251,8 @@ protected:
|
|||||||
bool m_parseSDF;
|
bool m_parseSDF;
|
||||||
int m_activeSdfModel;
|
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 parseInertia(UrdfInertia& inertia, class TiXmlElement* config, ErrorLogger* logger);
|
||||||
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
|
bool parseGeometry(UrdfGeometry& geom, class TiXmlElement* g, ErrorLogger* logger);
|
||||||
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
|
bool parseVisual(UrdfModel& model, UrdfVisual& visual, class TiXmlElement* config, ErrorLogger* logger);
|
||||||
@@ -277,6 +278,11 @@ public:
|
|||||||
{
|
{
|
||||||
return m_parseSDF;
|
return m_parseSDF;
|
||||||
}
|
}
|
||||||
|
void setGlobalScaling(btScalar scaling)
|
||||||
|
{
|
||||||
|
m_urdfScaling = scaling;
|
||||||
|
}
|
||||||
|
|
||||||
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
|
bool loadUrdf(const char* urdfText, ErrorLogger* logger, bool forceFixedBase);
|
||||||
bool loadSDF(const char* sdfText, ErrorLogger* logger);
|
bool loadSDF(const char* sdfText, ErrorLogger* logger);
|
||||||
|
|
||||||
|
|||||||
@@ -156,7 +156,7 @@ void InverseDynamicsExample::initPhysics()
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
BulletURDFImporter u2b(m_guiHelper,0);
|
BulletURDFImporter u2b(m_guiHelper,0,1);
|
||||||
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
|
bool loadOk = u2b.loadURDF("kuka_iiwa/model.urdf");// lwr / kuka.urdf");
|
||||||
if (loadOk)
|
if (loadOk)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -223,6 +223,18 @@ int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle,
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
|
||||||
|
{
|
||||||
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
b3Assert(command);
|
||||||
|
b3Assert(command->m_type == CMD_LOAD_URDF);
|
||||||
|
command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING;
|
||||||
|
command->m_urdfArguments.m_globalScaling = globalScaling;
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
|
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody)
|
||||||
{
|
{
|
||||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
@@ -234,6 +246,19 @@ int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, i
|
|||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling)
|
||||||
|
{
|
||||||
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
b3Assert(command);
|
||||||
|
b3Assert(command->m_type == CMD_LOAD_SDF);
|
||||||
|
command->m_updateFlags |=URDF_ARGS_USE_GLOBAL_SCALING;
|
||||||
|
command->m_sdfArguments.m_globalScaling = globalScaling;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
|
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase)
|
||||||
{
|
{
|
||||||
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
|
||||||
|
|||||||
@@ -267,6 +267,9 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
|
|||||||
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
||||||
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
|
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
|
||||||
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
|
int b3LoadUrdfCommandSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
|
||||||
|
int b3LoadUrdfCommandSetGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
b3SharedMemoryCommandHandle b3LoadBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
||||||
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
b3SharedMemoryCommandHandle b3SaveBulletCommandInit(b3PhysicsClientHandle physClient, const char* fileName);
|
||||||
@@ -301,6 +304,9 @@ int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle status
|
|||||||
|
|
||||||
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
||||||
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
int b3LoadSdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
|
||||||
|
int b3LoadSdfCommandSetUseGlobalScaling(b3SharedMemoryCommandHandle commandHandle, double globalScaling);
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
b3SharedMemoryCommandHandle b3SaveWorldCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
|
||||||
|
|
||||||
|
|||||||
@@ -2284,7 +2284,7 @@ bool PhysicsServerCommandProcessor::loadMjcf(const char* fileName, char* bufferS
|
|||||||
return loadOk;
|
return loadOk;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags)
|
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling)
|
||||||
{
|
{
|
||||||
btAssert(m_data->m_dynamicsWorld);
|
btAssert(m_data->m_dynamicsWorld);
|
||||||
if (!m_data->m_dynamicsWorld)
|
if (!m_data->m_dynamicsWorld)
|
||||||
@@ -2295,7 +2295,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
|||||||
|
|
||||||
m_data->m_sdfRecentLoadedBodies.clear();
|
m_data->m_sdfRecentLoadedBodies.clear();
|
||||||
|
|
||||||
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling);
|
||||||
|
|
||||||
bool forceFixedBase = false;
|
bool forceFixedBase = false;
|
||||||
bool loadOk =u2b.loadSDF(fileName,forceFixedBase);
|
bool loadOk =u2b.loadSDF(fileName,forceFixedBase);
|
||||||
@@ -2311,7 +2311,7 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe
|
|||||||
|
|
||||||
|
|
||||||
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
|
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
|
||||||
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags)
|
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling)
|
||||||
{
|
{
|
||||||
m_data->m_sdfRecentLoadedBodies.clear();
|
m_data->m_sdfRecentLoadedBodies.clear();
|
||||||
*bodyUniqueIdPtr = -1;
|
*bodyUniqueIdPtr = -1;
|
||||||
@@ -2326,7 +2326,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter, globalScaling);
|
||||||
|
|
||||||
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
|
bool loadOk = u2b.loadURDF(fileName, useFixedBase);
|
||||||
|
|
||||||
@@ -3579,7 +3579,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true;
|
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (sdfArgs.m_useMultiBody!=0) : true;
|
||||||
|
|
||||||
int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA
|
int flags = CUF_USE_SDF; //CUF_USE_URDF_INERTIA
|
||||||
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags);
|
btScalar globalScaling = 1.f;
|
||||||
|
if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
|
||||||
|
{
|
||||||
|
globalScaling = sdfArgs.m_globalScaling;
|
||||||
|
}
|
||||||
|
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes, useMultiBody, flags, globalScaling);
|
||||||
if (completedOk)
|
if (completedOk)
|
||||||
{
|
{
|
||||||
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
|
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
|
||||||
@@ -4016,10 +4021,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
|||||||
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true;
|
bool useMultiBody=(clientCmd.m_updateFlags & URDF_ARGS_USE_MULTIBODY) ? (urdfArgs.m_useMultiBody!=0) : true;
|
||||||
bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false;
|
bool useFixedBase = (clientCmd.m_updateFlags & URDF_ARGS_USE_FIXED_BASE) ? (urdfArgs.m_useFixedBase!=0): false;
|
||||||
int bodyUniqueId;
|
int bodyUniqueId;
|
||||||
|
btScalar globalScaling = 1.f;
|
||||||
|
if (clientCmd.m_updateFlags & URDF_ARGS_USE_GLOBAL_SCALING)
|
||||||
|
{
|
||||||
|
globalScaling = urdfArgs.m_globalScaling;
|
||||||
|
}
|
||||||
//load the actual URDF and send a report: completed or failed
|
//load the actual URDF and send a report: completed or failed
|
||||||
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
|
bool completedOk = loadUrdf(urdfArgs.m_urdfFileName,
|
||||||
initialPos,initialOrn,
|
initialPos,initialOrn,
|
||||||
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags);
|
useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes, urdfFlags, globalScaling);
|
||||||
|
|
||||||
if (completedOk && bodyUniqueId>=0)
|
if (completedOk && bodyUniqueId>=0)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -27,10 +27,10 @@ protected:
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
|
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags, btScalar globalScaling);
|
||||||
|
|
||||||
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
|
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
|
||||||
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags=0);
|
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes, int flags, btScalar globalScaling);
|
||||||
|
|
||||||
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
|
bool loadMjcf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes, bool useMultiBody, int flags);
|
||||||
|
|
||||||
|
|||||||
@@ -65,6 +65,7 @@ struct SdfArgs
|
|||||||
{
|
{
|
||||||
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
|
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
|
||||||
int m_useMultiBody;
|
int m_useMultiBody;
|
||||||
|
double m_globalScaling;
|
||||||
};
|
};
|
||||||
|
|
||||||
struct FileArgs
|
struct FileArgs
|
||||||
@@ -79,7 +80,8 @@ enum EnumUrdfArgsUpdateFlags
|
|||||||
URDF_ARGS_INITIAL_ORIENTATION=4,
|
URDF_ARGS_INITIAL_ORIENTATION=4,
|
||||||
URDF_ARGS_USE_MULTIBODY=8,
|
URDF_ARGS_USE_MULTIBODY=8,
|
||||||
URDF_ARGS_USE_FIXED_BASE=16,
|
URDF_ARGS_USE_FIXED_BASE=16,
|
||||||
URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32
|
URDF_ARGS_HAS_CUSTOM_URDF_FLAGS = 32,
|
||||||
|
URDF_ARGS_USE_GLOBAL_SCALING =64,
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
@@ -91,6 +93,7 @@ struct UrdfArgs
|
|||||||
int m_useMultiBody;
|
int m_useMultiBody;
|
||||||
int m_useFixedBase;
|
int m_useFixedBase;
|
||||||
int m_urdfFlags;
|
int m_urdfFlags;
|
||||||
|
double m_globalScaling;
|
||||||
};
|
};
|
||||||
|
|
||||||
|
|
||||||
|
|||||||
@@ -929,14 +929,14 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
|
|||||||
int physicsClientId = 0;
|
int physicsClientId = 0;
|
||||||
int flags = 0;
|
int flags = 0;
|
||||||
|
|
||||||
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","physicsClientId", NULL};
|
static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "useMaximalCoordinates", "useFixedBase", "flags","globalScaling", "physicsClientId", NULL};
|
||||||
|
|
||||||
static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL};
|
static char* kwlistBackwardCompatible4[] = {"fileName", "startPosX", "startPosY", "startPosZ", NULL};
|
||||||
static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL};
|
static char* kwlistBackwardCompatible8[] = {"fileName", "startPosX", "startPosY", "startPosZ", "startOrnX", "startOrnY", "startOrnZ", "startOrnW", NULL};
|
||||||
|
|
||||||
int bodyUniqueId= -1;
|
int bodyUniqueId= -1;
|
||||||
const char* urdfFileName = "";
|
const char* urdfFileName = "";
|
||||||
|
double globalScaling = -1;
|
||||||
double startPosX = 0.0;
|
double startPosX = 0.0;
|
||||||
double startPosY = 0.0;
|
double startPosY = 0.0;
|
||||||
double startPosZ = 0.0;
|
double startPosZ = 0.0;
|
||||||
@@ -976,7 +976,7 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
|
|||||||
double basePos[3];
|
double basePos[3];
|
||||||
double baseOrn[4];
|
double baseOrn[4];
|
||||||
|
|
||||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiii", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &physicsClientId))
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOiiidi", kwlist, &urdfFileName, &basePosObj, &baseOrnObj, &useMaximalCoordinates, &useFixedBase, &flags, &globalScaling, &physicsClientId))
|
||||||
{
|
{
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
@@ -1040,7 +1040,10 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args, PyObject* key
|
|||||||
{
|
{
|
||||||
b3LoadUrdfCommandSetUseFixedBase(command, 1);
|
b3LoadUrdfCommandSetUseFixedBase(command, 1);
|
||||||
}
|
}
|
||||||
|
if (globalScaling>0)
|
||||||
|
{
|
||||||
|
b3LoadUrdfCommandSetGlobalScaling(command,globalScaling);
|
||||||
|
}
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
if (statusType != CMD_URDF_LOADING_COMPLETED)
|
if (statusType != CMD_URDF_LOADING_COMPLETED)
|
||||||
@@ -1071,10 +1074,11 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
|
|||||||
int statusType;
|
int statusType;
|
||||||
b3SharedMemoryCommandHandle commandHandle;
|
b3SharedMemoryCommandHandle commandHandle;
|
||||||
b3PhysicsClientHandle sm = 0;
|
b3PhysicsClientHandle sm = 0;
|
||||||
|
double globalScaling = -1;
|
||||||
|
|
||||||
int physicsClientId = 0;
|
int physicsClientId = 0;
|
||||||
static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "physicsClientId", NULL};
|
static char* kwlist[] = {"sdfFileName", "useMaximalCoordinates", "globalScaling", "physicsClientId", NULL};
|
||||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|ii", kwlist, &sdfFileName, &useMaximalCoordinates, &physicsClientId))
|
if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|idi", kwlist, &sdfFileName, &useMaximalCoordinates, &globalScaling, &physicsClientId))
|
||||||
{
|
{
|
||||||
return NULL;
|
return NULL;
|
||||||
}
|
}
|
||||||
@@ -1090,7 +1094,10 @@ static PyObject* pybullet_loadSDF(PyObject* self, PyObject* args, PyObject* keyw
|
|||||||
{
|
{
|
||||||
b3LoadSdfCommandSetUseMultiBody(commandHandle,0);
|
b3LoadSdfCommandSetUseMultiBody(commandHandle,0);
|
||||||
}
|
}
|
||||||
|
if (globalScaling > 0)
|
||||||
|
{
|
||||||
|
b3LoadSdfCommandSetUseGlobalScaling(commandHandle,globalScaling);
|
||||||
|
}
|
||||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||||
statusType = b3GetStatusType(statusHandle);
|
statusType = b3GetStatusType(statusHandle);
|
||||||
if (statusType != CMD_SDF_LOADING_COMPLETED)
|
if (statusType != CMD_SDF_LOADING_COMPLETED)
|
||||||
|
|||||||
Reference in New Issue
Block a user