Merge pull request #343 from erwincoumans/master

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

4 files changed, 96 insertions, 9 deletions

diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp
index 470a0edb7..a777bee40 100644
--- a/Demos3/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/Demos3/ImportURDFDemo/ImportURDFSetup.cpp
@@ -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)
+    {
+    }
+    
+    btScalar 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());
+                            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;
+                            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.);
 	}
diff --git a/Demos3/ImportURDFDemo/ImportURDFSetup.h b/Demos3/ImportURDFDemo/ImportURDFSetup.h
index 482a36915..093079bbb 100644
--- a/Demos3/ImportURDFDemo/ImportURDFSetup.h
+++ b/Demos3/ImportURDFDemo/ImportURDFSetup.h
@@ -8,6 +8,8 @@ class ImportUrdfSetup : public CommonMultiBodySetup
 {
     char m_fileName[1024];
     
+    struct ImportUrdfInternalData* m_data;
+    
 public:
     ImportUrdfSetup();
     virtual ~ImportUrdfSetup();
diff --git a/Demos3/ImportURDFDemo/URDF2Bullet.cpp b/Demos3/ImportURDFDemo/URDF2Bullet.cpp
index 656996187..510fe9915 100644
--- a/Demos3/ImportURDFDemo/URDF2Bullet.cpp
+++ b/Demos3/ImportURDFDemo/URDF2Bullet.cpp
@@ -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
                     {
diff --git a/Demos3/ImportURDFDemo/URDF2Bullet.h b/Demos3/ImportURDFDemo/URDF2Bullet.h
index 9f4f83472..2d2a17747 100644
--- a/Demos3/ImportURDFDemo/URDF2Bullet.h
+++ b/Demos3/ImportURDFDemo/URDF2Bullet.h
@@ -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;
 };