Rolling friction demo for sphere and torsional friction demo for two point contact.

data/cube_gripper_left.urdf

@@ -6,7 +6,7 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="1.0"/>
+      <rolling_friction value="0.3"/>
       <inertia_scaling value="3.0"/>
       <contact_cfm value="0.0"/>
       <contact_erp value="1.0"/>

data/cube_gripper_right.urdf

@@ -6,7 +6,7 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="1.0"/>
+      <rolling_friction value="0.3"/>
       <inertia_scaling value="3.0"/>
       <contact_cfm value="0.0"/>
       <contact_erp value="1.0"/>

data/cube_small.urdf

@@ -3,7 +3,7 @@
   <link name="baseLink">
     <contact>
       <lateral_friction value="1.0"/>
-      <rolling_friction value="1.0"/>
+      <rolling_friction value="0.3"/>
       <inertia_scaling value="3.0"/>
       <contact_cfm value="0.0"/>
       <contact_erp value="1.0"/>

data/plane.urdf

@@ -1,6 +1,9 @@
 <?xml version="0.0" ?>
 <robot name="cube.urdf">
   <link name="baseLink">
+    <contact>
+      <rolling_friction value="0.3"/>
+    </contact>
     <inertial>
       <origin rpy="0 0 0" xyz="0 0 0"/>
        <mass value=".0"/>

data/sphere2_rolling_friction.urdf

@@ -0,0 +1,29 @@
+<?xml version="0.0" ?>
+<robot name="urdf_robot">
+  <link name="base_link">
+    <contact>
+      <rolling_friction value="0.3"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="10.0"/>
+       <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="1"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+				<mesh filename="textured_sphere_smooth.obj" scale="0.5 0.5 0.5"/>      
+      </geometry>
+      <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+	 <sphere radius="0.5"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>
+

examples/ExampleBrowser/ExampleEntries.cpp

@@ -261,6 +261,7 @@ static ExampleEntry gDefaultExamples[]=
 
 	ExampleEntry(1,"R2D2 Grasp","Load the R2D2 robot from URDF file and control it to grasp objects", R2D2GraspExampleCreateFunc, eROBOTIC_LEARN_GRASP),
 	ExampleEntry(1,"URDF Compliant Contact","Work-in-progress, experiment/improve compliant rigid contact using parameters from URDF file (contact_cfm, contact_erp, lateral_friction, rolling_friction)", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_COMPLIANT_CONTACT),
+    ExampleEntry(1,"Rolling friction","Experiment on multibody rolling friction", R2D2GraspExampleCreateFunc,eROBOTIC_LEARN_ROLLING_FRICTION),
     ExampleEntry(1,"Gripper Grasp","Grasp experiment with a gripper to improve contact model", GripperGraspExampleCreateFunc,eGRIPPER_GRASP),
     ExampleEntry(1,"Two Point Grasp","Grasp experiment with two point contact to test rolling friction", GripperGraspExampleCreateFunc, eTWO_POINT_GRASP),
 	

examples/RoboticsLearning/GripperGraspExample.cpp

@@ -172,15 +172,13 @@ public:
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
                 args.m_fileName = "cube_gripper_left.urdf";
-                //args.m_startPosition.setValue(0.0675, 0.02, 0.13);
                 args.m_startPosition.setValue(0.068, 0.02, 0.11);
-                //args.m_startOrientation.setEulerZYX(0, 0.785398, 0.785398);
                 args.m_useMultiBody = true;
                 m_robotSim.loadFile(args, results);
                 
                 b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
-                controlArgs.m_targetVelocity = -0.5;
+                controlArgs.m_targetVelocity = -0.1;
-                controlArgs.m_maxTorqueValue = 50.0;
+                controlArgs.m_maxTorqueValue = 10.0;
                 controlArgs.m_kd = 1.;
                 m_robotSim.setJointMotorControl(1,0,controlArgs);
             }
@@ -189,15 +187,13 @@ public:
                 b3RobotSimLoadFileArgs args("");
                 b3RobotSimLoadFileResults results;
                 args.m_fileName = "cube_gripper_right.urdf";
-                //args.m_startPosition.setValue(-0.0675, 0.02, 0.13);
                 args.m_startPosition.setValue(-0.068, 0.02, 0.11);
-                //args.m_startOrientation.setEulerZYX(0, -0.785398, -0.785398);
                 args.m_useMultiBody = true;
                 m_robotSim.loadFile(args, results);
                 
                 b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
-                controlArgs.m_targetVelocity = 0.5;
+                controlArgs.m_targetVelocity = 0.1;
-                controlArgs.m_maxTorqueValue = 50.0;
+                controlArgs.m_maxTorqueValue = 10.0;
                 controlArgs.m_kd = 1.;
                 m_robotSim.setJointMotorControl(2,0,controlArgs);
             }

examples/RoboticsLearning/R2D2GraspExample.cpp

@@ -142,6 +142,37 @@ public:
 			}
 		}
 	
+        if ((m_options & eROBOTIC_LEARN_ROLLING_FRICTION)!=0)
+        {
+            b3RobotSimLoadFileArgs args("");
+            b3RobotSimLoadFileResults results;
+            {
+                args.m_fileName = "sphere2_rolling_friction.urdf";
+                args.m_startPosition.setValue(0,0,2.5);
+                args.m_startOrientation.setEulerZYX(0,0,0);
+                args.m_useMultiBody = true;
+                m_robotSim.loadFile(args,results);
+            }
+            {
+                args.m_fileName = "sphere2.urdf";
+                args.m_startPosition.setValue(0,2,2.5);
+                args.m_startOrientation.setEulerZYX(0,0,0);
+                args.m_useMultiBody = true;
+                m_robotSim.loadFile(args,results);
+            }
+            {
+                b3RobotSimLoadFileArgs args("");
+                args.m_fileName = "plane.urdf";
+                args.m_startPosition.setValue(0,0,0);
+                args.m_startOrientation.setEulerZYX(0,0.2,0);
+                args.m_useMultiBody = true;
+                args.m_forceOverrideFixedBase = true;
+                b3RobotSimLoadFileResults results;
+                m_robotSim.loadFile(args,results);
+                m_robotSim.setGravity(b3MakeVector3(0,0,-10));
+            }
+        }
+
 		
     }
     virtual void    exitPhysics()

examples/RoboticsLearning/R2D2GraspExample.h

@@ -20,6 +20,7 @@ enum RobotLearningExampleOptions
 {
 	eROBOTIC_LEARN_GRASP=1,
 	eROBOTIC_LEARN_COMPLIANT_CONTACT=2,
+    eROBOTIC_LEARN_ROLLING_FRICTION=4,
 };
 
 class CommonExampleInterface*    R2D2GraspExampleCreateFunc(struct CommonExampleOptions& options);

src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp

@@ -598,18 +598,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
         btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
         
         btScalar* jac1=&m_data.m_jacobians[solverConstraint.m_jacAindex];
-        multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+        multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, cp.getPositionWorldOnA(), constraintNormal, btVector3(0,0,0), jac1, m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
         btScalar* delta = &m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
         multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
         
         btVector3 torqueAxis0 = constraintNormal;
         solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-        solverConstraint.m_contactNormal1 = constraintNormal;
+        solverConstraint.m_contactNormal1 = btVector3(0,0,0);
     } else
     {
         btVector3 torqueAxis0 = constraintNormal;
         solverConstraint.m_relpos1CrossNormal = torqueAxis0;
-        solverConstraint.m_contactNormal1 = constraintNormal;
+        solverConstraint.m_contactNormal1 = btVector3(0,0,0);
         solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
     }
     
@@ -641,18 +641,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
         m_data.m_deltaVelocitiesUnitImpulse.resize(m_data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
         btAssert(m_data.m_jacobians.size() == m_data.m_deltaVelocitiesUnitImpulse.size());
         
-        multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
+        multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, cp.getPositionWorldOnB(), -constraintNormal, btVector3(0,0,0), &m_data.m_jacobians[solverConstraint.m_jacBindex], m_data.scratch_r, m_data.scratch_v, m_data.scratch_m);
         multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
         
         btVector3 torqueAxis1 = constraintNormal;
         solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-        solverConstraint.m_contactNormal2 = -constraintNormal;
+        solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
         
     } else
     {
         btVector3 torqueAxis1 = constraintNormal;
         solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
-        solverConstraint.m_contactNormal2 = -constraintNormal;
+        solverConstraint.m_contactNormal2 = -btVector3(0,0,0);
         
         solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
     }