Merge pull request #2030 from erwincoumans/master

some fixes in inverse dynamics, PyBullet example comparing explicit pd, stable pd control, position control (constraint)

Author: erwincoumans

Extras/InverseDynamics/btMultiBodyTreeCreator.cpp

@@ -202,8 +202,16 @@ int btMultiBodyTreeCreator::createFromBtMultiBody(const btMultiBody *btmb, const
 				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
 				break;
 			case btMultibodyLink::eSpherical:
-				bt_id_error_message("spherical joints not implemented\n");
-				return -1;
+				
+				link.joint_type = SPHERICAL;
+				link.parent_r_parent_body_ref(0) = bt_link.m_eVector[0];
+				link.parent_r_parent_body_ref(1) = bt_link.m_eVector[1];
+				link.parent_r_parent_body_ref(2) = bt_link.m_eVector[2];
+				// random unit vector
+				link.body_axis_of_motion(0) = 0;
+				link.body_axis_of_motion(1) = 0;
+				link.body_axis_of_motion(2) = 1;
+				break;
 			case btMultibodyLink::ePlanar:
 				bt_id_error_message("planar joints not implemented\n");
 				return -1;

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -31,13 +31,17 @@ static btVector4 colors[4] =
 
 static btVector4 selectColor2()
 {
+#ifdef BT_THREADSAFE
 	static btSpinMutex sMutex;
 	sMutex.lock();
+#endif
 	static int curColor = 0;
 	btVector4 color = colors[curColor];
 	curColor++;
 	curColor &= 3;
+#ifdef BT_THREADSAFE
 	sMutex.unlock();
+#endif
 	return color;
 }
 

examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp

@@ -436,7 +436,7 @@ void SimpleOpenGL2Renderer::updateTexture(int textureIndex, const unsigned char*
 		}
 
 		b3Assert(glGetError() == GL_NO_ERROR);
-		glGenerateMipmap(GL_TEXTURE_2D);
+		//glGenerateMipmap(GL_TEXTURE_2D);
 		b3Assert(glGetError() == GL_NO_ERROR);
 	}
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -9756,7 +9756,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
 											   numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
 											   &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff);
 					}
-					serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
+					serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseKinematicsArguments.m_bodyUniqueId;
 					for (int i = 0; i < numDofs; i++)
 					{
 						serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];

examples/pybullet/examples/pdControl.py

@@ -1,26 +1,49 @@
 
 import pybullet as p
+from pdControllerExplicit import PDControllerExplicit
+from pdControllerExplicit import PDControllerStable
+
 import time
 
+
 useMaximalCoordinates=False
 p.connect(p.GUI)
-pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
-for i in range (p.getNumJoints(pole)):
+pole = p.loadURDF("cartpole.urdf", [0,0,0], useMaximalCoordinates=useMaximalCoordinates)
+pole2 = p.loadURDF("cartpole.urdf", [0,1,0], useMaximalCoordinates=useMaximalCoordinates)
+pole3 = p.loadURDF("cartpole.urdf", [0,2,0], useMaximalCoordinates=useMaximalCoordinates)
+pole4 = p.loadURDF("cartpole.urdf", [0,3,0], useMaximalCoordinates=useMaximalCoordinates)
+
+exPD = PDControllerExplicit(p)
+sPD = PDControllerStable(p)
+
+
+for i in range (p.getNumJoints(pole2)):
 	#disable default constraint-based motors
 	p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
-	print("joint",i,"=",p.getJointInfo(pole,i))
+	p.setJointMotorControl2(pole2,i,p.POSITION_CONTROL,targetPosition=0,force=0)
+	p.setJointMotorControl2(pole3,i,p.POSITION_CONTROL,targetPosition=0,force=0)
+	p.setJointMotorControl2(pole4,i,p.POSITION_CONTROL,targetPosition=0,force=0)
+	
+	#print("joint",i,"=",p.getJointInfo(pole2,i))
 
 
+timeStepId = p.addUserDebugParameter("timeStep",0.001,0.1,0.01)
 desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
 desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
-kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
+kpCartId = p.addUserDebugParameter("kpCart",0,500,1300)
 kdCartId = p.addUserDebugParameter("kdCart",0,300,150)
 maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
 
+textColor = [1,1,1]
+shift = 0.05
+p.addUserDebugText("explicit PD", [shift,0,.1],textColor,parentObjectUniqueId=pole,parentLinkIndex=1)
+p.addUserDebugText("explicit PD plugin", [shift,0,-.1],textColor,parentObjectUniqueId=pole2,parentLinkIndex=1)
+p.addUserDebugText("stablePD", [shift,0,.1],textColor,parentObjectUniqueId=pole4,parentLinkIndex=1)
+p.addUserDebugText("position constraint", [shift,0,-.1],textColor,parentObjectUniqueId=pole3,parentLinkIndex=1)
 
 desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
 desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
-kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
+kpPoleId = p.addUserDebugParameter("kpPole",0,500,1200)
 kdPoleId = p.addUserDebugParameter("kdPole",0,300,100)
 maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
 
@@ -29,34 +52,50 @@
 
 p.setGravity(0,0,-10)
 
-useRealTimeSim = True
+useRealTimeSim = False
 
 p.setRealTimeSimulation(useRealTimeSim)
 
-p.setTimeStep(0.001)
+timeStep = 0.001
 
 
 while p.isConnected():
+	p.getCameraImage(320,200)
+	timeStep = p.readUserDebugParameter(timeStepId)
+	p.setTimeStep(timeStep)
+
+	desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
+	desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
+	kpCart = p.readUserDebugParameter(kpCartId)
+	kdCart = p.readUserDebugParameter(kdCartId)
+	maxForceCart = p.readUserDebugParameter(maxForceCartId)
+
+	desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
+	desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
+	kpPole = p.readUserDebugParameter(kpPoleId)
+	kdPole = p.readUserDebugParameter(kdPoleId)
+	maxForcePole = p.readUserDebugParameter(maxForcePoleId)
+	
+	taus = exPD.computePD(pole, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
+	p.setJointMotorControlArray(pole, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
+		
 	if (pd>=0):
-		desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
-		desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
-		kpCart = p.readUserDebugParameter(kpCartId)
-		kdCart = p.readUserDebugParameter(kdCartId)
-		maxForceCart = p.readUserDebugParameter(maxForceCartId)
 		link = 0
-		p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosCart,targetVelocity=desiredVelCart,force=maxForceCart, positionGain=kpCart, velocityGain=kdCart)
-		
-		desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
-		desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
-		kpPole = p.readUserDebugParameter(kpPoleId)
-		kdPole = p.readUserDebugParameter(kdPoleId)
-		maxForcePole = p.readUserDebugParameter(maxForcePoleId)
+		p.setJointMotorControl2(bodyUniqueId=pole2,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosCart,targetVelocity=desiredVelCart,force=maxForceCart, positionGain=kpCart, velocityGain=kdCart)
 		link = 1
-		p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole)
+		p.setJointMotorControl2(bodyUniqueId=pole2,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole)
+	
+	
+	
+	
+	taus = sPD.computePD(pole4, [0,1], [desiredPosCart,desiredPosPole],[desiredVelCart,desiredVelPole], [kpCart,kpPole], [kdCart,kdPole],[maxForceCart,maxForcePole], timeStep)
+	p.setJointMotorControlArray(pole4, [0,1], controlMode=p.TORQUE_CONTROL, forces=taus)
 
-
+	p.setJointMotorControl2(pole3,0, p.POSITION_CONTROL, targetPosition=desiredPosCart, targetVelocity=desiredVelCart, positionGain=timeStep*(kpCart/150.), velocityGain=0.5, force=maxForceCart)
+	p.setJointMotorControl2(pole3,1, p.POSITION_CONTROL, targetPosition=desiredPosPole, targetVelocity=desiredVelPole, positionGain=timeStep*(kpPole/150.), velocityGain=0.5, force=maxForcePole)
+	
 	if (not useRealTimeSim):
 		p.stepSimulation()
-		time.sleep(1./240.)
+		time.sleep(timeStep)
 
 

examples/pybullet/examples/pdControllerExplicit.py

@@ -0,0 +1,67 @@
+import numpy as np
+
+class PDControllerExplicit(object):
+		def __init__(self, pb):
+			self._pb = pb
+
+		def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+			numJoints = self._pb.getNumJoints(bodyUniqueId)
+			jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+			q1 = []
+			qdot1 = []
+			for i in range (numJoints):
+				q1.append(jointStates[i][0])
+				qdot1.append(jointStates[i][1])
+			q = np.array(q1)
+			qdot=np.array(qdot1)
+			qdes = np.array(desiredPositions)
+			qdotdes = np.array(desiredVelocities)
+			qError = qdes - q
+			qdotError = qdotdes - qdot
+			Kp = np.diagflat(kps)
+			Kd = np.diagflat(kds)
+			forces = Kp.dot(qError) + Kd.dot(qdotError)
+			maxF = np.array(maxForces)
+			forces = np.clip(forces, -maxF , maxF )
+			return forces
+			
+			
+class PDControllerStable(object):
+		def __init__(self, pb):
+			self._pb = pb
+
+		def computePD(self, bodyUniqueId, jointIndices, desiredPositions, desiredVelocities, kps, kds, maxForces, timeStep):
+			numJoints = self._pb.getNumJoints(bodyUniqueId)
+			jointStates = self._pb.getJointStates(bodyUniqueId, jointIndices)
+			q1 = []
+			qdot1 = []
+			zeroAccelerations = []
+			for i in range (numJoints):
+				q1.append(jointStates[i][0])
+				qdot1.append(jointStates[i][1])
+				zeroAccelerations.append(0)
+			q = np.array(q1)
+			qdot=np.array(qdot1)
+			qdes = np.array(desiredPositions)
+			qdotdes = np.array(desiredVelocities)
+			qError = qdes - q
+			qdotError = qdotdes - qdot
+			Kp = np.diagflat(kps)
+			Kd = np.diagflat(kds)
+			p =  Kp.dot(qError)
+			d = Kd.dot(qdotError)
+			forces = p + d
+			
+			M1 = self._pb.calculateMassMatrix(bodyUniqueId,q1)
+			M2 = np.array(M1)
+			M = (M2 + Kd * timeStep)
+			c1 = self._pb.calculateInverseDynamics(bodyUniqueId, q1, qdot1, zeroAccelerations)
+			c = np.array(c1)
+			A = M
+			b = -c + p + d
+			qddot = np.linalg.solve(A, b)
+			tau = p + d - Kd.dot(qddot) * timeStep
+			maxF = np.array(maxForces)
+			forces = np.clip(tau, -maxF , maxF )
+			#print("c=",c)
+			return tau

src/BulletCollision/NarrowPhaseCollision/btConvexCast.h

@@ -22,6 +22,19 @@ subject to the following restrictions:
 class btMinkowskiSumShape;
 #include "LinearMath/btIDebugDraw.h"
 
+#ifdef BT_USE_DOUBLE_PRECISION
+#define MAX_ITERATIONS 64
+#define MAX_EPSILON (SIMD_EPSILON * 10)
+#else
+#define MAX_ITERATIONS 32
+#define MAX_EPSILON btScalar(0.0001)
+#endif
+///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
+///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
+//will need to digg deeper to make the algorithm more robust
+//since, a large epsilon can cause an early termination with false
+//positive results (ray intersections that shouldn't be there)
+
 /// btConvexCast is an interface for Casting
 class btConvexCast
 {
@@ -44,7 +57,9 @@ class btConvexCast
 		CastResult()
 			: m_fraction(btScalar(BT_LARGE_FLOAT)),
 			  m_debugDrawer(0),
-			  m_allowedPenetration(btScalar(0))
+			  m_allowedPenetration(btScalar(0)),
+			  m_subSimplexCastMaxIterations(MAX_ITERATIONS),
+			  m_subSimplexCastEpsilon(MAX_EPSILON)
 		{
 		}
 
@@ -57,6 +72,10 @@ class btConvexCast
 		btScalar m_fraction;  //input and output
 		btIDebugDraw* m_debugDrawer;
 		btScalar m_allowedPenetration;
+		
+		int m_subSimplexCastMaxIterations;
+		btScalar m_subSimplexCastEpsilon;
+
 	};
 
 	/// cast a convex against another convex object

src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp

@@ -31,8 +31,8 @@ bool btGjkEpaPenetrationDepthSolver::calcPenDepth(btSimplexSolverInterface& simp
 	(void)simplexSolver;
 
 	btVector3 guessVectors[] = {
-		btVector3(transformB.getOrigin() - transformA.getOrigin()).normalized(),
-		btVector3(transformA.getOrigin() - transformB.getOrigin()).normalized(),
+		btVector3(transformB.getOrigin() - transformA.getOrigin()).safeNormalize(),
+		btVector3(transformA.getOrigin() - transformB.getOrigin()).safeNormalize(),
 		btVector3(0, 0, 1),
 		btVector3(0, 1, 0),
 		btVector3(1, 0, 0),

src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp

@@ -28,13 +28,7 @@ btSubsimplexConvexCast::btSubsimplexConvexCast(const btConvexShape* convexA, con
 {
 }
 
-///Typically the conservative advancement reaches solution in a few iterations, clip it to 32 for degenerate cases.
-///See discussion about this here http://continuousphysics.com/Bullet/phpBB2/viewtopic.php?t=565
-#ifdef BT_USE_DOUBLE_PRECISION
-#define MAX_ITERATIONS 64
-#else
-#define MAX_ITERATIONS 32
-#endif
+
 bool btSubsimplexConvexCast::calcTimeOfImpact(
 	const btTransform& fromA,
 	const btTransform& toA,
@@ -60,7 +54,7 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
 	btVector3 supVertexA = fromA(m_convexA->localGetSupportingVertex(-r * fromA.getBasis()));
 	btVector3 supVertexB = fromB(m_convexB->localGetSupportingVertex(r * fromB.getBasis()));
 	v = supVertexA - supVertexB;
-	int maxIter = MAX_ITERATIONS;
+	int maxIter = result.m_subSimplexCastMaxIterations;
 
 	btVector3 n;
 	n.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
@@ -69,20 +63,12 @@ bool btSubsimplexConvexCast::calcTimeOfImpact(
 
 	btScalar dist2 = v.length2();
 
-#ifdef BT_USE_DOUBLE_PRECISION
-	btScalar epsilon = SIMD_EPSILON * 10;
-#else
-	//todo: epsilon kept for backward compatibility of unit tests.
-	//will need to digg deeper to make the algorithm more robust
-	//since, a large epsilon can cause an early termination with false
-	//positive results (ray intersections that shouldn't be there)
-	btScalar epsilon = btScalar(0.0001);
-#endif  //BT_USE_DOUBLE_PRECISION
+
 
 	btVector3 w, p;
 	btScalar VdotR;
 
-	while ((dist2 > epsilon) && maxIter--)
+	while ((dist2 > result.m_subSimplexCastEpsilon) && maxIter--)
 	{
 		supVertexA = interpolatedTransA(m_convexA->localGetSupportingVertex(-v * interpolatedTransA.getBasis()));
 		supVertexB = interpolatedTransB(m_convexB->localGetSupportingVertex(v * interpolatedTransB.getBasis()));

src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp

@@ -764,6 +764,12 @@ void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info
 	btVector3 ax1A = trA.getBasis().getColumn(2);
 	btVector3 ax1B = trB.getBasis().getColumn(2);
 	btVector3 ax1 = ax1A * factA + ax1B * factB;
+	if (ax1.length2()<SIMD_EPSILON)
+	{
+		factA=0.f;
+		factB=1.f;
+		ax1 = ax1A * factA + ax1B * factB;
+	}
 	ax1.normalize();
 	// fill first 3 rows
 	// we want: velA + wA x relA == velB + wB x relB

src/BulletDynamics/Featherstone/btMultiBody.h

@@ -134,6 +134,15 @@ btMultiBody
 		return m_baseCollider;
 	}
 
+	const btMultiBodyLinkCollider *getLinkCollider(int index) const
+	{
+		if (index >= 0 && index < getNumLinks())
+		{
+			return getLink(index).m_collider;
+		}
+		return 0;
+	}
+
 	btMultiBodyLinkCollider *getLinkCollider(int index)
 	{
 		if (index >= 0 && index < getNumLinks())