AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 05:10:08 +00:00
Code Issues Releases Wiki Activity

make one of the cubes in two_cubes.sdf static (immovable) using the <static> tag in SDF

Browse Source 
add an example using 'direct'
fix the send-desired-state commands, to add flags for arguments set, using default values.
Start exposing SDF loading in shared memory api, not fully implemented yet.
This commit is contained in:
erwin coumans 2016-06-03 19:03:56 -07:00
parent 0b69ba7f61
commit 4b2c0f6d89
11 changed files with 202 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
data/two_cubes.sdf
View File

@ -86,6 +86,7 @@
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='unit_box_0'>
<static>1</static>
<pose frame=''>0.512455 -1.58317 0.5 0 -0 0</pose>
<link name='unit_box_0::link'>
<inertial>

5
examples/ExampleBrowser/ExampleEntries.cpp
View File

@ -242,7 +242,10 @@ static ExampleEntry gDefaultExamples[]=
ExampleEntry(1,"Physics Server (Replay Log)", "Create a physics server that replay a command log from disk.",
PhysicsServerCreateFunc,PHYSICS_SERVER_REPLAY_FROM_COMMAND_LOG),
ExampleEntry(1, "Physics Client", "Create a physics client that can communicate with a physics server over shared memory", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Shared Mem)", "Create a physics client that can communicate with a physics server over shared memory.", PhysicsClientCreateFunc),
ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
#ifdef ENABLE_LUA

30
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -5,6 +5,29 @@
#include "SharedMemoryCommands.h"
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_LOAD_SDF;
int len = strlen(sdfFileName);
if (len<MAX_SDF_FILENAME_LENGTH)
{
strcpy(command->m_sdfArguments.m_sdfFileName,sdfFileName);
} else
{
command->m_sdfArguments.m_sdfFileName[0] = 0;
}
command->m_updateFlags = SDF_ARGS_FILE_NAME;
return (b3SharedMemoryCommandHandle) command;
}
b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName)
{
@ -146,6 +169,8 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
return 0;
}
@ -154,6 +179,7 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
return 0;
}
@ -162,6 +188,8 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
return 0;
}
@ -170,6 +198,7 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
return 0;
}
@ -179,6 +208,7 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}

2
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -88,6 +88,8 @@ int b3LoadUrdfCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHand
int b3LoadUrdfCommandSetUseMultiBody(b3SharedMemoryCommandHandle commandHandle, int useMultiBody);
int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle, int useFixedBase);
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///Set joint control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);

16
examples/SharedMemory/PhysicsClientExample.cpp
View File

@ -156,16 +156,12 @@ protected:
{
for (int i=0;i<m_numMotors;i++)
{
// btScalar targetVel = m_motorTargetVelocities[i].m_velTarget;
// int uIndex = m_motorTargetVelocities[i].m_uIndex;
// b3JointControlSetDesiredVelocity(commandHandle, uIndex,targetVel);
btScalar targetPos = m_motorTargetPositions[i].m_posTarget;
int qIndex = m_motorTargetPositions[i].m_qIndex;
int uIndex = m_motorTargetPositions[i].m_uIndex;
b3JointControlSetDesiredPosition(commandHandle, qIndex, targetPos);
b3JointControlSetKp(commandHandle, uIndex, 0.1);
b3JointControlSetKd(commandHandle, uIndex, 0.0);
b3JointControlSetMaximumForce(commandHandle,uIndex,1000);
}
@ -430,6 +426,11 @@ PhysicsClientExample::~PhysicsClientExample()
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas && (m_canvasIndex>=0))
{
m_canvas->destroyCanvas(m_canvasIndex);
}
b3Printf("~PhysicsClientExample\n");
}
@ -563,9 +564,14 @@ void PhysicsClientExample::initPhysics()
m_isOptionalServerConnected = m_physicsServer.connectSharedMemory( m_guiHelper);
}
if (m_options == eCLIENTEXAMPLE_DIRECT)
{
m_physicsClientHandle = b3ConnectPhysicsDirect();
} else
{
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
//m_physicsClientHandle = b3ConnectPhysicsDirect();
}
if (!b3CanSubmitCommand(m_physicsClientHandle))
{

2
examples/SharedMemory/PhysicsClientExample.h
View File

@ -4,7 +4,7 @@
enum ClientExampleOptions
{
eCLIENTEXAMPLE_LOOPBACK=1,
eCLIENTEAXMPLE_DIRECT=2,
eCLIENTEXAMPLE_DIRECT=2,
eCLIENTEXAMPLE_SERVER=3,
};

86
examples/SharedMemory/PhysicsDirect.cpp
View File

@ -33,6 +33,12 @@ struct PhysicsDirectInternalData
char m_bulletStreamDataServerToClient[SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE];
int m_cachedCameraPixelsWidth;
int m_cachedCameraPixelsHeight;
btAlignedObjectArray<unsigned char> m_cachedCameraPixelsRGBA;
btAlignedObjectArray<float> m_cachedCameraDepthBuffer;
PhysicsServerCommandProcessor* m_commandProcessor;
PhysicsDirectInternalData()
@ -167,6 +173,73 @@ bool PhysicsDirect::processDebugLines(const struct SharedMemoryCommand& orgComma
return m_data->m_hasStatus;
}
bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
btAssert(m_data->m_serverStatus.m_type == CMD_CAMERA_IMAGE_COMPLETED);
if (m_data->m_verboseOutput)
{
b3Printf("Camera image OK\n");
}
int numBytesPerPixel = 4;//RGBA
int numTotalPixels = serverCmd.m_sendPixelDataArguments.m_startingPixelIndex+
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied+
serverCmd.m_sendPixelDataArguments.m_numRemainingPixels;
m_data->m_cachedCameraPixelsWidth = 0;
m_data->m_cachedCameraPixelsHeight = 0;
int numPixels = serverCmd.m_sendPixelDataArguments.m_imageWidth*serverCmd.m_sendPixelDataArguments.m_imageHeight;
m_data->m_cachedCameraPixelsRGBA.reserve(numPixels*numBytesPerPixel);
m_data->m_cachedCameraDepthBuffer.resize(numTotalPixels);
m_data->m_cachedCameraPixelsRGBA.resize(numTotalPixels*numBytesPerPixel);
unsigned char* rgbaPixelsReceived =
(unsigned char*)&m_data->m_bulletStreamDataServerToClient[0];
printf("pixel = %d\n", rgbaPixelsReceived[0]);
for (int i=0;i<serverCmd.m_sendPixelDataArguments.m_numPixelsCopied*numBytesPerPixel;i++)
{
m_data->m_cachedCameraPixelsRGBA[i + serverCmd.m_sendPixelDataArguments.m_startingPixelIndex*numBytesPerPixel]
= rgbaPixelsReceived[i];
}
if (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0)
{
// continue requesting remaining pixels
command.m_type = CMD_REQUEST_CAMERA_IMAGE_DATA;
command.m_requestPixelDataArguments.m_startPixelIndex =
serverCmd.m_sendPixelDataArguments.m_startingPixelIndex +
serverCmd.m_sendPixelDataArguments.m_numPixelsCopied;
} else
{
m_data->m_cachedCameraPixelsWidth = serverCmd.m_sendPixelDataArguments.m_imageWidth;
m_data->m_cachedCameraPixelsHeight = serverCmd.m_sendPixelDataArguments.m_imageHeight;
}
}
} while (serverCmd.m_sendPixelDataArguments.m_numRemainingPixels > 0);
return m_data->m_hasStatus;
}
bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& command)
{
if (command.m_type==CMD_REQUEST_DEBUG_LINES)
@ -174,6 +247,11 @@ bool PhysicsDirect::submitClientCommand(const struct SharedMemoryCommand& comman
return processDebugLines(command);
}
if (command.m_type==CMD_REQUEST_CAMERA_IMAGE_DATA)
{
return processCamera(command);
}
bool hasStatus = m_data->m_commandProcessor->processCommand(command,m_data->m_serverStatus,&m_data->m_bulletStreamDataServerToClient[0],SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
m_data->m_hasStatus = hasStatus;
if (hasStatus)
@ -333,9 +411,9 @@ void PhysicsDirect::getCachedCameraImage(b3CameraImageData* cameraData)
{
if (cameraData)
{
cameraData->m_pixelHeight = 0;
cameraData->m_pixelWidth = 0;
cameraData->m_depthValues = 0;
cameraData->m_rgbColorData = 0;
cameraData->m_pixelWidth = m_data->m_cachedCameraPixelsWidth;
cameraData->m_pixelHeight = m_data->m_cachedCameraPixelsHeight;
cameraData->m_depthValues = m_data->m_cachedCameraDepthBuffer.size() ? &m_data->m_cachedCameraDepthBuffer[0] : 0;
cameraData->m_rgbColorData = m_data->m_cachedCameraPixelsRGBA.size() ? &m_data->m_cachedCameraPixelsRGBA[0] : 0;
}
}

2
examples/SharedMemory/PhysicsDirect.h
View File

@ -19,6 +19,8 @@ protected:
bool processDebugLines(const struct SharedMemoryCommand& orgCommand);
bool processCamera(const struct SharedMemoryCommand& orgCommand);
public:
PhysicsDirect();

47
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -1192,12 +1192,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->clearForcesAndTorques();
int torqueIndex = 0;
btVector3 f(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
btVector3 f(0,0,0);
btVector3 t(0,0,0);
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
{
f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]);
btVector3 t(clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4],
clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]);
}
torqueIndex+=6;
mb->addBaseForce(f);
mb->addBaseTorque(t);
@ -1206,7 +1212,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)
{
double torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
double torque = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex];
mb->addJointTorqueMultiDof(link,dof,torque);
torqueIndex++;
}
@ -1233,10 +1241,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (motorPtr)
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0)
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex];
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
@ -1247,6 +1260,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
}
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
if (m_data->m_verboseOutput)
@ -1271,11 +1285,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
btMultiBodyJointMotor* motor = *motorPtr;
btScalar desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
btScalar desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
btScalar desiredVelocity = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0)
desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex];
btScalar desiredPosition = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0)
desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex];
btScalar kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
btScalar kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
btScalar kp = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0)
kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex];
btScalar kd = 0.f;
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0)
kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex];
int dof1 = 0;
btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1];
@ -1288,9 +1310,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
motor->setVelocityTarget(desiredVelocity);
btScalar maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(1000);//maxImp);
if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0)
maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp);
numMotors++;
}

21
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -30,6 +30,7 @@
#define MAX_DEGREE_OF_FREEDOM 64
#define MAX_NUM_SENSORS 256
#define MAX_URDF_FILENAME_LENGTH 1024
#define MAX_SDF_FILENAME_LENGTH 1024
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
@ -54,6 +55,16 @@ TmpFloat3 CreateTmpFloat3(float x, float y, float z)
return tmp;
}
enum EnumSdfArgsUpdateFlags
{
SDF_ARGS_FILE_NAME=1,
};
struct SdfArgs
{
char m_sdfFileName[MAX_URDF_FILENAME_LENGTH];
};
enum EnumUrdfArgsUpdateFlags
{
URDF_ARGS_FILE_NAME=1,
@ -179,6 +190,15 @@ struct SendDesiredStateArgs
};
enum EnumSimDesiredStateUpdateFlags
{
SIM_DESIRED_STATE_HAS_Q=1,
SIM_DESIRED_STATE_HAS_QDOT=2,
SIM_DESIRED_STATE_HAS_KD=4,
SIM_DESIRED_STATE_HAS_KP=8,
SIM_DESIRED_STATE_HAS_MAX_FORCE=16,
};
enum EnumSimParamUpdateFlags
{
@ -287,6 +307,7 @@ struct SharedMemoryCommand
union
{
struct UrdfArgs m_urdfArguments;
struct SdfArgs m_sdfArguments;
struct InitPoseArgs m_initPoseArgs;
struct SendPhysicsSimulationParameters m_physSimParamArgs;
struct BulletDataStreamArgs m_dataStreamArguments;

4
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -5,6 +5,7 @@
enum EnumSharedMemoryClientCommand
{
CMD_LOAD_SDF,
CMD_LOAD_URDF,
CMD_SEND_BULLET_DATA_STREAM,
CMD_CREATE_BOX_COLLISION_SHAPE,
@ -35,7 +36,8 @@ enum EnumSharedMemoryServerStatus
CMD_CLIENT_COMMAND_COMPLETED,
//the server will skip unknown command and report a status 'CMD_UNKNOWN_COMMAND_FLUSHED'
CMD_UNKNOWN_COMMAND_FLUSHED,
CMD_SDF_LOADING_COMPLETED,
CMD_SDF_LOADING_FAILED,
CMD_URDF_LOADING_COMPLETED,
CMD_URDF_LOADING_FAILED,
CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,