AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-14 05:40:05 +00:00
Code Issues Releases Wiki Activity

tweak ImportMJCFSetup.cpp example MJCF humanoid a bit, clamp target positions to be within joint limits to avoid solver problems (conflicting constraints)

Browse Source 
add humanoid_manual_control.py PyBullet example which is similar to ImportMJCFSetup.cpp
This commit is contained in:
Erwin Coumans 2018-01-04 13:14:11 -08:00
parent 4743d2770a
commit c59a3763e5
4 changed files with 62 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
examples/Importers/ImportMJCFDemo/ImportMJCFSetup.cpp
View File

@ -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])
{

5
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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;

32
examples/pybullet/examples/humanoid_manual_control.py Normal file
View File

@ -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)

8
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@ -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);