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

@@ -74,6 +74,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);

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 {