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

re-enable auto-joint velocity target motors in URDF multibody import

Browse Source
This commit is contained in:
Erwin Coumans
2015-03-20 14:54:34 -07:00
parent 20a270bc94
commit 54b2bd9ff1
4 changed files with 96 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

96
Demos3/ImportURDFDemo/ImportURDFSetup.cpp
View File

@@ -8,6 +8,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "Bullet3Common/b3FileUtils.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"//to create a tesselation of a generic btConvexShape
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
static int bodyCollisionFilterGroup=btBroadphaseProxy::CharacterFilter;
static int bodyCollisionFilterMask=btBroadphaseProxy::AllFilter&(~btBroadphaseProxy::CharacterFilter);
@@ -37,6 +38,11 @@ class MyURDF2Bullet : public URDF2Bullet
mutable btMultiBody* m_bulletMultiBody;
public:
mutable btAlignedObjectArray<int> m_urdf2mbLink;
mutable btAlignedObjectArray<int> m_mb2urdfLink;
MyURDF2Bullet(my_shared_ptr<ModelInterface> robot,GraphicsPhysicsBridge& gfxBridge)
:m_robot(robot),
m_gfxBridge(gfxBridge),
@@ -49,6 +55,9 @@ public:
{
m_links[i]->m_link_index = i;
}
m_urdf2mbLink.resize(m_links.size(),-2);
m_mb2urdfLink.resize(m_links.size(),-2);
}
virtual int getRootLinkIndex() const
@@ -79,6 +88,11 @@ public:
return n;
}
virtual std::string getJointName(int linkIndex) const
{
return m_links[linkIndex]->parent_joint->name;
}
virtual void getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
if ((*m_links[linkIndex]).inertial)
@@ -231,6 +245,11 @@ public:
return c;
}
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const
{
m_urdf2mbLink[urdfLinkIndex] = mbLinkIndex;
m_mb2urdfLink[mbLinkIndex] = urdfLinkIndex;
}
virtual void createRigidBodyGraphicsInstance(int linkIndex, btRigidBody* body, const btVector3& colorRgba, int graphicsIndex) const
{
@@ -254,12 +273,30 @@ public:
btAlignedObjectArray<std::string> gFileNameArray;
#define MAX_NUM_MOTORS 1024
struct ImportUrdfInternalData
{
ImportUrdfInternalData()
:m_numMotors(0)
{
}
float m_motorTargetVelocities[MAX_NUM_MOTORS];
btMultiBodyJointMotor* m_jointMotors [MAX_NUM_MOTORS];
int m_numMotors;
};
ImportUrdfSetup::ImportUrdfSetup()
{
static int count = 0;
gFileNameArray.clear();
gFileNameArray.push_back("r2d2.urdf");
m_data = new ImportUrdfInternalData;
//load additional urdf file names from file
FILE* f = fopen("urdf_files.txt","r");
@@ -291,7 +328,7 @@ ImportUrdfSetup::ImportUrdfSetup()
ImportUrdfSetup::~ImportUrdfSetup()
{
delete m_data;
}
static btVector4 colors[4] =
@@ -1523,6 +1560,12 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
mappings.m_urdfLinkIndices2BulletLinkIndices.resize(numJoints+1,-2);//root and child links (=1+numJoints)
URDFvisual2BulletCollisionShape(root_link, gfxBridge, identityTrans,m_dynamicsWorld,mappings,pathPrefix);
mb = mappings.m_bulletMultiBody;
if (useFeatherstone)
{
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
}
} else
{
@@ -1532,19 +1575,51 @@ void ImportUrdfSetup::initPhysics(GraphicsPhysicsBridge& gfxBridge)
printf("urdf root link index = %d\n",rootLinkIndex);
ConvertURDF2Bullet(u2b,identityTrans,m_dynamicsWorld,useFeatherstone,pathPrefix);
mb = u2b.getBulletMultiBody();
if (useFeatherstone)
{
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
//create motors for each joint
for (int i=0;i<mb->getNumLinks();i++)
{
int mbLinkIndex = i;
if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute)
{
if (m_data->m_numMotors<MAX_NUM_MOTORS)
{
int urdfLinkIndex = u2b.m_mb2urdfLink[mbLinkIndex];
std::string jointName = u2b.getJointName(urdfLinkIndex);
char motorName[1024];
sprintf(motorName,"%s q'", jointName.c_str());
float* motorVel = &m_data->m_motorTargetVelocities[m_data->m_numMotors];
*motorVel = 0.f;
SliderParams slider(motorName,motorVel);
slider.m_minVal=-4;
slider.m_maxVal=4;
gfxBridge.getParameterInterface()->registerSliderFloatParameter(slider);
float maxMotorImpulse = 0.1f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,0,0,maxMotorImpulse);
m_data->m_jointMotors[m_data->m_numMotors]=motor;
m_dynamicsWorld->addMultiBodyConstraint(motor);
m_data->m_numMotors++;
}
}
}
}
}
if (useFeatherstone)
{
mb->setHasSelfCollision(false);
mb->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(mb);
}
}
//the btMultiBody support is work-in-progress :-)
useFeatherstone = !useFeatherstone;
//useFeatherstone = !useFeatherstone;
printf("numJoints/DOFS = %d\n", numJoints);
bool createGround=true;
@@ -1575,6 +1650,11 @@ void ImportUrdfSetup::stepSimulation(float deltaTime)
{
if (m_dynamicsWorld)
{
for (int i=0;i<m_data->m_numMotors;i++)
{
m_data->m_jointMotors[i]->setVelocityTarget(m_data->m_motorTargetVelocities[i]);
}
//the maximal coordinates/iterative MLCP solver requires a smallish timestep to converge
m_dynamicsWorld->stepSimulation(deltaTime,10,1./240.);
}

2
Demos3/ImportURDFDemo/ImportURDFSetup.h
View File

@@ -8,6 +8,8 @@ class ImportUrdfSetup : public CommonMultiBodySetup
{
char m_fileName[1024];
struct ImportUrdfInternalData* m_data;
public:
ImportUrdfSetup();
virtual ~ImportUrdfSetup();

4
Demos3/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -285,6 +285,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
btQuaternion rot = offsetInA.inverse().getRotation();//parent2joint.inverse().getRotation();
cache.m_bulletMultiBody->setupFixed(mbLinkIndex, mass, localInertiaDiagonal, mbParentIndex,
rot*offsetInB.getRotation(), offsetInA.getOrigin(),-offsetInB.getOrigin(),disableParentCollision);
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
btMatrix3x3 rm(rot);
btScalar y,p,r;
@@ -325,6 +326,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
-offsetInB.getOrigin(),
disableParentCollision);
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else
{
@@ -387,7 +389,7 @@ void ConvertURDF2BulletInternal(const URDF2Bullet& u2b, URDF2BulletCachedData& c
-offsetInB.getOrigin(),
disableParentCollision);
u2b.addLinkMapping(urdfLinkIndex,mbLinkIndex);
} else
{

3
Demos3/ImportURDFDemo/URDF2Bullet.h
View File

@@ -27,6 +27,8 @@ public:
///pure virtual interfaces, precondition is a valid linkIndex (you can assert/terminate if the linkIndex is out of range)
virtual std::string getLinkName(int linkIndex) const =0;
virtual std::string getJointName(int linkIndex) const = 0;
//fill mass and inertial data. If inertial data is missing, please initialize mass, inertia to sensitive values, and inertialFrame to identity.
virtual void getMassAndInertia(int urdfLinkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const =0;
@@ -54,6 +56,7 @@ public:
virtual class btMultiBodyLinkCollider* allocateMultiBodyLinkCollider(int urdfLinkIndex, int mbLinkIndex, btMultiBody* body) const = 0;
virtual void addLinkMapping(int urdfLinkIndex, int mbLinkIndex) const = 0;
};