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

Merge pull request #1091 from YunfeiBai/master

Browse Source 
Add APIs to reset object mass, lateral friction coefficient, and to get user constraint id.
This commit is contained in:
erwincoumans 2017-05-04 17:47:33 +00:00 committed by GitHub
commit 87a24dba84
16 changed files with 265 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
examples/SharedMemory/PhysicsClient.h
View File

@ -37,6 +37,8 @@ public:
virtual int getNumUserConstraints() const = 0;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const = 0;
virtual int getUserConstraintId(int serialIndex) const = 0;
virtual void setSharedMemoryKey(int key) = 0;

41
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -1179,6 +1179,13 @@ int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniq
return 0;
}
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
return cl->getUserConstraintId(serialIndex);
}
/// return the body unique id, given the index in range [0 , b3GetNumBodies() )
int b3GetBodyUniqueId(b3PhysicsClientHandle physClient, int serialIndex)
{
@ -1208,7 +1215,41 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd
return cl->getJointInfo(bodyIndex, jointIndex, *info);
}
b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_RESET_DYNAMIC_INFO;
command->m_updateFlags = 0;
return (b3SharedMemoryCommandHandle) command;
}
int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
b3Assert(mass > 0);
command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
command->m_resetDynamicInfoArgs.m_mass = mass;
command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_MASS;
return 0;
}
int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_RESET_DYNAMIC_INFO);
command->m_resetDynamicInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_resetDynamicInfoArgs.m_linkIndex = linkIndex;
command->m_resetDynamicInfoArgs.m_lateralFriction = lateralFriction;
command->m_updateFlags |= RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION;
return 0;
}
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{

9
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -74,6 +74,13 @@ int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
///given a body and joint index, return the joint information. See b3JointInfo in SharedMemoryPublic.h
int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointIndex, struct b3JointInfo* info);
///given a body unique id and link index, return the dynamic information. See b3DynamicInfo in SharedMemoryPublic.h
int b3GetDynamicInfo(b3PhysicsClientHandle physClient, int bodyUniqueId, int linkIndex, struct b3DynamicInfo* info);
b3SharedMemoryCommandHandle b3InitResetDynamicInfo(b3PhysicsClientHandle physClient);
int b3ResetDynamicInfoSetMass(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double mass);
int b3ResetDynamicInfoSetLateralFriction(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkIndex, double lateralFriction);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
@ -90,6 +97,8 @@ b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHa
int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet
///See btIDebugDraw::DebugDrawModes in Bullet/src/LinearMath/btIDebugDraw.h

9
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@ -163,6 +163,15 @@ int PhysicsClientSharedMemory::getUserConstraintInfo(int constraintUniqueId, str
return 0;
}
int PhysicsClientSharedMemory::getUserConstraintId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
{
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
PhysicsClientSharedMemory::PhysicsClientSharedMemory()
{

2
examples/SharedMemory/PhysicsClientSharedMemory.h
View File

@ -47,6 +47,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
virtual void setSharedMemoryKey(int key);

9
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -925,7 +925,14 @@ int PhysicsDirect::getUserConstraintInfo(int constraintUniqueId, struct b3UserCo
return 0;
}
int PhysicsDirect::getUserConstraintId(int serialIndex) const
{
if ((serialIndex >= 0) && (serialIndex < getNumUserConstraints()))
{
return m_data->m_userConstraintInfoMap.getKeyAtIndex(serialIndex).getUid1();
}
return -1;
}
int PhysicsDirect::getBodyUniqueId(int serialIndex) const
{

2
examples/SharedMemory/PhysicsDirect.h
View File

@ -66,6 +66,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint& info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

5
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@ -108,6 +108,11 @@ int PhysicsLoopBack::getUserConstraintInfo(int constraintUniqueId, struct b3User
return m_data->m_physicsClient->getUserConstraintInfo( constraintUniqueId, info);
}
int PhysicsLoopBack::getUserConstraintId(int serialIndex) const
{
return m_data->m_physicsClient->getUserConstraintId(serialIndex);
}
///todo: move this out of the interface
void PhysicsLoopBack::setSharedMemoryKey(int key)
{

2
examples/SharedMemory/PhysicsLoopBack.h
View File

@ -51,6 +51,8 @@ public:
virtual int getNumUserConstraints() const;
virtual int getUserConstraintInfo(int constraintUniqueId, struct b3UserConstraint&info) const;
virtual int getUserConstraintId(int serialIndex) const;
///todo: move this out of the
virtual void setSharedMemoryKey(int key);

57
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -3901,6 +3901,63 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
break;
};
case CMD_RESET_DYNAMIC_INFO:
{
BT_PROFILE("CMD_RESET_DYNAMIC_INFO");
if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_MASS)
{
int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId;
int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex;
double mass = clientCmd.m_resetDynamicInfoArgs.m_mass;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (linkIndex == -1)
{
mb->setBaseMass(mass);
}
else
{
mb->getLink(linkIndex).m_mass = mass;
}
}
}
if (clientCmd.m_updateFlags & RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION)
{
int bodyUniqueId = clientCmd.m_resetDynamicInfoArgs.m_bodyUniqueId;
int linkIndex = clientCmd.m_resetDynamicInfoArgs.m_linkIndex;
double lateralFriction = clientCmd.m_resetDynamicInfoArgs.m_lateralFriction;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
InteralBodyData* body = m_data->getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (linkIndex == -1)
{
mb->getBaseCollider()->setFriction(lateralFriction);
}
else
{
mb->getLinkCollider(linkIndex)->setFriction(lateralFriction);
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
};
case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS:
{
BT_PROFILE("CMD_SEND_PHYSICS_SIMULATION_PARAMETERS");

17
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -106,6 +106,22 @@ struct BulletDataStreamArgs
char m_bodyName[MAX_FILENAME_LENGTH];
};
enum EnumResetDynamicInfoFlags
{
RESET_DYNAMIC_INFO_SET_MASS=1,
RESET_DYNAMIC_INFO_SET_COM=2,
RESET_DYNAMIC_INFO_SET_LATERAL_FRICTION=4,
};
struct ResetDynamicInfoArgs
{
int m_bodyUniqueId;
int m_linkIndex;
double m_mass;
double m_COM[3];
double m_lateralFriction;
};
struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
@ -702,6 +718,7 @@ struct SharedMemoryCommand
struct MjcfArgs m_mjcfArguments;
struct FileArgs m_fileArguments;
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
struct ResetDynamicInfoArgs m_resetDynamicInfoArgs;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;

6
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -55,6 +55,7 @@ enum EnumSharedMemoryClientCommand
CMD_CONFIGURE_OPENGL_VISUALIZER,
CMD_REQUEST_KEYBOARD_EVENTS_DATA,
CMD_REQUEST_OPENGL_VISUALIZER_CAMERA,
CMD_RESET_DYNAMIC_INFO,
//don't go beyond this command!
CMD_MAX_CLIENT_COMMANDS,
@ -215,6 +216,11 @@ struct b3BodyInfo
const char* m_bodyName; // for btRigidBody, it does not have a base, but can still have a body name from urdf
};
struct b3DynamicInfo
{
double m_mass;
double m_localInertialPosition[3];
};
// copied from btMultiBodyLink.h
enum SensorType {

4
examples/pybullet/examples/constraint.py
View File

@ -9,6 +9,8 @@ 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])
print cid
print p.getConstraintUniqueId(0)
prev=[0,0,1]
a=-math.pi
while 1:
@ -21,4 +23,4 @@ while 1:
orn = p.getQuaternionFromEuler([a,0,0])
p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50)
p.removeConstraint(cid)
p.removeConstraint(cid)

20
examples/pybullet/examples/reset_dynamic_info.py Normal file
View File

@ -0,0 +1,20 @@
import pybullet as p
import time
import math
p.connect(p.GUI)
p.loadURDF(fileName="plane.urdf",baseOrientation=[0.25882,0,0,0.96593])
p.loadURDF(fileName="cube.urdf",baseOrientation=[0.25882,0,0,0.96593],basePosition=[0,0,2])
p.loadURDF(fileName="cube.urdf",baseOrientation=[0,0,0,1],basePosition=[0,0,4])
p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=0.1)
#p.resetDynamicInfo(bodyUniqueId=2,linkIndex=-1,mass=100.0)
p.setGravity(0,0,-10)
p.setRealTimeSimulation(0)
t=0
while 1:
t=t+1
if t > 400:
p.resetDynamicInfo(bodyUniqueId=0,linkIndex=-1,lateralFriction=0.01)
time.sleep(.01)
p.stepSimulation()

78
examples/pybullet/pybullet.c
View File

@ -604,6 +604,49 @@ static PyObject* pybullet_loadMJCF(PyObject* self, PyObject* args, PyObject* key
return pylist;
}
static PyObject* pybullet_resetDynamicInfo(PyObject* self, PyObject* args, PyObject* keywds)
{
int bodyUniqueId = -1;
int linkIndex = -2;
double mass = -1;
double lateralFriction = -1;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "lateralFriction", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ddi", kwlist, &bodyUniqueId, &linkIndex,&mass, &lateralFriction, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
b3SharedMemoryCommandHandle command = b3InitResetDynamicInfo(sm);
b3SharedMemoryStatusHandle statusHandle;
if (mass >= 0)
{
b3ResetDynamicInfoSetMass(command, bodyUniqueId, linkIndex, mass);
}
if (lateralFriction >= 0)
{
b3ResetDynamicInfoSetLateralFriction(command, bodyUniqueId, linkIndex, lateralFriction);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds)
{
double fixedTimeStep = -1;
@ -1809,6 +1852,36 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb
return NULL;
}
static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
int serialIndex = -1;
b3PhysicsClientHandle sm = 0;
static char* kwlist[] = {"serialIndex", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &serialIndex, &physicsClientId))
{
return NULL;
}
sm = getPhysicsClient(physicsClientId);
if (sm == 0)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int userConstraintId = -1;
userConstraintId = b3GetUserConstraintId(sm, serialIndex);
#if PY_MAJOR_VERSION >= 3
return PyLong_FromLong(userConstraintId);
#else
return PyInt_FromLong(userConstraintId);
#endif
}
}
static PyObject* pybullet_getNumConstraints(PyObject* self, PyObject* args, PyObject* keywds)
{
int numConstraints = 0;
@ -5393,7 +5466,7 @@ static PyMethodDef SpamMethods[] = {
{"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS,
"Get the user-created constraint info, given a constraint unique id."},
{"getConstraintUniqueId", (PyCFunction)pybullet_getBodyUniqueId, METH_VARARGS | METH_KEYWORDS,
{"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS,
"Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},
{"getBasePositionAndOrientation", (PyCFunction)pybullet_getBasePositionAndOrientation,
@ -5435,6 +5508,9 @@ static PyMethodDef SpamMethods[] = {
{"resetJointState", (PyCFunction)pybullet_resetJointState, METH_VARARGS | METH_KEYWORDS,
"Reset the state (position, velocity etc) for a joint on a body "
"instantaneously, not through physics simulation."},
{"resetDynamicInfo", (PyCFunction)pybullet_resetDynamicInfo, METH_VARARGS | METH_KEYWORDS,
"Reset dynamic information such as mass, lateral friction coefficient."},
{"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS,
"This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead."

5
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@ -140,6 +140,11 @@ public:
return m_baseCollider;
}
btMultiBodyLinkCollider* getLinkCollider(int index)
{
return m_colliders[index];
}
//
// get parent
// input: link num from 0 to num_links-1