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

Add API to reset mass.

Browse Source
This commit is contained in:
yunfeibai
2017-05-01 22:18:54 -07:00
parent fbd1a4ec2c
commit 4da2c076a7
6 changed files with 123 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

22
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1208,7 +1208,29 @@ 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);
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;
}
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info)
{

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -75,6 +75,12 @@ 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);
b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info);
///return a unique id for the user constraint, after successful creation, or -1 for an invalid constraint id

33
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -3732,6 +3732,39 @@ 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;
}
}
}
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");

15
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -106,6 +106,20 @@ struct BulletDataStreamArgs
char m_bodyName[MAX_FILENAME_LENGTH];
};
enum EnumResetDynamicInfoFlags
{
RESET_DYNAMIC_INFO_SET_MASS=1,
RESET_DYNAMIC_INFO_SET_COM=2,
};
struct ResetDynamicInfoArgs
{
int m_bodyUniqueId;
int m_linkIndex;
double m_mass;
double m_COM[3];
};
struct SetJointFeedbackArgs
{
int m_bodyUniqueId;
@@ -702,6 +716,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;

7
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,
@@ -207,6 +208,12 @@ 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];
double m_localInertialOrientation[4];
};
// copied from btMultiBodyLink.h
enum SensorType {

40
examples/pybullet/pybullet.c
View File

@@ -604,6 +604,43 @@ 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;
b3PhysicsClientHandle sm = 0;
int physicsClientId = 0;
static char* kwlist[] = {"bodyUniqueId", "linkIndex", "mass", "physicsClientId", NULL};
if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|di", kwlist, &bodyUniqueId, &linkIndex,&mass, &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);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* args, PyObject* keywds)
{
double fixedTimeStep = -1;
@@ -5436,6 +5473,9 @@ static PyMethodDef SpamMethods[] = {
"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, COM."},
{"setJointMotorControl", (PyCFunction)pybullet_setJointMotorControl, METH_VARARGS,
"This (obsolete) method cannot select non-zero physicsClientId, use setJointMotorControl2 instead."
"Set a single joint motor control mode and desired target value. There is "