mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-13 13:20:07 +00:00
Merge pull request #654 from erwincoumans/master
b3LoadSdfCommandInit , pybullet loadSDF , Fix pybullet Windows build errors
This commit is contained in:
erwincoumans
GitHub
commit
2b3cb55ac1
@ -191,14 +191,15 @@ IF (APPLE)
|
||||
ENDIF()
|
||||
|
||||
OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON)
|
||||
|
||||
FIND_PACKAGE(PythonLibs)
|
||||
OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF)
|
||||
|
||||
|
||||
IF(BUILD_PYBULLET)
|
||||
IF(WIN32)
|
||||
FIND_PACKAGE(PythonLibs 3.4 REQUIRED)
|
||||
SET(BUILD_SHARED_LIBS OFF CACHE BOOL "Shared Libs" FORCE)
|
||||
ELSE(WIN32)
|
||||
FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
|
||||
SET(BUILD_SHARED_LIBS ON CACHE BOOL "Shared Libs" FORCE)
|
||||
ENDIF(WIN32)
|
||||
ENDIF(BUILD_PYBULLET)
|
||||
|
||||
@ -519,6 +519,33 @@ int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
|
||||
return CMD_INVALID_STATUS;
|
||||
}
|
||||
|
||||
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity)
|
||||
{
|
||||
int numBodies = 0;
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
b3Assert(status);
|
||||
|
||||
if (status)
|
||||
{
|
||||
switch (status->m_type)
|
||||
{
|
||||
case CMD_SDF_LOADING_COMPLETED:
|
||||
{
|
||||
int i,maxBodies;
|
||||
numBodies = status->m_sdfLoadedArgs.m_numBodies;
|
||||
maxBodies = btMin(bodyIndicesCapacity, numBodies);
|
||||
for (i=0;i<maxBodies;i++)
|
||||
{
|
||||
bodyIndicesOut[i] = status->m_sdfLoadedArgs.m_bodyUniqueIds[i];
|
||||
}
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
return numBodies;
|
||||
}
|
||||
|
||||
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle)
|
||||
{
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
@ -558,6 +585,10 @@ int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
|
||||
const double* jointReactionForces[]) {
|
||||
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
|
||||
const SendActualStateArgs &args = status->m_sendActualStateArgs;
|
||||
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
|
||||
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
|
||||
return false;
|
||||
|
||||
if (bodyUniqueId) {
|
||||
*bodyUniqueId = args.m_bodyUniqueId;
|
||||
}
|
||||
|
||||
@ -40,6 +40,8 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
|
||||
/// Get the physics server return status type. See EnumSharedMemoryServerStatus in SharedMemoryPublic.h for error codes.
|
||||
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle);
|
||||
|
||||
int b3GetStatusBodyIndices(b3SharedMemoryStatusHandle statusHandle, int* bodyIndicesOut, int bodyIndicesCapacity);
|
||||
|
||||
int b3GetStatusBodyIndex(b3SharedMemoryStatusHandle statusHandle);
|
||||
|
||||
int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle,
|
||||
|
||||
@ -77,9 +77,9 @@ protected:
|
||||
|
||||
virtual void resetCamera()
|
||||
{
|
||||
float dist = 1.1;
|
||||
float pitch = 50;
|
||||
float yaw = 35;
|
||||
float dist = 4;
|
||||
float pitch = 193;
|
||||
float yaw = 25;
|
||||
float targetPos[3]={0,0,0.5};//-3,2.8,-2.5};
|
||||
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
|
||||
|
||||
@ -223,9 +223,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
{
|
||||
case CMD_LOAD_URDF:
|
||||
{
|
||||
|
||||
b3SharedMemoryCommandHandle commandHandle = b3LoadUrdfCommandInit(m_physicsClientHandle, "kuka_iiwa/model.urdf");
|
||||
|
||||
//setting the initial position, orientation and other arguments are optional
|
||||
double startPosX = 0;
|
||||
static double startPosY = 0;
|
||||
@ -234,7 +232,13 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
|
||||
startPosY += 2.f;
|
||||
// ret = b3LoadUrdfCommandSetUseFixedBase(commandHandle, 1);
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_LOAD_SDF:
|
||||
{
|
||||
b3SharedMemoryCommandHandle commandHandle = b3LoadSdfCommandInit(m_physicsClientHandle, "two_cubes.sdf");
|
||||
b3SubmitClientCommand(m_physicsClientHandle, commandHandle);
|
||||
break;
|
||||
}
|
||||
case CMD_REQUEST_CAMERA_IMAGE_DATA:
|
||||
@ -452,6 +456,7 @@ void PhysicsClientExample::createButtons()
|
||||
m_guiHelper->getParameterInterface()->removeAllParameters();
|
||||
|
||||
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
|
||||
createButton("Load SDF",CMD_LOAD_SDF, isTrigger);
|
||||
createButton("Get Camera Image",CMD_REQUEST_CAMERA_IMAGE_DATA,isTrigger);
|
||||
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
|
||||
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
|
||||
@ -649,11 +654,40 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
b3Warning("Camera image FAILED\n");
|
||||
}
|
||||
|
||||
|
||||
if (statusType == CMD_URDF_LOADING_COMPLETED)
|
||||
if (statusType == CMD_SDF_LOADING_COMPLETED)
|
||||
{
|
||||
int bodyIndex = b3GetStatusBodyIndex(status);
|
||||
if (bodyIndex>=0)
|
||||
//int bodyIndex = b3GetStatusBodyIndex(status);
|
||||
/*if (bodyIndex>=0)
|
||||
{
|
||||
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
|
||||
|
||||
for (int i=0;i<numJoints;i++)
|
||||
{
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
|
||||
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
}
|
||||
ComboBoxParams comboParams;
|
||||
comboParams.m_comboboxId = bodyIndex;
|
||||
comboParams.m_numItems = 1;
|
||||
comboParams.m_startItem = 0;
|
||||
comboParams.m_callback = MyComboBoxCallback;
|
||||
comboParams.m_userPointer = this;
|
||||
const char* bla = "bla";
|
||||
const char* blarray[1];
|
||||
blarray[0] = bla;
|
||||
|
||||
comboParams.m_items=blarray;//{&bla};
|
||||
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
|
||||
if (statusType == CMD_URDF_LOADING_COMPLETED)
|
||||
{
|
||||
int bodyIndex = b3GetStatusBodyIndex(status);
|
||||
if (bodyIndex>=0)
|
||||
{
|
||||
int numJoints = b3GetNumJoints(m_physicsClientHandle,bodyIndex);
|
||||
|
||||
@ -662,7 +696,6 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
b3JointInfo info;
|
||||
b3GetJointInfo(m_physicsClientHandle,bodyIndex,i,&info);
|
||||
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
|
||||
|
||||
}
|
||||
ComboBoxParams comboParams;
|
||||
comboParams.m_comboboxId = bodyIndex;
|
||||
@ -676,12 +709,8 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
|
||||
comboParams.m_items=blarray;//{&bla};
|
||||
m_guiHelper->getParameterInterface()->registerComboBox(comboParams);
|
||||
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
if (b3CanSubmitCommand(m_physicsClientHandle))
|
||||
@ -734,13 +763,12 @@ void PhysicsClientExample::stepSimulation(float deltaTime)
|
||||
if (m_numMotors)
|
||||
{
|
||||
enqueueCommand(CMD_SEND_DESIRED_STATE);
|
||||
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
|
||||
if (m_options != eCLIENTEXAMPLE_SERVER)
|
||||
{
|
||||
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
|
||||
}
|
||||
//enqueueCommand(CMD_REQUEST_ACTUAL_STATE);
|
||||
}
|
||||
enqueueCommand(CMD_STEP_FORWARD_SIMULATION);
|
||||
if (m_options != eCLIENTEXAMPLE_SERVER)
|
||||
{
|
||||
enqueueCommand(CMD_REQUEST_DEBUG_LINES);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -40,7 +40,7 @@ struct PhysicsClientSharedMemoryInternalData {
|
||||
SharedMemoryStatus m_lastServerStatus;
|
||||
|
||||
int m_counter;
|
||||
bool m_serverLoadUrdfOK;
|
||||
|
||||
bool m_isConnected;
|
||||
bool m_waitingForServer;
|
||||
bool m_hasLastServerStatus;
|
||||
@ -54,7 +54,6 @@ struct PhysicsClientSharedMemoryInternalData {
|
||||
m_counter(0),
|
||||
m_cachedCameraPixelsWidth(0),
|
||||
m_cachedCameraPixelsHeight(0),
|
||||
m_serverLoadUrdfOK(false),
|
||||
m_isConnected(false),
|
||||
m_waitingForServer(false),
|
||||
m_hasLastServerStatus(false),
|
||||
@ -204,8 +203,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CMD_SDF_LOADING_COMPLETED: {
|
||||
|
||||
if (m_data->m_verboseOutput) {
|
||||
b3Printf("Server loading the SDF OK\n");
|
||||
}
|
||||
break;
|
||||
}
|
||||
case CMD_URDF_LOADING_COMPLETED: {
|
||||
m_data->m_serverLoadUrdfOK = true;
|
||||
|
||||
if (m_data->m_verboseOutput) {
|
||||
b3Printf("Server loading the URDF OK\n");
|
||||
}
|
||||
@ -265,7 +271,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
if (m_data->m_verboseOutput) {
|
||||
b3Printf("Server failed loading the URDF...\n");
|
||||
}
|
||||
m_data->m_serverLoadUrdfOK = false;
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_SDF_LOADING_FAILED: {
|
||||
if (m_data->m_verboseOutput) {
|
||||
b3Printf("Server failed loading the SDF...\n");
|
||||
}
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
@ -490,6 +504,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
|
||||
m_data->m_waitingForServer = true;
|
||||
}
|
||||
|
||||
/*if (serverCmd.m_type == CMD_SDF_LOADING_COMPLETED)
|
||||
{
|
||||
int numBodies = serverCmd.m_sdfLoadedArgs.m_numBodies;
|
||||
if (numBodies>0)
|
||||
{
|
||||
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
||||
//now transfer the information of the individual objects etc.
|
||||
command.m_type = CMD_REQUEST_SDF_INFO;
|
||||
command.m_updateFlags = SDF_REQUEST_INFO_BODY;
|
||||
command.m_sdfRequestInfoArgs.m_infoIndex = 0;
|
||||
submitClientCommand(command);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
if (serverCmd.m_type == CMD_CAMERA_IMAGE_COMPLETED)
|
||||
{
|
||||
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
|
||||
|
||||
@ -393,6 +393,7 @@ struct PhysicsServerCommandProcessorInternalData
|
||||
btMultiBodyDynamicsWorld* m_dynamicsWorld;
|
||||
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
|
||||
|
||||
btAlignedObjectArray<int> m_sdfRecentLoadedBodies;
|
||||
|
||||
|
||||
|
||||
@ -681,6 +682,128 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
|
||||
}
|
||||
|
||||
|
||||
bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
{
|
||||
btAssert(m_data->m_dynamicsWorld);
|
||||
if (!m_data->m_dynamicsWorld)
|
||||
{
|
||||
b3Error("loadSdf: No valid m_dynamicsWorld");
|
||||
return false;
|
||||
}
|
||||
|
||||
m_data->m_sdfRecentLoadedBodies.clear();
|
||||
|
||||
BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter);
|
||||
|
||||
bool useFixedBase = false;
|
||||
bool loadOk = u2b.loadSDF(fileName, useFixedBase);
|
||||
if (loadOk)
|
||||
{
|
||||
for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
|
||||
{
|
||||
btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
|
||||
m_data->m_collisionShapes.push_back(shape);
|
||||
if (shape->isCompound())
|
||||
{
|
||||
btCompoundShape* compound = (btCompoundShape*) shape;
|
||||
for (int childIndex=0;childIndex<compound->getNumChildShapes();childIndex++)
|
||||
{
|
||||
m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex));
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
btTransform rootTrans;
|
||||
rootTrans.setIdentity();
|
||||
if (m_data->m_verboseOutput)
|
||||
{
|
||||
b3Printf("loaded %s OK!", fileName);
|
||||
}
|
||||
|
||||
for (int m =0; m<u2b.getNumModels();m++)
|
||||
{
|
||||
|
||||
u2b.activateModel(m);
|
||||
btMultiBody* mb = 0;
|
||||
|
||||
//get a body index
|
||||
int bodyUniqueId = m_data->allocHandle();
|
||||
|
||||
InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId);
|
||||
|
||||
{
|
||||
btScalar mass = 0;
|
||||
bodyHandle->m_rootLocalInertialFrame.setIdentity();
|
||||
btVector3 localInertiaDiagonal(0,0,0);
|
||||
int urdfLinkIndex = u2b.getRootLinkIndex();
|
||||
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame);
|
||||
}
|
||||
|
||||
|
||||
|
||||
//todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user
|
||||
int rootLinkIndex = u2b.getRootLinkIndex();
|
||||
b3Printf("urdf root link index = %d\n",rootLinkIndex);
|
||||
MyMultiBodyCreator creation(m_data->m_guiHelper);
|
||||
|
||||
u2b.getRootTransformInWorld(rootTrans);
|
||||
bool useMultiBody = true;
|
||||
ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true);
|
||||
|
||||
|
||||
|
||||
mb = creation.getBulletMultiBody();
|
||||
if (mb)
|
||||
{
|
||||
bodyHandle->m_multiBody = mb;
|
||||
|
||||
m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId);
|
||||
|
||||
createJointMotors(mb);
|
||||
|
||||
//serialize the btMultiBody and send the data to the client. This is one way to get the link/joint names across the (shared memory) wire
|
||||
|
||||
UrdfLinkNameMapUtil* util2 = new UrdfLinkNameMapUtil;
|
||||
m_data->m_urdfLinkNameMapper.push_back(util2);
|
||||
util2->m_mb = mb;
|
||||
util2->m_memSerializer = 0;
|
||||
//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
|
||||
|
||||
bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
|
||||
for (int i=0;i<mb->getNumLinks();i++)
|
||||
{
|
||||
//disable serialization of the collision objects
|
||||
|
||||
int urdfLinkIndex = creation.m_mb2urdfLink[i];
|
||||
btScalar mass;
|
||||
btVector3 localInertiaDiagonal(0,0,0);
|
||||
btTransform localInertialFrame;
|
||||
u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
|
||||
bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
|
||||
|
||||
std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
|
||||
m_data->m_strings.push_back(linkName);
|
||||
|
||||
mb->getLink(i).m_linkName = linkName->c_str();
|
||||
|
||||
std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str());
|
||||
m_data->m_strings.push_back(jointName);
|
||||
|
||||
mb->getLink(i).m_jointName = jointName->c_str();
|
||||
}
|
||||
std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex()));
|
||||
m_data->m_strings.push_back(baseName);
|
||||
mb->setBaseName(baseName->c_str());
|
||||
} else
|
||||
{
|
||||
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
return loadOk;
|
||||
}
|
||||
|
||||
bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
|
||||
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes)
|
||||
@ -731,8 +854,6 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
|
||||
ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
|
||||
|
||||
|
||||
///todo(erwincoumans) refactor this memory allocation issue
|
||||
for (int i=0;i<u2b.getNumAllocatedCollisionShapes();i++)
|
||||
{
|
||||
btCollisionShape* shape =u2b.getAllocatedCollisionShape(i);
|
||||
@ -751,11 +872,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
btMultiBody* mb = creation.getBulletMultiBody();
|
||||
if (useMultiBody)
|
||||
{
|
||||
|
||||
|
||||
|
||||
|
||||
if (mb)
|
||||
{
|
||||
|
||||
bodyHandle->m_multiBody = mb;
|
||||
|
||||
createJointMotors(mb);
|
||||
|
||||
|
||||
@ -805,12 +928,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
|
||||
const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer);
|
||||
util->m_memSerializer->finalizeChunk(chunk,structType,BT_MULTIBODY_CODE,mb);
|
||||
|
||||
return true;
|
||||
return true;
|
||||
} else
|
||||
{
|
||||
b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later.");
|
||||
return false;
|
||||
}
|
||||
|
||||
} else
|
||||
{
|
||||
btAssert(0);
|
||||
@ -1047,6 +1171,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
|
||||
break;
|
||||
}
|
||||
|
||||
case CMD_LOAD_SDF:
|
||||
{
|
||||
const SdfArgs& sdfArgs = clientCmd.m_sdfArguments;
|
||||
if (m_data->m_verboseOutput)
|
||||
{
|
||||
b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName);
|
||||
}
|
||||
|
||||
bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes);
|
||||
|
||||
//serverStatusOut.m_type = CMD_SDF_LOADING_FAILED;
|
||||
serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size();
|
||||
int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies);
|
||||
for (int i=0;i<maxBodies;i++)
|
||||
{
|
||||
serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i];
|
||||
}
|
||||
|
||||
serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED;
|
||||
hasStatus = true;
|
||||
break;
|
||||
}
|
||||
case CMD_LOAD_URDF:
|
||||
{
|
||||
|
||||
|
||||
@ -19,6 +19,10 @@ class PhysicsServerCommandProcessor
|
||||
protected:
|
||||
|
||||
|
||||
|
||||
|
||||
bool loadSdf(const char* fileName, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
bool loadUrdf(const char* fileName, const class btVector3& pos, const class btQuaternion& orn,
|
||||
bool useMultiBody, bool useFixedBase, int* bodyUniqueIdPtr, char* bufferServerToClient, int bufferSizeInBytes);
|
||||
|
||||
|
||||
@ -33,6 +33,7 @@
|
||||
#define MAX_SDF_FILENAME_LENGTH 1024
|
||||
#define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH
|
||||
#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM
|
||||
#define MAX_SDF_BODIES 1024
|
||||
|
||||
struct TmpFloat3
|
||||
{
|
||||
@ -298,6 +299,28 @@ struct CreateBoxShapeArgs
|
||||
double m_colorRGBA[4];
|
||||
};
|
||||
|
||||
struct SdfLoadedArgs
|
||||
{
|
||||
int m_numBodies;
|
||||
int m_bodyUniqueIds[MAX_SDF_BODIES];
|
||||
|
||||
///@todo(erwincoumans) load cameras, lights etc
|
||||
//int m_numCameras;
|
||||
//int m_numLights;
|
||||
};
|
||||
|
||||
|
||||
struct SdfRequestInfoArgs
|
||||
{
|
||||
int m_infoIndex;
|
||||
};
|
||||
|
||||
enum EnumSdfRequestInfoFlags
|
||||
{
|
||||
SDF_REQUEST_INFO_BODY=1,
|
||||
//SDF_REQUEST_INFO_CAMERA=2,
|
||||
};
|
||||
|
||||
struct SharedMemoryCommand
|
||||
{
|
||||
int m_type;
|
||||
@ -312,6 +335,7 @@ struct SharedMemoryCommand
|
||||
{
|
||||
struct UrdfArgs m_urdfArguments;
|
||||
struct SdfArgs m_sdfArguments;
|
||||
struct SdfRequestInfoArgs m_sdfRequestInfoArgs;
|
||||
struct InitPoseArgs m_initPoseArgs;
|
||||
struct SendPhysicsSimulationParameters m_physSimParamArgs;
|
||||
struct BulletDataStreamArgs m_dataStreamArguments;
|
||||
@ -340,6 +364,7 @@ struct SharedMemoryStatus
|
||||
union
|
||||
{
|
||||
struct BulletDataStreamArgs m_dataStreamArguments;
|
||||
struct SdfLoadedArgs m_sdfLoadedArgs;
|
||||
struct SendActualStateArgs m_sendActualStateArgs;
|
||||
struct SendDebugLinesArgs m_sendDebugLinesArgs;
|
||||
struct SendPixelDataArgs m_sendPixelDataArguments;
|
||||
|
||||
@ -19,6 +19,7 @@ enum EnumSharedMemoryClientCommand
|
||||
CMD_SEND_DESIRED_STATE,//todo: reconsider naming, for example SET_JOINT_CONTROL_VARIABLE?
|
||||
CMD_REQUEST_ACTUAL_STATE,
|
||||
CMD_REQUEST_DEBUG_LINES,
|
||||
CMD_REQUEST_SDF_INFO,
|
||||
CMD_STEP_FORWARD_SIMULATION,
|
||||
CMD_RESET_SIMULATION,
|
||||
CMD_PICK_BODY,
|
||||
|
||||
@ -182,6 +182,61 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
|
||||
return PyLong_FromLong(bodyIndex);
|
||||
}
|
||||
|
||||
#define MAX_SDF_BODIES 512
|
||||
|
||||
static PyObject*
|
||||
pybullet_loadSDF(PyObject* self, PyObject* args)
|
||||
{
|
||||
const char* sdfFileName="";
|
||||
int size= PySequence_Size(args);
|
||||
int numBodies = 0;
|
||||
int i;
|
||||
int bodyIndicesOut[MAX_SDF_BODIES];
|
||||
PyObject* pylist=0;
|
||||
b3SharedMemoryStatusHandle statusHandle;
|
||||
int statusType;
|
||||
b3SharedMemoryCommandHandle command;
|
||||
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
if (size==1)
|
||||
{
|
||||
if (!PyArg_ParseTuple(args, "s", &sdfFileName))
|
||||
return NULL;
|
||||
}
|
||||
|
||||
command = b3LoadSdfCommandInit(sm, sdfFileName);
|
||||
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
|
||||
statusType = b3GetStatusType(statusHandle);
|
||||
if (statusType!=CMD_SDF_LOADING_COMPLETED)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Cannot load SDF file.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
numBodies = b3GetStatusBodyIndices(statusHandle, bodyIndicesOut, MAX_SDF_BODIES);
|
||||
if (numBodies > MAX_SDF_BODIES)
|
||||
{
|
||||
PyErr_SetString(SpamError, "SDF exceeds body capacity");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
pylist = PyTuple_New(numBodies);
|
||||
|
||||
if (numBodies >0 && numBodies <= MAX_SDF_BODIES)
|
||||
{
|
||||
for (i=0;i<numBodies;i++)
|
||||
{
|
||||
PyTuple_SetItem(pylist,i,PyInt_FromLong(bodyIndicesOut[i]));
|
||||
}
|
||||
}
|
||||
return pylist;
|
||||
}
|
||||
|
||||
// Reset the simulation to remove all loaded objects
|
||||
static PyObject *
|
||||
pybullet_resetSimulation(PyObject* self, PyObject* args)
|
||||
@ -287,24 +342,26 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
|
||||
static PyObject *
|
||||
pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
|
||||
{
|
||||
int bodyIndex = -1;
|
||||
double basePosition[3];
|
||||
double baseOrientation[4];
|
||||
PyObject *pylistPos;
|
||||
PyObject *pylistOrientation;
|
||||
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int bodyIndex = -1;
|
||||
if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
|
||||
{
|
||||
PyErr_SetString(SpamError, "Expected a body index (integer).");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
double basePosition[3];
|
||||
double baseOrientation[4];
|
||||
|
||||
pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation);
|
||||
PyObject *pylistPos;
|
||||
|
||||
{
|
||||
|
||||
PyObject *item;
|
||||
@ -318,7 +375,7 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
|
||||
}
|
||||
|
||||
}
|
||||
PyObject *pylistOrientation;
|
||||
|
||||
{
|
||||
|
||||
PyObject *item;
|
||||
@ -446,13 +503,6 @@ static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16])
|
||||
// to see object based on camera position?
|
||||
static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
|
||||
{
|
||||
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
///request an image from a simulated camera, using a software renderer.
|
||||
struct b3CameraImageData imageData;
|
||||
PyObject* objViewMat,* objProjMat;
|
||||
@ -463,6 +513,14 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
|
||||
|
||||
// inialize cmd
|
||||
b3SharedMemoryCommandHandle command;
|
||||
|
||||
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
return NULL;
|
||||
}
|
||||
|
||||
command = b3InitRequestCameraImage(sm);
|
||||
|
||||
if (size==2) // only set camera resolution
|
||||
@ -498,6 +556,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
|
||||
{
|
||||
PyObject *item2;
|
||||
PyObject* pyResultList;//store 4 elements in this result: width, height, rgbData, depth
|
||||
PyObject *pylistRGB;
|
||||
PyObject* pylistDep;
|
||||
int i, j, p;
|
||||
|
||||
b3GetCameraImageData(sm, &imageData);
|
||||
//TODO(hellojas): error handling if image size is 0
|
||||
@ -505,9 +566,6 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
|
||||
PyTuple_SetItem(pyResultList, 0, PyInt_FromLong(imageData.m_pixelWidth));
|
||||
PyTuple_SetItem(pyResultList, 1, PyInt_FromLong(imageData.m_pixelHeight));
|
||||
|
||||
PyObject *pylistRGB;
|
||||
PyObject* pylistDep;
|
||||
int i, j, p;
|
||||
|
||||
{
|
||||
|
||||
@ -549,6 +607,9 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
|
||||
static PyMethodDef SpamMethods[] = {
|
||||
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
|
||||
"Create a multibody by loading a URDF file."},
|
||||
|
||||
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
|
||||
"Load multibodies from an SDF file."},
|
||||
|
||||
{"connect", pybullet_connectPhysicsServer, METH_VARARGS,
|
||||
"Connect to an existing physics server (using shared memory by default)."},
|
||||
|
||||
@ -145,11 +145,6 @@ public:
|
||||
// lo and hi limits for variables (set to -/+ infinity on entry).
|
||||
btScalar *m_lowerLimit,*m_upperLimit;
|
||||
|
||||
// findex vector for variables. see the LCP solver interface for a
|
||||
// description of what this does. this is set to -1 on entry.
|
||||
// note that the returned indexes are relative to the first index of
|
||||
// the constraint.
|
||||
int *findex;
|
||||
// number of solver iterations
|
||||
int m_numIterations;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user