int b3ResourcePath::getExePath(char* path, int maxPathLenInBytes)
{
int numBytes = 0;
#if __APPLE__
uint32_t bufsize = uint32_t(maxPathLenInBytes);
if (_NSGetExecutablePath(path, &bufsize)!=0)
{
b3Warning("Cannot find executable path\n");
return false;
} else
{
numBytes = strlen(path);
}
#else
#ifdef _WIN32
//https://msdn.microsoft.com/en-us/library/windows/desktop/ms683197(v=vs.85).aspx
HMODULE hModule = GetModuleHandle(NULL);
numBytes = GetModuleFileName(hModule, path, maxPathLenInBytes);
#else
///http://stackoverflow.com/questions/933850/how-to-find-the-location-of-the-executable-in-c
numBytes = (int)readlink("/proc/self/exe", path, maxPathLenInBytes-1);
if (numBytes > 0)
{
path[numBytes] = 0;
} else
{
b3Warning("Cannot find executable path\n");
}
#endif //_WIN32
#endif //__APPLE__
return numBytes;
}
void PhysicsClientExample::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createButtons();
} else
{
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
}
if (!m_physicsClient.connect())
{
b3Warning("Cannot connect to physics client");
}
}
void PhysicsClientExample::initPhysics()
{
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createButtons();
} else
{
MyCallback(CMD_LOAD_URDF, true, this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_STEP_FORWARD_SIMULATION,true,this);
MyCallback(CMD_RESET_SIMULATION,true,this);
}
m_selectedBody = -1;
m_prevSelectedBody = -1;
m_physicsClientHandle = b3ConnectSharedMemory(m_sharedMemoryKey);
//m_physicsClientHandle = b3ConnectPhysicsLoopback(SHARED_MEMORY_KEY);
//m_physicsClientHandle = b3ConnectPhysicsDirect();
if (!b3CanSubmitCommand(m_physicsClientHandle))
{
b3Warning("Cannot connect to physics client");
}
}
void RobotControlExample::prepareControlCommand(SharedMemoryCommand& command)
{
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 0;
}
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[i] = 0;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[i] = 1000;
}
for (int i=0;i<m_numMotors;i++)
{
btScalar targetVel = m_motorTargetState[i].m_velTarget;
int uIndex = m_motorTargetState[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
}
break;
}
case ROBOT_PD_CONTROL:
{
command.m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_POSITION_VELOCITY_PD;
for (int i=0;i<m_numMotors;i++)
{
int uIndex = m_motorTargetState[i].m_uIndex;
command.m_sendDesiredStateCommandArgument.m_Kp[uIndex] = m_motorTargetState[i].m_kp;
command.m_sendDesiredStateCommandArgument.m_Kd[uIndex] = m_motorTargetState[i].m_kd;
command.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[uIndex] = 10000;//max force
btScalar targetVel = m_motorTargetState[i].m_velTarget;
command.m_sendDesiredStateCommandArgument.m_desiredStateQdot[uIndex] = targetVel;
int posIndex = m_motorTargetState[i].m_posIndex;
btScalar targetPos = m_motorTargetState[i].m_posTarget;
command.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex] = targetPos;
}
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::prepareControlCommand");
}
};
}
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{
m_data->m_commandProcessor->setGuiHelper(guiHelper);
bool allowCreation = true;
if (m_data->m_isConnected)
{
b3Warning("connectSharedMemory, while already connected");
return m_data->m_isConnected;
}
int counter = 0;
do
{
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
int magicId =m_data->m_testBlock1->m_magicId;
if (m_data->m_verboseOutput)
{
b3Printf("magicId = %d\n", magicId);
}
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlock1);
if (m_data->m_verboseOutput)
{
b3Printf("Created and initialized shared memory block\n");
}
m_data->m_isConnected = true;
} else
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
m_data->m_isConnected = false;
}
} else
{
b3Error("Cannot connect to shared memory");
m_data->m_isConnected = false;
}
} while (counter++ < 10 && !m_data->m_isConnected);
if (!m_data->m_isConnected)
{
b3Error("Server cannot connect to shared memory.\n");
}
return m_data->m_isConnected;
}
void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances,btTransform& upAxisTrans, float& unitMeterScaling)
{
upAxisTrans.setIdentity();
unitMeterScaling=1;
GLInstanceGraphicsShape* shape = 0;
FILE* file = fopen(relativeFileName,"rb");
if (file)
{
int size=0;
if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
{
b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
} else
{
if (size)
{
//printf("Open DAE file of %d bytes\n",size);
Assimp::Importer importer;
//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
// importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
aiScene const* scene = importer.ReadFile(relativeFileName,
aiProcess_JoinIdenticalVertices |
//aiProcess_RemoveComponent |
aiProcess_SortByPType |
aiProcess_Triangulate);
if (scene)
{
shape = &visualShapes.expand();
shape->m_scaling[0] = 1;
shape->m_scaling[1] = 1;
shape->m_scaling[2] = 1;
shape->m_scaling[3] = 1;
int index = 0;
shape->m_indices = new b3AlignedObjectArray<int>();
shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
aiMatrix4x4 ident;
addMeshParts(scene, scene->mRootNode, shape, ident);
shape->m_numIndices = shape->m_indices->size();
shape->m_numvertices = shape->m_vertices->size();
ColladaGraphicsInstance& instance = visualShapeInstances.expand();
instance.m_shapeIndex = visualShapes.size()-1;
}
}
}
}
}
void PhysicsClientSharedMemory::uploadBulletFileToSharedMemory(const char* data, int len) {
btAssert(len < SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
if (len >= SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE) {
b3Warning("uploadBulletFileToSharedMemory %d exceeds max size %d\n", len,
SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
} else {
for (int i = 0; i < len; i++) {
m_data->m_testBlock1->m_bulletStreamDataClientToServer[i] = data[i];
}
}
}
void b3GpuNarrowPhase::setObjectVelocityCpu(float* linVel, float* angVel, int bodyIndex)
{
if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size())
{
m_data->m_bodyBufferCPU->at(bodyIndex).m_linVel.setValue(linVel[0],linVel[1],linVel[2]);
m_data->m_bodyBufferCPU->at(bodyIndex).m_angVel.setValue(angVel[0],angVel[1],angVel[2]);
} else
{
b3Warning("setObjectVelocityCpu out of range.\n");
}
}
void b3GpuNarrowPhase::setObjectTransformCpu(float* position, float* orientation , int bodyIndex)
{
if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size())
{
m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.setValue(position[0],position[1],position[2]);
m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]);
}
else
{
b3Warning("setObjectVelocityCpu out of range.\n");
}
}
bool PhysicsClientSharedMemory::connect() {
/// server always has to create and initialize shared memory
bool allowCreation = false;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(
m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1) {
if (m_data->m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER) {
b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey,
SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
} else {
if (m_data->m_verboseOutput) {
b3Printf("Connected to existing shared memory, status OK.\n");
}
m_data->m_isConnected = true;
}
} else {
b3Warning("Cannot connect to shared memory");
return false;
}
#if 0
if (m_data->m_isConnected)
{
//get all existing bodies and body info...
SharedMemoryCommand& command = m_data->m_testBlock1->m_clientCommands[0];
//now transfer the information of the individual objects etc.
command.m_type = CMD_REQUEST_BODY_INFO;
command.m_sdfRequestInfoArgs.m_bodyUniqueId = 37;
submitClientCommand(command);
int timeout = 1024 * 1024 * 1024;
const SharedMemoryStatus* status = 0;
while ((status == 0) && (timeout-- > 0))
{
status = processServerStatus();
}
//submitClientCommand(command);
}
#endif
return true;
}
void RobotControlExample::initPhysics()
{
///for this testing we use Z-axis up
int upAxis = 2;
m_guiHelper->setUpAxis(upAxis);
createEmptyDynamicsWorld();
//todo: create a special debug drawer that will cache the lines, so we can send the debug info over the wire
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
btVector3 grav(0,0,0);
grav[upAxis] = 0;//-9.8;
this->m_dynamicsWorld->setGravity(grav);
bool allowSharedMemoryInitialization = true;
m_physicsServer.connectSharedMemory(allowSharedMemoryInitialization, m_dynamicsWorld,m_guiHelper);
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
bool isTrigger = false;
createButton("Load URDF",CMD_LOAD_URDF, isTrigger);
createButton("Step Sim",CMD_STEP_FORWARD_SIMULATION, isTrigger);
createButton("Send Bullet Stream",CMD_SEND_BULLET_DATA_STREAM, isTrigger);
createButton("Get State",CMD_REQUEST_ACTUAL_STATE, isTrigger);
createButton("Send Desired State",CMD_SEND_DESIRED_STATE, isTrigger);
createButton("Create Box Collider",CMD_CREATE_BOX_COLLISION_SHAPE,isTrigger);
createButton("Set Physics Params",CMD_SEND_PHYSICS_SIMULATION_PARAMETERS,isTrigger);
createButton("Init Pose",CMD_INIT_POSE,isTrigger);
} else
{
/*
m_userCommandRequests.push_back(CMD_LOAD_URDF);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SEND_DESIRED_STATE);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
//m_userCommandRequests.push_back(CMD_SET_JOINT_FEEDBACK);
m_userCommandRequests.push_back(CMD_CREATE_BOX_COLLISION_SHAPE);
//m_userCommandRequests.push_back(CMD_CREATE_RIGID_BODY);
m_userCommandRequests.push_back(CMD_STEP_FORWARD_SIMULATION);
m_userCommandRequests.push_back(CMD_REQUEST_ACTUAL_STATE);
m_userCommandRequests.push_back(CMD_SHUTDOWN);
*/
}
if (!m_physicsClient.connect())
{
b3Warning("Cannot eonnect to physics client");
}
}
bool b3GpuNarrowPhase::getObjectTransformFromCpu(float* position, float* orientation , int bodyIndex) const
{
if (bodyIndex>=0 && bodyIndex<m_data->m_bodyBufferCPU->size())
{
position[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.x;
position[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.y;
position[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_pos.z;
position[3] = 1.f;//or 1
orientation[0] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.x;
orientation[1] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.y;
orientation[2] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.z;
orientation[3] = m_data->m_bodyBufferCPU->at(bodyIndex).m_quat.w;
return true;
}
b3Warning("getObjectTransformFromCpu out of range.\n");
return false;
}
///todo(erwincoumans) refactor this: merge with PhysicsDirect::processBodyJointInfo
void PhysicsClientSharedMemory::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd)
{
bParse::btBulletFile bf(
&this->m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor[0],
serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf.setFileDNAisMemoryDNA();
bf.parse(false);
BodyJointInfoCache* bodyJoints = new BodyJointInfoCache;
m_data->m_bodyJointMap.insert(bodyUniqueId,bodyJoints);
for (int i = 0; i < bf.m_multiBodies.size(); i++)
{
int flag = bf.getFlags();
if ((flag & bParse::FD_DOUBLE_PRECISION) != 0)
{
Bullet::btMultiBodyDoubleData* mb =
(Bullet::btMultiBodyDoubleData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
} else
{
Bullet::btMultiBodyFloatData* mb =
(Bullet::btMultiBodyFloatData*)bf.m_multiBodies[i];
bodyJoints->m_baseName = mb->m_baseName;
addJointInfoFromMultiBodyData(mb,bodyJoints, m_data->m_verboseOutput);
}
}
if (bf.ok()) {
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
bool PhysicsServerSharedMemory::connectSharedMemory( struct GUIHelperInterface* guiHelper)
{
m_data->m_guiHelper = guiHelper;
bool allowCreation = true;
bool allowConnectToExistingSharedMemory = false;
if (m_data->m_isConnected)
{
b3Warning("connectSharedMemory, while already connected");
return m_data->m_isConnected;
}
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE,allowCreation);
if (m_data->m_testBlock1)
{
int magicId =m_data->m_testBlock1->m_magicId;
b3Printf("magicId = %d\n", magicId);
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block\n");
m_data->m_isConnected = true;
} else
{
b3Error("Server cannot connect to existing shared memory, disconnecting shared memory.\n");
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
m_data->m_isConnected = false;
}
} else
{
b3Error("Cannot connect to shared memory");
m_data->m_isConnected = false;
}
return m_data->m_isConnected;
}
void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis)
{
// GLInstanceGraphicsShape* instance = 0;
//usually COLLADA files don't have that many visual geometries/shapes
visualShapes.reserve(MAX_VISUAL_SHAPES);
float extraScaling = 1;//0.01;
btHashMap<btHashString, int> name2ShapeIndex;
b3FileUtils f;
char filename[1024];
if (!f.findFile(relativeFileName,filename,1024))
{
b3Warning("File not found: %s\n", filename);
return;
}
TiXmlDocument doc(filename);
if (!doc.LoadFile())
return;
//We need units to be in meter, so apply a scaling using the asset/units meter
unitMeterScaling=1;
upAxisTransform.setIdentity();
//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling,clientUpAxis);
btMatrix4x4 ident;
ident.setIdentity();
readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);
}
void convertURDFToVisualShape(const UrdfVisual* visual, const char* urdfPathPrefix, const btTransform& visualTransform, btAlignedObjectArray<GLInstanceVertex>& verticesOut, btAlignedObjectArray<int>& indicesOut, btAlignedObjectArray<MyTexture2>& texturesOut)
{
GLInstanceGraphicsShape* glmesh = 0;
btConvexShape* convexColShape = 0;
switch (visual->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i = 0; i<numSteps; i++)
{
btScalar cylRadius = visual->m_geometry.m_cylinderRadius;
btScalar cylLength = visual->m_geometry.m_cylinderLength;
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i) / numSteps)), cylRadius*btCos(SIMD_2_PI*(float(i) / numSteps)), cylLength / 2.);
vertices.push_back(vert);
vert[2] = -cylLength / 2.;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
convexColShape = cylZShape;
break;
}
case URDF_GEOM_BOX:
{
btVector3 extents = visual->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
convexColShape = boxShape;
convexColShape->setMargin(0.001);
break;
}
case URDF_GEOM_SPHERE:
{
btScalar radius = visual->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
convexColShape = sphereShape;
convexColShape->setMargin(0.001);
break;
break;
}
case URDF_GEOM_MESH:
{
if (visual->m_name.length())
{
//b3Printf("visual->name=%s\n", visual->m_name.c_str());
}
if (1)//visual->m_geometry)
{
if (visual->m_geometry.m_meshFileName.length())
{
const char* filename = visual->m_geometry.m_meshFileName.c_str();
//b3Printf("mesh->filename=%s\n", filename);
char fullPath[1024];
int fileType = 0;
char tmpPathPrefix[1024];
std::string xml_string;
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char visualPathPrefix[1024];
sprintf(visualPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
b3FileUtils::toLower(fullPath);
if (strstr(fullPath, ".dae"))
{
fileType = MY_FILE_COLLADA;
}
if (strstr(fullPath, ".stl"))
{
fileType = MY_FILE_STL;
}
if (strstr(fullPath,".obj"))
{
fileType = MY_FILE_OBJ;
}
sprintf(fullPath, "%s%s", urdfPathPrefix, filename);
FILE* f = fopen(fullPath, "rb");
if (f)
{
fclose(f);
switch (fileType)
{
case MY_FILE_OBJ:
{
//glmesh = LoadMeshFromObj(fullPath,visualPathPrefix);
b3ImportMeshData meshData;
if (b3ImportMeshUtility::loadAndRegisterMeshFromFileInternal(fullPath, meshData))
{
if (meshData.m_textureImage)
{
MyTexture2 texData;
texData.m_width = meshData.m_textureWidth;
texData.m_height = meshData.m_textureHeight;
texData.textureData = meshData.m_textureImage;
texturesOut.push_back(texData);
}
glmesh = meshData.m_gfxShape;
}
break;
}
case MY_FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
case MY_FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans; upAxisTrans.setIdentity();
float unitMeterScaling = 1;
int upAxis = 2;
LoadMeshFromCollada(fullPath,
visualShapes,
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis);
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i<visualShapeInstances.size(); i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i = 0; i<gfxShape->m_vertices->size(); i++)
{
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
}
int curNumIndices = glmesh->m_indices->size();
int additionalIndices = gfxShape->m_indices->size();
glmesh->m_indices->resize(curNumIndices + additionalIndices);
for (int k = 0; k<additionalIndices; k++)
{
glmesh->m_indices->at(curNumIndices + k) = gfxShape->m_indices->at(k) + baseIndex;
}
//compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat;
upAxisMat.setIdentity();
// upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling, unitMeterScaling, unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*upAxisMat*instance->m_worldTransform;
//btMatrix4x4 worldMat = instance->m_worldTransform;
int curNumVertices = glmesh->m_vertices->size();
int additionalVertices = verts.size();
glmesh->m_vertices->reserve(curNumVertices + additionalVertices);
for (int v = 0; v<verts.size(); v++)
{
btVector3 pos(verts[v].xyzw[0], verts[v].xyzw[1], verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_vertices->push_back(verts[v]);
}
}
glmesh->m_numIndices = glmesh->m_indices->size();
glmesh->m_numvertices = glmesh->m_vertices->size();
//glmesh = LoadMeshFromCollada(fullPath);
break;
}
default:
{
b3Warning("Error: unsupported file type for Visual mesh: %s\n", fullPath);
btAssert(0);
}
}
if (glmesh && glmesh->m_vertices && (glmesh->m_numvertices>0))
{
//apply the geometry scaling
for (int i=0;i<glmesh->m_vertices->size();i++)
{
glmesh->m_vertices->at(i).xyzw[0] *= visual->m_geometry.m_meshScale[0];
glmesh->m_vertices->at(i).xyzw[1] *= visual->m_geometry.m_meshScale[1];
glmesh->m_vertices->at(i).xyzw[2] *= visual->m_geometry.m_meshScale[2];
}
}
else
{
b3Warning("issue extracting mesh from COLLADA/STL file %s\n", fullPath);
}
}
else
{
b3Warning("mesh geometry not found %s\n", fullPath);
}
}
}
break;
}
default:
{
b3Warning("Error: unknown visual geometry type\n");
}
}
//if we have a convex, tesselate into localVertices/localIndices
if ((glmesh==0) && convexColShape)
{
btShapeHull* hull = new btShapeHull(convexColShape);
hull->buildHull(0.0);
{
// int strideInBytes = 9*sizeof(float);
int numVertices = hull->numVertices();
int numIndices = hull->numIndices();
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i = 0; i < numVertices; i++)
{
GLInstanceVertex vtx;
btVector3 pos = hull->getVertexPointer()[i];
vtx.xyzw[0] = pos.x();
vtx.xyzw[1] = pos.y();
vtx.xyzw[2] = pos.z();
vtx.xyzw[3] = 1.f;
pos.normalize();
vtx.normal[0] = pos.x();
vtx.normal[1] = pos.y();
vtx.normal[2] = pos.z();
vtx.uv[0] = 0.5f;
vtx.uv[1] = 0.5f;
glmesh->m_vertices->push_back(vtx);
}
btAlignedObjectArray<int> indices;
for (int i = 0; i < numIndices; i++)
{
glmesh->m_indices->push_back(hull->getIndexPointer()[i]);
}
glmesh->m_numvertices = glmesh->m_vertices->size();
glmesh->m_numIndices = glmesh->m_indices->size();
}
delete hull;
delete convexColShape;
convexColShape = 0;
}
if (glmesh && glmesh->m_numIndices>0 && glmesh->m_numvertices >0)
{
int baseIndex = verticesOut.size();
for (int i = 0; i < glmesh->m_indices->size(); i++)
{
indicesOut.push_back(glmesh->m_indices->at(i) + baseIndex);
}
for (int i = 0; i < glmesh->m_vertices->size(); i++)
{
GLInstanceVertex& v = glmesh->m_vertices->at(i);
btVector3 vert(v.xyzw[0],v.xyzw[1],v.xyzw[2]);
btVector3 vt = visualTransform*vert;
v.xyzw[0] = vt[0];
v.xyzw[1] = vt[1];
v.xyzw[2] = vt[2];
btVector3 triNormal(v.normal[0],v.normal[1],v.normal[2]);
triNormal = visualTransform.getBasis()*triNormal;
v.normal[0] = triNormal[0];
v.normal[1] = triNormal[1];
v.normal[2] = triNormal[2];
verticesOut.push_back(v);
}
}
delete glmesh;
}
SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height)
{
gApp = this;
m_data = new SimpleInternalData;
m_data->m_frameDumpPngFileName = 0;
m_data->m_renderTexture = 0;
m_data->m_ffmpegFile = 0;
m_data->m_userPointer = 0;
m_window = new b3gDefaultOpenGLWindow();
b3gWindowConstructionInfo ci;
ci.m_title = title;
ci.m_width = width;
ci.m_height = height;
m_window->createWindow(ci);
m_window->setWindowTitle(title);
b3Assert(glGetError() ==GL_NO_ERROR);
glClearColor(0.9,0.9,1,1);
m_window->startRendering();
b3Assert(glGetError() ==GL_NO_ERROR);
#ifndef __APPLE__
#ifndef _WIN32
//some Linux implementations need the 'glewExperimental' to be true
glewExperimental = GL_TRUE;
#endif
if (glewInit() != GLEW_OK)
exit(1); // or handle the error in a nicer way
if (!GLEW_VERSION_2_1) // check that the machine supports the 2.1 API.
exit(1); // or handle the error in a nicer way
#endif
glGetError();//don't remove this call, it is needed for Ubuntu
b3Assert(glGetError() ==GL_NO_ERROR);
m_primRenderer = new GLPrimitiveRenderer(width,height);
m_parameterInterface = 0;
b3Assert(glGetError() ==GL_NO_ERROR);
m_instancingRenderer = new GLInstancingRenderer(128*1024,4*1024*1024);
m_instancingRenderer->init();
m_instancingRenderer->resize(width,height);
b3Assert(glGetError() ==GL_NO_ERROR);
m_instancingRenderer->InitShaders();
m_window->setMouseMoveCallback(b3DefaultMouseMoveCallback);
m_window->setMouseButtonCallback(b3DefaultMouseButtonCallback);
m_window->setKeyboardCallback(b3DefaultKeyboardCallback);
m_window->setWheelCallback(b3DefaultWheelCallback);
m_window->setResizeCallback(SimpleResizeCallback);
TwGenerateDefaultFonts();
m_data->m_fontTextureId = BindFont(g_DefaultNormalFont);
{
m_data->m_renderCallbacks = new OpenGL2RenderCallbacks(m_primRenderer);
m_data->m_fontStash = sth_create(512,512,m_data->m_renderCallbacks);//256,256);//,1024);//512,512);
b3Assert(glGetError() ==GL_NO_ERROR);
if (!m_data->m_fontStash)
{
b3Warning("Could not create stash");
//fprintf(stderr, "Could not create stash.\n");
}
char* data2 = OpenSansData;
unsigned char* data = (unsigned char*) data2;
if (!(m_data->m_droidRegular = sth_add_font_from_memory(m_data->m_fontStash, data)))
{
b3Warning("error!\n");
}
b3Assert(glGetError() ==GL_NO_ERROR);
}
}
virtual void reportWarning(const char* warning)
{
m_numWarnings++;
b3Warning(warning);
}
btCollisionShape* convertURDFToCollisionShape(const UrdfCollision* collision, const char* urdfPathPrefix)
{
btCollisionShape* shape = 0;
switch (collision->m_geometry.m_type)
{
case URDF_GEOM_CYLINDER:
{
btScalar cylRadius = collision->m_geometry.m_cylinderRadius;
btScalar cylLength = collision->m_geometry.m_cylinderLength;
btAlignedObjectArray<btVector3> vertices;
//int numVerts = sizeof(barrel_vertices)/(9*sizeof(float));
int numSteps = 32;
for (int i=0;i<numSteps;i++)
{
btVector3 vert(cylRadius*btSin(SIMD_2_PI*(float(i)/numSteps)),cylRadius*btCos(SIMD_2_PI*(float(i)/numSteps)),cylLength/2.);
vertices.push_back(vert);
vert[2] = -cylLength/2.;
vertices.push_back(vert);
}
btConvexHullShape* cylZShape = new btConvexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
cylZShape->setMargin(0.001);
cylZShape->initializePolyhedralFeatures();
//btConvexShape* cylZShape = new btConeShapeZ(cyl->radius,cyl->length);//(vexHullShape(&vertices[0].x(), vertices.size(), sizeof(btVector3));
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
shape = cylZShape;
break;
}
case URDF_GEOM_BOX:
{
btVector3 extents = collision->m_geometry.m_boxSize;
btBoxShape* boxShape = new btBoxShape(extents*0.5f);
//btConvexShape* boxShape = new btConeShapeX(extents[2]*0.5,extents[0]*0.5);
shape = boxShape;
shape ->setMargin(0.001);
break;
}
case URDF_GEOM_SPHERE:
{
btScalar radius = collision->m_geometry.m_sphereRadius;
btSphereShape* sphereShape = new btSphereShape(radius);
shape = sphereShape;
shape ->setMargin(0.001);
break;
break;
}
case URDF_GEOM_MESH:
{
if (collision->m_name.length())
{
//b3Printf("collision->name=%s\n",collision->m_name.c_str());
}
if (1)
{
if (collision->m_geometry.m_meshFileName.length())
{
const char* filename = collision->m_geometry.m_meshFileName.c_str();
//b3Printf("mesh->filename=%s\n",filename);
char fullPath[1024];
int fileType = 0;
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
b3FileUtils::toLower(fullPath);
char tmpPathPrefix[1024];
int maxPathLen = 1024;
b3FileUtils::extractPath(filename,tmpPathPrefix,maxPathLen);
char collisionPathPrefix[1024];
sprintf(collisionPathPrefix,"%s%s",urdfPathPrefix,tmpPathPrefix);
if (strstr(fullPath,".dae"))
{
fileType = FILE_COLLADA;
}
if (strstr(fullPath,".stl"))
{
fileType = FILE_STL;
}
if (strstr(fullPath,".obj"))
{
fileType = FILE_OBJ;
}
sprintf(fullPath,"%s%s",urdfPathPrefix,filename);
FILE* f = fopen(fullPath,"rb");
if (f)
{
fclose(f);
GLInstanceGraphicsShape* glmesh = 0;
switch (fileType)
{
case FILE_OBJ:
{
glmesh = LoadMeshFromObj(fullPath,collisionPathPrefix);
break;
}
case FILE_STL:
{
glmesh = LoadMeshFromSTL(fullPath);
break;
}
case FILE_COLLADA:
{
btAlignedObjectArray<GLInstanceGraphicsShape> visualShapes;
btAlignedObjectArray<ColladaGraphicsInstance> visualShapeInstances;
btTransform upAxisTrans;upAxisTrans.setIdentity();
float unitMeterScaling=1;
int upAxis = 2;
LoadMeshFromCollada(fullPath,
visualShapes,
visualShapeInstances,
upAxisTrans,
unitMeterScaling,
upAxis );
glmesh = new GLInstanceGraphicsShape;
// int index = 0;
glmesh->m_indices = new b3AlignedObjectArray<int>();
glmesh->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();
for (int i=0;i<visualShapeInstances.size();i++)
{
ColladaGraphicsInstance* instance = &visualShapeInstances[i];
GLInstanceGraphicsShape* gfxShape = &visualShapes[instance->m_shapeIndex];
b3AlignedObjectArray<GLInstanceVertex> verts;
verts.resize(gfxShape->m_vertices->size());
int baseIndex = glmesh->m_vertices->size();
for (int i=0;i<gfxShape->m_vertices->size();i++)
{
verts[i].normal[0] = gfxShape->m_vertices->at(i).normal[0];
verts[i].normal[1] = gfxShape->m_vertices->at(i).normal[1];
verts[i].normal[2] = gfxShape->m_vertices->at(i).normal[2];
verts[i].uv[0] = gfxShape->m_vertices->at(i).uv[0];
verts[i].uv[1] = gfxShape->m_vertices->at(i).uv[1];
verts[i].xyzw[0] = gfxShape->m_vertices->at(i).xyzw[0];
verts[i].xyzw[1] = gfxShape->m_vertices->at(i).xyzw[1];
verts[i].xyzw[2] = gfxShape->m_vertices->at(i).xyzw[2];
verts[i].xyzw[3] = gfxShape->m_vertices->at(i).xyzw[3];
}
int curNumIndices = glmesh->m_indices->size();
int additionalIndices = gfxShape->m_indices->size();
glmesh->m_indices->resize(curNumIndices+additionalIndices);
for (int k=0;k<additionalIndices;k++)
{
glmesh->m_indices->at(curNumIndices+k)=gfxShape->m_indices->at(k)+baseIndex;
}
//compensate upAxisTrans and unitMeterScaling here
btMatrix4x4 upAxisMat;
upAxisMat.setIdentity();
//upAxisMat.setPureRotation(upAxisTrans.getRotation());
btMatrix4x4 unitMeterScalingMat;
unitMeterScalingMat.setPureScaling(btVector3(unitMeterScaling,unitMeterScaling,unitMeterScaling));
btMatrix4x4 worldMat = unitMeterScalingMat*instance->m_worldTransform*upAxisMat;
//btMatrix4x4 worldMat = instance->m_worldTransform;
int curNumVertices = glmesh->m_vertices->size();
int additionalVertices = verts.size();
glmesh->m_vertices->reserve(curNumVertices+additionalVertices);
for(int v=0;v<verts.size();v++)
{
btVector3 pos(verts[v].xyzw[0],verts[v].xyzw[1],verts[v].xyzw[2]);
pos = worldMat*pos;
verts[v].xyzw[0] = float(pos[0]);
verts[v].xyzw[1] = float(pos[1]);
verts[v].xyzw[2] = float(pos[2]);
glmesh->m_vertices->push_back(verts[v]);
}
}
glmesh->m_numIndices = glmesh->m_indices->size();
glmesh->m_numvertices = glmesh->m_vertices->size();
//glmesh = LoadMeshFromCollada(fullPath);
break;
}
default:
{
b3Warning("Unsupported file type in Collision: %s\n",fullPath);
btAssert(0);
}
}
if (glmesh && (glmesh->m_numvertices>0))
{
//b3Printf("extracted %d verticed from STL file %s\n", glmesh->m_numvertices,fullPath);
//int shapeId = m_glApp->m_instancingRenderer->registerShape(&gvertices[0].pos[0],gvertices.size(),&indices[0],indices.size());
//convex->setUserIndex(shapeId);
btAlignedObjectArray<btVector3> convertedVerts;
convertedVerts.reserve(glmesh->m_numvertices);
for (int i=0;i<glmesh->m_numvertices;i++)
{
convertedVerts.push_back(btVector3(glmesh->m_vertices->at(i).xyzw[0],glmesh->m_vertices->at(i).xyzw[1],glmesh->m_vertices->at(i).xyzw[2]));
}
//btConvexHullShape* cylZShape = new btConvexHullShape(&glmesh->m_vertices->at(0).xyzw[0], glmesh->m_numvertices, sizeof(GLInstanceVertex));
btConvexHullShape* cylZShape = new btConvexHullShape(&convertedVerts[0].getX(), convertedVerts.size(), sizeof(btVector3));
//cylZShape->initializePolyhedralFeatures();
//btVector3 halfExtents(cyl->radius,cyl->radius,cyl->length/2.);
//btCylinderShapeZ* cylZShape = new btCylinderShapeZ(halfExtents);
cylZShape->setMargin(0.001);
shape = cylZShape;
} else
{
b3Warning("issue extracting mesh from STL file %s\n", fullPath);
}
delete glmesh;
} else
{
b3Warning("mesh geometry not found %s\n",fullPath);
}
}
}
break;
}
default:
{
b3Warning("Error: unknown visual geometry type\n");
}
}
return shape;
}
void MotionThreadFunc(void* userPtr,void* lsMemory)
{
printf("MotionThreadFunc thread started\n");
MotionThreadLocalStorage* localStorage = (MotionThreadLocalStorage*) lsMemory;
MotionArgs* args = (MotionArgs*) userPtr;
int workLeft = true;
b3Clock clock;
clock.reset();
bool init = true;
if (init)
{
args->m_cs->lock();
args->m_cs->setSharedParam(0,eMotionIsInitialized);
args->m_cs->unlock();
double deltaTimeInSeconds = 0;
do
{
deltaTimeInSeconds+= double(clock.getTimeMicroseconds())/1000000.;
if (deltaTimeInSeconds<(1./5000.))
{
skip++;
skip1++;
if (skip1>5)
{
b3Clock::usleep(250);
}
} else
{
skip1=0;
//process special controller commands, such as
//VR controller button press/release and controller motion
for (int c=0;c<MAX_VR_CONTROLLERS;c++)
{
btVector3 from = args->m_vrControllerPos[c];
btMatrix3x3 mat(args->m_vrControllerOrn[c]);
btScalar pickDistance = 1000.;
btVector3 toX = from+mat.getColumn(0);
btVector3 toY = from+mat.getColumn(1);
btVector3 toZ = from+mat.getColumn(2)*pickDistance;
if (args->m_isVrControllerTeleporting[c])
{
args->m_isVrControllerTeleporting[c] = false;
args->m_physicsServerPtr->pickBody(from,-toZ);
args->m_physicsServerPtr->removePickingConstraint();
}
if (!gCloseToKuka)
{
if (args->m_isVrControllerPicking[c])
{
args->m_isVrControllerPicking[c] = false;
args->m_isVrControllerDragging[c] = true;
args->m_physicsServerPtr->pickBody(from,-toZ);
//printf("PICK!\n");
}
}
if (args->m_isVrControllerDragging[c])
{
args->m_physicsServerPtr->movePickedBody(from,-toZ);
// printf(".");
}
if (args->m_isVrControllerReleasing[c])
{
args->m_isVrControllerDragging[c] = false;
args->m_isVrControllerReleasing[c] = false;
args->m_physicsServerPtr->removePickingConstraint();
//printf("Release pick\n");
}
}
//don't simulate over a huge timestep if we had some interruption (debugger breakpoint etc)
if (deltaTimeInSeconds>clampedDeltaTime)
{
deltaTimeInSeconds = clampedDeltaTime;
b3Warning("Clamp deltaTime from %f to %f",deltaTimeInSeconds, clampedDeltaTime);
}
clock.reset();
args->m_physicsServerPtr->stepSimulationRealTime(deltaTimeInSeconds);
deltaTimeInSeconds = 0;
}
args->m_physicsServerPtr->processClientCommands();
} while (args->m_cs->getSharedParam(0)!=eRequestTerminateMotion);
} else
{
args->m_cs->lock();
args->m_cs->setSharedParam(0,eMotionInitializationFailed);
args->m_cs->unlock();
}
printf("finished, #skip = %d, skip1 = %d\n",skip,skip1);
skip=0;
skip1=0;
//do nothing
}
bool PhysicsServerSharedMemory::connectSharedMemory(struct GUIHelperInterface* guiHelper)
{
m_data->m_commandProcessor->setGuiHelper(guiHelper);
bool allowCreation = true;
bool allConnected = false;
int numConnected = 0;
int counter = 0;
for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
{
if (m_data->m_areConnected[block])
{
allConnected = true;
numConnected++;
b3Warning("connectSharedMemory, while already connected");
continue;
}
do
{
m_data->m_testBlocks[block] = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlocks[block])
{
int magicId = m_data->m_testBlocks[block]->m_magicId;
if (m_data->m_verboseOutput)
{
b3Printf("magicId = %d\n", magicId);
}
if (m_data->m_testBlocks[block]->m_magicId != SHARED_MEMORY_MAGIC_NUMBER)
{
InitSharedMemoryBlock(m_data->m_testBlocks[block]);
if (m_data->m_verboseOutput)
{
b3Printf("Created and initialized shared memory block\n");
}
m_data->m_areConnected[block] = true;
numConnected++;
}
else
{
m_data->m_sharedMemory->releaseSharedMemory(m_data->m_sharedMemoryKey + block, SHARED_MEMORY_SIZE);
m_data->m_testBlocks[block] = 0;
m_data->m_areConnected[block] = false;
}
}
else
{
//b3Error("Cannot connect to shared memory");
m_data->m_areConnected[block] = false;
}
} while (counter++ < 10 && !m_data->m_areConnected[block]);
if (!m_data->m_areConnected[block])
{
b3Error("Server cannot connect to shared memory.\n");
}
}
allConnected = (numConnected == MAX_SHARED_MEMORY_BLOCKS);
return allConnected;
}
bool PhysicsClientSharedMemory::processServerStatus(SharedMemoryStatus& serverStatus)
{
bool hasStatus = false;
if (!m_data->m_testBlock1)
{
serverStatus.m_type = CMD_SHARED_MEMORY_NOT_INITIALIZED;
return true;
}
if (!m_data->m_waitingForServer)
{
serverStatus.m_type = CMD_WAITING_FOR_CLIENT_COMMAND;
return true;
}
if (m_data->m_testBlock1->m_numServerCommands> m_data->m_testBlock1->m_numProcessedServerCommands)
{
btAssert(m_data->m_testBlock1->m_numServerCommands==m_data->m_testBlock1->m_numProcessedServerCommands+1);
const SharedMemoryStatus& serverCmd =m_data->m_testBlock1->m_serverCommands[0];
hasStatus = true;
serverStatus = serverCmd;
EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
//consume the command
switch (serverCmd.m_type)
{
case CMD_CLIENT_COMMAND_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server completed command");
}
break;
}
case CMD_URDF_LOADING_COMPLETED:
{
m_data->m_serverLoadUrdfOK = true;
if (m_data->m_verboseOutput)
{
b3Printf("Server loading the URDF OK\n");
}
if (serverCmd.m_dataStreamArguments.m_streamChunkLength>0)
{
bParse::btBulletFile* bf = new bParse::btBulletFile(this->m_data->m_testBlock1->m_bulletStreamDataServerToClient,serverCmd.m_dataStreamArguments.m_streamChunkLength);
bf->setFileDNAisMemoryDNA();
bf->parse(false);
m_data->m_robotMultiBodyData.push_back(bf);
for (int i=0; i<bf->m_multiBodies.size(); i++)
{
int flag = bf->getFlags();
int qOffset = 7;
int uOffset=6;
if ((flag&bParse::FD_DOUBLE_PRECISION)!=0)
{
Bullet::btMultiBodyDoubleData* mb = (Bullet::btMultiBodyDoubleData*)bf->m_multiBodies[i];
if (mb->m_baseName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
}
}
for (int link=0; link<mb->m_numLinks; link++)
{
{
b3JointInfo info;
info.m_flags = 0;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
info.m_linkIndex = link;
if (mb->m_links[link].m_linkName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
}
info.m_linkName = mb->m_links[link].m_linkName;
}
if (mb->m_links[link].m_jointName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
}
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
}
} else
{
Bullet::btMultiBodyFloatData* mb = (Bullet::btMultiBodyFloatData*) bf->m_multiBodies[i];
if (mb->m_baseName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_baseName = %s\n",mb->m_baseName);
}
}
for (int link=0; link<mb->m_numLinks; link++)
{
{
b3JointInfo info;
info.m_flags = 0;
info.m_qIndex = qOffset;
info.m_uIndex = uOffset;
if (mb->m_links[link].m_linkName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_linkName = %s\n",link,mb->m_links[link].m_linkName);
}
info.m_linkName = mb->m_links[link].m_linkName;
}
if (mb->m_links[link].m_jointName)
{
if (m_data->m_verboseOutput)
{
b3Printf("mb->m_links[%d].m_jointName = %s\n",link,mb->m_links[link].m_jointName);
}
info.m_jointName = mb->m_links[link].m_jointName;
info.m_jointType = mb->m_links[link].m_jointType;
}
if ((mb->m_links[link].m_jointType == eRevoluteType)||
(mb->m_links[link].m_jointType == ePrismaticType))
{
info.m_flags |= JOINT_HAS_MOTORIZED_POWER;
}
m_data->m_jointInfo.push_back(info);
}
qOffset+= mb->m_links[link].m_posVarCount;
uOffset+= mb->m_links[link].m_dofCount;
}
}
}
if (bf->ok())
{
if (m_data->m_verboseOutput)
{
b3Printf("Received robot description ok!\n");
}
} else
{
b3Warning("Robot description not received");
}
}
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server received desired state");
}
break;
}
case CMD_STEP_FORWARD_SIMULATION_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server completed step simulation");
}
break;
}
case CMD_URDF_LOADING_FAILED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server failed loading the URDF...\n");
}
m_data->m_serverLoadUrdfOK = false;
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server received bullet data stream OK\n");
}
break;
}
case CMD_BULLET_DATA_STREAM_RECEIVED_FAILED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Server failed receiving bullet data stream\n");
}
break;
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Received actual state\n");
}
SharedMemoryStatus& command = m_data->m_testBlock1->m_serverCommands[0];
int numQ = command.m_sendActualStateArgs.m_numDegreeOfFreedomQ;
int numU = command.m_sendActualStateArgs.m_numDegreeOfFreedomU;
if (m_data->m_verboseOutput)
{
b3Printf("size Q = %d, size U = %d\n", numQ,numU);
}
char msg[1024];
{
sprintf(msg,"Q=[");
for (int i=0; i<numQ; i++)
{
if (i<numQ-1)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQ[i]);
}
}
sprintf(msg,"%s]",msg);
}
if (m_data->m_verboseOutput)
{
b3Printf(msg);
}
{
sprintf(msg,"U=[");
for (int i=0; i<numU; i++)
{
if (i<numU-1)
{
sprintf(msg,"%s%f,",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
} else
{
sprintf(msg,"%s%f",msg,command.m_sendActualStateArgs.m_actualStateQdot[i]);
}
}
sprintf(msg,"%s]",msg);
}
if (m_data->m_verboseOutput)
{
b3Printf(msg);
}
if (m_data->m_verboseOutput)
{
b3Printf("\n");
}
break;
}
case CMD_DEBUG_LINES_COMPLETED:
{
if (m_data->m_verboseOutput)
{
b3Printf("Success receiving %d debug lines",serverCmd.m_sendDebugLinesArgs.m_numDebugLines);
}
int numLines = serverCmd.m_sendDebugLinesArgs.m_numDebugLines;
btScalar* linesFrom = (btScalar*)&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0];
btScalar* linesTo = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+numLines*sizeof(btVector3));
btScalar* linesColor = (btScalar*)(&m_data->m_testBlock1->m_bulletStreamDataServerToClient[0]+2*numLines*sizeof(btVector3));
m_data->m_debugLinesFrom.resize(numLines);
m_data->m_debugLinesTo.resize(numLines);
m_data->m_debugLinesColor.resize(numLines);
for (int i=0; i<numLines; i++)
{
btVector3 from(linesFrom[i*4],linesFrom[i*4+1],linesFrom[i*4+2]);
btVector3 to(linesTo[i*4],linesTo[i*4+1],linesTo[i*4+2]);
btVector3 color(linesColor[i*4],linesColor[i*4+1],linesColor[i*4+2]);
m_data->m_debugLinesFrom[i] = from;
m_data->m_debugLinesTo[i] = to;
m_data->m_debugLinesColor[i] = color;
}
break;
}
case CMD_DEBUG_LINES_OVERFLOW_FAILED:
{
b3Warning("Error receiving debug lines");
m_data->m_debugLinesFrom.resize(0);
m_data->m_debugLinesTo.resize(0);
m_data->m_debugLinesColor.resize(0);
break;
}
default:
{
b3Error("Unknown server status\n");
btAssert(0);
}
};
m_data->m_testBlock1->m_numProcessedServerCommands++;
//we don't have more than 1 command outstanding (in total, either server or client)
btAssert(m_data->m_testBlock1->m_numProcessedServerCommands == m_data->m_testBlock1->m_numServerCommands);
if (m_data->m_testBlock1->m_numServerCommands == m_data->m_testBlock1->m_numProcessedServerCommands)
{
m_data->m_waitingForServer = false;
} else
{
m_data->m_waitingForServer = true;
}
} else
{
if (m_data->m_verboseOutput)
{
b3Printf("m_numServerStatus = %d, processed = %d\n", m_data->m_testBlock1->m_numServerCommands,
m_data->m_testBlock1->m_numProcessedServerCommands);
}
}
return hasStatus;
}
void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays, b3AlignedObjectArray<b3RayHit>& hitResults,
int numBodies,const struct b3RigidBodyData* bodies, int numCollidables,const struct b3Collidable* collidables, const struct b3GpuNarrowPhaseInternalData* narrowphaseData)
{
// return castRays(rays,hitResults,numBodies,bodies,numCollidables,collidables);
B3_PROFILE("castRaysHost");
for (int r=0;r<rays.size();r++)
{
b3Vector3 rayFrom = rays[r].m_from;
b3Vector3 rayTo = rays[r].m_to;
float hitFraction = hitResults[r].m_hitFraction;
int hitBodyIndex= -1;
b3Vector3 hitNormal;
for (int b=0;b<numBodies;b++)
{
const b3Vector3& pos = bodies[b].m_pos;
const b3Quaternion& orn = bodies[b].m_quat;
switch (collidables[bodies[b].m_collidableIdx].m_shapeType)
{
case SHAPE_SPHERE:
{
b3Scalar radius = collidables[bodies[b].m_collidableIdx].m_radius;
if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction))
{
hitBodyIndex = b;
b3Vector3 hitPoint;
hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction);
hitNormal = (hitPoint-bodies[b].m_pos).normalize();
}
}
case SHAPE_CONVEX_HULL:
{
b3Transform convexWorldTransform;
convexWorldTransform.setIdentity();
convexWorldTransform.setOrigin(bodies[b].m_pos);
convexWorldTransform.setRotation(bodies[b].m_quat);
b3Transform convexWorld2Local = convexWorldTransform.inverse();
b3Vector3 rayFromLocal = convexWorld2Local(rayFrom);
b3Vector3 rayToLocal = convexWorld2Local(rayTo);
int shapeIndex = collidables[bodies[b].m_collidableIdx].m_shapeIndex;
const b3ConvexPolyhedronData& poly = narrowphaseData->m_convexPolyhedra[shapeIndex];
if (rayConvex(rayFromLocal, rayToLocal,poly,narrowphaseData->m_convexFaces, hitFraction, hitNormal))
{
hitBodyIndex = b;
}
break;
}
default:
{
static bool once=true;
if (once)
{
once=false;
b3Warning("Raytest: unsupported shape type\n");
}
}
}
}
if (hitBodyIndex>=0)
{
hitResults[r].m_hitFraction = hitFraction;
hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction);
hitResults[r].m_hitNormal = hitNormal;
hitResults[r].m_hitBody = hitBodyIndex;
}
}
}
void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
{
constraint->setUserConstraintPtr(0);
m_constraints.push_back(constraint);
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
int rbA = p->getRigidBodyA().getUserIndex();
int rbB = p->getRigidBodyB().getUserIndex();
btVector3 pivotInB = p->getPivotInB();
if (rbB<=0)
{
pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(p2p);
m_rigidBodyPipeline->addConstraint(p2p);
} else
{
b3Error("invalid body index in addConstraint,b3Point2PointConstraint\n");
}
break;
}
case D6_CONSTRAINT_TYPE:
{
btGeneric6DofConstraint* dof2 = (btGeneric6DofConstraint*) constraint;
const b3RigidBodyCL* bodiesCL = m_np->getBodiesCpu();
int rbA = dof2->getRigidBodyA().getUserIndex();
int rbB = dof2->getRigidBodyB().getUserIndex();
btTransform frameInA = dof2->getFrameOffsetB();
btTransform frameInB = dof2->getFrameOffsetB();
if (rbA<=0)
{
frameInA = dof2->getRigidBodyB().getWorldTransform()*dof2->getFrameOffsetB();
rbA = m_staticBody->getUserIndex();
}
if (rbB<=0)
{
frameInB = dof2->getRigidBodyA().getWorldTransform()*dof2->getFrameOffsetA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Generic6DofConstraint* dof3 = new b3Generic6DofConstraint(rbA,rbB,(b3Transform&)frameInA,(b3Transform&)frameInB,false,bodiesCL);//(();//(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
{
btVector3 limit(0,0,0);
dof2->getLinearLowerLimit(limit);
dof3->setLinearLowerLimit((const b3Vector3&)limit);
dof2->setLinearUpperLimit(limit);
dof3->setLinearUpperLimit((const b3Vector3&)limit);
dof2->setAngularLowerLimit(limit);
dof3->setAngularLowerLimit((const b3Vector3&)limit);
dof2->setAngularUpperLimit(limit);
dof3->setAngularUpperLimit((const b3Vector3&)limit);
/* for (int i=0;i<6;i++)
{
dof3->setParam(BT_CONSTRAINT_STOP_CFM,dof2->getParam(BT_CONSTRAINT_STOP_CFM,i),i);
dof3->setParam(BT_CONSTRAINT_STOP_ERP,dof2->getParam(BT_CONSTRAINT_STOP_ERP,i),i);
}
*/
dof3->setBreakingImpulseThreshold(dof2->getBreakingImpulseThreshold());
}
// p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(dof3);
m_rigidBodyPipeline->addConstraint(dof3);
} else
{
b3Error("invalid body index in addConstraint, btGeneric6DofConstraint.\n");
}
// b3Generic6DofConstraint
break;
}
default:
b3Warning("Warning: b3GpuDynamicsWorld::addConstraint with unsupported constraint type\n");
};
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
//update sensor feedback: joint force/torque data and measured joint positions
for (int i=0;i<m_numMotors;i++)
{
int jointIndex = m_motorTargetState[i].m_jointIndex;
int positionIndex = m_motorTargetState[i].m_posIndex;
int velocityIndex = m_motorTargetState[i].m_uIndex;
m_motorTargetState[i].m_measuredJointPosition = status.m_sendActualStateArgs.m_actualStateQ[positionIndex];
m_motorTargetState[i].m_measuredJointVelocity = status.m_sendActualStateArgs.m_actualStateQdot[velocityIndex];
m_motorTargetState[i].m_measuredJointForce.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+1],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+2]);
m_motorTargetState[i].m_measuredJointTorque.setValue(status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+3],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+4],
status.m_sendActualStateArgs.m_jointReactionForces[6*jointIndex+5]);
if (m_motorTargetState[i].m_measuredJointPosition>0.1)
{
m_motorTargetState[i].m_velTarget = -1.5;
} else
{
m_motorTargetState[i].m_velTarget = 1.5;
}
b3Printf("Joint Force (Linear) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointForce.x(),m_motorTargetState[i].m_measuredJointForce.y(),m_motorTargetState[i].m_measuredJointForce.z());
b3Printf("Joint Torque (Angular) [%s]=(%f,%f,%f)\n",m_motorTargetState[i].m_jointName.c_str(),m_motorTargetState[i].m_measuredJointTorque.x(),m_motorTargetState[i].m_measuredJointTorque.y(),m_motorTargetState[i].m_measuredJointTorque.z());
}
}
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
SharedMemoryCommand sensorCommand;
sensorCommand.m_type = CMD_CREATE_SENSOR;
sensorCommand.m_createSensorArguments.m_numJointSensorChanges = 0;
for (int jointIndex=0;jointIndex<m_physicsClient.getNumJoints();jointIndex++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(jointIndex,info);
if (m_verboseOutput)
{
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
break;
}
case ROBOT_PD_CONTROL:
{
char motorName[1024];
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
break;
}
case ROBOT_PING_PONG_JOINT_FEEDBACK:
{
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_jointName = info.m_jointName;
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_jointIndex = jointIndex;
sensorCommand.m_createSensorArguments.m_jointIndex[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = jointIndex;
sensorCommand.m_createSensorArguments.m_enableJointForceSensor[sensorCommand.m_createSensorArguments.m_numJointSensorChanges] = true;
sensorCommand.m_createSensorArguments.m_numJointSensorChanges++;
m_numMotors++;
}
}
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
}
};
}
}
}
if (sensorCommand.m_createSensorArguments.m_numJointSensorChanges)
{
enqueueCommand(sensorCommand);
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
if (m_verboseOutput)
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
}
SharedMemoryCommand cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_CREATE_SENSOR)
{
b3Printf("CMD_CREATE_SENSOR!\n");
}
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
command.m_type = CMD_REQUEST_ACTUAL_STATE;
enqueueCommand(command);
}
}
}
}
}
bool PhysicsClientSharedMemory::submitClientCommand(const SharedMemoryCommand& command)
{
if (!m_data->m_waitingForServer)
{
//process command
{
m_data->m_waitingForServer = true;
switch (command.m_type)
{
case CMD_LOAD_URDF:
{
if (!m_data->m_serverLoadUrdfOK)
{
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_LOAD_URDF;
sprintf(m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_urdfFileName,"r2d2.urdf");
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[0] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[1] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialPosition[2] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[0] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[1] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[2] = 0.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_initialOrientation[3] = 1.0;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useFixedBase = false;
m_data->m_testBlock1->m_clientCommands[0].m_urdfArguments.m_useMultiBody = true;
m_data->m_testBlock1->m_numClientCommands++;
b3Printf("Client created CMD_LOAD_URDF\n");
} else
{
b3Warning("Server already loaded URDF, no client command submitted\n");
}
break;
}
case CMD_CREATE_BOX_COLLISION_SHAPE:
{
if (m_data->m_serverLoadUrdfOK)
{
b3Printf("Requesting create box collision shape\n");
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_CREATE_BOX_COLLISION_SHAPE;
m_data->m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("No URDF loaded\n");
}
break;
}
case CMD_SEND_BULLET_DATA_STREAM:
{
b3Printf("Sending a Bullet Data Stream\n");
///The idea is to pass a stream of chunks from client to server
///over shared memory. The server will process it
///Initially we will just copy an entire .bullet file into shared
///memory but we can also send individual chunks one at a time
///so it becomes a streaming solution
///In addition, we can make a separate API to create those chunks
///if needed, instead of using a 3d modeler or the Bullet SDK btSerializer
char relativeFileName[1024];
const char* fileName = "slope.bullet";
bool fileFound = b3ResourcePath::findResourcePath(fileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
fread(m_data->m_testBlock1->m_bulletStreamDataClientToServer, mFileLen, 1, fp);
fclose(fp);
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_BULLET_DATA_STREAM;
m_data->m_testBlock1->m_clientCommands[0].m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_data->m_testBlock1->m_numClientCommands++;
b3Printf("Send bullet data stream command\n");
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", fileName);
}
break;
}
case CMD_REQUEST_ACTUAL_STATE:
{
if (m_data->m_serverLoadUrdfOK)
{
b3Printf("Requesting actual state\n");
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_REQUEST_ACTUAL_STATE;
m_data->m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("No URDF loaded\n");
}
break;
}
case CMD_SEND_DESIRED_STATE:
{
if (m_data->m_serverLoadUrdfOK)
{
b3Printf("Sending desired state (pos, vel, torque)\n");
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SEND_DESIRED_STATE;
//todo: expose a drop box in the GUI for this
int controlMode = CONTROL_MODE_VELOCITY;//CONTROL_MODE_TORQUE;
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_VELOCITY;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
m_data->m_testBlock1->m_desiredStateQdot[i] = 1;
m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
case CONTROL_MODE_TORQUE:
{
m_data->m_testBlock1->m_clientCommands[0].m_sendDesiredStateCommandArgument.m_controlMode = CONTROL_MODE_TORQUE;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
m_data->m_testBlock1->m_desiredStateForceTorque[i] = 100;
}
break;
}
default:
{
b3Printf("Unknown control mode in client CMD_SEND_DESIRED_STATE");
btAssert(0);
}
}
m_data->m_testBlock1->m_numClientCommands++;
} else
{
b3Warning("Cannot send CMD_SEND_DESIRED_STATE, no URDF loaded\n");
}
break;
}
case CMD_STEP_FORWARD_SIMULATION:
{
if (m_data->m_serverLoadUrdfOK)
{
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_STEP_FORWARD_SIMULATION;
m_data->m_testBlock1->m_clientCommands[0].m_stepSimulationArguments.m_deltaTimeInSeconds = 1./60.;
m_data->m_testBlock1->m_numClientCommands++;
b3Printf("client created CMD_STEP_FORWARD_SIMULATION %d\n", m_data->m_counter++);
} else
{
b3Warning("No URDF loaded yet, no client CMD_STEP_FORWARD_SIMULATION submitted\n");
}
break;
}
case CMD_SHUTDOWN:
{
m_data->m_testBlock1->m_clientCommands[0].m_type =CMD_SHUTDOWN;
m_data->m_testBlock1->m_numClientCommands++;
m_data->m_serverLoadUrdfOK = false;
b3Printf("client created CMD_SHUTDOWN\n");
break;
}
default:
{
b3Error("unknown command requested\n");
}
}
}
}
return true;
}
SimpleOpenGL3App::SimpleOpenGL3App( const char* title, int width,int height)
{
gApp = this;
m_data = new SimpleInternalData;
m_window = new b3gDefaultOpenGLWindow();
b3gWindowConstructionInfo ci;
ci.m_title = title;
ci.m_width = width;
ci.m_height = height;
m_window->createWindow(ci);
m_window->setWindowTitle(title);
glClearColor(1,1,1,1);
m_window->startRendering();
#ifndef __APPLE__
glewInit();
#endif
m_primRenderer = new GLPrimitiveRenderer(width,height);
m_instancingRenderer = new GLInstancingRenderer(128*1024,4*1024*1024);
m_instancingRenderer->init();
m_instancingRenderer->resize(width,height);
m_instancingRenderer->InitShaders();
m_window->setMouseMoveCallback(b3DefaultMouseMoveCallback);
m_window->setMouseButtonCallback(b3DefaultMouseButtonCallback);
m_window->setKeyboardCallback(b3DefaultKeyboardCallback);
m_window->setWheelCallback(b3DefaultWheelCallback);
m_window->setResizeCallback(SimpleResizeCallback);
TwGenerateDefaultFonts();
m_data->m_fontTextureId = BindFont(g_DefaultNormalFont);
{
GLint err;
int datasize;
float sx,sy,dx,dy,lh;
GLuint texture;
m_data->m_renderCallbacks = new OpenGL2RenderCallbacks(m_primRenderer);
m_data->m_fontStash = sth_create(512,512,m_data->m_renderCallbacks);//256,256);//,1024);//512,512);
err = glGetError();
assert(err==GL_NO_ERROR);
if (!m_data->m_fontStash)
{
b3Warning("Could not create stash");
//fprintf(stderr, "Could not create stash.\n");
}
int droidRegular=0;
char* data2 = OpenSansData;
unsigned char* data = (unsigned char*) data2;
if (!(m_data->m_droidRegular = sth_add_font_from_memory(m_data->m_fontStash, data)))
{
b3Warning("error!\n");
}
err = glGetError();
assert(err==GL_NO_ERROR);
}
}
void RobotControlExample::stepSimulation(float deltaTime)
{
m_physicsServer.processClientCommands();
if (m_physicsClient.isConnected())
{
SharedMemoryStatus status;
bool hasStatus = m_physicsClient.processServerStatus(status);
if (hasStatus && status.m_type == CMD_URDF_LOADING_COMPLETED)
{
for (int i=0;i<m_physicsClient.getNumJoints();i++)
{
b3JointInfo info;
m_physicsClient.getJointInfo(i,info);
if (m_verboseOutput)
{
b3Printf("Joint %s at q-index %d and u-index %d\n",info.m_jointName,info.m_qIndex,info.m_uIndex);
}
if (info.m_flags & JOINT_HAS_MOTORIZED_POWER)
{
if (m_numMotors<MAX_NUM_MOTORS)
{
switch (m_option)
{
case ROBOT_VELOCITY_CONTROL:
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
m_numMotors++;
break;
}
case ROBOT_PD_CONTROL:
{
char motorName[1024];
MyMotorInfo* motorInfo = &m_motorTargetState[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_posIndex = info.m_qIndex;
motorInfo->m_kp = 1;
motorInfo->m_kd = 0;
{
sprintf(motorName,"%s kp", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kp);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-SIMD_PI;
slider.m_maxVal=SIMD_PI;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s kd", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_kd);
slider.m_minVal=0;
slider.m_maxVal=1;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
{
sprintf(motorName,"%s q'", info.m_jointName);
SliderParams slider(motorName,&motorInfo->m_velTarget);
slider.m_minVal=-10;
slider.m_maxVal=10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
break;
}
default:
{
b3Warning("Unknown control mode in RobotControlExample::stepSimulation");
}
};
}
}
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
if (m_verboseOutput)
{
b3Printf("Outstanding user command requests: %d\n", m_userCommandRequests.size());
}
SharedMemoryCommand cmd = m_userCommandRequests[0];
//a manual 'pop_front', we don't use 'remove' because it will re-order the commands
for (int i=1;i<m_userCommandRequests.size();i++)
{
m_userCommandRequests[i-1] = m_userCommandRequests[i];
}
m_userCommandRequests.pop_back();
if (cmd.m_type == CMD_SEND_BULLET_DATA_STREAM)
{
char relativeFileName[1024];
bool fileFound = b3ResourcePath::findResourcePath(cmd.m_dataStreamArguments.m_bulletFileName,relativeFileName,1024);
if (fileFound)
{
FILE *fp = fopen(relativeFileName, "rb");
if (fp)
{
fseek(fp, 0L, SEEK_END);
int mFileLen = ftell(fp);
fseek(fp, 0L, SEEK_SET);
if (mFileLen<SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE)
{
char* data = (char*)malloc(mFileLen);
fread(data, mFileLen, 1, fp);
fclose(fp);
cmd.m_dataStreamArguments.m_streamChunkLength = mFileLen;
m_physicsClient.uploadBulletFileToSharedMemory(data,mFileLen);
if (m_verboseOutput)
{
b3Printf("Loaded bullet data chunks into shared memory\n");
}
free(data);
} else
{
b3Warning("Bullet file size (%d) exceeds of streaming memory chunk size (%d)\n", mFileLen,SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
}
} else
{
b3Warning("Cannot open file %s\n", relativeFileName);
}
} else
{
b3Warning("Cannot find file %s\n", cmd.m_dataStreamArguments.m_bulletFileName);
}
}
m_physicsClient.submitClientCommand(cmd);
} else
{
if (m_numMotors)
{
SharedMemoryCommand command;
command.m_type =CMD_SEND_DESIRED_STATE;
prepareControlCommand(command);
enqueueCommand(command);
command.m_type =CMD_STEP_FORWARD_SIMULATION;
enqueueCommand(command);
}
}
}
}
}
void* Win32SharedMemory::allocateSharedMemory(int key, int size, bool allowCreation)
{
{
Win32SharedMemorySegment* seg = 0;
int i=0;
for (i=0;i<m_internalData->m_segments.size();i++)
{
if (m_internalData->m_segments[i].m_key == key)
{
seg = &m_internalData->m_segments[i];
break;
}
}
if (seg)
{
b3Error("already created shared memory segment using same key");
return seg->m_buf;
}
}
Win32SharedMemorySegment seg;
seg.m_key = key;
#ifdef UNICODE
swprintf_s (seg.m_szName,TEXT("MyFileMappingObject%d"),key);
#else
sprintf(seg.m_szName,"MyFileMappingObject%d",key);
#endif
seg.m_hMapFile = OpenFileMapping(
FILE_MAP_ALL_ACCESS, // read/write access
FALSE, // do not inherit the name
seg.m_szName); // name of mapping object
if (seg.m_hMapFile==NULL)
{
if (allowCreation)
{
seg.m_hMapFile = CreateFileMapping(
INVALID_HANDLE_VALUE, // use paging file
NULL, // default security
PAGE_READWRITE, // read/write access
0, // maximum object size (high-order DWORD)
size, // maximum object size (low-order DWORD)
seg.m_szName); // name of mapping object
} else
{
b3Warning("Could not create file mapping object (%d).\n",GetLastError());
return 0;
}
}
seg.m_buf = MapViewOfFile(seg.m_hMapFile, // handle to map object
FILE_MAP_ALL_ACCESS, // read/write permission
0,
0,
size);
if (seg.m_buf == NULL)
{
b3Warning("Could not map view of file (%d).\n",GetLastError());
CloseHandle(seg.m_hMapFile);
return 0;
}
m_internalData->m_segments.push_back(seg);
return seg.m_buf;
}
virtual void reportWarning(const char* warning)
{
b3Warning(warning);
}
