Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Merge pull request #915 from erwincoumans/master

Browse Source 
placeholder for EGLOpenGLWindow and fix some memory leaks in example code
This commit is contained in:
erwincoumans
2017-01-12 11:07:56 -08:00
committed by GitHub
parent 7c348e934d 9aa5a839d5
commit 6ad616b29c
32 changed files with 798 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
examples/ExampleBrowser/GwenGUISupport/GwenProfileWindow.cpp
View File

@@ -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

2
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -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;

15
examples/ExampleBrowser/OpenGLExampleBrowser.cpp
View File

@@ -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;
}

4
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -18,6 +18,7 @@ UrdfParser::~UrdfParser()
for (int i=0;i<m_tmpModels.size();i++)
{
cleanModel(m_tmpModels[i]);
delete m_tmpModels[i];
}
m_sdfModels.clear();
m_tmpModels.clear();
@@ -540,13 +541,14 @@ bool UrdfParser::parseVisual(UrdfModel& model, UrdfVisual& visual, TiXmlElement*
matPtr->m_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;
}

1
examples/MultiThreading/b3Win32ThreadSupport.cpp
View File

@@ -311,6 +311,7 @@ void b3Win32ThreadSupport::stopThreads()
WaitForSingleObject(threadStatus.m_eventCompletetHandle, INFINITE);
}
delete threadStatus.m_lsMemory;
threadStatus.m_userPtr = 0;
SetEvent(threadStatus.m_eventStartHandle);

207
examples/OpenGLWindow/EGLOpenGLWindow.cpp Normal file
View File

@@ -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 <elmindreda@glfw.org>
//
// 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

76
examples/OpenGLWindow/EGLOpenGLWindow.h Normal file
View File

@@ -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

2
examples/RoboticsLearning/b3RobotSimAPI.cpp
View File

@@ -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;

2
examples/SharedMemory/BodyJointInfoUtility.h
View File

@@ -34,6 +34,8 @@ template <typename T, typename U> 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 =

65
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -3,6 +3,7 @@
#include "Bullet3Common/b3Scalar.h"
#include "Bullet3Common/b3Vector3.h"
#include "Bullet3Common/b3Matrix3x3.h"
#include "Bullet3Common/b3Transform.h"
#include <string.h>
#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;

4
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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

2
examples/SharedMemory/PhysicsClientSharedMemory2.cpp
View File

@@ -9,7 +9,7 @@
PhysicsClientSharedMemory2::PhysicsClientSharedMemory2(SharedMemoryCommandProcessor* proc)
:PhysicsDirect(proc)
:PhysicsDirect(proc,false)
{
m_proc = proc;
}

2
examples/SharedMemory/PhysicsClientSharedMemory2_C_API.cpp
View File

@@ -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);

2
examples/SharedMemory/PhysicsClientUDP_C_API.cpp
View File

@@ -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");

68
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -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; i<m_data->m_bodyJointMap.size(); i++)
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
for (int j = 0; j<bodyJoints->m_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; i<m_data->m_bodyJointMap.size(); i++)
{
BodyJointInfoCache2** bodyJointsPtr = m_data->m_bodyJointMap.getAtIndex(i);
if (bodyJointsPtr && *bodyJointsPtr)
{
BodyJointInfoCache2* bodyJoints = *bodyJointsPtr;
for (int j = 0; j<bodyJoints->m_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;
}

3
examples/SharedMemory/PhysicsDirect.h
View File

@@ -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();

2
examples/SharedMemory/PhysicsDirectC_API.cpp
View File

@@ -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;
}

58
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -779,6 +779,7 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
for (int i=0;i<m_data->m_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;

13
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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;

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -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

2
examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
View File

@@ -82,7 +82,7 @@ public:
newargv[argc+1] = t1;
m_data = btCreateInProcessExampleBrowser(newargc,newargv);
SharedMemoryInterface* shMem = btGetSharedMemoryInterface(m_data);
free(newargv);
setSharedMemoryInterface(shMem);
}

5
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -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

8
examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
View File

@@ -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;o<ptr->m_renderObjects.size();o++)
{
delete ptr->m_renderObjects[o];
}
}
delete ptr;
}
}

24
examples/pybullet/constraint.py Normal file
View File

@@ -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)

23
examples/pybullet/hello_pybullet.py Normal file
View File

@@ -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()

85
examples/pybullet/inverse_kinematics.py Normal file
View File

@@ -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

149
examples/pybullet/pybullet.c
View File

@@ -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."
},

4
src/BulletDynamics/Featherstone/btMultiBodyConstraint.h
View File

@@ -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;

4
src/BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h
View File

@@ -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;
}

3
src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.h
View File

@@ -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);
};

4
src/BulletDynamics/Featherstone/btMultiBodySliderConstraint.h
View File

@@ -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;
}

12
test/SharedMemory/test.c
View File

@@ -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));