From 3eedb2a6f2679f80874bf532e3f792e5c11874f4 Mon Sep 17 00:00:00 2001
From: Erwin Coumans <erwincoumans@google.com>
Date: Wed, 24 Jul 2019 22:07:16 -0700
Subject: [PATCH] fix rotational friction between btMultiBody and btRigidBody

---
 .../pybullet/examples/rotationalFriction.py   | 15 +++++++
 .../gym/pybullet_data/cube_rotate.urdf        | 41 +++++++++++++++++++
 .../btMultiBodyConstraintSolver.cpp           |  8 ++--
 3 files changed, 60 insertions(+), 4 deletions(-)
 create mode 100644 examples/pybullet/examples/rotationalFriction.py
 create mode 100644 examples/pybullet/gym/pybullet_data/cube_rotate.urdf

diff --git a/examples/pybullet/examples/rotationalFriction.py b/examples/pybullet/examples/rotationalFriction.py
new file mode 100644
index 000000000..f38cba38c
--- /dev/null
+++ b/examples/pybullet/examples/rotationalFriction.py
@@ -0,0 +1,15 @@
+import time
+import pybullet as p
+import pybullet_data as pd
+useMaximalCoordinatesA = True
+useMaximalCoordinatesB = True
+p.connect(p.GUI)
+p.setAdditionalSearchPath(pd.getDataPath())
+cube=p.loadURDF("cube_rotate.urdf",useMaximalCoordinates=useMaximalCoordinatesA)
+p.loadURDF("sphere2.urdf",[0,0,2],useMaximalCoordinates=useMaximalCoordinatesB)
+p.setGravity(0,0,-10)
+p.setJointMotorControl2(cube,0,p.VELOCITY_CONTROL,targetVelocity=1, force=100)
+while (1):
+  p.stepSimulation()
+  time.sleep(1./240.)
+
diff --git a/examples/pybullet/gym/pybullet_data/cube_rotate.urdf b/examples/pybullet/gym/pybullet_data/cube_rotate.urdf
new file mode 100644
index 000000000..5859c4db6
--- /dev/null
+++ b/examples/pybullet/gym/pybullet_data/cube_rotate.urdf
@@ -0,0 +1,41 @@
+<?xml version="1.0" ?>
+<robot name="cube">
+  <link name="world"/>
+  
+  <link name="baseLink">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+       <mass value="1.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="cube.obj" scale="1 1 1"/>
+      </geometry>
+       <material name="white">
+        <color rgba="1 1 1 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+<box size="1 1 1"/>
+      </geometry>
+    </collision>
+  </link>
+  <joint name="joint_baseLink_world" type="continuous">
+    <parent link="world"/>
+    <child link="baseLink"/>
+<dynamics damping="1.0" friction="0.0001"/>
+    <origin xyz="0 0 0"/>
+<axis xyz="0 0 1"/>
+  </joint>
+</robot>
+
diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
index 23e163f0e..9b58c1a49 100644
--- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
+++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
@@ -944,13 +944,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
 		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;
+		btVector3 torqueAxis0 = constraintNormal;
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
 	}
 	else
 	{
-		btVector3 torqueAxis0 = -constraintNormal;
+		btVector3 torqueAxis0 = constraintNormal;
 		solverConstraint.m_relpos1CrossNormal = torqueAxis0;
 		solverConstraint.m_contactNormal1 = btVector3(0, 0, 0);
 		solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
@@ -986,13 +986,13 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu
 		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;
+		btVector3 torqueAxis1 = -constraintNormal;
 		solverConstraint.m_relpos2CrossNormal = torqueAxis1;
 		solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);
 	}
 	else
 	{
-		btVector3 torqueAxis1 = constraintNormal;
+		btVector3 torqueAxis1 = -constraintNormal;
 		solverConstraint.m_relpos2CrossNormal = torqueAxis1;
 		solverConstraint.m_contactNormal2 = -btVector3(0, 0, 0);