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

enable btGearConstraint, expose 'changeDynamics' for gearRatio, only works for maximalCoordinates rigid bodies.

Browse Source 
See examples\pybullet\examples\mimicJointConstraint.py
This commit is contained in:
Erwin Coumans
2017-06-07 13:44:34 -07:00
parent d08f3e5f91
commit 60e3887456
7 changed files with 94 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1704,7 +1704,18 @@ int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHan
return 0;
}
int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio)
{
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_GEAR_RATIO;
command->m_userConstraintArguments.m_gearRatio = gearRatio;
return 0;
}
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -102,7 +102,9 @@ b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHa
int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]);
int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]);
int b3InitChangeUserConstraintSetMaxForce(b3SharedMemoryCommandHandle commandHandle, double maxAppliedForce);
int b3InitChangeUserConstraintSetGearRatio(b3SharedMemoryCommandHandle commandHandle, double gearRatio);
b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);

54
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5877,7 +5877,48 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
}
}
else
{
InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->m_bodyHandles.getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0;
if (parentBody && childBody)
{
if (parentBody->m_rigidBody)
{
if (clientCmd.m_userConstraintArguments.m_jointType == eGearType)
{
btRigidBody* childRb = childBody->m_rigidBody;
if (childRb)
{
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(*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;
}
}
}
}
}
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT)
@@ -5921,7 +5962,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (userConstraintPtr->m_rbConstraint)
{
//todo
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
if (userConstraintPtr->m_rbConstraint->getObjectType()==GEAR_CONSTRAINT_TYPE)
{
btGearConstraint* gear = (btGearConstraint*) userConstraintPtr->m_rbConstraint;
gear->setRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
}
}
}
serverCmd.m_userConstraintResultArgs = clientCmd.m_userConstraintArguments;
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidChange;
@@ -5944,7 +5992,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
if (userConstraintPtr->m_rbConstraint)
{
m_data->m_dynamicsWorld->removeConstraint(userConstraintPtr->m_rbConstraint);
delete userConstraintPtr->m_rbConstraint;
m_data->m_userConstraints.remove(userConstraintUidRemove);
}
serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = userConstraintUidRemove;
serverCmd.m_type = CMD_REMOVE_USER_CONSTRAINT_COMPLETED;

2
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -627,7 +627,7 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16,
USER_CONSTRAINT_CHANGE_MAX_FORCE=32,
USER_CONSTRAINT_REQUEST_INFO=64,
USER_CONSTRAINT_CHANGE_GEAR_RATIO=128,
};
enum EnumBodyChangeFlags

3
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -184,6 +184,7 @@ enum JointType {
ePlanarType = 3,
eFixedType = 4,
ePoint2PointType = 5,
eGearType=6
};
@@ -227,6 +228,8 @@ struct b3UserConstraint
int m_jointType;
double m_maxAppliedForce;
int m_userConstraintUniqueId;
double m_gearRatio;
};
struct b3BodyInfo

16
examples/pybullet/examples/mimicJointConstraint.py Normal file
View File

@@ -0,0 +1,16 @@
#mimic joint currently only works for 'maximal coordinate' rigid bodies
#one way to use it would be to attach maximal coordinate rigid bodies to multibody links using
#fixed constraints
import pybullet as p
import time
p.connect(p.GUI)
wheelA = p.loadURDF("wheel.urdf",[0,0,0],useMaximalCoordinates=1)
wheelB = p.loadURDF("wheel.urdf",[0,0,1],useMaximalCoordinates=1)
c = p.createConstraint(wheelA,-1,wheelB,-1,jointType=p.JOINT_GEAR,jointAxis =[0,1,0],parentFramePosition=[0,0,0],childFramePosition=[0,0,0])
p.changeConstraint(c,gearRatio=-0.1)
p.setRealTimeSimulation(1)
while(1):
time.sleep(0.01)
p.removeConstraint(c)

12
examples/pybullet/pybullet.c
View File

@@ -4608,7 +4608,7 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py
static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject* keywds)
{
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "physicsClientId", NULL};
static char* kwlist[] = {"userConstraintUniqueId", "jointChildPivot", "jointChildFrameOrientation", "maxForce", "gearRatio", "physicsClientId", NULL};
int userConstraintUniqueId = -1;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -4620,8 +4620,8 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
double jointChildPivot[3];
double jointChildFrameOrn[4];
double maxForce = -1;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOdi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &physicsClientId))
double gearRatio = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOddi", kwlist, &userConstraintUniqueId, &jointChildPivotObj, &jointChildFrameOrnObj, &maxForce, &gearRatio, &physicsClientId))
{
return NULL;
}
@@ -4647,7 +4647,10 @@ static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, P
{
b3InitChangeUserConstraintSetMaxForce(commandHandle, maxForce);
}
if (gearRatio!=0)
{
b3InitChangeUserConstraintSetGearRatio(commandHandle,gearRatio);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
Py_INCREF(Py_None);
@@ -6769,6 +6772,7 @@ initpybullet(void)
PyModule_AddIntConstant(m, "JOINT_PLANAR", ePlanarType); // user read
PyModule_AddIntConstant(m, "JOINT_FIXED", eFixedType); // user read
PyModule_AddIntConstant(m, "JOINT_POINT2POINT", ePoint2PointType); // user read
PyModule_AddIntConstant(m, "JOINT_GEAR", eGearType); // user read
PyModule_AddIntConstant(m, "SENSOR_FORCE_TORQUE", eSensorForceTorqueType); // user read