From 6d1948e79e5caf74cfa2e3344ee308bd81fe2c33 Mon Sep 17 00:00:00 2001 From: "Erwin Coumans (Google)" Date: Fri, 24 Jun 2016 07:31:17 -0700 Subject: [PATCH] tweaks in pybullet and shared memory C-API: allow to reset the state of a single joint allow to set the target/mode for a single joint motor at a time rename pybullet API: initializeJointPositions -> resetJointState --- examples/BasicDemo/BasicExample.cpp | 2 +- examples/SharedMemory/PhysicsClientC_API.cpp | 33 + examples/SharedMemory/PhysicsClientC_API.h | 7 +- .../PhysicsServerCommandProcessor.cpp | 579 +++++++++--------- examples/SharedMemory/SharedMemoryCommands.h | 4 + examples/pybullet/pybullet.c | 247 ++++---- test/SharedMemory/test.c | 4 +- 7 files changed, 463 insertions(+), 413 deletions(-) diff --git a/examples/BasicDemo/BasicExample.cpp b/examples/BasicDemo/BasicExample.cpp index f5bd9486c..b6365df36 100644 --- a/examples/BasicDemo/BasicExample.cpp +++ b/examples/BasicDemo/BasicExample.cpp @@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase float dist = 41; float pitch = 52; float yaw = 35; - float targetPos[3]={0,0.46,0}; + float targetPos[3]={0,0,0}; m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]); } }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 57121f0c8..203e0ebbe 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -167,6 +167,10 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle ph command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode; command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId; command->m_updateFlags = 0; + for (int i=0;im_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0; + } return (b3SharedMemoryCommandHandle) command; } @@ -176,6 +180,7 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q; return 0; } @@ -186,6 +191,8 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP; + return 0; } @@ -195,6 +202,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD; return 0; } @@ -205,6 +213,8 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT; + return 0; } @@ -215,6 +225,8 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + return 0; } @@ -224,6 +236,8 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl b3Assert(command); command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE; + return 0; } @@ -372,6 +386,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex) { + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); @@ -380,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl command->m_type = CMD_INIT_POSE; command->m_updateFlags =0; command->m_initPoseArgs.m_bodyUniqueId = bodyIndex; + //a bit slow, initialing the full range to zero... + for (int i=0;im_initPoseArgs.m_hasInitialStateQ[i] = 0; + } return (b3SharedMemoryCommandHandle) command; } @@ -392,6 +412,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle command->m_initPoseArgs.m_initialStateQ[0] = startPosX; command->m_initPoseArgs.m_initialStateQ[1] = startPosY; command->m_initPoseArgs.m_initialStateQ[2] = startPosZ; + + command->m_initPoseArgs.m_hasInitialStateQ[0] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[1] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[2] = 1; + return 0; } @@ -405,6 +430,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan command->m_initPoseArgs.m_initialStateQ[4] = startOrnY; command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ; command->m_initPoseArgs.m_initialStateQ[6] = startOrnW; + + command->m_initPoseArgs.m_hasInitialStateQ[3] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[4] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[5] = 1; + command->m_initPoseArgs.m_hasInitialStateQ[6] = 1; + return 0; } @@ -417,6 +448,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand for (int i=0;im_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i]; + command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1; } return 0; } @@ -433,6 +465,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0) { command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition; + command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1; } return 0; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 0f036415c..aee561d8f 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -99,15 +99,16 @@ b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClien b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode); -///Set joint control variables such as desired position/angle, desired velocity, +///Set joint motor control variables such as desired position/angle, desired velocity, ///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE) b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode); - ///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD +///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value); int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); -//Only use when controlMode is CONTROL_MODE_VELOCITY + +///Only use when controlMode is CONTROL_MODE_VELOCITY int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); ///Only use if when controlMode is CONTROL_MODE_TORQUE, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index b2c3c5c05..3cdaac902 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -55,12 +55,12 @@ struct SharedMemoryDebugDrawer : public btIDebugDraw virtual void draw3dText(const btVector3& location,const char* textString) { } - + virtual void setDebugMode(int debugMode) { m_debugMode = debugMode; } - + virtual int getDebugMode() const { return m_debugMode; @@ -100,11 +100,11 @@ struct InternalBodyHandle : public InteralBodyData BT_DECLARE_ALIGNED_ALLOCATOR(); int m_nextFreeHandle; - void SetNextFree(int next) + void SetNextFree(int next) { m_nextFreeHandle = next; } - int GetNextFree() const + int GetNextFree() const { return m_nextFreeHandle; } @@ -153,7 +153,7 @@ public: struct CommandLogger { FILE* m_file; - + void writeHeader(unsigned char* buffer) const { @@ -185,15 +185,15 @@ struct CommandLogger buffer[9] = 0; buffer[10] = 0; buffer[11] = 0; - + int ver = btGetVersion(); if (ver>=0 && ver<999) { sprintf((char*)&buffer[9],"%d",ver); } - + } - + void logCommand(const SharedMemoryCommand& command) { btCommandChunk chunk; @@ -240,10 +240,10 @@ struct CommandLogPlayback } unsigned char c = m_header[7]; m_fileIs64bit = (c=='-'); - + const bool VOID_IS_8 = ((sizeof(void*)==8)); m_bitsVary = (VOID_IS_8 != m_fileIs64bit); - + } @@ -260,7 +260,7 @@ struct CommandLogPlayback if (m_file) { size_t s = 0; - + if (m_fileIs64bit) { @@ -271,7 +271,7 @@ struct CommandLogPlayback bCommandChunkPtr4 chunk4; s = fread((void*)&chunk4,sizeof(bCommandChunkPtr4),1,m_file); } - + if (s==1) { s = fread(cmd,sizeof(SharedMemoryCommand),1,m_file); @@ -324,7 +324,7 @@ struct PhysicsServerCommandProcessorInternalData { m_numUsedHandles = 0; m_firstFreeHandle = -1; - + increaseHandleCapacity(1); } @@ -349,10 +349,10 @@ struct PhysicsServerCommandProcessorInternalData int additionalCapacity= m_bodyHandles.size(); increaseHandleCapacity(additionalCapacity); - + getHandle(handle)->SetNextFree(m_firstFreeHandle); } - + return handle; } @@ -369,16 +369,16 @@ struct PhysicsServerCommandProcessorInternalData } ///end handle management - - + + CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; - + btScalar m_physicsDeltaTime; btAlignedObjectArray m_multiBodyJointFeedbacks; - + btAlignedObjectArray m_worldImporters; btAlignedObjectArray m_urdfLinkNameMapper; @@ -391,17 +391,17 @@ struct PhysicsServerCommandProcessorInternalData btDefaultCollisionConfiguration* m_collisionConfiguration; btMultiBodyDynamicsWorld* m_dynamicsWorld; SharedMemoryDebugDrawer* m_remoteDebugDrawer; - - btAlignedObjectArray m_sdfRecentLoadedBodies; - - + btAlignedObjectArray m_sdfRecentLoadedBodies; + + + struct GUIHelperInterface* m_guiHelper; int m_sharedMemoryKey; bool m_verboseOutput; - - + + //data for picking objects class btRigidBody* m_pickedBody; class btTypedConstraint* m_pickedConstraint; @@ -426,7 +426,7 @@ struct PhysicsServerCommandProcessorInternalData m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0) { - + initHandles(); #if 0 btAlignedObjectArray bla; @@ -478,16 +478,16 @@ struct PhysicsServerCommandProcessorInternalData void PhysicsServerCommandProcessor::setGuiHelper(struct GUIHelperInterface* guiHelper) { - + if (guiHelper) { - + guiHelper->createPhysicsDebugDrawer(m_data->m_dynamicsWorld); } else { if (m_data->m_guiHelper && m_data->m_dynamicsWorld && m_data->m_dynamicsWorld->getDebugDrawer()) { - + m_data->m_dynamicsWorld->setDebugDrawer(0); } } @@ -521,45 +521,45 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() ///collision configuration contains default setup for memory, collision setup m_data->m_collisionConfiguration = new btDefaultCollisionConfiguration(); //m_collisionConfiguration->setConvexConvexMultipointIterations(); - + ///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded) m_data->m_dispatcher = new btCollisionDispatcher(m_data->m_collisionConfiguration); - + m_data->m_broadphase = new btDbvtBroadphase(); - + m_data->m_solver = new btMultiBodyConstraintSolver; - + m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration); m_data->m_remoteDebugDrawer = new SharedMemoryDebugDrawer(); - - + + m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0)); } void PhysicsServerCommandProcessor::deleteDynamicsWorld() { - + for (int i=0;im_multiBodyJointFeedbacks.size();i++) { delete m_data->m_multiBodyJointFeedbacks[i]; } m_data->m_multiBodyJointFeedbacks.clear(); - - + + for (int i=0;im_worldImporters.size();i++) { delete m_data->m_worldImporters[i]; } m_data->m_worldImporters.clear(); - + for (int i=0;im_urdfLinkNameMapper.size();i++) { delete m_data->m_urdfLinkNameMapper[i]; } m_data->m_urdfLinkNameMapper.clear(); - - + + for (int i=0;im_strings.size();i++) { delete m_data->m_strings[i]; @@ -568,11 +568,11 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() btAlignedObjectArray constraints; btAlignedObjectArray mbconstraints; - - + + if (m_data->m_dynamicsWorld) { - + int i; for (i = m_data->m_dynamicsWorld->getNumConstraints() - 1; i >= 0; i--) { @@ -586,7 +586,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() mbconstraints.push_back(mbconstraint); m_data->m_dynamicsWorld->removeMultiBodyConstraint(mbconstraint); } - + for (i = m_data->m_dynamicsWorld->getNumCollisionObjects() - 1; i >= 0; i--) { btCollisionObject* obj = m_data->m_dynamicsWorld->getCollisionObjectArray()[i]; @@ -662,7 +662,7 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb) for (int i=0;igetLink(mbLinkIndex).m_userPtr = motor; m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); motor->finalizeMultiDof(); - + } } @@ -688,11 +688,11 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe b3Error("loadSdf: No valid m_dynamicsWorld"); return false; } - + m_data->m_sdfRecentLoadedBodies.clear(); - + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); - + bool useFixedBase = false; bool loadOk = u2b.loadSDF(fileName, useFixedBase); if (loadOk) @@ -709,16 +709,16 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe m_data->m_collisionShapes.push_back(compound->getChildShape(childIndex)); } } - + } - + btTransform rootTrans; rootTrans.setIdentity(); if (m_data->m_verboseOutput) { b3Printf("loaded %s OK!", fileName); } - + for (int m =0; mallocHandle(); - + InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); { @@ -737,8 +737,8 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe int urdfLinkIndex = u2b.getRootLinkIndex(); u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,bodyHandle->m_rootLocalInertialFrame); } - - + + //todo: move these internal API called inside the 'ConvertURDF2Bullet' call, hidden from the user int rootLinkIndex = u2b.getRootLinkIndex(); @@ -748,26 +748,26 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe u2b.getRootTransformInWorld(rootTrans); bool useMultiBody = true; ConvertURDF2Bullet(u2b,creation, rootTrans,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix(),true); - - - + + + mb = creation.getBulletMultiBody(); if (mb) { bodyHandle->m_multiBody = mb; - + m_data->m_sdfRecentLoadedBodies.push_back(bodyUniqueId); - + createJointMotors(mb); - + //disable serialization of the collision objects (they are too big, and the client likely doesn't need them); bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks()); for (int i=0;igetNumLinks();i++) { //disable serialization of the collision objects - + int urdfLinkIndex = creation.m_mb2urdfLink[i]; btScalar mass; btVector3 localInertiaDiagonal(0,0,0); @@ -777,12 +777,12 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str()); m_data->m_strings.push_back(linkName); - + mb->getLink(i).m_linkName = linkName->c_str(); std::string* jointName = new std::string(u2b.getJointName(urdfLinkIndex).c_str()); m_data->m_strings.push_back(jointName); - + mb->getLink(i).m_jointName = jointName->c_str(); } std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); @@ -792,10 +792,10 @@ bool PhysicsServerCommandProcessor::loadSdf(const char* fileName, char* bufferSe { b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); } - + } } - return loadOk; + return loadOk; } bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVector3& pos, const btQuaternion& orn, @@ -809,13 +809,13 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto } - + BulletURDFImporter u2b(m_data->m_guiHelper, &m_data->m_visualConverter); - + bool loadOk = u2b.loadURDF(fileName, useFixedBase); - - + + if (loadOk) { //get a body index @@ -836,7 +836,7 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto { b3Printf("loaded %s OK!", fileName); } - + btTransform tr; tr.setIdentity(); tr.setOrigin(pos); @@ -844,9 +844,9 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto //int rootLinkIndex = u2b.getRootLinkIndex(); // printf("urdf root link index = %d\n",rootLinkIndex); MyMultiBodyCreator creation(m_data->m_guiHelper); - + ConvertURDF2Bullet(u2b,creation, tr,m_data->m_dynamicsWorld,useMultiBody,u2b.getPathPrefix()); - + for (int i=0;im_collisionShapes.push_back(compound->getChildShape(childIndex)); } } - + } - + btMultiBody* mb = creation.getBulletMultiBody(); if (useMultiBody) { - - + + if (mb) { - + bodyHandle->m_multiBody = mb; - + createJointMotors(mb); @@ -905,17 +905,17 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto util->m_memSerializer->registerNameForPointer(jointName->c_str(),jointName->c_str()); mb->getLink(i).m_jointName = jointName->c_str(); } - + std::string* baseName = new std::string(u2b.getLinkName(u2b.getRootLinkIndex())); m_data->m_strings.push_back(baseName); - + util->m_memSerializer->registerNameForPointer(baseName->c_str(),baseName->c_str()); mb->setBaseName(baseName->c_str()); - - + + util->m_memSerializer->insertHeader(); - + int len = mb->calculateSerializeBufferSize(); btChunk* chunk = util->m_memSerializer->allocate(len,1); const char* structType = mb->serialize(chunk->m_oldPtr, util->m_memSerializer); @@ -927,16 +927,16 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto b3Warning("No multibody loaded from URDF. Could add btRigidBody+btTypedConstraint solution later."); return false; } - + } else { btAssert(0); - + return true; } - + } - + return false; } @@ -944,10 +944,10 @@ void PhysicsServerCommandProcessor::replayLogCommand(char* bufferServerToClient, { if (m_data->m_logPlayback) { - + SharedMemoryCommand clientCmd; SharedMemoryStatus serverStatus; - + bool hasCommand = m_data->m_logPlayback->processNextCommand(&clientCmd); if (hasCommand) { @@ -982,7 +982,7 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char* } util->m_memSerializer->registerNameForPointer(mb->getBaseName(),mb->getBaseName()); - + util->m_memSerializer->insertHeader(); int len = mb->calculateSerializeBufferSize(); @@ -1002,12 +1002,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { ///we ignore overflow of integer for now - + { - + //until we implement a proper ring buffer, we assume always maximum of 1 outstanding commands - - + + //const SharedMemoryCommand& clientCmd =m_data->m_testBlock1->m_clientCommands[0]; #if 1 if (m_data->m_commandLogger) @@ -1017,7 +1017,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm #endif //m_data->m_testBlock1->m_numProcessedClientCommands++; - + //no timestamp yet int timeStamp = 0; @@ -1031,11 +1031,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_SEND_BULLET_DATA_STREAM length %d",clientCmd.m_dataStreamArguments.m_streamChunkLength); } - + btBulletWorldImporter* worldImporter = new btBulletWorldImporter(m_data->m_dynamicsWorld); m_data->m_worldImporters.push_back(worldImporter); bool completedOk = worldImporter->loadFileFromMemory(m_data->m_testBlock1->m_bulletStreamDataClientToServer,clientCmd.m_dataStreamArguments.m_streamChunkLength); - + if (completedOk) { SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_COMPLETED,clientCmd.m_sequenceNumber,timeStamp); @@ -1046,7 +1046,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& status = m_data->createServerStatus(CMD_BULLET_DATA_STREAM_RECEIVED_FAILED,clientCmd.m_sequenceNumber,timeStamp); m_data->submitServerStatus(status); } - + break; } #endif @@ -1054,7 +1054,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm case CMD_REQUEST_DEBUG_LINES: { int curFlags =m_data->m_remoteDebugDrawer->getDebugMode(); - + int debugMode = clientCmd.m_requestDebugLinesArguments.m_debugMode;//clientCmd.btIDebugDraw::DBG_DrawWireframe|btIDebugDraw::DBG_DrawAabb; int startingLineIndex = clientCmd.m_requestDebugLinesArguments.m_startingLineIndex; if (startingLineIndex<0) @@ -1062,7 +1062,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("startingLineIndex should be non-negative"); startingLineIndex = 0; } - + if (clientCmd.m_requestDebugLinesArguments.m_startingLineIndex==0) { m_data->m_remoteDebugDrawer->m_lines2.resize(0); @@ -1083,9 +1083,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("m_startingLineIndex exceeds total number of debug lines"); startingLineIndex =m_data->m_remoteDebugDrawer->m_lines2.size(); } - + int numLines = btMin(maxNumLines,m_data->m_remoteDebugDrawer->m_lines2.size()-startingLineIndex); - + if (numLines) { @@ -1108,66 +1108,66 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm linesColor[i*3+2] = m_data->m_remoteDebugDrawer->m_lines2[i+startingLineIndex].m_color.z(); } } - + serverStatusOut.m_type = CMD_DEBUG_LINES_COMPLETED; serverStatusOut.m_sendDebugLinesArgs.m_numDebugLines = numLines; serverStatusOut.m_sendDebugLinesArgs.m_startingLineIndex = startingLineIndex; serverStatusOut.m_sendDebugLinesArgs.m_numRemainingDebugLines = m_data->m_remoteDebugDrawer->m_lines2.size()-(startingLineIndex+numLines); hasStatus = true; - + break; } case CMD_REQUEST_CAMERA_IMAGE_DATA: { - + int startPixelIndex = clientCmd.m_requestPixelDataArguments.m_startPixelIndex; int width, height; int numPixelsCopied = 0; - + if ( - (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && + (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) && (clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_SET_PIXEL_WIDTH_HEIGHT)!=0) { m_data->m_visualConverter.setWidthAndHeight(clientCmd.m_requestPixelDataArguments.m_pixelWidth, clientCmd.m_requestPixelDataArguments.m_pixelHeight); - } - + } + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(0,0,0,0,0,&width,&height,0); - } + } else { m_data->m_visualConverter.getWidthAndHeight(width,height); } - - - + + + int numTotalPixels = width*height; int numRemainingPixels = numTotalPixels - startPixelIndex; - - + + if (numRemainingPixels>0) { int maxNumPixels = bufferSizeInBytes/8-1; unsigned char* pixelRGBA = (unsigned char*)bufferServerToClient; int numRequestedPixels = btMin(maxNumPixels,numRemainingPixels); - + float* depthBuffer = (float*)(bufferServerToClient+numRequestedPixels*4); - + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_USE_HARDWARE_OPENGL)!=0) { m_data->m_guiHelper->copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); } else { - + if (clientCmd.m_requestPixelDataArguments.m_startPixelIndex==0) { // printf("-------------------------------\nRendering\n"); - - + + if ((clientCmd.m_updateFlags & REQUEST_PIXEL_ARGS_HAS_CAMERA_MATRICES)!=0) { m_data->m_visualConverter.render( @@ -1177,19 +1177,19 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_visualConverter.render(); } - + } - + m_data->m_visualConverter.copyCameraImageData(pixelRGBA,numRequestedPixels,depthBuffer,numRequestedPixels,startPixelIndex,&width,&height,&numPixelsCopied); } - + //each pixel takes 4 RGBA values and 1 float = 8 bytes - + } else { - + } - + serverStatusOut.m_type = CMD_CAMERA_IMAGE_COMPLETED; serverStatusOut.m_sendPixelDataArguments.m_numPixelsCopied = numPixelsCopied; serverStatusOut.m_sendPixelDataArguments.m_numRemainingPixels = numRemainingPixels - numPixelsCopied; @@ -1197,7 +1197,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverStatusOut.m_sendPixelDataArguments.m_imageWidth = width; serverStatusOut.m_sendPixelDataArguments.m_imageHeight= height; hasStatus = true; - + break; } @@ -1206,7 +1206,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm const SdfRequestInfoArgs& sdfInfoArgs = clientCmd.m_sdfRequestInfoArgs; //stream info into memory int streamSizeInBytes = createBodyInfoStream(sdfInfoArgs.m_bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - + serverStatusOut.m_type = CMD_BODY_INFO_COMPLETED; serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = sdfInfoArgs.m_bodyUniqueId; serverStatusOut.m_dataStreamArguments.m_streamChunkLength = streamSizeInBytes; @@ -1220,9 +1220,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_LOAD_SDF:%s", sdfArgs.m_sdfFileName); } - + bool completedOk = loadSdf(sdfArgs.m_sdfFileName,bufferServerToClient, bufferSizeInBytes); - + //serverStatusOut.m_type = CMD_SDF_LOADING_FAILED; serverStatusOut.m_sdfLoadedArgs.m_numBodies = m_data->m_sdfRecentLoadedBodies.size(); int maxBodies = btMin(MAX_SDF_BODIES, serverStatusOut.m_sdfLoadedArgs.m_numBodies); @@ -1230,14 +1230,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { serverStatusOut.m_sdfLoadedArgs.m_bodyUniqueIds[i] = m_data->m_sdfRecentLoadedBodies[i]; } - + serverStatusOut.m_type = CMD_SDF_LOADING_COMPLETED; hasStatus = true; break; } case CMD_LOAD_URDF: { - + const UrdfArgs& urdfArgs = clientCmd.m_urdfArguments; if (m_data->m_verboseOutput) { @@ -1267,32 +1267,32 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm bool completedOk = loadUrdf(urdfArgs.m_urdfFileName, initialPos,initialOrn, useMultiBody, useFixedBase,&bodyUniqueId, bufferServerToClient, bufferSizeInBytes); - - + + if (completedOk) { - + m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld); - + serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED; serverStatusOut.m_dataStreamArguments.m_streamChunkLength = 0; - + if (m_data->m_urdfLinkNameMapper.size()) { serverStatusOut.m_dataStreamArguments.m_streamChunkLength = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize(); } serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId; hasStatus = true; - + } else { serverStatusOut.m_type = CMD_URDF_LOADING_FAILED; hasStatus = true; } - - - + + + break; } case CMD_CREATE_SENSOR: @@ -1322,7 +1322,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm mb->getLink(jointIndex).m_jointFeedback = fb; m_data->m_multiBodyJointFeedbacks.push_back(fb); }; - + } else { if (mb->getLink(jointIndex).m_jointFeedback) @@ -1337,7 +1337,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - + } else { b3Warning("No btMultiBody in the world. btRigidBody/btTypedConstraint sensor not hooked up yet"); @@ -1352,15 +1352,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btJointFeedback* fb = new btJointFeedback(); m_data->m_jointFeedbacks.push_back(fb); c->setJointFeedback(fb); - - + + } */ #endif - + serverStatusOut.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; - + break; } case CMD_SEND_DESIRED_STATE: @@ -1369,7 +1369,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Processed CMD_SEND_DESIRED_STATE"); } - + int bodyUniqueId = clientCmd.m_sendDesiredStateCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); @@ -1377,7 +1377,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btMultiBody* mb = body->m_multiBody; btAssert(mb); - + switch (clientCmd.m_sendDesiredStateCommandArgument.m_controlMode) { case CONTROL_MODE_TORQUE: @@ -1386,38 +1386,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Using CONTROL_MODE_TORQUE"); } - mb->clearForcesAndTorques(); - + // mb->clearForcesAndTorques(); int torqueIndex = 0; - #if 0 - //it is inconsistent to allow application of base force/torque, there is no 'joint' motor. Use a separate API for this. - - btVector3 f(0,0,0); - btVector3 t(0,0,0); - - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - { - f = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[0], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[1], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[2]); - t = btVector3 (clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[3], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[4], - clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[5]); - } - torqueIndex+=6; - mb->addBaseForce(f); - mb->addBaseTorque(t); - #endif - for (int link=0;linkgetNumLinks();link++) + if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) { - - for (int dof=0;dofgetLink(link).m_dofCount;dof++) - { - double torque = 0.f; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; - mb->addJointTorqueMultiDof(link,dof,torque); - torqueIndex++; + for (int link=0;linkgetNumLinks();link++) + { + + for (int dof=0;dofgetLink(link).m_dofCount;dof++) + { + double torque = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[torqueIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + torque = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[torqueIndex]; + mb->addJointTorqueMultiDof(link,dof,torque); + } + torqueIndex++; + } } } break; @@ -1428,7 +1413,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { b3Printf("Using CONTROL_MODE_VELOCITY"); } - + int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque { @@ -1437,22 +1422,30 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (supportsJointMotor(mb,link)) { - + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - - + + if (motor) { btScalar desiredVelocity = 0.f; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_QDOT)!=0) - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; - motor->setVelocityTarget(desiredVelocity); + bool hasDesiredVelocity = false; - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; - if ((clientCmd.m_updateFlags&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(maxImp); + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex]; + motor->setVelocityTarget(desiredVelocity); + hasDesiredVelocity = true; + } + if (hasDesiredVelocity) + { + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex]&SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + { + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex]*m_data->m_physicsDeltaTime; + } + motor->setMaxAppliedImpulse(maxImp); + } numMotors++; } @@ -1470,7 +1463,6 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Printf("Using CONTROL_MODE_POSITION_VELOCITY_PD"); } //compute the force base on PD control - mb->clearForcesAndTorques(); int numMotors = 0; //find the joint motors and apply the desired velocity and maximum force/torque @@ -1481,44 +1473,61 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { if (supportsJointMotor(mb,link)) { - + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)mb->getLink(link).m_userPtr; - + if (motor) { - - btScalar desiredVelocity = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_QDOT)!=0) - desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; - btScalar desiredPosition = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_Q)!=0) - desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + bool hasDesiredPosOrVel = false; btScalar kp = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KP)!=0) - kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; - btScalar kd = 0.f; - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_KD)!=0) - kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + btScalar kd = 0.f; + btScalar desiredVelocity = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_QDOT)!=0) + { + hasDesiredPosOrVel = true; + desiredVelocity = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQdot[velIndex]; + kd = 0.1; + } + btScalar desiredPosition = 0.f; + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[posIndex] & SIM_DESIRED_STATE_HAS_Q)!=0) + { + hasDesiredPosOrVel = true; + desiredPosition = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateQ[posIndex]; + kp = 0.1; + } - int dof1 = 0; - btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; - btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; - btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; - btScalar velocityError = (desiredVelocity - currentVelocity); - - desiredVelocity = kp * positionStabiliationTerm + - kd * velocityError; - - motor->setVelocityTarget(desiredVelocity); - - btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + if (hasDesiredPosOrVel) + { - if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) - maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; - - motor->setMaxAppliedImpulse(maxImp); + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KP)!=0) + { + kp = clientCmd.m_sendDesiredStateCommandArgument.m_Kp[velIndex]; + } + + if ((clientCmd.m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[velIndex] & SIM_DESIRED_STATE_HAS_KD)!=0) + { + kd = clientCmd.m_sendDesiredStateCommandArgument.m_Kd[velIndex]; + } + + int dof1 = 0; + btScalar currentPosition = mb->getJointPosMultiDof(link)[dof1]; + btScalar currentVelocity = mb->getJointVelMultiDof(link)[dof1]; + btScalar positionStabiliationTerm = (desiredPosition-currentPosition)/m_data->m_physicsDeltaTime; + btScalar velocityError = (desiredVelocity - currentVelocity); + + desiredVelocity = kp * positionStabiliationTerm + + kd * velocityError; + + motor->setVelocityTarget(desiredVelocity); + btScalar maxImp = 1000000.f*m_data->m_physicsDeltaTime; + + if ((clientCmd.m_updateFlags & SIM_DESIRED_STATE_HAS_MAX_FORCE)!=0) + maxImp = clientCmd.m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[velIndex]*m_data->m_physicsDeltaTime; + + motor->setMaxAppliedImpulse(maxImp); + } numMotors++; } @@ -1535,10 +1544,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm b3Warning("m_controlMode not implemented yet"); break; } - + } } - + serverStatusOut.m_type = CMD_DESIRED_STATE_RECEIVED_COMPLETED; hasStatus = true; break; @@ -1551,7 +1560,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId; InteralBodyData* body = m_data->getHandle(bodyUniqueId); - + if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; @@ -1560,9 +1569,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId; int totalDegreeOfFreedomQ = 0; - int totalDegreeOfFreedomU = 0; - - + int totalDegreeOfFreedomU = 0; + + //always add the base, even for static (non-moving objects) //so that we can easily move the 'fixed' base when needed //do we don't use this conditional "if (!mb->hasFixedBase())" @@ -1570,7 +1579,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btTransform tr; tr.setOrigin(mb->getBasePos()); tr.setRotation(mb->getWorldToBaseRot().inverse()); - + serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[0] = body->m_rootLocalInertialFrame.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_rootLocalInertialFrame[1] = @@ -1588,14 +1597,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm body->m_rootLocalInertialFrame.getRotation()[3]; - + //base position in world space, carthesian serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; @@ -1605,7 +1614,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2]; - + //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1]; @@ -1622,7 +1631,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d]; } - + if (0 == mb->getLink(l).m_jointFeedback) { for (int d=0;d<6;d++) @@ -1633,23 +1642,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear(); btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular(); - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+0] = sensedForce[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+1] = sensedForce[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+2] = sensedForce[2]; - + serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+3] = sensedTorque[0]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+4] = sensedTorque[1]; serverCmd.m_sendActualStateArgs.m_jointReactionForces[l*6+5] = sensedTorque[2]; } - + serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0; - + if (supportsJointMotor(mb,l)) { - + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)body->m_multiBody->getLink(l).m_userPtr; - + if (motor && m_data->m_physicsDeltaTime>btScalar(0)) { btScalar force =motor->getAppliedImpulse(0)/m_data->m_physicsDeltaTime; @@ -1663,10 +1672,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin(); btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation(); - + btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); - + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ(); @@ -1674,24 +1683,24 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z(); serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w(); - + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ(); - + serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z(); serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w(); - + } - + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; - + hasStatus = true; - + } else { if (body && body->m_rigidBody) @@ -1711,7 +1720,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2]; //base orientation, quaternion x,y,z,w, in world space, carthesian - serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; + serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2]; serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3]; @@ -1721,7 +1730,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2]; - + //base angular velocity (in world space, carthesian) serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0]; serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1]; @@ -1745,7 +1754,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } case CMD_STEP_FORWARD_SIMULATION: { - + if (m_data->m_verboseOutput) { b3Printf("Step simulation request"); @@ -1756,13 +1765,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm applyJointDamping(i); } m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; hasStatus = true; break; } - + case CMD_SEND_PHYSICS_SIMULATION_PARAMETERS: { if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_DELTA_TIME) @@ -1781,12 +1790,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; - + }; case CMD_INIT_POSE: { @@ -1803,6 +1812,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { btVector3 zero(0,0,0); + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[0] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); + mb->setBaseVel(zero); mb->setBasePos(btVector3( clientCmd.m_initPoseArgs.m_initialStateQ[0], @@ -1811,6 +1824,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_ORIENTATION) { + btAssert(clientCmd.m_initPoseArgs.m_hasInitialStateQ[3] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[4] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && + clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); + mb->setBaseOmega(btVector3(0,0,0)); mb->setWorldToBaseRot(btQuaternion( clientCmd.m_initPoseArgs.m_initialStateQ[3], @@ -1823,9 +1841,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm int dofIndex = 7; for (int i=0;igetNumLinks();i++) { - if (mb->getLink(i).m_dofCount==1) + if ( (clientCmd.m_initPoseArgs.m_hasInitialStateQ[dofIndex]) && (mb->getLink(i).m_dofCount==1)) { - mb->setJointPos(i,clientCmd.m_initPoseArgs.m_initialStateQ[dofIndex]); mb->setJointVel(i,0); } @@ -1833,14 +1850,14 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } } - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; break; } - + case CMD_RESET_SIMULATION: { @@ -1857,7 +1874,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm createEmptyDynamicsWorld(); m_data->exitHandles(); m_data->initHandles(); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_RESET_SIMULATION_COMPLETED; hasStatus = true; @@ -1887,7 +1904,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_updateFlags & BOX_SHAPE_HAS_INITIAL_ORIENTATION) { - + startTrans.setRotation(btQuaternion( clientCmd.m_createBoxShapeArguments.m_initialOrientation[0], clientCmd.m_createBoxShapeArguments.m_initialOrientation[1], @@ -1912,7 +1929,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm m_data->m_worldImporters.push_back(worldImporter); btCollisionShape* shape = 0; - + switch (shapeType) { case COLLISION_SHAPE_TYPE_CYLINDER_X: @@ -1969,8 +1986,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm shape = worldImporter->createBoxShape(halfExtents); } } - - + + bool isDynamic = (mass>0); btRigidBody* rb = worldImporter->createRigidBody(isDynamic,mass,startTrans,shape,0); rb->setRollingFriction(0.2); @@ -1985,11 +2002,11 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } m_data->m_guiHelper->createCollisionShapeGraphicsObject(rb->getCollisionShape()); m_data->m_guiHelper->createCollisionObjectGraphicsObject(rb,colorRGBA); - - + + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_RIGID_BODY_CREATION_COMPLETED; - + int bodyUniqueId = m_data->allocHandle(); InternalBodyHandle* bodyHandle = m_data->getHandle(bodyUniqueId); @@ -2008,7 +2025,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_pickBodyArguments.m_rayToWorld[1], clientCmd.m_pickBodyArguments.m_rayToWorld[2])); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED; hasStatus = true; @@ -2040,7 +2057,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm default: { b3Error("Unknown command encountered"); - + SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_UNKNOWN_COMMAND_FLUSHED; hasStatus = true; @@ -2048,7 +2065,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } }; - + } } return hasStatus; @@ -2060,10 +2077,10 @@ void PhysicsServerCommandProcessor::renderScene() if (m_data->m_guiHelper) { m_data->m_guiHelper->syncPhysicsToGraphics(m_data->m_dynamicsWorld); - + m_data->m_guiHelper->render(m_data->m_dynamicsWorld); } - + } void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) @@ -2114,7 +2131,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); if (multiCol && multiCol->m_multiBody) { - + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); multiCol->m_multiBody->setCanSleep(false); @@ -2127,10 +2144,10 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) btScalar scaling=1; p2p->setMaxAppliedImpulse(2*scaling); - + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; + m_data->m_pickingMultiBodyPoint2Point =p2p; } } @@ -2155,7 +2172,7 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld if (pickCon) { //keep it at the same picking distance - + btVector3 dir = rayToWorld-rayFromWorld; dir.normalize(); dir *= m_data->m_oldPickingDist; @@ -2164,21 +2181,21 @@ bool PhysicsServerCommandProcessor::movePickedBody(const btVector3& rayFromWorld pickCon->setPivotB(newPivotB); } } - + if (m_data->m_pickingMultiBodyPoint2Point) { //keep it at the same picking distance - + btVector3 dir = rayToWorld-rayFromWorld; dir.normalize(); dir *= m_data->m_oldPickingDist; btVector3 newPivotB = rayFromWorld + dir; - + m_data->m_pickingMultiBodyPoint2Point->setPivotInB(newPivotB); } - + return false; } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 83fd91c34..9745b8eb6 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -115,6 +115,7 @@ enum EnumInitPoseFlags struct InitPoseArgs { int m_bodyUniqueId; + int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; }; @@ -179,11 +180,14 @@ struct SendDesiredStateArgs double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link + int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM]; + //desired state is only written by the client, read-only access by server is expected //m_desiredStateQ is indexed by position variables, //starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM]; + //m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM]; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 0d2303fc5..b978feb47 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3,6 +3,7 @@ #include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h" + #ifdef __APPLE__ #include #else @@ -139,9 +140,11 @@ pybullet_loadURDF(PyObject* self, PyObject* args) int bodyIndex = -1; const char* urdfFileName=""; + float startPosX =0; - float startPosY =0; - float startPosZ = 1; + float startPosY =0; + float startPosZ = 0; + float startOrnX = 0; float startOrnY = 0; float startOrnZ = 0; @@ -282,7 +285,7 @@ pybullet_resetSimulation(PyObject* self, PyObject* args) return Py_None; } -static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) +static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) { //todo(erwincoumans): set max forces, kp, kd @@ -296,25 +299,22 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) size= PySequence_Size(args); - if (size==3) + if (size==4) { - int bodyIndex, controlMode; - PyObject* targetValues; - if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues)) + int bodyIndex, jointIndex, controlMode; + double targetValue; + + if (PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) { - PyObject* seq; - int numJoints, len; - seq = PySequence_Fast(targetValues, "expected a sequence"); - len = PySequence_Size(targetValues); - numJoints = b3GetNumJoints(sm,bodyIndex); + int numJoints; b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryStatusHandle statusHandle; - int qIndex; - - if (len!=numJoints) - { - PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints."); - Py_DECREF(seq); + struct b3JointInfo info; + + numJoints = b3GetNumJoints(sm,bodyIndex); + if ((jointIndex >= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } @@ -323,49 +323,43 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args) (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { PyErr_SetString(SpamError, "Illegral control mode."); - Py_DECREF(seq); return NULL; } commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode); - - for (qIndex=0;qIndex= numJoints) || (jointIndex < 0)) + { + PyErr_SetString(SpamError, "Joint index out-of-range."); return NULL; } + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); - commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue); - for (qIndex=0;qIndex