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

merge bullet master

Browse Source
This commit is contained in:
Jasmine Hsu
2016-06-24 15:20:46 -07:00
parent d2a2797201 84f6d60ab3
commit aa91042ac8
13 changed files with 629 additions and 434 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
build_and_run_cmake.sh
View File

@@ -2,6 +2,6 @@
rm CMakeCache.txt
mkdir build_cmake
cd build_cmake
cmake ..
cmake -DBUILD_PYTHON=OFF -CMAKE_BUILD_TYPE=Release ..
make -j12
examples/ExampleBrowser/App_ExampleBrowser

2
examples/BasicDemo/BasicExample.cpp
View File

@@ -42,7 +42,7 @@ struct BasicExample : public CommonRigidBodyBase
float dist = 41;
float pitch = 52;
float yaw = 35;
float targetPos[3]={0,0.46,0};
float targetPos[3]={0,0,0};
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
}
};

11
examples/Importers/ImportSTLDemo/LoadMeshFromSTL.h
View File

@@ -66,17 +66,18 @@ static GLInstanceGraphicsShape* LoadMeshFromSTL(const char* relativeFileName)
for (int i=0;i<numTriangles;i++)
{
char* curPtr = &memoryBuffer[84+i*50];
MySTLTriangle* tri = (MySTLTriangle*) curPtr;
MySTLTriangle tmp;
memcpy(&tmp,curPtr,sizeof(MySTLTriangle));
GLInstanceVertex v0,v1,v2;
v0.uv[0] = v1.uv[0] = v2.uv[0] = 0.5;
v0.uv[1] = v1.uv[1] = v2.uv[1] = 0.5;
for (int v=0;v<3;v++)
{
v0.xyzw[v] = tri->vertex0[v];
v1.xyzw[v] = tri->vertex1[v];
v2.xyzw[v] = tri->vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tri->normal[v];
v0.xyzw[v] = tmp.vertex0[v];
v1.xyzw[v] = tmp.vertex1[v];
v2.xyzw[v] = tmp.vertex2[v];
v0.normal[v] = v1.normal[v] = v2.normal[v] = tmp.normal[v];
}
v0.xyzw[3] = v1.xyzw[3] = v2.xyzw[3] = 0.f;

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

@@ -393,13 +393,22 @@ bool UrdfParser::parseGeometry(UrdfGeometry& geom, TiXmlElement* g, ErrorLogger*
}
geom.m_meshFileName = shape->Attribute("filename");
if (shape->Attribute("scale"))
geom.m_meshScale.setValue(1,1,1);
if (shape->Attribute("scale"))
{
parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger);
if (!parseVector3(geom.m_meshScale,shape->Attribute("scale"),logger))
{
logger->reportWarning("scale should be a vector3, not single scalar. Workaround activated.\n");
std::string scalar_str = shape->Attribute("scale");
double scaleFactor = urdfLexicalCast<double>(scalar_str.c_str());
if (scaleFactor)
{
geom.m_meshScale.setValue(scaleFactor,scaleFactor,scaleFactor);
}
}
} else
{
geom.m_meshScale.setValue(1,1,1);
}
}
}

41
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -151,7 +151,12 @@ b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHand
}
b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient,0,controlMode);
}
b3SharedMemoryCommandHandle b3JointControlCommandInit2( b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
@@ -162,6 +167,10 @@ b3SharedMemoryCommandHandle b3JointControlCommandInit( b3PhysicsClientHandle phy
command->m_sendDesiredStateCommandArgument.m_controlMode = controlMode;
command->m_sendDesiredStateCommandArgument.m_bodyUniqueId = bodyUniqueId;
command->m_updateFlags = 0;
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -171,6 +180,7 @@ int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle,
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQ[qIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_Q;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[qIndex] |= SIM_DESIRED_STATE_HAS_Q;
return 0;
}
@@ -181,6 +191,8 @@ int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kp[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KP;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KP;
return 0;
}
@@ -190,6 +202,7 @@ int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex,
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_Kd[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_KD;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_KD;
return 0;
}
@@ -200,6 +213,8 @@ int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle,
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateQdot[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_QDOT;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_QDOT;
return 0;
}
@@ -210,6 +225,8 @@ int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -218,6 +235,9 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
command->m_sendDesiredStateCommandArgument.m_hasDesiredStateFlags[dofIndex] |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0;
}
@@ -366,6 +386,7 @@ int b3CreateBoxCommandSetStartOrientation(b3SharedMemoryCommandHandle commandHan
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
@@ -374,6 +395,11 @@ b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physCl
command->m_type = CMD_INIT_POSE;
command->m_updateFlags =0;
command->m_initPoseArgs.m_bodyUniqueId = bodyIndex;
//a bit slow, initialing the full range to zero...
for (int i=0;i<MAX_DEGREE_OF_FREEDOM;i++)
{
command->m_initPoseArgs.m_hasInitialStateQ[i] = 0;
}
return (b3SharedMemoryCommandHandle) command;
}
@@ -386,6 +412,11 @@ int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle
command->m_initPoseArgs.m_initialStateQ[0] = startPosX;
command->m_initPoseArgs.m_initialStateQ[1] = startPosY;
command->m_initPoseArgs.m_initialStateQ[2] = startPosZ;
command->m_initPoseArgs.m_hasInitialStateQ[0] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[1] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[2] = 1;
return 0;
}
@@ -399,6 +430,12 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan
command->m_initPoseArgs.m_initialStateQ[4] = startOrnY;
command->m_initPoseArgs.m_initialStateQ[5] = startOrnZ;
command->m_initPoseArgs.m_initialStateQ[6] = startOrnW;
command->m_initPoseArgs.m_hasInitialStateQ[3] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[4] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[5] = 1;
command->m_initPoseArgs.m_hasInitialStateQ[6] = 1;
return 0;
}
@@ -411,6 +448,7 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand
for (int i=0;i<numJointPositions;i++)
{
command->m_initPoseArgs.m_initialStateQ[i+7] = jointPositions[i];
command->m_initPoseArgs.m_hasInitialStateQ[i+7] = 1;
}
return 0;
}
@@ -427,6 +465,7 @@ int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3Shar
if ((info.m_flags & JOINT_HAS_MOTORIZED_POWER) && info.m_qIndex >=0)
{
command->m_initPoseArgs.m_initialStateQ[info.m_qIndex] = jointPosition;
command->m_initPoseArgs.m_hasInitialStateQ[info.m_qIndex] = 1;
}
return 0;
}

12
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -95,14 +95,20 @@ int b3LoadUrdfCommandSetUseFixedBase(b3SharedMemoryCommandHandle commandHandle,
b3SharedMemoryCommandHandle b3LoadSdfCommandInit(b3PhysicsClientHandle physClient, const char* sdfFileName);
///Set joint control variables such as desired position/angle, desired velocity,
///The b3JointControlCommandInit method is obsolete, use b3JointControlCommandInit2 instead
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode);
///Set joint motor control variables such as desired position/angle, desired velocity,
///applied joint forces, dependent on the control mode (CONTROL_MODE_VELOCITY or CONTROL_MODE_TORQUE)
b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
b3SharedMemoryCommandHandle b3JointControlCommandInit2(b3PhysicsClientHandle physClient, int bodyUniqueId, int controlMode);
///Only use when controlMode is CONTROL_MODE_POSITION_VELOCITY_PD
int b3JointControlSetDesiredPosition(b3SharedMemoryCommandHandle commandHandle, int qIndex, double value);
int b3JointControlSetKp(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
int b3JointControlSetKd(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
//Only use when controlMode is CONTROL_MODE_VELOCITY
///Only use when controlMode is CONTROL_MODE_VELOCITY
int b3JointControlSetDesiredVelocity(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value); /* find a better name for dof/q/u indices, point to b3JointInfo */
int b3JointControlSetMaximumForce(b3SharedMemoryCommandHandle commandHandle, int dofIndex, double value);
///Only use if when controlMode is CONTROL_MODE_TORQUE,

2
examples/SharedMemory/PhysicsClientExample.cpp
View File

@@ -377,7 +377,7 @@ void PhysicsClientExample::prepareAndSubmitCommand(int commandId)
if (m_selectedBody>=0)
{
// b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_VELOCITY);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_physicsClientHandle, m_selectedBody, CONTROL_MODE_POSITION_VELOCITY_PD);
// b3Printf("prepare control command for body %d", m_selectedBody);
prepareControlCommand(command);

577
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

File diff suppressed because it is too large Load Diff

4
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -115,6 +115,7 @@ enum EnumInitPoseFlags
struct InitPoseArgs
{
int m_bodyUniqueId;
int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM];
double m_initialStateQ[MAX_DEGREE_OF_FREEDOM];
};
@@ -179,11 +180,14 @@ struct SendDesiredStateArgs
double m_Kp[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
double m_Kd[MAX_DEGREE_OF_FREEDOM];//indexed by degree of freedom, 6 for base, and then the dofs for each link
int m_hasDesiredStateFlags[MAX_DEGREE_OF_FREEDOM];
//desired state is only written by the client, read-only access by server is expected
//m_desiredStateQ is indexed by position variables,
//starting with 3 base position variables, 4 base orientation variables (quaternion), then link position variables
double m_desiredStateQ[MAX_DEGREE_OF_FREEDOM];
//m_desiredStateQdot is index by velocity degrees of freedom, 3 linear and 3 angular variables for the base and then link velocity variables
double m_desiredStateQdot[MAX_DEGREE_OF_FREEDOM];

329
examples/pybullet/pybullet.c
View File

@@ -3,12 +3,19 @@
#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
#ifdef __APPLE__
#include <Python/Python.h>
#else
#include <Python.h>
#endif
#if PY_MAJOR_VERSION >= 3
#define PyInt_FromLong PyLong_FromLong
#define PyString_FromString PyBytes_FromString
#endif
enum eCONNECT_METHOD
{
eCONNECT_GUI=1,
@@ -133,6 +140,7 @@ pybullet_loadURDF(PyObject* self, PyObject* args)
int bodyIndex = -1;
const char* urdfFileName="";
float startPosX =0;
float startPosY =0;
float startPosZ = 0;
@@ -218,7 +226,7 @@ pybullet_loadSDF(PyObject* self, PyObject* args)
PyObject* pylist=0;
b3SharedMemoryStatusHandle statusHandle;
int statusType;
b3SharedMemoryCommandHandle command;
b3SharedMemoryCommandHandle commandHandle;
if (0==sm)
{
@@ -232,8 +240,8 @@ pybullet_loadSDF(PyObject* self, PyObject* args)
return NULL;
}
command = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle);
if (statusType!=CMD_SDF_LOADING_COMPLETED)
{
@@ -278,12 +286,18 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
return Py_None;
}
static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
//todo(erwincoumans): set max forces, kp, kd
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double maxForce=100000;
double gains=0.1;
int valid = 0;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -292,72 +306,98 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
size= PySequence_Size(args);
if (size==3)
if (size==4)
{
int bodyIndex, controlMode;
PyObject* targetValues;
if (PyArg_ParseTuple(args, "iiO", &bodyIndex, &controlMode, &targetValues))
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyObject* seq;
int numJoints, len;
seq = PySequence_Fast(targetValues, "expected a sequence");
len = PySequence_Size(targetValues);
numJoints = b3GetNumJoints(sm,bodyIndex);
b3SharedMemoryCommandHandle commandHandle;
int qIndex;
if (len!=numJoints)
{
PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints.");
Py_DECREF(seq);
return NULL;
}
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
{
PyErr_SetString(SpamError, "Illegral control mode.");
Py_DECREF(seq);
return NULL;
}
commandHandle = b3JointControlCommandInit(sm, bodyIndex,controlMode);
for (qIndex=0;qIndex<numJoints;qIndex++)
{
float value = pybullet_internalGetFloatFromSequence(seq,qIndex);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, qIndex, value);
break;
}
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==5)
{
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (size==6)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
}
if (valid)
{
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
struct b3JointInfo info;
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, qIndex, value);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition( commandHandle, qIndex, value);
break;
}
default:
{
}
};
}
Py_DECREF(seq);
Py_INCREF(Py_None);
return Py_None;
numJoints = b3GetNumJoints(sm,bodyIndex);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
if ((controlMode != CONTROL_MODE_VELOCITY) &&
(controlMode != CONTROL_MODE_TORQUE) &&
(controlMode != CONTROL_MODE_POSITION_VELOCITY_PD))
{
PyErr_SetString(SpamError, "Illegral control mode.");
return NULL;
}
commandHandle = b3JointControlCommandInit2(sm, bodyIndex,controlMode);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
switch (controlMode)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
double kd = gains;
b3JointControlSetKd(commandHandle,info.m_uIndex,kd);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
double kp = gains;
b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
break;
}
default:
{
}
};
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "error in setJointControl.");
return NULL;
@@ -548,37 +588,53 @@ pybullet_getNumJoints(PyObject* self, PyObject* args)
// Initalize all joint positions given a list of values
static PyObject*
pybullet_initializeJointPositions(PyObject* self, PyObject* args)
pybullet_resetJointState(PyObject* self, PyObject* args)
{
int size;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
// TODO(hellojas): initialize all joint positions given a pylist of values
//
//
/*
///b3CreatePoseCommandInit will initialize (teleport) the pose of a body/robot. You can individually set the base position,
///base orientation and joint angles. This will set all velocities of base and joints to zero.
///This is not a robot control command using actuators/joint motors, but manual repositioning the robot.
b3SharedMemoryCommandHandle b3CreatePoseCommandInit(b3PhysicsClientHandle physClient, int bodyIndex);
int b3CreatePoseCommandSetBasePosition(b3SharedMemoryCommandHandle commandHandle, double startPosX,double startPosY,double startPosZ);
int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHandle, double startOrnX,double startOrnY,double startOrnZ, double startOrnW);
int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions);
int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition);
*/
//
//
Py_INCREF(Py_None);
return Py_None;
size= PySequence_Size(args);
if (size==3)
{
int bodyIndex;
int jointIndex;
double targetValue;
if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &targetValue))
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int numJoints;
numJoints = b3GetNumJoints(sm,bodyIndex);
if ((jointIndex >= numJoints) || (jointIndex < 0))
{
PyErr_SetString(SpamError, "Joint index out-of-range.");
return NULL;
}
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, targetValue);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "error in resetJointState.");
return NULL;
}
// CURRENTLY NOT SUPPORTED
// Initalize a single joint position for a specific body index
//
@@ -622,7 +678,7 @@ pybullet_initializeJointPositions(PyObject* self, PyObject* args)
// Py_INCREF(Py_None);
// return Py_None;
// }
#if 0
static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) {
int i, j;
int numDegreeQ;
@@ -710,7 +766,7 @@ pybullet_getJointPositions(PyObject* self, PyObject* args)
Py_INCREF(Py_None);
return Py_None;
}
#endif
// Get the a single joint info for a specific bodyIndex
//
@@ -729,13 +785,7 @@ pybullet_getJointPositions(PyObject* self, PyObject* args)
static PyObject*
pybullet_getJointInfo(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
PyObject *pyListJointInfo;
PyObject *pyListJointInfo;
struct b3JointInfo info;
@@ -745,6 +795,14 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
int size= PySequence_Size(args);
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size==2) // get body index and joint index
{
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
@@ -771,9 +829,8 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
// info.m_flags,
// info.m_jointDamping,
// info.m_jointFriction);
PyTuple_SetItem(pyListJointInfo, 0,
PyInt_FromLong(info.m_jointIndex));
PyInt_FromLong(info.m_jointIndex));
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyTuple_SetItem(pyListJointInfo, 2,
@@ -788,7 +845,6 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
PyFloat_FromDouble(info.m_jointDamping));
PyTuple_SetItem(pyListJointInfo, 7,
PyFloat_FromDouble(info.m_jointFriction));
return pyListJointInfo;
}
}
@@ -814,14 +870,7 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
static PyObject*
pybullet_getJointState(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
PyObject *pyListJointForceTorque;
PyObject *pyListJointForceTorque;
PyObject *pyListJointState;
PyObject *item;
@@ -837,6 +886,15 @@ pybullet_getJointState(PyObject* self, PyObject* args)
int size= PySequence_Size(args);
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size==2) // get body index and joint index
{
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
@@ -1036,11 +1094,6 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
static PyMethodDef SpamMethods[] = {
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
"Load multibodies from an SDF file."},
{"connect", pybullet_connectPhysicsServer, METH_VARARGS,
"Connect to an existing physics server (using shared memory by default)."},
@@ -1057,34 +1110,44 @@ static PyMethodDef SpamMethods[] = {
{"setGravity", pybullet_setGravity, METH_VARARGS,
"Set the gravity acceleration (x,y,z)."},
{"initializeJointPositions", pybullet_initializeJointPositions, METH_VARARGS,
"Initialize the joint positions for all joints. This method skips any physics simulation and teleports all joints to the new positions."},
// CURRENTLY NOT SUPPORTED
// {"initializeJointPosition", pybullet_initializeJointPosition, METH_VARARGS,
// "Initialize the joint position for one joint. This method skips any physics simulation and teleports the joint to the new position."},
{"getJointInfo", pybullet_getJointInfo, METH_VARARGS,
"Get the joint metadata info for a joint on a body. This includes joint index, name, type, q-index and u-index."},
{"getJointState", pybullet_getJointState, METH_VARARGS,
"Get the joint metadata info for a joint on a body."},
{"setJointControl", pybullet_setJointControl, METH_VARARGS,
"Set the joint control mode and desired target values."},
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},
{"loadSDF", pybullet_loadSDF, METH_VARARGS,
"Load multibodies from an SDF file."},
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
"Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"getJointPositions", pybullet_getJointPositions, METH_VARARGS,
"Get the all the joint positions for a given body index."},
// {"resetBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
// "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."},
{"getJointInfo", pybullet_getJointInfo, METH_VARARGS,
"Get the name and type info for a joint on a body."},
{"getJointState", pybullet_getJointState, METH_VARARGS,
"Get the state (position, velocity etc) for a joint on a body."},
{"resetJointState", pybullet_resetJointState, METH_VARARGS,
"Reset the state (position, velocity etc) for a joint on a body instantaneously, not through physics simulation."},
{"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS,
"Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."},
//saveSnapshot
//loadSnapshot
//collision info
//raycast info
//applyBaseForce
//applyLinkForce
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{NULL, NULL, 0, NULL} /* Sentinel */
};

29
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -23,7 +23,10 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(0.1),
m_kp(0)
{
m_maxAppliedImpulse = maxMotorImpulse;
@@ -51,7 +54,10 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
m_desiredVelocity(desiredVelocity),
m_desiredPosition(0),
m_kd(0.1),
m_kp(0)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -113,9 +119,22 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
for (int row=0;row<getNumRows();row++)
{
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
int dof = 0;
btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof];
btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof];
btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep;
btScalar velocityError = (m_desiredVelocity - currentVelocity);
btScalar rhs = m_kp * positionStabiliationTerm + m_kd * velocityError;
printf("m_kd = %f\n", m_kd);
printf("m_kp = %f\n", m_kp);
printf("m_desiredVelocity = %f\n", m_desiredVelocity);
printf("m_desiredPosition = %f\n", m_desiredPosition);
printf("m_maxAppliedImpulse = %f\n", m_maxAppliedImpulse);
printf("rhs = %f\n", rhs);
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,rhs);
constraintRow.m_orgConstraint = this;
constraintRow.m_orgDofIndex = row;
{

15
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h
View File

@@ -25,8 +25,11 @@ class btMultiBodyJointMotor : public btMultiBodyConstraint
{
protected:
btScalar m_desiredVelocity;
btScalar m_desiredPosition;
btScalar m_kd;
btScalar m_kp;
public:
@@ -42,11 +45,19 @@ public:
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal);
virtual void setVelocityTarget(btScalar velTarget)
virtual void setVelocityTarget(btScalar velTarget, btScalar kd = 0.1f)
{
m_desiredVelocity = velTarget;
m_kd = kd;
}
virtual void setPositionTarget(btScalar posTarget, btScalar kp = 0.1f)
{
m_desiredPosition = posTarget;
m_kp = kp;
}
virtual void debugDraw(class btIDebugDraw* drawer)
{
//todo(erwincoumans)

22
test/SharedMemory/test.c
View File

@@ -68,6 +68,9 @@ void testSharedMemory(b3PhysicsClientHandle sm)
int bodyUniqueId = bodyIndicesOut[0];
numJoints = b3GetNumJoints(sm,bodyUniqueId);
ASSERT_EQ(numJoints,7);
#if 0
b3Printf("numJoints: %d\n", numJoints);
for (i=0;i<numJoints;i++)
{
@@ -77,9 +80,26 @@ void testSharedMemory(b3PhysicsClientHandle sm)
b3Printf("jointInfo[%d].m_jointName=%s\n",i,jointInfo.m_jointName);
}
}
//ASSERT_EQ(numBodies ==1);
#endif
{
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle commandHandle;
double jointAngle = 0.f;
int jointIndex;
commandHandle = b3CreatePoseCommandInit(sm, bodyUniqueId);
for (jointIndex=0;jointIndex<numJoints;jointIndex++)
{
b3CreatePoseCommandSetJointPosition(sm, commandHandle, jointIndex, jointAngle);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
}
{
b3SharedMemoryStatusHandle statusHandle;
int statusType;