Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests 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
parent 7c439b2b6a 939d6ead32
commit 87a24dba84
16 changed files with 265 additions and 3 deletions
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 {