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

Add b3GetStatusActualState() to C_API.

Browse Source 
Change pick-and-move C_API to use handlers
This commit is contained in:
Erwin Coumans
2015-11-04 16:08:28 -08:00
parent d6464ce40d
commit 2317307a22
4 changed files with 100 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files

73
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -480,6 +480,41 @@ int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
} }
return bodyId; return bodyId;
} }
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
if (bodyUniqueId) {
*bodyUniqueId = args.m_bodyUniqueId;
}
if (numDegreeOfFreedomQ) {
*numDegreeOfFreedomQ = args.m_numDegreeOfFreedomQ;
}
if (numDegreeOfFreedomU) {
*numDegreeOfFreedomU = args.m_numDegreeOfFreedomU;
}
if (rootLocalInertialFrame) {
*rootLocalInertialFrame = args.m_rootLocalInertialFrame;
}
if (actualStateQ) {
*actualStateQ = args.m_actualStateQ;
}
if (actualStateQdot) {
*actualStateQdot = args.m_actualStateQdot;
}
if (jointReactionForces) {
*jointReactionForces = args.m_jointReactionForces;
}
return true;
}
int b3CanSubmitCommand(b3PhysicsClientHandle physClient) int b3CanSubmitCommand(b3PhysicsClientHandle physClient)
{ {
PhysicsClient* cl = (PhysicsClient* ) physClient; PhysicsClient* cl = (PhysicsClient* ) physClient;
@@ -526,42 +561,54 @@ void b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int linkInd
cl->getJointInfo(bodyIndex, linkIndex,*info); cl->getJointInfo(bodyIndex, linkIndex,*info);
} }
int b3PickBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ) double rayToWorldX, double rayToWorldY, double rayToWorldZ)
{ {
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_PICK_BODY); command->m_type = CMD_PICK_BODY;
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return 0; return (b3SharedMemoryCommandHandle)command;
} }
int b3MovePickedBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ) double rayToWorldX, double rayToWorldY,
double rayToWorldZ)
{ {
PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_MOVE_PICKED_BODY); command->m_type = CMD_MOVE_PICKED_BODY;
command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX; command->m_pickBodyArguments.m_rayFromWorld[0] = rayFromWorldX;
command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY; command->m_pickBodyArguments.m_rayFromWorld[1] = rayFromWorldY;
command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ; command->m_pickBodyArguments.m_rayFromWorld[2] = rayFromWorldZ;
command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX; command->m_pickBodyArguments.m_rayToWorld[0] = rayToWorldX;
command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY; command->m_pickBodyArguments.m_rayToWorld[1] = rayToWorldY;
command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ; command->m_pickBodyArguments.m_rayToWorld[2] = rayToWorldZ;
return 0; return (b3SharedMemoryCommandHandle)command;
} }
int b3RemovePickingConstraint(b3SharedMemoryCommandHandle commandHandle) b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient)
{ {
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; PhysicsClient *cl = (PhysicsClient *)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand *command = cl->getAvailableSharedMemoryCommand();
b3Assert(command); b3Assert(command);
b3Assert(command->m_type == CMD_REMOVE_PICKING_CONSTRAINT_BODY); command->m_type = CMD_REMOVE_PICKING_CONSTRAINT_BODY;
return 0; return (b3SharedMemoryCommandHandle)command;
} }
b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode) b3SharedMemoryCommandHandle b3InitRequestDebugLinesCommand(b3PhysicsClientHandle physClient, int debugMode)

22
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -36,6 +36,14 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle); int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* numDegreeOfFreedomQ,
int* numDegreeOfFreedomU,
const double* rootLocalInertialFrame[],
const double* actualStateQ[],
const double* actualStateQdot[],
const double* jointReactionForces[]);
int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex); int b3GetNumJoints(b3PhysicsClientHandle physClient, int bodyIndex);
@@ -103,14 +111,14 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in
b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId);
void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state);
int b3PickBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ); double rayToWorldX, double rayToWorldY, double rayToWorldZ);
int b3MovePickedBody(struct SharedMemoryCommand *command, b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ); double rayToWorldX, double rayToWorldY,
int b3RemovePickingConstraint(struct SharedMemoryCommand *command); double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
#ifdef __cplusplus #ifdef __cplusplus
} }

4
examples/SharedMemory/PhysicsServer.cpp
View File

@@ -795,9 +795,11 @@ void PhysicsServerSharedMemory::createJointMotors(btMultiBody* mb)
bool PhysicsServerSharedMemory::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, bool PhysicsServerSharedMemory::loadUrdf(const char* fileName2, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr) bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr)
{ {
const char* fileName="tmp/model/simulation/model_s_one_s15p1.urdf";
btAssert(m_data->m_dynamicsWorld); btAssert(m_data->m_dynamicsWorld);
if (!m_data->m_dynamicsWorld) if (!m_data->m_dynamicsWorld)
{ {

8
test/SharedMemory/test.c
View File

@@ -128,13 +128,12 @@ int main(int argc, char* argv[])
if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{ {
#if 0 b3GetStatusActualState(statusHandle,
posVarCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomQ; 0, &posVarCount, &dofCount,
dofCount =status.m_sendActualStateArgs.m_numDegreeOfFreedomU; 0, 0, 0, 0);
b3Printf("posVarCount = %d\n",posVarCount); b3Printf("posVarCount = %d\n",posVarCount);
printf("dofCount = %d\n",dofCount); printf("dofCount = %d\n",dofCount);
#endif
} }
} }
@@ -177,7 +176,6 @@ int main(int argc, char* argv[])
struct b3JointSensorState sensorState; struct b3JointSensorState sensorState;
b3GetJointState(sm,state,sensorJointIndexRight,&sensorState); b3GetJointState(sm,state,sensorJointIndexRight,&sensorState);
b3GetJointInfo(sm,bodyIndex,sensorJointIndexRight,&sensorState);
b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight, b3Printf("Sensor for joint [%d] = %f,%f,%f\n", sensorJointIndexRight,
sensorState.m_jointForceTorque[0], sensorState.m_jointForceTorque[0],
sensorState.m_jointForceTorque[1], sensorState.m_jointForceTorque[1],