Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

implement pybullet.createUserConstraint for maximalCoordinates (fixed, p2p, gear)

Browse Source 
maximalCoordinates, make sure parent is bodyA, child is BodyB
This commit is contained in:
erwincoumans
2017-08-30 09:54:38 -07:00
parent 9d404f5316
commit 8dfa76e924
2 changed files with 143 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

6
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -390,7 +390,7 @@ void ConvertURDF2BulletInternal(
{ {
//b3Printf("Fixed joint\n"); //b3Printf("Fixed joint\n");
btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA); btGeneric6DofSpring2Constraint* dof6 = creation.createFixedJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB);
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
@@ -417,7 +417,7 @@ void ConvertURDF2BulletInternal(
} else } else
{ {
btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit); btGeneric6DofSpring2Constraint* dof6 = creation.createRevoluteJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit, jointUpperLimit);
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);
@@ -449,9 +449,7 @@ void ConvertURDF2BulletInternal(
} else } else
{ {
//btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*linkRigidBody, *parentRigidBody, offsetInB, offsetInA,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit); btGeneric6DofSpring2Constraint* dof6 = creation.createPrismaticJoint(urdfLinkIndex,*parentRigidBody, *linkRigidBody, offsetInA, offsetInB,jointAxisInJointSpace,jointLowerLimit,jointUpperLimit);
if (enableConstraints) if (enableConstraints)
world1->addConstraint(dof6,true); world1->addConstraint(dof6,true);

167
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6716,36 +6716,151 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
if (parentBody->m_rigidBody) if (parentBody->m_rigidBody)
{ {
if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
btRigidBody* parentRb = 0;
if (clientCmd.m_userConstraintArguments.m_parentJointIndex==-1)
{ {
btRigidBody* childRb = childBody->m_rigidBody; parentRb = parentBody->m_rigidBody;
if (childRb) } else
{
if ((clientCmd.m_userConstraintArguments.m_parentJointIndex>=0) &&
(clientCmd.m_userConstraintArguments.m_parentJointIndex<parentBody->m_rigidBodyJoints.size()))
{ {
btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0], parentRb = &parentBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_parentJointIndex]->getRigidBodyB();
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
//for now we use the same local axis for both objects
btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
btScalar ratio=1;
btGearConstraint* gear = new btGearConstraint(*parentBody->m_rigidBody,*childRb, axisA,axisB,ratio);
m_data->m_dynamicsWorld->addConstraint(gear,true);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = gear;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
} }
} }
btRigidBody* childRb = 0;
if (childBody->m_rigidBody)
{
if (clientCmd.m_userConstraintArguments.m_childJointIndex==-1)
{
childRb = childBody->m_rigidBody;
}
else
{
if ((clientCmd.m_userConstraintArguments.m_childJointIndex>=0)
&& (clientCmd.m_userConstraintArguments.m_childJointIndex<childBody->m_rigidBodyJoints.size()))
{
childRb = &childBody->m_rigidBodyJoints[clientCmd.m_userConstraintArguments.m_childJointIndex]->getRigidBodyB();
}
}
}
switch (clientCmd.m_userConstraintArguments.m_jointType)
{
case eRevoluteType:
{
break;
}
case ePrismaticType:
{
break;
}
case eFixedType:
{
if (childRb && parentRb && (childRb!=parentRb))
{
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btTransform offsetTrA,offsetTrB;
offsetTrA.setIdentity();
offsetTrA.setOrigin(pivotInParent);
offsetTrB.setIdentity();
offsetTrB.setOrigin(pivotInChild);
btGeneric6DofSpring2Constraint* dof6 = new btGeneric6DofSpring2Constraint(*parentRb, *childRb, offsetTrA, offsetTrB);
dof6->setLinearLowerLimit(btVector3(0,0,0));
dof6->setLinearUpperLimit(btVector3(0,0,0));
dof6->setAngularLowerLimit(btVector3(0,0,0));
dof6->setAngularUpperLimit(btVector3(0,0,0));
m_data->m_dynamicsWorld->addConstraint(dof6);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = dof6;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
break;
}
case ePoint2PointType:
{
if (childRb && parentRb && (childRb!=parentRb))
{
btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]);
btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]);
btPoint2PointConstraint* p2p = new btPoint2PointConstraint(*parentRb,*childRb,pivotInParent,pivotInChild);
p2p->m_setting.m_impulseClamp = defaultMaxForce;
m_data->m_dynamicsWorld->addConstraint(p2p);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = p2p;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
break;
}
case eGearType:
{
if (childRb && parentRb && (childRb!=parentRb))
{
btVector3 axisA(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
//for now we use the same local axis for both objects
btVector3 axisB(clientCmd.m_userConstraintArguments.m_jointAxis[0],
clientCmd.m_userConstraintArguments.m_jointAxis[1],
clientCmd.m_userConstraintArguments.m_jointAxis[2]);
btScalar ratio=1;
btGearConstraint* gear = new btGearConstraint(*parentRb,*childRb, axisA,axisB,ratio);
m_data->m_dynamicsWorld->addConstraint(gear,true);
InteralUserConstraintData userConstraintData;
userConstraintData.m_rbConstraint = gear;
int uid = m_data->m_userConstraintUIDGenerator++;
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = uid;
serverCmd.m_userConstraintResultArgs.m_maxAppliedForce = defaultMaxForce;
userConstraintData.m_userConstraintData = serverCmd.m_userConstraintResultArgs;
m_data->m_userConstraints.insert(uid,userConstraintData);
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
break;
}
case eSphericalType:
{
b3Warning("constraint type not handled yet");
break;
}
case ePlanarType:
{
b3Warning("constraint type not handled yet");
break;
}
default:
{
b3Warning("unknown constraint type");
}
};
} }
} }
} }