void PhysicsServerSharedMemory::disconnectSharedMemory(bool deInitializeSharedMemory)
{
//m_data->m_commandProcessor->deleteDynamicsWorld();
m_data->m_commandProcessor->setGuiHelper(0);
if (m_data->m_verboseOutput)
{
b3Printf("releaseSharedMemory1\n");
}
for (int block = 0; block < MAX_SHARED_MEMORY_BLOCKS; block++)
{
if (m_data->m_testBlocks[block])
{
if (m_data->m_verboseOutput)
{
b3Printf("m_testBlock1\n");
}
if (deInitializeSharedMemory)
{
m_data->m_testBlocks[block]->m_magicId = 0;
if (m_data->m_verboseOutput)
{
b3Printf("De-initialized shared memory, magic id = %d\n", m_data->m_testBlocks[block]->m_magicId);
}
}
btAssert(m_data->m_sharedMemory);
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;
}
}
void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix)
{
b3SetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
b3SetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
//also for Bullet 2.x API
btSetCustomEnterProfileZoneFunc(MyDummyEnterProfileZoneFunc);
btSetCustomLeaveProfileZoneFunc(MyDummyLeaveProfileZoneFunc);
char fileName[1024];
static int fileCounter = 0;
sprintf(fileName, "%s_%d.json", fileNamePrefix, fileCounter++);
gTimingFile = fopen(fileName, "w");
if (gTimingFile)
{
fprintf(gTimingFile, "{\"traceEvents\":[\n");
//dump the content to file
for (int i = 0; i < BT_QUICKPROF_MAX_THREAD_COUNT; i++)
{
if (gTimings[i].m_numTimings)
{
printf("Writing %d timings for thread %d\n", gTimings[i].m_numTimings, i);
gTimings[i].flush();
}
}
fprintf(gTimingFile, "\n],\n\"displayTimeUnit\": \"ns\"}");
fclose(gTimingFile);
}
else
{
b3Printf("Error opening file");
b3Printf(fileName);
}
gTimingFile = 0;
}
bool PhysicsClientSharedMemory::connect(bool allowSharedMemoryInitialization)
{
bool allowCreation = true;
m_data->m_testBlock1 = (SharedMemoryBlock*)m_data->m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE, allowCreation);
if (m_data->m_testBlock1)
{
if (m_data->m_testBlock1->m_magicId !=SHARED_MEMORY_MAGIC_NUMBER)
{
if (allowSharedMemoryInitialization)
{
InitSharedMemoryBlock(m_data->m_testBlock1);
b3Printf("Created and initialized shared memory block");
m_data->m_isConnected = true;
} else
{
b3Error("Error: please start server before client\n");
m_data->m_sharedMemory->releaseSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
m_data->m_testBlock1 = 0;
return false;
}
} else
{
b3Printf("Connected to existing shared memory, status OK.\n");
m_data->m_isConnected = true;
}
} else
{
b3Error("Cannot connect to shared memory");
return false;
}
return true;
}
void PhysicsServerSharedMemory::releaseSharedMemory()
{
if (m_data->m_verboseOutput)
{
b3Printf("releaseSharedMemory1\n");
}
if (m_data->m_testBlock1)
{
if (m_data->m_verboseOutput)
{
b3Printf("m_testBlock1\n");
}
m_data->m_testBlock1->m_magicId = 0;
if (m_data->m_verboseOutput)
{
b3Printf("magic id = %d\n",m_data->m_testBlock1->m_magicId);
}
btAssert(m_data->m_sharedMemory);
m_data->m_sharedMemory->releaseSharedMemory( m_data->m_sharedMemoryKey
, SHARED_MEMORY_SIZE);
}
if (m_data->m_sharedMemory)
{
if (m_data->m_verboseOutput)
{
b3Printf("m_sharedMemory\n");
}
if (m_data->m_ownsSharedMemory)
{
delete m_data->m_sharedMemory;
}
m_data->m_sharedMemory = 0;
m_data->m_testBlock1 = 0;
}
}
static int gLoadMultiBodyFromUrdf(lua_State *L)
{
int argc = lua_gettop(L);
if (argc==4)
{
if (!lua_isuserdata(L,1))
{
std::cerr << "error: first argument to b3CreateRigidbody should be world";
return 0;
}
luaL_checktype(L,3, LUA_TTABLE);
btVector3 pos = getLuaVectorArg(L,3);
btQuaternion orn = getLuaQuaternionArg(L,4);
btDiscreteDynamicsWorld* world = (btDiscreteDynamicsWorld*) lua_touserdata(L,1);
if (world != sLuaDemo->m_dynamicsWorld)
{
std::cerr << "error: first argument expected to be a world";
return 0;
}
const char* fileName = lua_tostring(L,2);
#if 1
BulletURDFImporter u2b(sLuaDemo->m_guiHelper);
bool loadOk = u2b.loadURDF(fileName);
if (loadOk)
{
b3Printf("loaded %s OK!", fileName);
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(orn);
int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(sLuaDemo->m_guiHelper);
bool m_useMultiBody = true;
ConvertURDF2Bullet(u2b,creation, tr,sLuaDemo->m_dynamicsWorld,m_useMultiBody,u2b.getPathPrefix());
btMultiBody* mb = creation.getBulletMultiBody();
if (mb)
{
lua_pushlightuserdata (L, mb);
return 1;
}
} else
{
b3Printf("can't find %s",fileName);
}
#endif
}
return 0;
}
void PhysicsServer::initPhysics()
{
createEmptyDynamicsWorld();
m_testBlock1 = (SharedMemoryExampleData*) m_sharedMemory->allocateSharedMemory(SHARED_MEMORY_KEY, SHARED_MEMORY_SIZE);
// btAssert(m_testBlock1);
if (m_testBlock1)
{
// btAssert(m_testBlock1->m_magicId != SHARED_MEMORY_MAGIC_NUMBER);
if (m_testBlock1->m_magicId == SHARED_MEMORY_MAGIC_NUMBER)
{
b3Printf("Warning: shared memory is already initialized, did you already spawn a server?\n");
}
m_testBlock1->m_numClientCommands = 0;
m_testBlock1->m_numServerCommands = 0;
m_testBlock1->m_numProcessedClientCommands=0;
m_testBlock1->m_numProcessedServerCommands=0;
m_testBlock1->m_magicId = SHARED_MEMORY_MAGIC_NUMBER;
b3Printf("Shared memory succesfully allocated\n");
} else
{
b3Error("Couldn't allocated shared memory, is it implemented on your operating system?\n");
}
}
virtual void initPhysics()
{
///create some graphics proxy for the tracking target
///the endeffector tries to track it using Inverse Kinematics
{
int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM);
b3Quaternion orn(0, 0, 0, 1);
b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1);
b3Vector3 scaling = b3MakeVector3(.02, .02, .02);
m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling);
}
m_app->m_renderer->writeTransforms();
int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
// 0;//m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d",connected);
{
m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf");
if (m_kukaIndex >=0)
{
int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
b3Printf("numJoints = %d",numJoints);
for (int i=0;i<numJoints;i++)
{
b3JointInfo jointInfo;
m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo);
b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName);
}
/*
int wheelJointIndices[4]={2,3,6,7};
int wheelTargetVelocities[4]={-10,-10,-10,-10};
for (int i=0;i<4;i++)
{
b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY);
controlArgs.m_targetVelocity = wheelTargetVelocities[i];
controlArgs.m_maxTorqueValue = 1e30;
m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs);
}
*/
}
{
m_robotSim.loadURDF("plane.urdf");
m_robotSim.setGravity(b3MakeVector3(0,0,0));
}
}
}
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);
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)
{
char motorName[1024];
sprintf(motorName,"%s q'", info.m_jointName);
MyMotorInfo* motorInfo = &m_motorTargetVelocities[m_numMotors];
motorInfo->m_velTarget = 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++;
}
}
}
}
if (m_physicsClient.canSubmitCommand())
{
if (m_userCommandRequests.size())
{
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();
m_physicsClient.submitClientCommand(cmd);
}
}
}
}
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 MultiBodyConstraintFeedbackSetup::stepSimulation(float deltaTime)
{
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
btScalar torque = m_multiBody->getJointTorque(0);
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
}
btScalar timeStep = 1./240.f;
m_dynamicsWorld->stepSimulation(timeStep,0);
static int count = 0;
if ((count& 0x0f)==0)
{
if (m_motor)
{
float force = m_motor->getAppliedImpulse(0)/timeStep;
b3Printf("motor applied force = %f\n", force);
}
for (int i=0;i<m_jointFeedbacks.size();i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
}
count++;
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
// b3Printf("child angvel = %f",jointVel);
}
void TestHingeTorque::stepSimulation(float deltaTime)
{
if (0)//m_once)
{
m_once=false;
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
btRigidBody& bodyA = hinge->getRigidBodyA();
btTransform trA = bodyA.getWorldTransform();
btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10);
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10);
}
m_dynamicsWorld->stepSimulation(1./240,0);
static int count = 0;
if ((count& 0x0f)==0)
{
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
base->getAngularVelocity()[1],
base->getAngularVelocity()[2]);
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
child->getAngularVelocity()[1],
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
{
b3Printf("Applied force at the COM/Inertial frame B[%d]:(%f,%f,%f), torque B:(%f,%f,%f)\n", i,
m_jointFeedback[i]->m_appliedForceBodyB.x(),
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
}
}
count++;
//CommonRigidBodyBase::stepSimulation(deltaTime);
}
void PosixSharedMemory::releaseSharedMemory(int key, int size)
{
#ifdef TEST_SHARED_MEMORY
btSharedMemorySegment* 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 (0==seg)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory key not found");
return;
}
if (seg->m_sharedMemoryId < 0)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shared memory id is not set");
} else
{
if (seg->m_createdSharedMemory)
{
int result = shmctl(seg->m_sharedMemoryId,IPC_RMID,0);
if (result == -1)
{
b3Error("PosixSharedMemory::releaseSharedMemory: shmat returned -1");
} else
{
b3Printf("PosixSharedMemory::releaseSharedMemory removed shared memory");
}
seg->m_createdSharedMemory = false;
seg->m_sharedMemoryId = -1;
}
if (seg->m_sharedMemoryPtr)
{
shmdt(seg->m_sharedMemoryPtr);
seg->m_sharedMemoryPtr = 0;
b3Printf("PosixSharedMemory::releaseSharedMemory detached shared memory\n");
}
}
m_internalData->m_segments.removeAtIndex(i);
#endif
}
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsShutdown(false)
{
b3Printf("Started PhysicsServer\n");
bool useServer = true;
}
b3Scalar resolveCollision(LWRigidBody& bodyA,
LWRigidBody& bodyB,
LWContactPoint& contactPoint)
{
b3Assert(contactPoint.m_distance<=0);
btScalar appliedImpulse = 0.f;
b3Vector3 rel_pos1 = contactPoint.m_ptOnAWorld - bodyA.m_worldPose.m_position;
b3Vector3 rel_pos2 = contactPoint.m_ptOnBWorld - bodyB.getPosition();
btScalar rel_vel = contactPoint.m_normalOnB.dot(bodyA.getVelocity(rel_pos1) - bodyB.getVelocity(rel_pos2));
if (rel_vel < -B3_EPSILON)
{
b3Vector3 temp1 = bodyA.m_invInertiaTensorWorld * rel_pos1.cross(contactPoint.m_normalOnB);
b3Vector3 temp2 = bodyB.m_invInertiaTensorWorld * rel_pos2.cross(contactPoint.m_normalOnB);
btScalar impulse = -(1.0f + gRestitution) * rel_vel /
(bodyA.m_invMass + bodyB.m_invMass + contactPoint.m_normalOnB.dot(temp1.cross(rel_pos1) + temp2.cross(rel_pos2)));
b3Vector3 impulse_vector = contactPoint.m_normalOnB * impulse;
b3Printf("impulse = %f\n", impulse);
appliedImpulse = impulse;
bodyA.applyImpulse(impulse_vector, rel_pos1);
bodyB.applyImpulse(-impulse_vector, rel_pos2);
}
return appliedImpulse;
}
void GpuConvexScene::setupScene()
{
m_raycaster = new b3GpuRaycast(m_clData->m_clContext,m_clData->m_clDevice,m_clData->m_clQueue);
int index=0;
createStaticEnvironment();
index+=createDynamicsObjects();
m_data->m_rigidBodyPipeline->writeAllInstancesToGpu();
float camPos[4]={0,0,0,0};//ci.arraySizeX,ci.arraySizeY/2,ci.arraySizeZ,0};
//float camPos[4]={1,12.5,1.5,0};
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraTargetPosition(camPos[0],camPos[1],camPos[2]);
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(150);
//m_instancingRenderer->setCameraYaw(85);
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraYaw(30);
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(225);
m_guiHelper->getRenderInterface()->updateCamera(1);//>updateCamera();
char msg[1024];
int numInstances = index;
sprintf(msg,"Num objects = %d",numInstances);
b3Printf(msg);
//if (ci.m_gui)
// ci.m_gui->setStatusBarMessage(msg,true);
}
PhysicsClientExample::~PhysicsClientExample()
{
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
if (m_options == eCLIENTEXAMPLE_SERVER)
{
bool deInitializeSharedMemory = true;
m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory);
}
if (m_canvas)
{
if (m_canvasRGBIndex >= 0)
m_canvas->destroyCanvas(m_canvasRGBIndex);
if (m_canvasDepthIndex >= 0)
m_canvas->destroyCanvas(m_canvasDepthIndex);
if (m_canvasSegMaskIndex >= 0)
m_canvas->destroyCanvas(m_canvasSegMaskIndex);
}
b3Printf("~PhysicsClientExample\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
{
b3Error("Cannot connect to shared memory");
return false;
}
return true;
}
bool PhysicsServer::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn,
bool useMultiBody, bool useFixedBase)
{
MyURDFImporter u2b(m_guiHelper);
bool loadOk = u2b.loadURDF(fileName);
if (loadOk)
{
b3Printf("loaded %s OK!", fileName);
btTransform tr;
tr.setIdentity();
tr.setOrigin(pos);
tr.setRotation(orn);
int rootLinkIndex = u2b.getRootLinkIndex();
// printf("urdf root link index = %d\n",rootLinkIndex);
MyMultiBodyCreator creation(m_guiHelper);
ConvertURDF2Bullet(u2b,creation, tr,m_dynamicsWorld,useMultiBody,u2b.getPathPrefix());
btMultiBody* mb = creation.getBulletMultiBody();
return true;
}
return false;
}
PhysicsClientExample::PhysicsClientExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsTermination(false),
m_numMotors(0)
{
b3Printf("Started PhysicsClientExample\n");
}
PhysicsServerExample::PhysicsServerExample(GUIHelperInterface* helper)
:SharedMemoryCommon(helper),
m_wantsShutdown(false),
m_isConnected(false)
{
b3Printf("Started PhysicsServer\n");
}
void* b3AlignedAllocInternal (size_t size, int alignment,int line,char* filename)
{
void *ret;
char *real;
b3g_totalBytesAlignedAllocs += size;
b3g_numAlignedAllocs++;
real = (char *)b3s_allocFunc(size + 2*sizeof(void *) + (alignment-1));
if (real) {
ret = (void*) b3AlignPointer(real + 2*sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
*((int*)(ret)-2) = size;
} else {
ret = (void *)(real);//??
}
b3Printf("allocation#%d at address %x, from %s,line %d, size %d\n",b3g_numAlignedAllocs,real, filename,line,size);
int* ptr = (int*)ret;
*ptr = 12;
return (ret);
}
void b3GpuDynamicsWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback) const
{
b3AlignedObjectArray<b3RayInfo> rays;
b3RayInfo ray;
ray.m_from = (const b3Vector3&)rayFromWorld;
ray.m_to = (const b3Vector3&)rayToWorld;
rays.push_back(ray);
b3AlignedObjectArray<b3RayHit> hitResults;
b3RayHit hit;
hit.m_hitFraction = 1.f;
hitResults.push_back(hit);
m_rigidBodyPipeline->castRays(rays,hitResults);
b3Printf("hit = %f\n", hitResults[0].m_hitFraction);
if (hitResults[0].m_hitFraction<1.f)
{
b3Assert(hitResults[0].m_hitBody >=0);
b3Assert(hitResults[0].m_hitBody < m_collisionObjects.size());
b3Vector3 hitNormalLocal = hitResults[0].m_hitNormal;
btCollisionObject* colObj = m_collisionObjects[hitResults[0].m_hitBody];
LocalRayResult rayResult(colObj,0,(btVector3&)hitNormalLocal,hitResults[0].m_hitFraction);
rayResult.m_hitFraction = hitResults[0].m_hitFraction;
resultCallback.addSingleResult(rayResult,true);
}
}
ImportUrdfSetup::ImportUrdfSetup(struct GUIHelperInterface* helper, int option, const char* fileName)
:CommonMultiBodyBase(helper)
{
m_data = new ImportUrdfInternalData;
if (option==1)
{
m_useMultiBody = true;
} else
{
m_useMultiBody = false;
}
static int count = 0;
if (fileName)
{
setFileName(fileName);
} else
{
gFileNameArray.clear();
//load additional urdf file names from file
FILE* f = fopen("urdf_files.txt","r");
if (f)
{
int result;
//warning: we don't avoid string buffer overflow in this basic example in fscanf
char fileName[1024];
do
{
result = fscanf(f,"%s",fileName);
b3Printf("urdf_files.txt entry %s",fileName);
if (result==1)
{
gFileNameArray.push_back(fileName);
}
} while (result==1);
fclose(f);
}
if (gFileNameArray.size()==0)
{
gFileNameArray.push_back("r2d2.urdf");
}
int numFileNames = gFileNameArray.size();
if (count>=numFileNames)
{
count=0;
}
sprintf(m_fileName,"%s",gFileNameArray[count++].c_str());
}
}
void PhysicsClientExample::createButtons()
{
bool isTrigger = false;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->removeAllParameters();
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("Create Cylinder Body",CMD_CREATE_RIGID_BODY,isTrigger);
createButton("Reset Simulation",CMD_RESET_SIMULATION,isTrigger);
createButton("Initialize Pose",CMD_INIT_POSE, isTrigger);
createButton("Set gravity", CMD_SEND_PHYSICS_SIMULATION_PARAMETERS, isTrigger);
if (m_physicsClientHandle && m_selectedBody>=0)
{
int numJoints = b3GetNumJoints(m_physicsClientHandle,m_selectedBody);
for (int i=0;i<numJoints;i++)
{
b3JointInfo info;
b3GetJointInfo(m_physicsClientHandle,m_selectedBody,i,&info);
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)
{
char motorName[1024];
sprintf(motorName,"%s q", info.m_jointName);
// MyMotorInfo2* motorInfo = &m_motorTargetVelocities[m_numMotors];
MyMotorInfo2* motorInfo = &m_motorTargetPositions[m_numMotors];
motorInfo->m_velTarget = 0.f;
motorInfo->m_posTarget = 0.f;
motorInfo->m_uIndex = info.m_uIndex;
motorInfo->m_qIndex = info.m_qIndex;
// SliderParams slider(motorName,&motorInfo->m_velTarget);
// slider.m_minVal=-4;
// slider.m_maxVal=4;
SliderParams slider(motorName,&motorInfo->m_posTarget);
slider.m_minVal=-4;
slider.m_maxVal=4;
if (m_guiHelper && m_guiHelper->getParameterInterface())
{
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
}
m_numMotors++;
}
}
}
}
}
}
static void cleanup(int signo)
{
if (!interrupted) { // this is the second time, we're hanging somewhere
b3Printf("Aborting and deleting SharedMemoryCommon object");
delete sExampleBrowser;
sleep(1);
sExampleBrowser = 0;
errx(EXIT_FAILURE, "aborted example on signal %d", signo);
} else
{
b3Printf("no action");
exit(EXIT_FAILURE);
}
interrupted = true;
warnx("caught signal %d", signo);
}
virtual void exitPhysics()
{
b3Printf("exitPhysics, stopping threads");
bool blockingWait =false;
int arg0,arg1;
args.m_cs->lock();
//terminate all threads
args.m_cs->setSharedParam(1,MAGIC_RESET_NUMBER);
args.m_cs->unlock();
if (blockingWait)
{
for (int i=0;i<m_numThreads;i++)
{
m_threadSupport->waitForResponse(&arg0,&arg1);
printf("finished waiting for response: %d %d\n", arg0,arg1);
}
} else
{
int numActiveThreads = m_numThreads;
while (numActiveThreads)
{
if (m_threadSupport->isTaskCompleted(&arg0,&arg1,0))
{
numActiveThreads--;
printf("numActiveThreads = %d\n",numActiveThreads);
} else
{
// printf("polling..");
}
};
}
delete m_threadSupport;
b3Printf("Threads stopped");
for (int i=0;i<m_jobs.size();i++)
{
delete m_jobs[i];
}
m_jobs.clear();
}
PhysicsClientExample::~PhysicsClientExample()
{
if (m_physicsClientHandle)
{
b3ProcessServerStatus(m_physicsClientHandle);
b3DisconnectSharedMemory(m_physicsClientHandle);
}
b3Printf("~PhysicsClientExample\n");
}
void MultiPendulumExample::applyPendulumForce(btScalar pendulumForce){
if(pendulumForce != 0){
b3Printf("Apply %f to pendulum",pendulumForce);
for (int i = 0; i < gDisplacedPendula; i++) {
if (gDisplacedPendula >= 0 && gDisplacedPendula <= gPendulaQty)
pendula[i]->applyCentralForce(btVector3(pendulumForce, 0, 0));
}
}
}
void enqueueCommand(const SharedMemoryCommand& orgCommand)
{
m_userCommandRequests.push_back(orgCommand);
SharedMemoryCommand& cmd = m_userCommandRequests[m_userCommandRequests.size()-1];
cmd.m_sequenceNumber = m_sequenceNumberGenerator++;
cmd.m_timeStamp = m_realtimeClock.getTimeMicroseconds();
b3Printf("User put command request %d on queue (queue length = %d)\n",cmd.m_type, m_userCommandRequests.size());
}
bool PhysicsDirect::processContactPointData(const struct SharedMemoryCommand& orgCommand)
{
SharedMemoryCommand command = orgCommand;
const SharedMemoryStatus& serverCmd = m_data->m_serverStatus;
do
{
bool hasStatus = m_data->m_commandProcessor->processCommand(command, m_data->m_serverStatus, &m_data->m_bulletStreamDataServerToClient[0], SHARED_MEMORY_MAX_STREAM_CHUNK_SIZE);
b3Clock clock;
double startTime = clock.getTimeInSeconds();
double timeOutInSeconds = m_data->m_timeOutInSeconds;
while ((!hasStatus) && (clock.getTimeInSeconds() - startTime < timeOutInSeconds))
{
const SharedMemoryStatus* stat = processServerStatus();
if (stat)
{
hasStatus = true;
}
}
m_data->m_hasStatus = hasStatus;
if (hasStatus)
{
if (m_data->m_verboseOutput)
{
b3Printf("Contact Point Information Request OK\n");
}
int startContactIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex;
int numContactsCopied = serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
m_data->m_cachedContactPoints.resize(startContactIndex + numContactsCopied);
b3ContactPointData* contactData = (b3ContactPointData*)&m_data->m_bulletStreamDataServerToClient[0];
for (int i = 0; i < numContactsCopied; i++)
{
m_data->m_cachedContactPoints[startContactIndex + i] = contactData[i];
}
if (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied)
{
m_data->m_hasStatus = false;
command.m_type = CMD_REQUEST_CONTACT_POINT_INFORMATION;
command.m_requestContactPointArguments.m_startingContactPointIndex = serverCmd.m_sendContactPointArgs.m_startingContactPointIndex + serverCmd.m_sendContactPointArgs.m_numContactPointsCopied;
command.m_requestContactPointArguments.m_objectAIndexFilter = -1;
command.m_requestContactPointArguments.m_objectBIndexFilter = -1;
}
}
} while (serverCmd.m_sendContactPointArgs.m_numRemainingContactPoints > 0 && serverCmd.m_sendContactPointArgs.m_numContactPointsCopied);
return m_data->m_hasStatus;
}
