diff --git a/data/cube_gripper_left.urdf b/data/cube_gripper_left.urdf
index 08951a2b9..f46f69bcf 100644
--- a/data/cube_gripper_left.urdf
+++ b/data/cube_gripper_left.urdf
@@ -6,7 +6,7 @@
-
+
diff --git a/data/cube_gripper_right.urdf b/data/cube_gripper_right.urdf
index a70bcfef2..68a202175 100644
--- a/data/cube_gripper_right.urdf
+++ b/data/cube_gripper_right.urdf
@@ -6,7 +6,7 @@
-
+
diff --git a/data/cube_small.urdf b/data/cube_small.urdf
index e94692b50..23ae7fa3b 100644
--- a/data/cube_small.urdf
+++ b/data/cube_small.urdf
@@ -3,7 +3,7 @@
-
+
diff --git a/data/plane.urdf b/data/plane.urdf
index 57b746104..fb7c124af 100644
--- a/data/plane.urdf
+++ b/data/plane.urdf
@@ -1,6 +1,9 @@
+
+
+
diff --git a/data/sphere2_rolling_friction.urdf b/data/sphere2_rolling_friction.urdf
new file mode 100644
index 000000000..e4ff66341
--- /dev/null
+++ b/data/sphere2_rolling_friction.urdf
@@ -0,0 +1,29 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp
index a9fcd4bcf..9a98f5e0c 100644
--- a/examples/ExampleBrowser/ExampleEntries.cpp
+++ b/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),
diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp
index 46db7d59c..434454985 100644
--- a/examples/RoboticsLearning/GripperGraspExample.cpp
+++ b/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_maxTorqueValue = 50.0;
+ controlArgs.m_targetVelocity = -0.1;
+ 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_maxTorqueValue = 50.0;
+ controlArgs.m_targetVelocity = 0.1;
+ controlArgs.m_maxTorqueValue = 10.0;
controlArgs.m_kd = 1.;
m_robotSim.setJointMotorControl(2,0,controlArgs);
}
diff --git a/examples/RoboticsLearning/R2D2GraspExample.cpp b/examples/RoboticsLearning/R2D2GraspExample.cpp
index 4da5cc22a..3a33ba653 100644
--- a/examples/RoboticsLearning/R2D2GraspExample.cpp
+++ b/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()
diff --git a/examples/RoboticsLearning/R2D2GraspExample.h b/examples/RoboticsLearning/R2D2GraspExample.h
index 353f684e5..1861b4607 100644
--- a/examples/RoboticsLearning/R2D2GraspExample.h
+++ b/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);
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index b8c2ab011..161b1b7ed 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/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);
}
@@ -762,7 +762,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult
rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
}
}
-
+
solverConstraint.m_friction = cp.m_combinedRollingFriction;
if(!isFriction)