Merge pull request #2030 from erwincoumans/master

some fixes in inverse dynamics, PyBullet example comparing explicit pd, stable pd control, position control (constraint)
2018-12-22 16:17:26 -08:00
parent 4a66d6c80b d477d18ad6
commit 8bc1c8e01b
14 changed files with 204 additions and 51 deletions
--- a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp
+++ b/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;
--- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+++ b/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;
 }

--- a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp
+++ b/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);
 	}
 }
--- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+++ b/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];
--- a/examples/pybullet/examples/pdControl.py
+++ b/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 @@ pd = p.loadPlugin("pdControlPlugin")
 	
 p.setGravity(0,0,-10)

-useRealTimeSim = True
+useRealTimeSim = False

 p.setRealTimeSimulation(useRealTimeSim)
 	
-p.setTimeStep(0.001)
+timeStep = 0.001
 	
 	
 while p.isConnected():
-	if (pd>=0):
+	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)
-		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)
-		link = 1
-		p.setJointMotorControl2(bodyUniqueId=pole,jointIndex=link,controlMode=p.PD_CONTROL,targetPosition=desiredPosPole,targetVelocity=desiredVelPole,force=maxForcePole, positionGain=kpPole, velocityGain=kdPole)
 	
+	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):
+		link = 0
+		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=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)
 	
 	
--- a/examples/pybullet/examples/pdControllerExplicit.py
+++ b/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
--- a/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h
+++ b/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 @@ public:
 		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 @@ public:
 		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
--- a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp
+++ b/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),
--- a/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp
+++ b/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()));
--- a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp
+++ b/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
--- a/src/BulletDynamics/Featherstone/btMultiBody.h
+++ b/src/BulletDynamics/Featherstone/btMultiBody.h
@@ -134,6 +134,15 @@ public:
 		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())
--- a/src/BulletInverseDynamics/MultiBodyTree.cpp
+++ b/src/BulletInverseDynamics/MultiBodyTree.cpp
@@ -281,6 +281,8 @@ int MultiBodyTree::addBody(int body_index, int parent_index, JointType joint_typ
 			break;
 		case FLOATING:
 			break;
+		case SPHERICAL:
+			break;
 		default:
 			bt_id_error_message("unknown joint type %d\n", joint_type);
 			return -1;
@@ -437,6 +439,16 @@ int MultiBodyTree::finalize()
 				rigid_body.m_Jac_JT(1) = 0.0;
 				rigid_body.m_Jac_JT(2) = 0.0;
 				break;
+			case SPHERICAL:
+				// NOTE/TODO: this is not really correct.
+				// the Jacobians should be 3x3 matrices here !
+				rigid_body.m_Jac_JR(0) = 0.0;
+				rigid_body.m_Jac_JR(1) = 0.0;
+				rigid_body.m_Jac_JR(2) = 0.0;
+				rigid_body.m_Jac_JT(0) = 0.0;
+				rigid_body.m_Jac_JT(1) = 0.0;
+				rigid_body.m_Jac_JT(2) = 0.0;
+				break;
 			case FLOATING:
 				// NOTE/TODO: this is not really correct.
 				// the Jacobians should be 3x3 matrices here !
--- a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
+++ b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp
@@ -28,6 +28,9 @@ int MultiBodyTree::InitCache::addBody(const int body_index, const int parent_ind
 			// does not add a degree of freedom
 			// m_num_dofs+=0;
 			break;
+		case SPHERICAL:
+			m_num_dofs += 3;
+			break;
 		case FLOATING:
 			m_num_dofs += 6;
 			break;
--- a/src/LinearMath/btConvexHullComputer.cpp
+++ b/src/LinearMath/btConvexHullComputer.cpp
@@ -926,7 +926,7 @@ int btConvexHullInternal::Rational64::compare(const Rational64& b) const
 		"decb %%bh\n\t"        // now bx=0x0000 if difference is zero, 0xff01 if it is negative, 0x0001 if it is positive (i.e., same sign as difference)
 		"shll $16, %%ebx\n\t"  // ebx has same sign as difference
 		: "=&b"(result), [tmp] "=&r"(tmp), "=a"(dummy)
-		: "a"(denominator), [bn] "g"(b.numerator), [tn] "g"(numerator), [bd] "g"(b.denominator)
+		: "a"(m_denominator), [bn] "g"(b.m_numerator), [tn] "g"(m_numerator), [bd] "g"(b.m_denominator)
 		: "%rdx", "cc");
 	return result ? result ^ sign  // if sign is +1, only bit 0 of result is inverted, which does not change the sign of result (and cannot result in zero)
 								   // if sign is -1, all bits of result are inverted, which changes the sign of result (and again cannot result in zero)