From 0b69ba7f610824b653c8f86493c363e73c1452d3 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Thu, 2 Jun 2016 18:04:22 -0700 Subject: [PATCH] Support the field under in SDF to make the model immovable, similar to setting the mass to zero. Add joint velocity motors in ImportSDF example. --- .../ImportSDFDemo/ImportSDFSetup.cpp | 54 +++++++++++++++++++ .../ImportURDFDemo/BulletUrdfImporter.cpp | 17 ++++-- .../Importers/ImportURDFDemo/UrdfParser.cpp | 14 ++++- .../Importers/ImportURDFDemo/UrdfParser.h | 7 +++ 4 files changed, 88 insertions(+), 4 deletions(-) diff --git a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp index 54883298b..f55cc9758 100644 --- a/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp +++ b/examples/Importers/ImportSDFDemo/ImportSDFSetup.cpp @@ -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;iregisterNameForPointer(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_numMotorsc_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++; + } + } + + } + } } diff --git a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp index 7e9624d64..4838079a0 100644 --- a/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp +++ b/examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp @@ -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 { diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index 60b40b512..430593b5e 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -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"); diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.h b/examples/Importers/ImportURDFDemo/UrdfParser.h index 1a855f281..b83c0b3c8 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.h +++ b/examples/Importers/ImportURDFDemo/UrdfParser.h @@ -134,6 +134,13 @@ struct UrdfModel btHashMap m_joints; btArray m_rootLinks; + bool m_overrideFixedBase; + + UrdfModel() + :m_overrideFixedBase(false) + { + m_rootTransformInWorld.setIdentity(); + } };