mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 05:40:05 +00:00
add pybullet.changeConstraint / b3InitChangeUserConstraintCommand/ b3InitChangeUserConstraintSetPivotInB /b3InitChangeUserConstraintSetFrameInB command, to change an existing user constraint.
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
parent
1c79798583
commit
9aa5a839d5
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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
|
||||
|
||||
24
examples/pybullet/constraint.py
Normal file
24
examples/pybullet/constraint.py
Normal file
@ -0,0 +1,24 @@
|
||||
import pybullet as p
|
||||
import time
|
||||
import math
|
||||
|
||||
p.connect(p.GUI)
|
||||
|
||||
p.loadURDF("plane.urdf")
|
||||
cubeId = p.loadURDF("cube_small.urdf",0,0,1)
|
||||
p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(1)
|
||||
cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1])
|
||||
|
||||
a=-math.pi
|
||||
while 1:
|
||||
a=a+0.01
|
||||
if (a>math.pi):
|
||||
a=-math.pi
|
||||
time.sleep(.01)
|
||||
p.setGravity(0,0,-10)
|
||||
pivot=[a,0,1]
|
||||
orn = p.getQuaternionFromEuler([a,0,0])
|
||||
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn)
|
||||
|
||||
p.removeConstraint(cid)
|
||||
@ -3028,6 +3028,51 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
|
||||
return Py_None;
|
||||
}
|
||||
|
||||
|
||||
static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation", "physicsClientId", NULL};
|
||||
int userConstraintUniqueId=-1;
|
||||
b3SharedMemoryCommandHandle commandHandle;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
int physicsClientId = 0;
|
||||
b3PhysicsClientHandle sm = 0;
|
||||
PyObject* jointChildPivotObj=0;
|
||||
PyObject* jointChildFrameOrnObj=0;
|
||||
double jointChildPivot[3];
|
||||
double jointChildFrameOrn[4];
|
||||
|
||||
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&physicsClientId))
|
||||
{
|
||||
return NULL;
|
||||
}
|
||||
|
||||
sm = getPhysicsClient(physicsClientId);
|
||||
if (sm == 0)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
commandHandle = b3InitChangeUserConstraintCommand(sm,userConstraintUniqueId);
|
||||
|
||||
if (pybullet_internalSetVectord(jointChildPivotObj,jointChildPivot))
|
||||
{
|
||||
b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot);
|
||||
}
|
||||
if (pybullet_internalSetVector4d(jointChildFrameOrnObj,jointChildFrameOrn))
|
||||
{
|
||||
b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn);
|
||||
}
|
||||
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
Py_INCREF(Py_None);
|
||||
return Py_None;
|
||||
};
|
||||
|
||||
|
||||
static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds)
|
||||
{
|
||||
static char *kwlist[] = { "userConstraintUniqueId","physicsClientId", NULL};
|
||||
@ -4372,6 +4417,11 @@ static PyMethodDef SpamMethods[] = {
|
||||
"Create a constraint between two bodies. Returns a (int) unique id, if successfull."
|
||||
},
|
||||
|
||||
{"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS,
|
||||
"Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id."
|
||||
},
|
||||
|
||||
|
||||
{"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS,
|
||||
"Remove a constraint using its unique id."
|
||||
},
|
||||
|
||||
@ -90,6 +90,10 @@ public:
|
||||
void updateJacobianSizes();
|
||||
void allocateJacobiansMultiDof();
|
||||
|
||||
//many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later.
|
||||
virtual void setFrameInB(const btMatrix3x3& frameInB) {}
|
||||
virtual void setPivotInB(const btVector3& pivotInB){}
|
||||
|
||||
virtual void finalizeMultiDof()=0;
|
||||
|
||||
virtual int getIslandIdA() const =0;
|
||||
|
||||
@ -62,7 +62,7 @@ public:
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
void setPivotInB(const btVector3& pivotInB)
|
||||
virtual void setPivotInB(const btVector3& pivotInB)
|
||||
{
|
||||
m_pivotInB = pivotInB;
|
||||
}
|
||||
@ -82,7 +82,7 @@ public:
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
void setFrameInB(const btMatrix3x3& frameInB)
|
||||
virtual void setFrameInB(const btMatrix3x3& frameInB)
|
||||
{
|
||||
m_frameInB = frameInB;
|
||||
}
|
||||
|
||||
@ -53,11 +53,12 @@ public:
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
void setPivotInB(const btVector3& pivotInB)
|
||||
virtual void setPivotInB(const btVector3& pivotInB)
|
||||
{
|
||||
m_pivotInB = pivotInB;
|
||||
}
|
||||
|
||||
|
||||
virtual void debugDraw(class btIDebugDraw* drawer);
|
||||
|
||||
};
|
||||
|
||||
@ -63,7 +63,7 @@ public:
|
||||
return m_pivotInB;
|
||||
}
|
||||
|
||||
void setPivotInB(const btVector3& pivotInB)
|
||||
virtual void setPivotInB(const btVector3& pivotInB)
|
||||
{
|
||||
m_pivotInB = pivotInB;
|
||||
}
|
||||
@ -83,7 +83,7 @@ public:
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
void setFrameInB(const btMatrix3x3& frameInB)
|
||||
virtual void setFrameInB(const btMatrix3x3& frameInB)
|
||||
{
|
||||
m_frameInB = frameInB;
|
||||
}
|
||||
|
||||
Loading…
Reference in New Issue
Block a user