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

implement a few more pybullet methods:

Browse Source 
pybullet_applyExternalForce, pybullet_applyExternalTorque, pybullet_setTimeStep,
pybullet_resetBasePositionAndOrientation,
pybullet_getQuaternionFromEuler,
pybullet_getEulerFromQuaternion
This commit is contained in:
Erwin Coumans
2016-06-26 18:18:30 -07:00
parent a15eb3035e
commit 013dbda023
7 changed files with 547 additions and 149 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -903,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_APPLY_EXTERNAL_FORCE;
command->m_updateFlags = 0;
command->m_externalForceArguments.m_numForcesAndTorques = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
command->m_externalForceArguments.m_positions[index+i] = position[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -159,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
#ifdef __cplusplus
}
#endif

64
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -2056,6 +2056,70 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_APPLY_EXTERNAL_FORCE:
{
for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
{
InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
btVector3 positionLocal(
clientCmd.m_externalForceArguments.m_positions[i*3+0],
clientCmd.m_externalForceArguments.m_positions[i*3+1],
clientCmd.m_externalForceArguments.m_positions[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal;
mb->addBaseForce(forceWorld);
mb->addBaseTorque(relPosWorld.cross(forceWorld));
//b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
} else
{
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal;
btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal;
mb->addLinkForce(link, forceWorld);
mb->addLinkTorque(link,relPosWorld.cross(forceWorld));
//b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
}
}
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0)
{
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
mb->addBaseTorque(torqueWorld);
//b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
} else
{
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal;
mb->addLinkTorque(link, torqueWorld);
//b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");

22
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -319,6 +319,27 @@ struct SdfRequestInfoArgs
int m_bodyUniqueId;
};
///flags for b3ApplyExternalTorque and b3ApplyExternalForce
enum EnumExternalForcePrivateFlags
{
// EF_LINK_FRAME=1,
// EF_WORLD_FRAME=2,
EF_TORQUE=4,
EF_FORCE=8,
};
struct ExternalForceArgs
{
int m_numForcesAndTorques;
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_linkIds[MAX_SDF_BODIES];
double m_forcesAndTorques[3*MAX_SDF_BODIES];
double m_positions[3*MAX_SDF_BODIES];
int m_forceFlags[MAX_SDF_BODIES];
};
enum EnumSdfRequestInfoFlags
{
SDF_REQUEST_INFO_BODY=1,
@@ -350,6 +371,7 @@ struct SharedMemoryCommand
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
struct RequestPixelDataArgs m_requestPixelDataArguments;
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
};
};

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -26,6 +26,7 @@ enum EnumSharedMemoryClientCommand
CMD_MOVE_PICKED_BODY,
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_MAX_CLIENT_COMMANDS
};
@@ -143,5 +144,11 @@ enum {
CONTROL_MODE_POSITION_VELOCITY_PD,
};
///flags for b3ApplyExternalTorque and b3ApplyExternalForce
enum EnumExternalForceFlags
{
EF_LINK_FRAME=1,
EF_WORLD_FRAME=2,
};
#endif//SHARED_MEMORY_PUBLIC_H

2
examples/SharedMemory/premake4.lua
View File

@@ -26,6 +26,8 @@ files {
"PhysicsServer.h",
"main.cpp",
"PhysicsClientC_API.cpp",
"SharedMemoryCommands.h",
"SharedMemoryPublic.h",
"PhysicsServer.cpp",
"PosixSharedMemory.cpp",
"Win32SharedMemory.cpp",