From f74e3782aaf62c4f4647fca77eaad553bd2c4f1c Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwin.coumans@gmail.com>
Date: Wed, 3 Jun 2015 18:02:15 -0700
Subject: [PATCH] enable prismatic joint motor in URDF demo, by default. Thanks
 to Takuya! prepare for prismatic joint limit for btMultiBody in the URDF
 demo. There is an issue with it, working on fixing this.

---
 examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp | 4 +++-
 examples/Importers/ImportURDFDemo/URDF2Bullet.cpp     | 6 +++++-
 2 files changed, 8 insertions(+), 2 deletions(-)

diff --git a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
index a3160a799..7ffda4b72 100644
--- a/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
+++ b/examples/Importers/ImportURDFDemo/ImportURDFSetup.cpp
@@ -217,7 +217,9 @@ void ImportUrdfSetup::initPhysics()
 				for (int i=0;i<mb->getNumLinks();i++)
 				{
 					int mbLinkIndex = i;
-					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute)
+					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute 
+					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
+					)
 					{
 						if (m_data->m_numMotors<MAX_NUM_MOTORS)
 						{
diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
index c389d6b3c..87adbe87c 100644
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
@@ -5,6 +5,7 @@
 #include "BulletCollision/CollisionShapes/btCompoundShape.h"
 #include "BulletDynamics/Dynamics/btRigidBody.h"
 #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
+#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
 #include "BulletDynamics/ConstraintSolver/btGeneric6DofSpring2Constraint.h"
 #include "URDFImporterInterface.h"
 #include "MultiBodyCreationInterface.h"
@@ -389,7 +390,10 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                                                                    disableParentCollision);
                         
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
-                        
+                       btMultiBodyConstraint* con = new btMultiBodyJointLimitConstraint(cache.m_bulletMultiBody,mbLinkIndex,jointLowerLimit, jointUpperLimit);
+			//world1->addMultiBodyConstraint(con);
+			printf("joint lower limit=%d, upper limit = %f\n", jointLowerLimit, jointUpperLimit);
+ 
                     } else
                     {
                         btGeneric6DofSpring2Constraint* dof6 = creation.allocateGeneric6DofSpring2Constraint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);