diff --git a/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp b/Extras/InverseDynamics/btMultiBodyTreeCreator.cpp index d70e28220..02dcf3819 100644 --- 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; diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 59a961772..bde786ffe 100644 --- 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; } diff --git a/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp b/examples/OpenGLWindow/SimpleOpenGL2Renderer.cpp index 287af31fc..7e1a54e83 100644 --- 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); } } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 54329ec27..2bf9c99ec 100644 --- 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]; diff --git a/examples/pybullet/examples/pdControl.py b/examples/pybullet/examples/pdControl.py index 505ee3109..311063be2 100644 --- 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): - 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) - + 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): + 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) diff --git a/examples/pybullet/examples/pdControllerExplicit.py b/examples/pybullet/examples/pdControllerExplicit.py new file mode 100644 index 000000000..beaca965d --- /dev/null +++ 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 \ No newline at end of file diff --git a/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h b/src/BulletCollision/NarrowPhaseCollision/btConvexCast.h index 98b0d5084..76f54699c 100644 --- 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 diff --git a/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp b/src/BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.cpp index b44fab5d8..07629229a 100644 --- 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), diff --git a/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp b/src/BulletCollision/NarrowPhaseCollision/btSubSimplexConvexCast.cpp index a2af32fdd..37458339e 100644 --- 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())); diff --git a/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp b/src/BulletDynamics/ConstraintSolver/btHingeConstraint.cpp index 121290d0d..aa6f69000 100644 --- 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()= 0 && index < getNumLinks()) + { + return getLink(index).m_collider; + } + return 0; + } + btMultiBodyLinkCollider *getLinkCollider(int index) { if (index >= 0 && index < getNumLinks()) diff --git a/src/BulletInverseDynamics/MultiBodyTree.cpp b/src/BulletInverseDynamics/MultiBodyTree.cpp index dadf0bab0..f150b5ae4 100644 --- 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 ! diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeInitCache.cpp index cf41af5b2..a718db051 100644 --- 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; diff --git a/src/LinearMath/btConvexHullComputer.cpp b/src/LinearMath/btConvexHullComputer.cpp index df4ad0629..8bbfdc5f2 100644 --- 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)