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

add pybullet.changeConstraint / b3InitChangeUserConstraintCommand/ b3InitChangeUserConstraintSetPivotInB /b3InitChangeUserConstraintSetFrameInB command, to change an existing user constraint.

Browse Source 
add constraint.py example.
allow pybullet.createConstraint to create user constraint without a child body ('fixed' to the world)
This commit is contained in:
Erwin Coumans
2017-01-12 10:30:46 -08:00
parent 1c79798583
commit 9aa5a839d5
10 changed files with 186 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1064,6 +1064,52 @@ b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHan
return (b3SharedMemoryCommandHandle)command;
}
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_CHANGE_CONSTRAINT;
command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double pivotInB[3])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B;
command->m_userConstraintArguments.m_childFrame[0] = pivotInB[0];
command->m_userConstraintArguments.m_childFrame[1] = pivotInB[1];
command->m_userConstraintArguments.m_childFrame[2] = pivotInB[2];
return 0;
}
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double frameOrnInB[4])
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_USER_CONSTRAINT);
b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT);
command->m_updateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B;
command->m_userConstraintArguments.m_childFrame[3] = frameOrnInB[0];
command->m_userConstraintArguments.m_childFrame[4] = frameOrnInB[1];
command->m_userConstraintArguments.m_childFrame[5] = frameOrnInB[2];
command->m_userConstraintArguments.m_childFrame[6] = frameOrnInB[3];
return 0;
}
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -74,6 +74,10 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle);
b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet

57
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3460,8 +3460,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex);
if (parentBody && parentBody->m_multiBody)
{
InteralBodyData* childBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex);
if (childBody)
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
//also create a constraint with just a single multibody/rigid body without child
//if (childBody)
{
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]);
@@ -3470,7 +3471,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]);
if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType)
{
if (childBody->m_multiBody)
if (childBody && childBody->m_multiBody)
{
btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild);
multibodyFixed->setMaxAppliedImpulse(500.0);
@@ -3485,7 +3486,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
else
{
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild);
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild);
rigidbodyFixed->setMaxAppliedImpulse(500.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodyFixed);
@@ -3500,7 +3502,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType)
{
if (childBody->m_multiBody)
if (childBody && childBody->m_multiBody)
{
btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
multibodySlider->setMaxAppliedImpulse(500.0);
@@ -3514,7 +3516,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
else
{
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis);
rigidbodySlider->setMaxAppliedImpulse(500.0);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(rigidbodySlider);
@@ -3528,7 +3532,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType)
{
if (childBody->m_multiBody)
if (childBody && childBody->m_multiBody)
{
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500);
@@ -3542,7 +3546,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
else
{
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild);
btRigidBody* rb = childBody? childBody->m_rigidBody : 0;
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild);
p2p->setMaxAppliedImpulse(500);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(p2p);
@@ -3564,6 +3570,41 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
{
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove);
if (userConstraintPtr)
{
if (userConstraintPtr->m_mbConstraint)
{
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B)
{
btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0],
clientCmd.m_userConstraintArguments.m_childFrame[1],
clientCmd.m_userConstraintArguments.m_childFrame[2]);
userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB);
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B)
{
btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3],
clientCmd.m_userConstraintArguments.m_childFrame[4],
clientCmd.m_userConstraintArguments.m_childFrame[5],
clientCmd.m_userConstraintArguments.m_childFrame[6]);
btMatrix3x3 childFrameBasis(childFrameOrn);
userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis);
}
}
if (userConstraintPtr->m_rbConstraint)
{
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1;
serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED;
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT)
{
int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -535,7 +535,9 @@ enum EnumUserConstraintFlags
{
USER_CONSTRAINT_ADD_CONSTRAINT=1,
USER_CONSTRAINT_REMOVE_CONSTRAINT=2,
USER_CONSTRAINT_CHANGE_CONSTRAINT=4
USER_CONSTRAINT_CHANGE_CONSTRAINT=4,
USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8,
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
};
struct UserConstraintArgs