diff --git a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp index cce4ebacf..4cc6e63a6 100644 --- a/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp +++ b/examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp @@ -312,7 +312,10 @@ void profileWindowSetVisible(MyProfileWindow* window, bool visible) void destroyProfileWindow(MyProfileWindow* window) { CProfileManager::Release_Iterator(window->profIter); + delete window->m_menuItems; delete window; + CProfileManager::CleanupMemory(); + } #endif //BT_NO_PROFILE diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 5dde14f30..3060eaaee 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -389,6 +389,8 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data) }; printf("btShutDownExampleBrowser stopping threads\n"); + data->m_threadSupport->deleteCriticalSection(data->m_args.m_cs); + delete data->m_threadSupport; delete data->m_sharedMem; delete data; diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 6842e03c2..ee27e72e7 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -379,6 +379,7 @@ void deleteDemo() sCurrentDemo=0; delete s_guiHelper; s_guiHelper = 0; + // CProfileManager::CleanupMemory(); } } @@ -1014,9 +1015,15 @@ OpenGLExampleBrowser::~OpenGLExampleBrowser() delete s_app->m_2dCanvasInterface; s_app->m_2dCanvasInterface = 0; +#ifndef BT_NO_PROFILE + destroyProfileWindow(m_internalData->m_profWindow); +#endif + m_internalData->m_gui->exit(); + + delete m_internalData->m_gui; delete m_internalData->m_gwenRenderer; delete m_internalData->m_myTexLoader; @@ -1029,13 +1036,15 @@ OpenGLExampleBrowser::~OpenGLExampleBrowser() s_app = 0; - -// delete m_internalData->m_profWindow; - + + + delete m_internalData; gFileImporterByExtension.clear(); gAllExamples = 0; + + } diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index bcac5eec7..9f06dbfa0 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -18,6 +18,7 @@ UrdfParser::~UrdfParser() for (int i=0;im_name = "mat"; if (name_char) matPtr->m_name = name_char; + model.m_materials.insert(matPtr->m_name.c_str(),matPtr); TiXmlElement *diffuse = mat->FirstChildElement("diffuse"); if (diffuse) { std::string diffuseText = diffuse->GetText(); btVector4 rgba(1,0,0,1); parseVector4(rgba,diffuseText); matPtr->m_rgbaColor = rgba; - model.m_materials.insert(matPtr->m_name.c_str(),matPtr); + visual.m_materialName = matPtr->m_name; visual.m_hasLocalMaterial = true; } diff --git a/examples/MultiThreading/b3Win32ThreadSupport.cpp b/examples/MultiThreading/b3Win32ThreadSupport.cpp index 199533b1b..e8625be06 100644 --- a/examples/MultiThreading/b3Win32ThreadSupport.cpp +++ b/examples/MultiThreading/b3Win32ThreadSupport.cpp @@ -311,6 +311,7 @@ void b3Win32ThreadSupport::stopThreads() WaitForSingleObject(threadStatus.m_eventCompletetHandle, INFINITE); } + delete threadStatus.m_lsMemory; threadStatus.m_userPtr = 0; SetEvent(threadStatus.m_eventStartHandle); diff --git a/examples/OpenGLWindow/EGLOpenGLWindow.cpp b/examples/OpenGLWindow/EGLOpenGLWindow.cpp new file mode 100644 index 000000000..d5a8c70df --- /dev/null +++ b/examples/OpenGLWindow/EGLOpenGLWindow.cpp @@ -0,0 +1,207 @@ + +//portions of this file are copied from GLFW egl_context.c/egl_context.h + +//======================================================================== +// GLFW 3.3 EGL - www.glfw.org +//------------------------------------------------------------------------ +// Copyright (c) 2002-2006 Marcus Geelnard +// Copyright (c) 2006-2016 Camilla Löwy +// +// This software is provided 'as-is', without any express or implied +// warranty. In no event will the authors be held liable for any damages +// arising from the use of this software. +// +// Permission is granted to anyone to use this software for any purpose, +// including commercial applications, and to alter it and redistribute it +// freely, subject to the following restrictions: +// +// 1. The origin of this software must not be misrepresented; you must not +// claim that you wrote the original software. If you use this software +// in a product, an acknowledgment in the product documentation would +// be appreciated but is not required. +// +// 2. Altered source versions must be plainly marked as such, and must not +// be misrepresented as being the original software. +// +// 3. This notice may not be removed or altered from any source +// distribution. +// +//======================================================================== + +#ifdef BT_USE_EGL + +#include "EGLOpenGLWindow.h" + + +struct EGLInternalData2 +{ + bool m_isInitialized; + + int m_windowWidth; + int m_windowHeight; + + b3KeyboardCallback m_keyboardCallback; + b3WheelCallback m_wheelCallback; + b3ResizeCallback m_resizeCallback; + b3MouseButtonCallback m_mouseButtonCallback; + b3MouseMoveCallback m_mouseMoveCallback; + + EGLInternalData2() + :m_isInitialized(false), + m_windowWidth(0), + m_windowHeight(0), + m_keyboardCallback(0), + m_wheelCallback(0), + m_resizeCallback(0), + m_mouseButtonCallback(0), + m_mouseMoveCallback(0) + { + + } +}; + +EGLOpenGLWindow::EGLOpenGLWindow() +{ + m_data = new EGLInternalData2(); + +} + +EGLOpenGLWindow::~EGLOpenGLWindow() +{ + delete m_data; +} + + +void EGLOpenGLWindow::createWindow(const b3gWindowConstructionInfo& ci) +{ + +} + +void EGLOpenGLWindow::closeWindow() +{ + +} + +void EGLOpenGLWindow::runMainLoop() +{ + +} + +float EGLOpenGLWindow::getTimeInSeconds() +{ + return 0.; +} + +bool EGLOpenGLWindow::requestedExit() const +{ + return false; +} + +void EGLOpenGLWindow::setRequestExit() +{ + +} + +void EGLOpenGLWindow::startRendering() +{ + +} + +void EGLOpenGLWindow::endRendering() +{ + +} + +bool EGLOpenGLWindow::isModifierKeyPressed(int key) +{ + return false; +} + +void EGLOpenGLWindow::setMouseMoveCallback(b3MouseMoveCallback mouseCallback) +{ + m_data->m_mouseMoveCallback = mouseCallback; +} + +b3MouseMoveCallback EGLOpenGLWindow::getMouseMoveCallback() +{ + return m_data->m_mouseMoveCallback; +} + + +void EGLOpenGLWindow::setMouseButtonCallback(b3MouseButtonCallback mouseCallback) +{ + m_data->m_mouseButtonCallback = mouseCallback; +} + +b3MouseButtonCallback EGLOpenGLWindow::getMouseButtonCallback() +{ + return m_data->m_mouseButtonCallback; +} + +void EGLOpenGLWindow::setResizeCallback(b3ResizeCallback resizeCallback) +{ + m_data->m_resizeCallback = resizeCallback; +} + +b3ResizeCallback EGLOpenGLWindow::getResizeCallback() +{ + return m_data->m_resizeCallback; +} + +void EGLOpenGLWindow::setWheelCallback(b3WheelCallback wheelCallback) +{ + m_data->m_wheelCallback = wheelCallback; +} + +b3WheelCallback EGLOpenGLWindow::getWheelCallback() +{ + return m_data->m_wheelCallback; +} + +void EGLOpenGLWindow::setKeyboardCallback( b3KeyboardCallback keyboardCallback) +{ + m_data->m_keyboardCallback = keyboardCallback; +} + +b3KeyboardCallback EGLOpenGLWindow::getKeyboardCallback() +{ + return m_data->m_keyboardCallback; +} + +void EGLOpenGLWindow::setRenderCallback( b3RenderCallback renderCallback) +{ + +} +void EGLOpenGLWindow::setWindowTitle(const char* title) +{ + +} + +float EGLOpenGLWindow::getRetinaScale() const +{ + return 1.f; +} + +void EGLOpenGLWindow::setAllowRetina(bool allow) +{ + +} + + +int EGLOpenGLWindow::getWidth() const +{ + return m_data->m_windowWidth; +} + +int EGLOpenGLWindow::getHeight() const +{ + return m_data->m_windowHeight; +} + +int EGLOpenGLWindow::fileOpenDialog(char* fileName, int maxFileNameLength) +{ + return 0; +} + +#endif //BT_USE_EGL + diff --git a/examples/OpenGLWindow/EGLOpenGLWindow.h b/examples/OpenGLWindow/EGLOpenGLWindow.h new file mode 100644 index 000000000..66ffb3148 --- /dev/null +++ b/examples/OpenGLWindow/EGLOpenGLWindow.h @@ -0,0 +1,76 @@ +#ifndef EGL_OPENGL_WINDOW_H +#define EGL_OPENGL_WINDOW_H + +#ifdef BT_USE_EGL + +#define b3gDefaultOpenGLWindow EGLOpenGLWindow + +#include "../CommonInterfaces/CommonWindowInterface.h" + +class EGLOpenGLWindow : public CommonWindowInterface +{ + struct EGLInternalData2* m_data; + bool m_OpenGLInitialized; + bool m_requestedExit; +public: + + EGLOpenGLWindow(); + virtual ~EGLOpenGLWindow(); + + virtual void createDefaultWindow(int width, int height, const char* title) + { + b3gWindowConstructionInfo ci(width,height); + ci.m_title = title; + createWindow(ci); + } + + virtual void createWindow(const b3gWindowConstructionInfo& ci); + + virtual void closeWindow(); + + virtual void runMainLoop(); + virtual float getTimeInSeconds(); + + virtual bool requestedExit() const; + virtual void setRequestExit(); + + virtual void startRendering(); + + virtual void endRendering(); + + virtual bool isModifierKeyPressed(int key); + + virtual void setMouseMoveCallback(b3MouseMoveCallback mouseCallback); + virtual b3MouseMoveCallback getMouseMoveCallback(); + + virtual void setMouseButtonCallback(b3MouseButtonCallback mouseCallback); + virtual b3MouseButtonCallback getMouseButtonCallback(); + + virtual void setResizeCallback(b3ResizeCallback resizeCallback); + virtual b3ResizeCallback getResizeCallback(); + + virtual void setWheelCallback(b3WheelCallback wheelCallback); + virtual b3WheelCallback getWheelCallback(); + + virtual void setKeyboardCallback( b3KeyboardCallback keyboardCallback); + virtual b3KeyboardCallback getKeyboardCallback(); + + virtual void setRenderCallback( b3RenderCallback renderCallback); + + virtual void setWindowTitle(const char* title); + + virtual float getRetinaScale() const; + virtual void setAllowRetina(bool allow); + + virtual int getWidth() const; + virtual int getHeight() const; + + virtual int fileOpenDialog(char* fileName, int maxFileNameLength); + + +}; + +#endif //BT_USE_EGL + +#endif //EGL_OPENGL_WINDOW_H + diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index f4ad1fcb5..a2ab15632 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -911,7 +911,7 @@ bool b3RobotSimAPI::connect(GUIHelperInterface* guiHelper) else { PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor; - m_data->m_clientServerDirect = new PhysicsDirect(sdk); + m_data->m_clientServerDirect = new PhysicsDirect(sdk,true); bool connected = m_data->m_clientServerDirect->connect(guiHelper); m_data->m_physicsClient = (b3PhysicsClientHandle)m_data->m_clientServerDirect; diff --git a/examples/SharedMemory/BodyJointInfoUtility.h b/examples/SharedMemory/BodyJointInfoUtility.h index 4488d68fc..a959b40dd 100644 --- a/examples/SharedMemory/BodyJointInfoUtility.h +++ b/examples/SharedMemory/BodyJointInfoUtility.h @@ -34,6 +34,8 @@ template void addJointInfoFromMultiBodyData(const T* mb { { b3JointInfo info; + info.m_jointName = 0; + info.m_linkName = 0; info.m_flags = 0; info.m_jointIndex = link; info.m_qIndex = diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 56fe09ee6..251f4ea86 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3,6 +3,7 @@ #include "Bullet3Common/b3Scalar.h" #include "Bullet3Common/b3Vector3.h" #include "Bullet3Common/b3Matrix3x3.h" +#include "Bullet3Common/b3Transform.h" #include #include "SharedMemoryCommands.h" @@ -534,6 +535,10 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle b3Assert(bodyIndex>=0); if (bodyIndex>=0) { + b3Transform wlf,com,inertial; + + + for (int i = 0; i < 3; ++i) { state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i]; @@ -544,6 +549,20 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i]; state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i]; } + com.setOrigin(b3MakeVector3(state->m_worldPosition[0],state->m_worldPosition[1],state->m_worldPosition[2])); + com.setRotation(b3Quaternion(state->m_worldOrientation[0],state->m_worldOrientation[1],state->m_worldOrientation[2],state->m_worldOrientation[3])); + inertial.setOrigin(b3MakeVector3(state->m_localInertialPosition[0],state->m_localInertialPosition[1],state->m_localInertialPosition[2])); + inertial.setRotation(b3Quaternion(state->m_localInertialOrientation[0],state->m_localInertialOrientation[1],state->m_localInertialOrientation[2],state->m_localInertialOrientation[3])); + wlf = com*inertial.inverse(); + for (int i = 0; i < 3; ++i) + { + state->m_worldLinkFramePosition[i] = wlf.getOrigin()[i]; + } + b3Quaternion wlfOrn = wlf.getRotation(); + for (int i = 0; i < 4; ++i) + { + state->m_worldLinkFrameOrientation[i] = wlfOrn[i]; + } } } @@ -1045,6 +1064,52 @@ b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHan return (b3SharedMemoryCommandHandle)command; } +b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) +{ + PhysicsClient* cl = (PhysicsClient* ) physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + command->m_type = CMD_USER_CONSTRAINT; + command->m_updateFlags = USER_CONSTRAINT_CHANGE_CONSTRAINT; + command->m_userConstraintArguments.m_userConstraintUniqueId = userConstraintUniqueId; + return (b3SharedMemoryCommandHandle)command; +} + +int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double pivotInB[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |= USER_CONSTRAINT_CHANGE_PIVOT_IN_B; + + command->m_userConstraintArguments.m_childFrame[0] = pivotInB[0]; + command->m_userConstraintArguments.m_childFrame[1] = pivotInB[1]; + command->m_userConstraintArguments.m_childFrame[2] = pivotInB[2]; + return 0; +} +int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double frameOrnInB[4]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_USER_CONSTRAINT); + b3Assert(command->m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT); + + command->m_updateFlags |= USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B; + + command->m_userConstraintArguments.m_childFrame[3] = frameOrnInB[0]; + command->m_userConstraintArguments.m_childFrame[4] = frameOrnInB[1]; + command->m_userConstraintArguments.m_childFrame[5] = frameOrnInB[2]; + command->m_userConstraintArguments.m_childFrame[6] = frameOrnInB[3]; + + return 0; +} + + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 2f553ea73..54daaa77c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -74,6 +74,10 @@ int b3GetJointInfo(b3PhysicsClientHandle physClient, int bodyIndex, int jointInd b3SharedMemoryCommandHandle b3InitCreateUserConstraintCommand(b3PhysicsClientHandle physClient, int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, struct b3JointInfo* info); int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle statusHandle); +b3SharedMemoryCommandHandle b3InitChangeUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); +int b3InitChangeUserConstraintSetPivotInB(b3SharedMemoryCommandHandle commandHandle, double jointChildPivot[3]); +int b3InitChangeUserConstraintSetFrameInB(b3SharedMemoryCommandHandle commandHandle, double jointChildFrameOrn[4]); + b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); ///Request physics debug lines for debug visualization. The flags in debugMode are the same as used in Bullet diff --git a/examples/SharedMemory/PhysicsClientSharedMemory2.cpp b/examples/SharedMemory/PhysicsClientSharedMemory2.cpp index 7eea2298d..3e4c6cf73 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory2.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory2.cpp @@ -9,7 +9,7 @@ PhysicsClientSharedMemory2::PhysicsClientSharedMemory2(SharedMemoryCommandProcessor* proc) - :PhysicsDirect(proc) + :PhysicsDirect(proc,false) { m_proc = proc; } diff --git a/examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp b/examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp index 7323205d0..0645795e8 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp @@ -8,7 +8,7 @@ b3PhysicsClientHandle b3ConnectSharedMemory2(int key) SharedMemoryCommandProcessor* cmdProc = new SharedMemoryCommandProcessor(); cmdProc->setSharedMemoryKey(key); - PhysicsDirect* cl = new PhysicsDirect(cmdProc); + PhysicsDirect* cl = new PhysicsDirect(cmdProc,true); cl->setSharedMemoryKey(key); diff --git a/examples/SharedMemory/PhysicsClientUDP_C_API.cpp b/examples/SharedMemory/PhysicsClientUDP_C_API.cpp index f48fdedb8..824c77f33 100644 --- a/examples/SharedMemory/PhysicsClientUDP_C_API.cpp +++ b/examples/SharedMemory/PhysicsClientUDP_C_API.cpp @@ -10,7 +10,7 @@ b3PhysicsClientHandle b3ConnectPhysicsUDP(const char* hostName, int port) UdpNetworkedPhysicsProcessor* udp = new UdpNetworkedPhysicsProcessor(hostName, port); - PhysicsDirect* direct = new PhysicsDirect(udp); + PhysicsDirect* direct = new PhysicsDirect(udp,true); bool connected = direct->connect(); printf("direct!\n"); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 2dd96b161..1fc760482 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -62,11 +62,11 @@ struct PhysicsDirectInternalData } }; -PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk) +PhysicsDirect::PhysicsDirect(PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership) { m_data = new PhysicsDirectInternalData; m_data->m_commandProcessor = physSdk; - m_data->m_ownsCommandProcessor = false; + m_data->m_ownsCommandProcessor = passSdkOwnership; } PhysicsDirect::~PhysicsDirect() @@ -79,9 +79,40 @@ PhysicsDirect::~PhysicsDirect() { delete m_data->m_commandProcessor; } + + resetData(); + + + delete m_data; } +void PhysicsDirect::resetData() +{ + m_data->m_debugLinesFrom.clear(); + m_data->m_debugLinesTo.clear(); + m_data->m_debugLinesColor.clear(); + for (int i = 0; im_bodyJointMap.size(); i++) + { + BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); + if (bodyJointsPtr && *bodyJointsPtr) + { + BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; + for (int j = 0; jm_jointInfo.size(); j++) { + if (bodyJoints->m_jointInfo[j].m_jointName) + { + free(bodyJoints->m_jointInfo[j].m_jointName); + } + if (bodyJoints->m_jointInfo[j].m_linkName) + { + free(bodyJoints->m_jointInfo[j].m_linkName); + } + } + delete (*bodyJointsPtr); + } + } + m_data->m_bodyJointMap.clear(); +} // return true if connection succesfull, can also check 'isConnected' bool PhysicsDirect::connect() @@ -525,6 +556,14 @@ bool PhysicsDirect::processCamera(const struct SharedMemoryCommand& orgCommand) void PhysicsDirect::processBodyJointInfo(int bodyUniqueId, const SharedMemoryStatus& serverCmd) { + + BodyJointInfoCache2** cachePtr = m_data->m_bodyJointMap[bodyUniqueId]; + //don't process same bodyUniqueId multiple times + if (cachePtr) + { + return; + } + bParse::btBulletFile bf( &m_data->m_bulletStreamDataServerToClient[0], serverCmd.m_numDataStreamBytes); @@ -627,30 +666,7 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } case CMD_RESET_SIMULATION_COMPLETED: { - m_data->m_debugLinesFrom.clear(); - m_data->m_debugLinesTo.clear(); - m_data->m_debugLinesColor.clear(); - for (int i = 0; im_bodyJointMap.size(); i++) - { - BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i); - if (bodyJointsPtr && *bodyJointsPtr) - { - BodyJointInfoCache2* bodyJoints = *bodyJointsPtr; - for (int j = 0; jm_jointInfo.size(); j++) { - if (bodyJoints->m_jointInfo[j].m_jointName) - { - free(bodyJoints->m_jointInfo[j].m_jointName); - } - if (bodyJoints->m_jointInfo[j].m_linkName) - { - free(bodyJoints->m_jointInfo[j].m_linkName); - } - } - delete (*bodyJointsPtr); - } - } - m_data->m_bodyJointMap.clear(); - + resetData(); break; } diff --git a/examples/SharedMemory/PhysicsDirect.h b/examples/SharedMemory/PhysicsDirect.h index 6b6b526be..bdaced974 100644 --- a/examples/SharedMemory/PhysicsDirect.h +++ b/examples/SharedMemory/PhysicsDirect.h @@ -28,9 +28,10 @@ protected: void postProcessStatus(const struct SharedMemoryStatus& serverCmd); + void resetData(); public: - PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk); + PhysicsDirect(class PhysicsCommandProcessorInterface* physSdk, bool passSdkOwnership); virtual ~PhysicsDirect(); diff --git a/examples/SharedMemory/PhysicsDirectC_API.cpp b/examples/SharedMemory/PhysicsDirectC_API.cpp index 61ceef26f..ded3b81f2 100644 --- a/examples/SharedMemory/PhysicsDirectC_API.cpp +++ b/examples/SharedMemory/PhysicsDirectC_API.cpp @@ -11,7 +11,7 @@ b3PhysicsClientHandle b3ConnectPhysicsDirect() { PhysicsServerCommandProcessor* sdk = new PhysicsServerCommandProcessor; - PhysicsDirect* direct = new PhysicsDirect(sdk); + PhysicsDirect* direct = new PhysicsDirect(sdk,true); bool connected = direct->connect(); return (b3PhysicsClientHandle )direct; } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 7f98b8dfb..92acae284 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -779,6 +779,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld() for (int i=0;im_worldImporters.size();i++) { + m_data->m_worldImporters[i]->deleteAllData(); delete m_data->m_worldImporters[i]; } m_data->m_worldImporters.clear(); @@ -3459,8 +3460,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm InteralBodyData* parentBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_parentBodyIndex); if (parentBody && parentBody->m_multiBody) { - InteralBodyData* childBody = m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex); - if (childBody) + InteralBodyData* childBody = clientCmd.m_userConstraintArguments.m_childBodyIndex>=0 ? m_data->getHandle(clientCmd.m_userConstraintArguments.m_childBodyIndex):0; + //also create a constraint with just a single multibody/rigid body without child + //if (childBody) { btVector3 pivotInParent(clientCmd.m_userConstraintArguments.m_parentFrame[0], clientCmd.m_userConstraintArguments.m_parentFrame[1], clientCmd.m_userConstraintArguments.m_parentFrame[2]); btVector3 pivotInChild(clientCmd.m_userConstraintArguments.m_childFrame[0], clientCmd.m_userConstraintArguments.m_childFrame[1], clientCmd.m_userConstraintArguments.m_childFrame[2]); @@ -3469,7 +3471,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btVector3 jointAxis(clientCmd.m_userConstraintArguments.m_jointAxis[0], clientCmd.m_userConstraintArguments.m_jointAxis[1], clientCmd.m_userConstraintArguments.m_jointAxis[2]); if (clientCmd.m_userConstraintArguments.m_jointType == eFixedType) { - if (childBody->m_multiBody) + if (childBody && childBody->m_multiBody) { btMultiBodyFixedConstraint* multibodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild); multibodyFixed->setMaxAppliedImpulse(500.0); @@ -3484,7 +3486,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild); + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + btMultiBodyFixedConstraint* rigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild); rigidbodyFixed->setMaxAppliedImpulse(500.0); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(rigidbodyFixed); @@ -3499,7 +3502,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else if (clientCmd.m_userConstraintArguments.m_jointType == ePrismaticType) { - if (childBody->m_multiBody) + if (childBody && childBody->m_multiBody) { btMultiBodySliderConstraint* multibodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); multibodySlider->setMaxAppliedImpulse(500.0); @@ -3513,7 +3516,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodySliderConstraint* rigidbodySlider = new btMultiBodySliderConstraint(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild,frameInParent,frameInChild,jointAxis); rigidbodySlider->setMaxAppliedImpulse(500.0); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(rigidbodySlider); @@ -3527,7 +3532,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else if (clientCmd.m_userConstraintArguments.m_jointType == ePoint2PointType) { - if (childBody->m_multiBody) + if (childBody && childBody->m_multiBody) { btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_multiBody,clientCmd.m_userConstraintArguments.m_childJointIndex,pivotInParent,pivotInChild); p2p->setMaxAppliedImpulse(500); @@ -3541,7 +3546,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } else { - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,childBody->m_rigidBody,pivotInParent,pivotInChild); + btRigidBody* rb = childBody? childBody->m_rigidBody : 0; + + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(parentBody->m_multiBody,clientCmd.m_userConstraintArguments.m_parentJointIndex,rb,pivotInParent,pivotInChild); p2p->setMaxAppliedImpulse(500); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; world->addMultiBodyConstraint(p2p); @@ -3563,6 +3570,41 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm } } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_CONSTRAINT) + { + int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(userConstraintUidRemove); + if (userConstraintPtr) + { + if (userConstraintPtr->m_mbConstraint) + { + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_PIVOT_IN_B) + { + btVector3 pivotInB(clientCmd.m_userConstraintArguments.m_childFrame[0], + clientCmd.m_userConstraintArguments.m_childFrame[1], + clientCmd.m_userConstraintArguments.m_childFrame[2]); + + userConstraintPtr->m_mbConstraint->setPivotInB(pivotInB); + } + if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B) + { + btQuaternion childFrameOrn(clientCmd.m_userConstraintArguments.m_childFrame[3], + clientCmd.m_userConstraintArguments.m_childFrame[4], + clientCmd.m_userConstraintArguments.m_childFrame[5], + clientCmd.m_userConstraintArguments.m_childFrame[6]); + + btMatrix3x3 childFrameBasis(childFrameOrn); + userConstraintPtr->m_mbConstraint->setFrameInB(childFrameBasis); + } + } + if (userConstraintPtr->m_rbConstraint) + { + + } + serverCmd.m_userConstraintResultArgs.m_userConstraintUniqueId = -1; + serverCmd.m_type = CMD_USER_CONSTRAINT_COMPLETED; + } + } if (clientCmd.m_updateFlags & USER_CONSTRAINT_REMOVE_CONSTRAINT) { int userConstraintUidRemove = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 6eee8dad3..4fe4eea38 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -573,7 +573,7 @@ public: virtual ~MultiThreadedOpenGLGuiHelper() { - delete m_childGuiHelper; + //delete m_childGuiHelper; } void setCriticalSection(b3CriticalSection* cs) @@ -1152,9 +1152,12 @@ PhysicsServerExample::~PhysicsServerExample() #ifdef BT_ENABLE_VR delete m_tinyVrGui; #endif + + bool deInitializeSharedMemory = true; m_physicsServer.disconnectSharedMemory(deInitializeSharedMemory); m_isConnected = false; + delete m_multiThreadedHelper; } bool PhysicsServerExample::isConnected() @@ -1243,10 +1246,18 @@ void PhysicsServerExample::exitPhysics() { b3Clock::usleep(1000); } + //we need to call 'stepSimulation' to make sure that + //other threads get out of blocking state (workerThreadWait) + stepSimulation(0); }; printf("stopping threads\n"); + m_threadSupport->deleteCriticalSection(m_args[0].m_cs); + m_threadSupport->deleteCriticalSection(m_args[0].m_cs2); + m_threadSupport->deleteCriticalSection(m_args[0].m_cs3); + m_threadSupport->deleteCriticalSection(m_args[0].m_csGUI); + delete m_threadSupport; m_threadSupport = 0; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index cd22178f3..061c9f7ba 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -535,7 +535,9 @@ enum EnumUserConstraintFlags { USER_CONSTRAINT_ADD_CONSTRAINT=1, USER_CONSTRAINT_REMOVE_CONSTRAINT=2, - USER_CONSTRAINT_CHANGE_CONSTRAINT=4 + USER_CONSTRAINT_CHANGE_CONSTRAINT=4, + USER_CONSTRAINT_CHANGE_PIVOT_IN_B=8, + USER_CONSTRAINT_CHANGE_FRAME_ORN_IN_B=16, }; struct UserConstraintArgs diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index deb8349cf..1bd307b4e 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -82,7 +82,7 @@ public: newargv[argc+1] = t1; m_data = btCreateInProcessExampleBrowser(newargc,newargv); SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data); - + free(newargv); setSharedMemoryInterface(shMem); } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index a517659bb..2cf694afe 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -325,11 +325,16 @@ struct b3VisualShapeInformation ///use URDF link frame = link COM frame * inertiaFrame.inverse() struct b3LinkState { + //m_worldPosition and m_worldOrientation of the Center Of Mass (COM) double m_worldPosition[3]; double m_worldOrientation[4]; double m_localInertialPosition[3]; double m_localInertialOrientation[4]; + + ///world position and orientation of the (URDF) link frame + double m_worldLinkFramePosition[3]; + double m_worldLinkFrameOrientation[4]; }; //todo: discuss and decide about control mode and combinations diff --git a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp index 830e629e8..0b3c59703 100644 --- a/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp +++ b/examples/SharedMemory/TinyRendererVisualShapeConverter.cpp @@ -122,6 +122,7 @@ TinyRendererVisualShapeConverter::TinyRendererVisualShapeConverter() } TinyRendererVisualShapeConverter::~TinyRendererVisualShapeConverter() { + resetAll(); delete m_data; } @@ -972,6 +973,13 @@ void TinyRendererVisualShapeConverter::resetAll() if (ptrptr && *ptrptr) { TinyRendererObjectArray* ptr = *ptrptr; + if (ptr) + { + for (int o=0;om_renderObjects.size();o++) + { + delete ptr->m_renderObjects[o]; + } + } delete ptr; } } diff --git a/examples/pybullet/constraint.py b/examples/pybullet/constraint.py new file mode 100644 index 000000000..53f1a5505 --- /dev/null +++ b/examples/pybullet/constraint.py @@ -0,0 +1,24 @@ +import pybullet as p +import time +import math + +p.connect(p.GUI) + +p.loadURDF("plane.urdf") +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]) + +a=-math.pi +while 1: + a=a+0.01 + if (a>math.pi): + a=-math.pi + time.sleep(.01) + p.setGravity(0,0,-10) + pivot=[a,0,1] + orn = p.getQuaternionFromEuler([a,0,0]) + p.changeConstraint(cid,pivot,jointChildFrameOrientation=orn) + +p.removeConstraint(cid) \ No newline at end of file diff --git a/examples/pybullet/hello_pybullet.py b/examples/pybullet/hello_pybullet.py new file mode 100644 index 000000000..28cd0a049 --- /dev/null +++ b/examples/pybullet/hello_pybullet.py @@ -0,0 +1,23 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +p.setGravity(0,0,-10) +planeId = p.loadURDF("plane.urdf") +cubeStartPos = [0,0,1] +cubeStartOrientation = p.getQuaternionFromEuler([0,0,0]) +boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) +cubePos, cubeOrn = p.getBasePositionAndOrientation(boxId) + +useRealTimeSimulation = 0 + +if (useRealTimeSimulation): + p.setRealTimeSimulation(1) + +while 1: + if (useRealTimeSimulation): + p.setGravity(0,0,-10) + sleep(0.01) # Time in seconds. + else: + p.stepSimulation() \ No newline at end of file diff --git a/examples/pybullet/inverse_kinematics.py b/examples/pybullet/inverse_kinematics.py new file mode 100644 index 000000000..f71707097 --- /dev/null +++ b/examples/pybullet/inverse_kinematics.py @@ -0,0 +1,85 @@ +import pybullet as p +import time +import math +from datetime import datetime + +#clid = p.connect(p.SHARED_MEMORY) +p.connect(p.GUI) +p.loadURDF("plane.urdf",[0,0,-0.3]) +kukaId = p.loadURDF("kuka_iiwa/model.urdf",[0,0,0]) +p.resetBasePositionAndOrientation(kukaId,[0,0,0],[0,0,0,1]) +kukaEndEffectorIndex = 6 +numJoints = p.getNumJoints(kukaId) +if (numJoints!=7): + exit() + + +#lower limits for null space +ll=[-.967,-2 ,-2.96,0.19,-2.96,-2.09,-3.05] +#upper limits for null space +ul=[.967,2 ,2.96,2.29,2.96,2.09,3.05] +#joint ranges for null space +jr=[5.8,4,5.8,4,5.8,4,6] +#restposes for null space +rp=[0,0,0,0.5*math.pi,0,-math.pi*0.5*0.66,0] + +for i in range (numJoints): + p.resetJointState(kukaId,i,rp[i]) + +p.setGravity(0,0,0) +t=0. +prevPose=[0,0,0] +prevPose1=[0,0,0] +hasPrevPose = 0 +useNullSpace = 0 + +useOrientation = 1 +useSimulation = 1 +useRealTimeSimulation = 1 +p.setRealTimeSimulation(useRealTimeSimulation) +#trailDuration is duration (in seconds) after debug lines will be removed automatically +#use 0 for no-removal +trailDuration = 15 + + +while 1: + if (useRealTimeSimulation): + dt = datetime.now() + t = (dt.second/60.)*2.*math.pi + else: + t=t+0.1 + + if (useSimulation and useRealTimeSimulation==0): + p.stepSimulation() + + for i in range (1): + pos = [-0.4,0.2*math.cos(t),0.+0.2*math.sin(t)] + #end effector points down, not up (in case useOrientation==1) + orn = p.getQuaternionFromEuler([0,-math.pi,0]) + + if (useNullSpace==1): + if (useOrientation==1): + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn,ll,ul,jr,rp) + else: + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,lowerLimits=ll, upperLimits=ul, jointRanges=jr, restPoses=rp) + else: + if (useOrientation==1): + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos,orn) + else: + jointPoses = p.calculateInverseKinematics(kukaId,kukaEndEffectorIndex,pos) + + if (useSimulation): + for i in range (numJoints): + p.setJointMotorControl2(bodyIndex=kukaId,jointIndex=i,controlMode=p.POSITION_CONTROL,targetPosition=jointPoses[i],targetVelocity=0,force=500,positionGain=0.03,velocityGain=1) + else: + #reset the joint state (ignoring all dynamics, not recommended to use during simulation) + for i in range (numJoints): + p.resetJointState(kukaId,i,jointPoses[i]) + + ls = p.getLinkState(kukaId,kukaEndEffectorIndex) + if (hasPrevPose): + p.addUserDebugLine(prevPose,pos,[0,0,0.3],1,trailDuration) + p.addUserDebugLine(prevPose1,ls[4],[1,0,0],1,trailDuration) + prevPose=pos + prevPose1=ls[4] + hasPrevPose = 1 diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 5f23c6b73..d9f6156af 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2020,6 +2020,8 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args,PyObject* PyObject* pyLinkStateWorldOrientation; PyObject* pyLinkStateLocalInertialPosition; PyObject* pyLinkStateLocalInertialOrientation; + PyObject* pyLinkStateWorldLinkFramePosition; + PyObject* pyLinkStateWorldLinkFrameOrientation; struct b3LinkState linkState; @@ -2094,11 +2096,26 @@ b3PhysicsClientHandle sm = 0; PyFloat_FromDouble(linkState.m_localInertialOrientation[i])); } - pyLinkState = PyTuple_New(4); + pyLinkStateWorldLinkFramePosition = PyTuple_New(3); + for (i = 0; i < 3; ++i) { + PyTuple_SetItem(pyLinkStateWorldLinkFramePosition , i, + PyFloat_FromDouble(linkState.m_worldLinkFramePosition[i])); + } + + pyLinkStateWorldLinkFrameOrientation = PyTuple_New(4); + for (i = 0; i < 4; ++i) { + PyTuple_SetItem(pyLinkStateWorldLinkFrameOrientation, i, + PyFloat_FromDouble(linkState.m_worldLinkFrameOrientation[i])); + } + + + pyLinkState = PyTuple_New(6); PyTuple_SetItem(pyLinkState, 0, pyLinkStateWorldPosition); PyTuple_SetItem(pyLinkState, 1, pyLinkStateWorldOrientation); PyTuple_SetItem(pyLinkState, 2, pyLinkStateLocalInertialPosition); PyTuple_SetItem(pyLinkState, 3, pyLinkStateLocalInertialOrientation); + PyTuple_SetItem(pyLinkState, 4, pyLinkStateWorldLinkFramePosition ); + PyTuple_SetItem(pyLinkState, 5, pyLinkStateWorldLinkFrameOrientation); return pyLinkState; } @@ -3011,6 +3028,51 @@ static PyObject* pybullet_getClosestPointData(PyObject* self, PyObject* args, Py return Py_None; } + +static PyObject* pybullet_changeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "userConstraintUniqueId","jointChildPivot", "jointChildFrameOrientation", "physicsClientId", NULL}; + int userConstraintUniqueId=-1; + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* jointChildPivotObj=0; + PyObject* jointChildFrameOrnObj=0; + double jointChildPivot[3]; + double jointChildFrameOrn[4]; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OOi", kwlist,&userConstraintUniqueId,&jointChildPivotObj, &jointChildFrameOrnObj,&physicsClientId)) + { + return NULL; + } + + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + commandHandle = b3InitChangeUserConstraintCommand(sm,userConstraintUniqueId); + + if (pybullet_internalSetVectord(jointChildPivotObj,jointChildPivot)) + { + b3InitChangeUserConstraintSetPivotInB(commandHandle, jointChildPivot); + } + if (pybullet_internalSetVector4d(jointChildFrameOrnObj,jointChildFrameOrn)) + { + b3InitChangeUserConstraintSetFrameInB(commandHandle, jointChildFrameOrn); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + statusType = b3GetStatusType(statusHandle); + Py_INCREF(Py_None); + return Py_None; +}; + + static PyObject* pybullet_removeUserConstraint(PyObject* self, PyObject* args, PyObject *keywds) { static char *kwlist[] = { "userConstraintUniqueId","physicsClientId", NULL}; @@ -4048,13 +4110,18 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int bodyIndex; int endEffectorLinkIndex; - PyObject* targetPosObj; - PyObject* targetOrnObj; + PyObject* targetPosObj=0; + PyObject* targetOrnObj=0; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOO|i", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&physicsClientId)) + PyObject* lowerLimitsObj = 0; + PyObject* upperLimitsObj = 0; + PyObject* jointRangesObj = 0; + PyObject* restPosesObj = 0; + + static char *kwlist[] = { "bodyIndex", "endEffectorLinkIndex", "targetPosition", "targetOrientation","lowerLimits", "upperLimits", "jointRanges", "restPoses", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiO|OOOOOi", kwlist, &bodyIndex, &endEffectorLinkIndex, &targetPosObj,&targetOrnObj,&lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &physicsClientId)) { return NULL; } @@ -4067,8 +4134,49 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, { double pos[3]; double ori[4]={0,1.0,0,0}; - - if (pybullet_internalSetVectord(targetPosObj,pos) && pybullet_internalSetVector4d(targetOrnObj,ori)) + int hasPos =pybullet_internalSetVectord(targetPosObj,pos); + int hasOrn = pybullet_internalSetVector4d(targetOrnObj,ori); + + int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0; + int szUpperLimits = upperLimitsObj? PySequence_Size(upperLimitsObj): 0; + int szJointRanges = jointRangesObj? PySequence_Size(jointRangesObj):0; + int szRestPoses = restPosesObj? PySequence_Size(restPosesObj):0; + + int numJoints = b3GetNumJoints(sm, bodyIndex); + + + int hasNullSpace = 0; + double* lowerLimits = 0; + double* upperLimits = 0; + double* jointRanges = 0; + double* restPoses = 0; + + if (numJoints && (szLowerLimits == numJoints) && (szUpperLimits == numJoints) && + (szJointRanges == numJoints) && (szRestPoses == numJoints)) + { + int szInBytes = sizeof(double) * numJoints; + int i; + PyObject* pylist = 0; + lowerLimits = (double*)malloc(szInBytes); + upperLimits = (double*)malloc(szInBytes); + jointRanges = (double*)malloc(szInBytes); + restPoses = (double*)malloc(szInBytes); + + for (i = 0; i < numJoints; i++) + { + lowerLimits[i] = + pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = + pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = + pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = + pybullet_internalGetFloatFromSequence(restPosesObj, i); + } + hasNullSpace = 1; + } + + if (hasPos) { b3SharedMemoryStatusHandle statusHandle; int numPos=0; @@ -4076,7 +4184,27 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, int result; b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm,bodyIndex); - b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,6,pos,ori); + + if (hasNullSpace) + { + if (hasOrn) + { + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, numJoints, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); + } else + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command,numJoints, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + } + } else + { + if (hasOrn) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,endEffectorLinkIndex,pos,ori); + } else + { + b3CalculateInverseKinematicsAddTargetPurePosition(command,endEffectorLinkIndex,pos); + } + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); result = b3GetStatusInverseKinematicsJointPositions(statusHandle, @@ -4289,6 +4417,11 @@ static PyMethodDef SpamMethods[] = { "Create a constraint between two bodies. Returns a (int) unique id, if successfull." }, + {"changeConstraint", (PyCFunction)pybullet_changeUserConstraint, METH_VARARGS | METH_KEYWORDS, + "Change some parameters of an existing constraint, such as the child pivot or child frame orientation, using its unique id." + }, + + {"removeConstraint", (PyCFunction)pybullet_removeUserConstraint, METH_VARARGS | METH_KEYWORDS, "Remove a constraint using its unique id." }, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h index 74c6f5a81..c219b4376 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.h @@ -90,6 +90,10 @@ public: void updateJacobianSizes(); void allocateJacobiansMultiDof(); + //many constraints have setFrameInB/setPivotInB. Will use 'getConstraintType' later. + virtual void setFrameInB(const btMatrix3x3& frameInB) {} + virtual void setPivotInB(const btVector3& pivotInB){} + virtual void finalizeMultiDof()=0; virtual int getIslandIdA() const =0; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h index 26e28a74e..036025136 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h @@ -62,7 +62,7 @@ public: return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } @@ -82,7 +82,7 @@ public: return m_frameInB; } - void setFrameInB(const btMatrix3x3& frameInB) + virtual void setFrameInB(const btMatrix3x3& frameInB) { m_frameInB = frameInB; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h index b2e219ac1..ea3d85abf 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h @@ -53,11 +53,12 @@ public: return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } + virtual void debugDraw(class btIDebugDraw* drawer); }; diff --git a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h index 571dcd53b..0a6cf3df1 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h @@ -63,7 +63,7 @@ public: return m_pivotInB; } - void setPivotInB(const btVector3& pivotInB) + virtual void setPivotInB(const btVector3& pivotInB) { m_pivotInB = pivotInB; } @@ -83,7 +83,7 @@ public: return m_frameInB; } - void setFrameInB(const btMatrix3x3& frameInB) + virtual void setFrameInB(const btMatrix3x3& frameInB) { m_frameInB = frameInB; } diff --git a/test/SharedMemory/test.c b/test/SharedMemory/test.c index 6037b693e..f7eeb18ad 100644 --- a/test/SharedMemory/test.c +++ b/test/SharedMemory/test.c @@ -1,3 +1,4 @@ + //#include "SharedMemoryCommands.h" #ifdef PHYSICS_SHARED_MEMORY #include "SharedMemory/PhysicsClientC_API.h" @@ -244,6 +245,17 @@ void testSharedMemory(b3PhysicsClientHandle sm) } } + { + b3SharedMemoryCommandHandle command; + b3SharedMemoryStatusHandle statusHandle; + int width = 1024; + int height = 1024; + + command = b3InitRequestCameraImage(sm); + + b3RequestCameraImageSetPixelResolution(command, width, height); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + } if (b3CanSubmitCommand(sm)) { b3SharedMemoryStatusHandle state = b3SubmitClientCommandAndWaitStatus(sm, b3RequestActualStateCommandInit(sm,bodyIndex));