add infrastructure float btMultiBodyLink m_jointDamping, m_jointFriction (actual damping/friction is in a separate commit)

add door.urdf for testing damping/friction

Extras/Serialize/BulletFileLoader/autogenerated/bullet.h

@@ -67,6 +67,8 @@ typedef struct bInvalidHandle {
     class btConvexHullShapeData;
     class btCollisionObjectDoubleData;
     class btCollisionObjectFloatData;
+    class btContactSolverInfoDoubleData;
+    class btContactSolverInfoFloatData;
     class btDynamicsWorldDoubleData;
     class btDynamicsWorldFloatData;
     class btRigidBodyFloatData;
@@ -93,8 +95,6 @@ typedef struct bInvalidHandle {
     class btSliderConstraintDoubleData;
     class btGearConstraintFloatData;
     class btGearConstraintDoubleData;
-    class btContactSolverInfoDoubleData;
-    class btContactSolverInfoFloatData;
     class SoftBodyMaterialData;
     class SoftBodyNodeData;
     class SoftBodyLinkData;
@@ -600,6 +600,81 @@ typedef struct bInvalidHandle {
     };
 
 
+// -------------------------------------------------- //
+    class btContactSolverInfoDoubleData
+    {
+    public:
+        double m_tau;
+        double m_damping;
+        double m_friction;
+        double m_timeStep;
+        double m_restitution;
+        double m_maxErrorReduction;
+        double m_sor;
+        double m_erp;
+        double m_erp2;
+        double m_globalCfm;
+        double m_splitImpulsePenetrationThreshold;
+        double m_splitImpulseTurnErp;
+        double m_linearSlop;
+        double m_warmstartingFactor;
+        double m_maxGyroscopicForce;
+        double m_singleAxisRollingFrictionThreshold;
+        int m_numIterations;
+        int m_solverMode;
+        int m_restingContactRestitutionThreshold;
+        int m_minimumSolverBatchSize;
+        int m_splitImpulse;
+        char m_padding[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btContactSolverInfoFloatData
+    {
+    public:
+        float m_tau;
+        float m_damping;
+        float m_friction;
+        float m_timeStep;
+        float m_restitution;
+        float m_maxErrorReduction;
+        float m_sor;
+        float m_erp;
+        float m_erp2;
+        float m_globalCfm;
+        float m_splitImpulsePenetrationThreshold;
+        float m_splitImpulseTurnErp;
+        float m_linearSlop;
+        float m_warmstartingFactor;
+        float m_maxGyroscopicForce;
+        float m_singleAxisRollingFrictionThreshold;
+        int m_numIterations;
+        int m_solverMode;
+        int m_restingContactRestitutionThreshold;
+        int m_minimumSolverBatchSize;
+        int m_splitImpulse;
+        char m_padding[4];
+    };
+
+
+// -------------------------------------------------- //
+    class btDynamicsWorldDoubleData
+    {
+    public:
+        btContactSolverInfoDoubleData m_solverInfo;
+        btVector3DoubleData m_gravity;
+    };
+
+
+// -------------------------------------------------- //
+    class btDynamicsWorldFloatData
+    {
+    public:
+        btContactSolverInfoFloatData m_solverInfo;
+        btVector3FloatData m_gravity;
+    };
+
 
 // -------------------------------------------------- //
     class btRigidBodyFloatData
@@ -1061,82 +1136,6 @@ typedef struct bInvalidHandle {
     };
 
 
-// -------------------------------------------------- //
-    class btContactSolverInfoDoubleData
-    {
-    public:
-        double m_tau;
-        double m_damping;
-        double m_friction;
-        double m_timeStep;
-        double m_restitution;
-        double m_maxErrorReduction;
-        double m_sor;
-        double m_erp;
-        double m_erp2;
-        double m_globalCfm;
-        double m_splitImpulsePenetrationThreshold;
-        double m_splitImpulseTurnErp;
-        double m_linearSlop;
-        double m_warmstartingFactor;
-        double m_maxGyroscopicForce;
-        double m_singleAxisRollingFrictionThreshold;
-        int m_numIterations;
-        int m_solverMode;
-        int m_restingContactRestitutionThreshold;
-        int m_minimumSolverBatchSize;
-        int m_splitImpulse;
-        char m_padding[4];
-    };
-
-
-// -------------------------------------------------- //
-    class btContactSolverInfoFloatData
-    {
-    public:
-        float m_tau;
-        float m_damping;
-        float m_friction;
-        float m_timeStep;
-        float m_restitution;
-        float m_maxErrorReduction;
-        float m_sor;
-        float m_erp;
-        float m_erp2;
-        float m_globalCfm;
-        float m_splitImpulsePenetrationThreshold;
-        float m_splitImpulseTurnErp;
-        float m_linearSlop;
-        float m_warmstartingFactor;
-        float m_maxGyroscopicForce;
-        float m_singleAxisRollingFrictionThreshold;
-        int m_numIterations;
-        int m_solverMode;
-        int m_restingContactRestitutionThreshold;
-        int m_minimumSolverBatchSize;
-        int m_splitImpulse;
-        char m_padding[4];
-    };
-
-	// -------------------------------------------------- //
-    class btDynamicsWorldDoubleData
-    {
-    public:
-        btContactSolverInfoDoubleData m_solverInfo;
-        btVector3DoubleData m_gravity;
-    };
-
-
-// -------------------------------------------------- //
-    class btDynamicsWorldFloatData
-    {
-    public:
-        btContactSolverInfoFloatData m_solverInfo;
-        btVector3FloatData m_gravity;
-    };
-
-
-
 // -------------------------------------------------- //
     class SoftBodyMaterialData
     {
@@ -1366,6 +1365,8 @@ typedef struct bInvalidHandle {
         double m_jointPos[7];
         double m_jointVel[6];
         double m_jointTorque[6];
+        double m_jointDamping;
+        double m_jointFriction;
     };
 
 
@@ -1390,6 +1391,8 @@ typedef struct bInvalidHandle {
         float m_jointVel[6];
         float m_jointTorque[6];
         int m_posVarCount;
+        float m_jointDamping;
+        float m_jointFriction;
     };
 
 

data/door.urdf

@@ -0,0 +1,98 @@
+<?xml version="0.0" ?>
+<robot name="urdf_door">
+  <link name="baseLink">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0.35"/>
+       <mass value="0.0"/>
+       <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.05 0 0.5"/>
+      <geometry>
+      <box size="0.1 0.1 1"/>
+      </geometry>
+	   <material name="framemat0">
+      <color
+                    rgba="0.9 0.4 0. 1" />
+      </material>
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.95 0 0.5"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+	  <material name="framemat0"/>	 
+    </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.5 0 0.95"/>
+      <geometry>
+        <box size="1 0.1 0.1"/>
+      </geometry>
+	  <material name="framemat0"/>
+	  </visual>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.5 0 0.05"/>
+      <geometry>
+        <box size="1 0.1 0.1"/>
+      </geometry>
+	  <material name="framemat0"/>
+	  </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.05 0 0.5"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.95 0 0.5"/>
+      <geometry>
+        <box size="0.1 0.1 1"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.5 0 0.95"/>
+      <geometry>
+        <box size="1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    <collision>
+      <origin rpy="0 0 0" xyz="0.5 0 0.05"/>
+      <geometry>
+        <box size="1 0.1 0.1"/>
+      </geometry>
+    </collision>
+    
+  </link>
+   <link name="childA">
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 -0.36"/>
+      <mass value="1.0"/>
+      <inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0.4 0 0.4"/>
+      <geometry>
+        <box size="0.9  0.05 0.8"/>
+      </geometry>
+	   <material name="doormat0">
+      <color rgba="0.8 0.8 0.3 1" />
+      </material>
+
+	  </visual>
+     <collision>
+       <origin rpy="0 0 0" xyz="0.4 0 0.4"/>
+       <geometry>
+         <box size="0.9  0.05 0.8"/>
+       </geometry>
+     </collision>
+
+   </link>
+   <joint name="joint_baseLink_childA" type="continuous">
+    <parent link="baseLink"/>
+    <child link="childA"/>
+	<dynamics damping="1.0" friction="0.0001"/>
+    <origin xyz="0.05 0 0.1"/>
+		<axis xyz="0 0 1"/>
+  </joint>
+</robot>
+

examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp

@@ -236,11 +236,13 @@ void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVect
     }
 }
     
-bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
+bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
 {
     jointLowerLimit = 0.f;
     jointUpperLimit = 0.f;
-	
+	jointDamping = 0.f;
+	jointFriction = 0.f;
+
 	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
 	btAssert(linkPtr);
 	if (linkPtr)
@@ -256,6 +258,9 @@ bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joi
 			jointAxisInJointSpace = pj->m_localJointAxis;
 			jointLowerLimit = pj->m_lowerLimit;
 			jointUpperLimit = pj->m_upperLimit;
+			jointDamping = pj->m_jointDamping;
+			jointFriction = pj->m_jointFriction;
+
 			return true;
 		} else
 		{

examples/Importers/ImportURDFDemo/BulletUrdfImporter.h

@@ -34,7 +34,7 @@ public:
     
     virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
 
-    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const;
 
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
 

examples/Importers/ImportURDFDemo/ROSURDFImporter.cpp

@@ -223,18 +223,22 @@ void  ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3
     }
 }
     
-bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
+bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
 {
     jointLowerLimit = 0.f;
     jointUpperLimit = 0.f;
-        
+    jointDamping = 0.f;
+	jointFriction = 0.f;
+
     if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
     {
         my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
             
         const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
         const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
-            
+		jointDamping = pj->dynamics->damping;
+		jointFriction = pj->dynamics->friction;
+
         jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
         parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
         parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));

examples/Importers/ImportURDFDemo/ROSURDFImporter.h

@@ -33,7 +33,7 @@ public:
     
     virtual void  getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const;
 
-    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const;
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit,btScalar& jointDamping, btScalar& jointFriction) const;
 
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& localInertiaFrame) const;
 

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -195,9 +195,11 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
     btVector3 jointAxisInJointSpace;
     btScalar jointLowerLimit;
     btScalar jointUpperLimit;
+    btScalar jointDamping;
+    btScalar jointFriction;
 
 
-    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit);
+    bool hasParentJoint = u2b.getJointInfo(urdfLinkIndex, parent2joint, jointAxisInJointSpace, jointType,jointLowerLimit,jointUpperLimit, jointDamping, jointFriction);
 
 
     linkTransformInWorldSpace =parentTransformInWorldSpace*parent2joint;
@@ -297,6 +299,8 @@ void ConvertURDF2BulletInternal(const URDFImporterInterface& u2b, MultiBodyCreat
                                                                   offsetInA.inverse().getRotation()*offsetInB.getRotation(), quatRotate(offsetInB.inverse().getRotation(),jointAxisInJointSpace), offsetInA.getOrigin(),//parent2joint.getOrigin(),
                                                                   -offsetInB.getOrigin(),
                                                                   disableParentCollision);
+						cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointDamping = jointDamping;
+						cache.m_bulletMultiBody->getLink(mbLinkIndex).m_jointFriction= jointFriction;
                         creation.addLinkMapping(urdfLinkIndex,mbLinkIndex);
 
                     } else

examples/Importers/ImportURDFDemo/URDFImporterInterface.h

@@ -35,7 +35,7 @@ public:
     ///fill an array of child link indices for this link, btAlignedObjectArray behaves like a std::vector so just use push_back and resize(0) if needed
     virtual void getLinkChildIndices(int urdfLinkIndex, btAlignedObjectArray<int>& childLinkIndices) const =0;
     
-    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const =0;
+    virtual bool getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const =0;
     
 	///quick hack: need to rethink the API/dependencies of this
 	virtual int convertLinkVisualShapes(int linkIndex, const char* pathPrefix, const btTransform& inertialFrame) const = 0;

examples/SharedMemory/BodyJointInfoUtility.h

@@ -57,6 +57,8 @@ template <typename T, typename U> void addJointInfoFromMultiBodyData(const T* mb
 			}
 
 			info.m_jointType = mb->m_links[link].m_jointType;
+			info.m_jointDamping = mb->m_links[link].m_jointDamping;
+			info.m_jointFriction = mb->m_links[link].m_jointFriction;
 
 			if ((mb->m_links[link].m_jointType == eRevoluteType) ||
 				(mb->m_links[link].m_jointType == ePrismaticType)) {

examples/SharedMemory/SharedMemoryPublic.h

@@ -85,6 +85,8 @@ struct b3JointInfo
         int m_uIndex;
         int m_jointIndex;
         int m_flags;
+		double m_jointDamping;
+		double m_jointFriction;
 };
 
 struct b3JointSensorState

src/BulletDynamics/Featherstone/btMultiBody.cpp

@@ -1899,6 +1899,9 @@ const char*	btMultiBody::serialize(void* dataBuffer, class btSerializer* seriali
 				getLink(i).m_inertiaLocal.serialize(memPtr->m_linkInertia);
 				memPtr->m_linkMass = getLink(i).m_mass;
 				memPtr->m_parentIndex = getLink(i).m_parent;
+				memPtr->m_jointDamping = getLink(i).m_jointDamping;
+				memPtr->m_jointFriction = getLink(i).m_jointFriction;
+
 				getLink(i).m_eVector.serialize(memPtr->m_parentComToThisComOffset);
 				getLink(i).m_dVector.serialize(memPtr->m_thisPivotToThisComOffset);
 				getLink(i).m_zeroRotParentToThis.serialize(memPtr->m_zeroRotParentToThis);

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -691,8 +691,9 @@ struct btMultiBodyLinkDoubleData
 	double					m_jointPos[7];
 	double					m_jointVel[6];
 	double					m_jointTorque[6];
-	
+
-	
+	double					m_jointDamping;
+	double					m_jointFriction;
 	
 };
 
@@ -721,7 +722,8 @@ struct btMultiBodyLinkFloatData
 	float					m_jointVel[6];
 	float					m_jointTorque[6];
 	int						m_posVarCount;
-	
+	float					m_jointDamping;
+	float					m_jointFriction;
 
 };
 

src/BulletDynamics/Featherstone/btMultiBodyLink.h

@@ -132,8 +132,10 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 
 	const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
 	const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
+	btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
+	btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
 
-    // ctor: set some sensible defaults
+	// ctor: set some sensible defaults
 	btMultibodyLink()
 		: 	m_mass(1),
 			m_parent(-1),
@@ -146,7 +148,9 @@ btVector3 m_appliedConstraintForce;    // In WORLD frame
 			m_jointType(btMultibodyLink::eInvalid),
 			m_jointFeedback(0),
 			m_linkName(0),
-			m_jointName(0)
+			m_jointName(0),
+			m_jointDamping(0),
+			m_jointFriction(0)
 	{
 		
 		m_inertiaLocal.setValue(1, 1, 1);