Merge branch 'master' of https://github.com/erwincoumans/bullet3

examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp

@@ -119,9 +119,10 @@ ImportMJCFSetup::ImportMJCFSetup(struct GUIHelperInterface* helper, int option,
 		
 		if (gMCFJFileNameArray.size()==0)
 		{
+			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
+
 			gMCFJFileNameArray.push_back("MPL/MPL.xml");
 
-			gMCFJFileNameArray.push_back("mjcf/humanoid.xml");
 			gMCFJFileNameArray.push_back("mjcf/inverted_pendulum.xml");
 			gMCFJFileNameArray.push_back("mjcf/ant.xml");
 			gMCFJFileNameArray.push_back("mjcf/hello_mjcf.xml");
@@ -198,7 +199,7 @@ void ImportMJCFSetup::initPhysics()
 	//@todo also use the modified collision filter for raycast and other collision related queries
 	m_filterCallback->m_filterMode = FILTER_GROUPAMASKB_OR_GROUPBMASKA2;
 	
-	//m_dynamicsWorld->getSolverInfo().m_numIterations = 100;
+	//m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
 	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
 	m_dynamicsWorld->getDebugDrawer()->setDebugMode(
 		btIDebugDraw::DBG_DrawConstraints
@@ -255,6 +256,8 @@ void ImportMJCFSetup::initPhysics()
 				s->registerNameForPointer(name->c_str(),name->c_str());
 #endif//TEST_MULTIBODY_SERIALIZATION
 				mb->setBaseName(name->c_str());
+				mb->getBaseCollider()->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
+
 				//create motors for each btMultiBody joint
 				int numLinks = mb->getNumLinks();
 				for (int i=0;i<numLinks;i++)
@@ -270,10 +273,11 @@ void ImportMJCFSetup::initPhysics()
 #endif//TEST_MULTIBODY_SERIALIZATION
 					m_nameMemory.push_back(jointName);
 					m_nameMemory.push_back(linkName);
+					mb->getLinkCollider(i)->setCollisionFlags(mb->getBaseCollider()->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
 
 					mb->getLink(i).m_linkName = linkName->c_str();
 					mb->getLink(i).m_jointName = jointName->c_str();
-							
+					m_data->m_mb = mb;
 					if (mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::eRevolute
 					    ||mb->getLink(mbLinkIndex).m_jointType==btMultibodyLink::ePrismatic
 					)
@@ -363,7 +367,16 @@ void ImportMJCFSetup::stepSimulation(float deltaTime)
         {
 			if (m_data->m_jointMotors[i])
 			{
-				m_data->m_jointMotors[i]->setPositionTarget(m_data->m_motorTargetPositions[i]);
+				btScalar pos = m_data->m_motorTargetPositions[i];
+
+				int link = m_data->m_jointMotors[i]->getLinkA();
+				btScalar lowerLimit = m_data->m_mb->getLink(link).m_jointLowerLimit;
+				btScalar upperLimit = m_data->m_mb->getLink(link).m_jointUpperLimit;
+				if (lowerLimit < upperLimit)
+				{
+					btClamp(pos, lowerLimit, upperLimit);
+				}
+				m_data->m_jointMotors[i]->setPositionTarget(pos);
 			}
 			if (m_data->m_generic6DofJointMotors[i])
 			{

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -4633,6 +4633,11 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct
 									}
 
 									motor->setVelocityTarget(desiredVelocity,kd);
+									//clamp position
+									if (mb->getLink(link).m_jointLowerLimit <= mb->getLink(link).m_jointUpperLimit)
+									{
+										btClamp(desiredPosition, mb->getLink(link).m_jointLowerLimit, mb->getLink(link).m_jointUpperLimit);
+									}
 									motor->setPositionTarget(desiredPosition,kp);
 
 									btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;

examples/pybullet/examples/humanoid_manual_control.py

@@ -0,0 +1,32 @@
+import pybullet as p
+import time
+
+p.connect(p.GUI)
+obUids = p.loadMJCF("mjcf/humanoid.xml")
+humanoid = obUids[1]
+
+gravId = p.addUserDebugParameter("gravity",-10,10,-10)
+jointIds=[]
+paramIds=[]
+
+p.setPhysicsEngineParameter(numSolverIterations=10)
+p.changeDynamics(humanoid,-1,linearDamping=0, angularDamping=0)
+
+for j in range (p.getNumJoints(humanoid)):
+	p.changeDynamics(humanoid,j,linearDamping=0, angularDamping=0)
+	info = p.getJointInfo(humanoid,j)
+	#print(info)
+	jointName = info[1]
+	jointType = info[2]
+	if (jointType==p.JOINT_PRISMATIC or jointType==p.JOINT_REVOLUTE):
+		jointIds.append(j)
+		paramIds.append(p.addUserDebugParameter(jointName.decode("utf-8"),-4,4,0))
+
+p.setRealTimeSimulation(1)
+while(1):
+	p.setGravity(0,0,p.readUserDebugParameter(gravId))
+	for i in range(len(paramIds)):
+		c = paramIds[i]
+		targetPos = p.readUserDebugParameter(c)
+		p.setJointMotorControl2(humanoid,jointIds[i],p.POSITION_CONTROL,targetPos, force=5*240.)
+	time.sleep(0.01)

src/BulletDynamics/Featherstone/btMultiBodyConstraint.h

@@ -119,6 +119,14 @@ public:
 		return m_bodyB;
 	}
 
+	int getLinkA() const
+	{
+		return m_linkA;
+	}
+	int getLinkB() const
+	{
+		return m_linkB;
+	}
 	void	internalSetAppliedImpulse(int dof, btScalar appliedImpulse)
 	{
 		btAssert(dof>=0);