diff --git a/data/door.urdf b/data/door.urdf index 012597d07..999568c2b 100644 --- a/data/door.urdf +++ b/data/door.urdf @@ -72,7 +72,7 @@ - + diff --git a/examples/CommonInterfaces/CommonGUIHelperInterface.h b/examples/CommonInterfaces/CommonGUIHelperInterface.h index eb1cf6cbb..5ecd98273 100644 --- a/examples/CommonInterfaces/CommonGUIHelperInterface.h +++ b/examples/CommonInterfaces/CommonGUIHelperInterface.h @@ -72,6 +72,9 @@ struct GUIHelperInterface virtual int addUserDebugText3D( const char* txt, const double posisionXYZ[3], const double textColorRGB[3], double size, double lifeTime){return -1;}; virtual int addUserDebugLine(const double debugLineFromXYZ[3], const double debugLineToXYZ[3], const double debugLineColorRGB[3], double lineWidth, double lifeTime ){return -1;}; + virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue){return -1;}; + virtual int readUserDebugParameter(int itemUniqueId, double* value) { return 0;} + virtual void removeUserDebugItem( int debugItemUniqueId){}; virtual void removeAllUserDebugItems( ){}; diff --git a/examples/ExampleBrowser/OpenGLGuiHelper.h b/examples/ExampleBrowser/OpenGLGuiHelper.h index d0c9cdb2b..642b9dea1 100644 --- a/examples/ExampleBrowser/OpenGLGuiHelper.h +++ b/examples/ExampleBrowser/OpenGLGuiHelper.h @@ -63,6 +63,11 @@ struct OpenGLGuiHelper : public GUIHelperInterface { return -1; } + virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue) + { + return -1; + } + virtual void removeUserDebugItem( int debugItemUniqueId) { } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 897ea2e24..74c06f765 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -1338,6 +1338,55 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle p return (b3SharedMemoryCommandHandle) command; } +b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_ADD_PARAMETER; + int len = strlen(txt); + if (lenm_userDebugDrawArgs.m_text,txt); + } else + { + command->m_userDebugDrawArgs.m_text[0] = 0; + } + command->m_userDebugDrawArgs.m_rangeMin = rangeMin; + command->m_userDebugDrawArgs.m_rangeMax = rangeMax; + command->m_userDebugDrawArgs.m_startValue = startValue; + return (b3SharedMemoryCommandHandle)command; +} + +b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type =CMD_USER_DEBUG_DRAW; + command->m_updateFlags = USER_DEBUG_READ_PARAMETER; + command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId; + return (b3SharedMemoryCommandHandle) command; +} + +int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); + if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED)) + { + *paramValue = status->m_userDebugDrawArgs.m_parameterValue; + return 1; + } + return 0; +} + + b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; @@ -1347,7 +1396,7 @@ b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle phys b3Assert(command); command->m_type =CMD_USER_DEBUG_DRAW; command->m_updateFlags = USER_DEBUG_REMOVE_ONE_ITEM; - command->m_userDebugDrawArgs.m_removeItemUniqueId = debugItemUniqueId; + command->m_userDebugDrawArgs.m_itemUniqueId = debugItemUniqueId; return (b3SharedMemoryCommandHandle) command; } diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 0c465224e..4a2333749 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -93,6 +93,10 @@ void b3GetDebugLines(b3PhysicsClientHandle physClient, struct b3DebugLines* l /// Add/remove user-specific debug lines and debug text messages b3SharedMemoryCommandHandle b3InitUserDebugDrawAddLine3D(b3PhysicsClientHandle physClient, double fromXYZ[3], double toXYZ[3], double colorRGB[3], double lineWidth, double lifeTime); b3SharedMemoryCommandHandle b3InitUserDebugDrawAddText3D(b3PhysicsClientHandle physClient, const char* txt, double positionXYZ[3], double colorRGB[3], double textSize, double lifeTime); +b3SharedMemoryCommandHandle b3InitUserDebugAddParameter(b3PhysicsClientHandle physClient, const char* txt, double rangeMin, double rangeMax, double startValue); +b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3PhysicsClientHandle physClient, int debugItemUniqueId); +int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue); + b3SharedMemoryCommandHandle b3InitUserDebugDrawRemove(b3PhysicsClientHandle physClient, int debugItemUniqueId); b3SharedMemoryCommandHandle b3InitUserDebugDrawRemoveAll(b3PhysicsClientHandle physClient); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index ead4132fd..5b3f3ac81 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -792,6 +792,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { b3Warning("Save .bullet failed"); break; } + case CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED: case CMD_USER_DEBUG_DRAW_COMPLETED: { break; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 100446e9a..6c0f23b4f 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -4042,7 +4042,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_USER_DEBUG_DRAW_FAILED; hasStatus = true; - + if (clientCmd.m_updateFlags & USER_DEBUG_ADD_PARAMETER) + { + int uid = m_data->m_guiHelper->addUserDebugParameter( + clientCmd.m_userDebugDrawArgs.m_text, + clientCmd.m_userDebugDrawArgs.m_rangeMin, + clientCmd.m_userDebugDrawArgs.m_rangeMax, + clientCmd.m_userDebugDrawArgs.m_startValue + ); + serverCmd.m_userDebugDrawArgs.m_debugItemUniqueId = uid; + serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; + } + if (clientCmd.m_updateFlags &USER_DEBUG_READ_PARAMETER) + { + + int ok = m_data->m_guiHelper->readUserDebugParameter( + clientCmd.m_userDebugDrawArgs.m_itemUniqueId, + &serverCmd.m_userDebugDrawArgs.m_parameterValue); + if (ok) + { + serverCmd.m_type = CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED; + } + } if ((clientCmd.m_updateFlags & USER_DEBUG_SET_CUSTOM_OBJECT_COLOR) || (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR)) { int bodyUniqueId = clientCmd.m_userDebugDrawArgs.m_objectUniqueId; @@ -4131,7 +4152,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } if (clientCmd.m_updateFlags & USER_DEBUG_REMOVE_ONE_ITEM) { - m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_removeItemUniqueId); + m_data->m_guiHelper->removeUserDebugItem(clientCmd.m_userDebugDrawArgs.m_itemUniqueId); serverCmd.m_type = CMD_USER_DEBUG_DRAW_COMPLETED; } @@ -4229,7 +4250,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons } else { btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); - if (multiCol && multiCol->m_multiBody && multiCol->m_multiBody->getBaseMass()>0.) + if (multiCol && multiCol->m_multiBody) { m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); @@ -4483,6 +4504,8 @@ void PhysicsServerCommandProcessor::resetSimulation() m_data->m_visualConverter.resetAll(); } + removePickingConstraint(); + deleteDynamicsWorld(); createEmptyDynamicsWorld(); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index bca3e5c32..3cb55fad0 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -176,6 +176,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperAutogenerateGraphicsObjects, eGUIUserDebugAddText, eGUIUserDebugAddLine, + eGUIUserDebugAddParameter, eGUIUserDebugRemoveItem, eGUIUserDebugRemoveAllItems, }; @@ -481,6 +482,15 @@ struct UserDebugDrawLine int m_itemUniqueId; }; +struct UserDebugParameter +{ + char m_text[1024]; + double m_rangeMin; + double m_rangeMax; + double m_value; + int m_itemUniqueId; +}; + struct UserDebugText { char m_text[1024]; @@ -807,8 +817,6 @@ public: } - - btAlignedObjectArray m_userDebugText; UserDebugText m_tmpText; @@ -835,6 +843,38 @@ public: return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId; } + btAlignedObjectArray m_userDebugParams; + UserDebugParameter m_tmpParam; + + virtual int readUserDebugParameter(int itemUniqueId, double* value) + { + for (int i=0;im_itemUniqueId == itemUniqueId) + { + *value = m_userDebugParams[i]->m_value; + return 1; + } + } + return 0; + } + + virtual int addUserDebugParameter(const char* txt, double rangeMin, double rangeMax, double startValue) + { + strcpy(m_tmpParam.m_text,txt); + m_tmpParam.m_rangeMin = rangeMin; + m_tmpParam.m_rangeMax = rangeMax; + m_tmpParam.m_value = startValue; + m_tmpParam.m_itemUniqueId = m_uidGenerator++; + + m_cs->lock(); + m_cs->setSharedParam(1, eGUIUserDebugAddParameter); + workerThreadWait(); + + return (*m_userDebugParams[m_userDebugParams.size()-1]).m_itemUniqueId; + } + + btAlignedObjectArray m_userDebugLines; UserDebugDrawLine m_tmpLine; @@ -1407,6 +1447,24 @@ void PhysicsServerExample::stepSimulation(float deltaTime) m_multiThreadedHelper->mainThreadRelease(); break; } + case eGUIUserDebugAddParameter: + { + UserDebugParameter* param = new UserDebugParameter(m_multiThreadedHelper->m_tmpParam); + m_multiThreadedHelper->m_userDebugParams.push_back(param); + + { + SliderParams slider(param->m_text,¶m->m_value); + slider.m_minVal=param->m_rangeMin; + slider.m_maxVal=param->m_rangeMax; + + if (m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()) + m_multiThreadedHelper->m_childGuiHelper->getParameterInterface()->registerSliderFloatParameter(slider); + } + + //also add actual menu + m_multiThreadedHelper->mainThreadRelease(); + break; + } case eGUIUserDebugAddLine: { m_multiThreadedHelper->m_userDebugLines.push_back(m_multiThreadedHelper->m_tmpLine); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 91049ebaf..f87e44c0c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -571,6 +571,8 @@ enum EnumUserDebugDrawFlags USER_DEBUG_REMOVE_ALL=8, USER_DEBUG_SET_CUSTOM_OBJECT_COLOR = 16, USER_DEBUG_REMOVE_CUSTOM_OBJECT_COLOR = 32, + USER_DEBUG_ADD_PARAMETER=64, + USER_DEBUG_READ_PARAMETER=128, }; @@ -582,13 +584,17 @@ struct UserDebugDrawArgs double m_lineWidth; double m_lifeTime; - int m_removeItemUniqueId; + int m_itemUniqueId; char m_text[MAX_FILENAME_LENGTH]; double m_textPositionXYZ[3]; double m_textColorRGB[3]; double m_textSize; + double m_rangeMin; + double m_rangeMax; + double m_startValue; + double m_objectDebugColorRGB[3]; int m_objectUniqueId; int m_linkIndex; @@ -599,6 +605,7 @@ struct UserDebugDrawArgs struct UserDebugDrawResultArgs { int m_debugItemUniqueId; + double m_parameterValue; }; struct SendVREvents diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2cf694afe..571aa05e7 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -109,6 +109,7 @@ enum EnumSharedMemoryServerStatus CMD_LOAD_TEXTURE_COMPLETED, CMD_LOAD_TEXTURE_FAILED, CMD_USER_DEBUG_DRAW_COMPLETED, + CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED, CMD_USER_DEBUG_DRAW_FAILED, CMD_USER_CONSTRAINT_COMPLETED, CMD_USER_CONSTRAINT_FAILED, diff --git a/examples/pybullet/constraint.py b/examples/pybullet/constraint.py index 53f1a5505..040708664 100644 --- a/examples/pybullet/constraint.py +++ b/examples/pybullet/constraint.py @@ -9,7 +9,7 @@ cubeId = p.loadURDF("cube_small.urdf",0,0,1) p.setGravity(0,0,-10) p.setRealTimeSimulation(1) cid = p.createConstraint(cubeId,-1,-1,-1,p.JOINT_FIXED,[0,0,0],[0,0,0],[0,0,1]) - +prev=[0,0,1] a=-math.pi while 1: a=a+0.01 @@ -19,6 +19,6 @@ while 1: p.setGravity(0,0,-10) pivot=[a,0,1] orn = p.getQuaternionFromEuler([a,0,0]) - p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn) + p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn, maxForce=50) p.removeConstraint(cid) \ No newline at end of file diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 05196cda1..0b856bd4b 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -20,6 +20,7 @@ project ("pybullet") } if os.is("MacOSX") then +-- targetextension {"so"} links{"Cocoa.framework","Python"} end @@ -38,6 +39,7 @@ if not _OPTIONS["no-enet"] then includedirs {"../../examples/ThirdPartyLibs/enet/include"} if os.is("Windows") then +-- targetextension {"dylib"} defines { "WIN32" } links {"Ws2_32","Winmm"} end diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 57ad9b634..07af59b6d 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2126,6 +2126,103 @@ b3PhysicsClientHandle sm = 0; } +static PyObject* pybullet_readUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) +{ + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int itemUniqueId; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + + static char *kwlist[] = { "itemUniqueId", "physicsClientId", NULL }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist,&itemUniqueId, &physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitUserDebugReadParameter(sm,itemUniqueId); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED) + { + double paramValue=0.f; + int ok = b3GetStatusDebugParameterValue(statusHandle,¶mValue); + if (ok) + { + PyObject* item = PyFloat_FromDouble(paramValue); + return item; + } + } + + PyErr_SetString(SpamError, "Failed to read parameter."); + return NULL; + +} + + + +static PyObject* pybullet_addUserDebugParameter(PyObject* self, PyObject* args, PyObject *keywds) +{ + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int res = 0; + + char* text; + double posXYZ[3]; + double colorRGB[3]={1,1,1}; + + + PyObject* textPositionObj=0; + double rangeMin = 0.f; + double rangeMax = 1.f; + double startValue = 0.f; + int physicsClientId = 0; + b3PhysicsClientHandle sm=0; + static char *kwlist[] = { "paramName", "rangeMin", "rangeMax","startValue", "physicsClientId", NULL }; + + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|dddi", kwlist, &text, &rangeMin, &rangeMax,&startValue, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitUserDebugAddParameter(sm,text,rangeMin,rangeMax, startValue); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + + if (statusType == CMD_USER_DEBUG_DRAW_COMPLETED) + { + int debugItemUniqueId = b3GetDebugItemUniqueId(statusHandle); + PyObject* item = PyInt_FromLong(debugItemUniqueId); + return item; + } + + + PyErr_SetString(SpamError, "Error in addUserDebugParameter."); + return NULL; +} + + static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) { @@ -4543,6 +4640,13 @@ static PyMethodDef SpamMethods[] = { "A lifeTime of 0 means permanent until removed. Returns a unique id for the user debug item." }, + { "addUserDebugParameter", (PyCFunction)pybullet_addUserDebugParameter, METH_VARARGS | METH_KEYWORDS, + "Add a user debug parameter, such as a slider, that can be controlled using a GUI." + }, + { "readUserDebugParameter", (PyCFunction)pybullet_readUserDebugParameter, METH_VARARGS | METH_KEYWORDS, + "Read the current value of a user debug parameter, given the user debug item unique id." + }, + { "removeUserDebugItem", (PyCFunction)pybullet_removeUserDebugItem, METH_VARARGS | METH_KEYWORDS, "remove a user debug draw item, giving its unique id" }, diff --git a/test/InverseDynamics/test_invdyn_jacobian.cpp b/test/InverseDynamics/test_invdyn_jacobian.cpp index 01907b2b2..dccc40e40 100644 --- a/test/InverseDynamics/test_invdyn_jacobian.cpp +++ b/test/InverseDynamics/test_invdyn_jacobian.cpp @@ -76,7 +76,7 @@ void calculateDotJacUError(const MultiBodyTreeCreator& creator, const int nloops EXPECT_EQ(0, tree2->calculatePositionAndVelocityKinematics(q, u)); EXPECT_EQ(0, tree2->calculateJacobians(q, u)); - for (size_t idx = 0; idx < nbodies; idx++) { + for (int idx = 0; idx < nbodies; idx++) { vec3 tmp1, tmp2; vec3 diff; EXPECT_EQ(0, tree1->getBodyLinearAcceleration(idx, &tmp1)); @@ -134,7 +134,7 @@ void calculateJacobianError(const MultiBodyTreeCreator& creator, const int nloop EXPECT_EQ(0, tree2->calculatePositionKinematics(q)); EXPECT_EQ(0, tree2->calculateJacobians(q)); - for (size_t idx = 0; idx < nbodies; idx++) { + for (int idx = 0; idx < nbodies; idx++) { mat3x ref_jac_r(3, ndofs); mat3x ref_jac_t(3, ndofs); ref_jac_r.setZero(); @@ -211,7 +211,7 @@ void calculateVelocityJacobianError(const MultiBodyTreeCreator& creator, const i EXPECT_EQ(0, tree2->calculatePositionKinematics(q)); EXPECT_EQ(0, tree2->calculateJacobians(q)); - for (size_t idx = 0; idx < nbodies; idx++) { + for (int idx = 0; idx < nbodies; idx++) { vec3 vel1; vec3 omg1; vec3 vel2;