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

[pybullet] implement addUserDebugParameter / readUserDebugParameter

Browse Source 
fix inertial frame for door.urdf
allow picking of links of multi-bodies with a fixed base
This commit is contained in:
Erwin Coumans
2017-01-17 15:42:32 -08:00
parent 966ebb04fe
commit 52761f5578
14 changed files with 270 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
data/door.urdf
View File

@@ -72,7 +72,7 @@
</link>
<link name="childA">
<inertial>
<origin rpy="0 0 0" xyz="0 0 -0.36"/>
<origin rpy="0 0 0" xyz="0.4 0 0.4"/>
<mass value="1.0"/>
<inertia ixx="0.048966669" ixy="0" ixz="0" iyy="0.046466667" iyz="0" izz="0.0041666669"/>
</inertial>

3
examples/CommonInterfaces/CommonGUIHelperInterface.h
View File

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

5
examples/ExampleBrowser/OpenGLGuiHelper.h
View File

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

51
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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 (len<MAX_FILENAME_LENGTH)
{
strcpy(command->m_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;
}

4
examples/SharedMemory/PhysicsClientC_API.h
View File

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

1
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

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

29
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

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

62
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -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<UserDebugText> m_userDebugText;
UserDebugText m_tmpText;
@@ -835,6 +843,38 @@ public:
return m_userDebugText[m_userDebugText.size()-1].m_itemUniqueId;
}
btAlignedObjectArray<UserDebugParameter*> m_userDebugParams;
UserDebugParameter m_tmpParam;
virtual int readUserDebugParameter(int itemUniqueId, double* value)
{
for (int i=0;i<m_userDebugParams.size();i++)
{
if (m_userDebugParams[i]->m_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<UserDebugDrawLine> 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,&param->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);

9
examples/SharedMemory/SharedMemoryCommands.h
View File

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

1
examples/SharedMemory/SharedMemoryPublic.h
View File

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

4
examples/pybullet/constraint.py
View File

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

2
examples/pybullet/premake4.lua
View File

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

104
examples/pybullet/pybullet.c
View File

@@ -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,&paramValue);
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"
},

6
test/InverseDynamics/test_invdyn_jacobian.cpp
View File

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