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

Support the <static> field under <model> in SDF to make the model immovable, similar to setting the mass to zero.

Browse Source 
Add joint velocity motors in ImportSDF example.
This commit is contained in:
erwin coumans 2016-06-02 18:04:22 -07:00
parent 5bf125cab5
commit 0b69ba7f61
4 changed files with 88 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp
View File

@ -235,6 +235,60 @@ void ImportSDFSetup::initPhysics()
ConvertURDF2Bullet(u2b,creation, rootTrans,m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix(),true);
mb = creation.getBulletMultiBody();
if (m_useMultiBody && mb )
{
std::string* name = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
m_nameMemory.push_back(name);
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(name->c_str(),name->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
mb->setBaseName(name->c_str());
//create motors for each btMultiBody joint
int numLinks = mb->getNumLinks();
for (int i=0;i<numLinks;i++)
{
int mbLinkIndex = i;
int urdfLinkIndex = creation.m_mb2urdfLink[mbLinkIndex];
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex));
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
#ifdef TEST_MULTIBODY_SERIALIZATION
s->registerNameForPointer(jointName->c_str(),jointName->c_str());
s->registerNameForPointer(linkName->c_str(),linkName->c_str());
#endif//TEST_MULTIBODY_SERIALIZATION
m_nameMemory.push_back(jointName);
m_nameMemory.push_back(linkName);
mb->getLink(i).m_linkName = linkName->c_str();
mb->getLink(i).m_jointName = jointName->c_str();
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
char motorName[1024];
sprintf(motorName,"%s q'", jointName->c_str());
btScalar* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 10.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
//motor->setMaxAppliedImpulse(0);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
}
}

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

@ -267,14 +267,25 @@ void BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
//the link->m_inertia is NOT necessarily aligned with the inertial frame
//so an additional transform might need to be computed
UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
btAssert(linkPtr);
if (linkPtr)
{
UrdfLink* link = *linkPtr;
mass = link->m_inertia.m_mass;
if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
{
mass = 0.f;
localInertiaDiagonal.setValue(0,0,0);
}
else
{
mass = link->m_inertia.m_mass;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
inertialFrame = link->m_inertia.m_linkLocalFrame;
localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
link->m_inertia.m_izz);
}
else
{

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

@ -529,6 +529,8 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
link.m_name = linkName;
if (m_parseSDF) {
TiXmlElement* pose = config->FirstChildElement("pose");
if (0==pose)
{
@ -572,7 +574,7 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
logger->reportWarning(link.m_name.c_str());
}
}
// Multiple Visuals (optional)
for (TiXmlElement* vis_xml = config->FirstChildElement("visual"); vis_xml; vis_xml = vis_xml->NextSiblingElement("visual"))
{
@ -1240,6 +1242,16 @@ bool UrdfParser::loadSDF(const char* sdfText, ErrorLogger* logger)
UrdfModel* localModel = new UrdfModel;
m_tmpModels.push_back(localModel);
TiXmlElement* stat = robot_xml->FirstChildElement("static");
if (0!=stat)
{
int val = int(atof(stat->GetText()));
if (val==1)
{
localModel->m_overrideFixedBase = true;
}
}
// Get robot name
const char *name = robot_xml->Attribute("name");

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

@ -134,6 +134,13 @@ struct UrdfModel
btHashMap<btHashString, UrdfJoint*> m_joints;
btArray<UrdfLink*> m_rootLinks;
bool m_overrideFixedBase;
UrdfModel()
:m_overrideFixedBase(false)
{
m_rootTransformInWorld.setIdentity();
}
};