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

Merge remote-tracking branch 'bp/master'

Browse Source
This commit is contained in:
YunfeiBai
2016-08-19 12:02:48 -07:00
parent 4611f5f097 e5626b954f
commit 94161246d9
20 changed files with 412 additions and 241 deletions
Download Patch File Download Diff File Expand all files Collapse all files

215
examples/pybullet/pybullet.c
View File

@@ -296,10 +296,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double targetPosition=0;
double targetVelocity=0;
double maxForce=100000;
double appliedForce = 0;
double kp=0.1;
double kd=1.0;
int valid = 0;
@@ -313,34 +314,107 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
size= PySequence_Size(args);
if (size==4)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
double targetValue = 0;
// see switch statement below for convertsions dependent on controlMode
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
valid = 1;
switch (controlMode)
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
targetPosition = targetValue;
break;
}
case CONTROL_MODE_VELOCITY:
{
targetVelocity = targetValue;
break;
}
case CONTROL_MODE_TORQUE:
{
appliedForce = targetValue;
break;
}
default:
{
valid = 0;
}
}
}
if (size==5)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
double targetValue = 0;
//See switch statement for conversions
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
switch (controlMode)
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
targetPosition = targetValue;
break;
}
case CONTROL_MODE_VELOCITY:
{
targetVelocity = targetValue;
break;
}
case CONTROL_MODE_TORQUE:
{
valid = 0;
break;
}
default:
{
valid = 0;
}
}
}
if (size==6)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
double gain;
double targetValue = 0;
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gain))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
}
valid = 1;
valid = 1;
switch (controlMode)
{
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
targetPosition = targetValue;
kp = gain;
break;
}
case CONTROL_MODE_VELOCITY:
{
targetVelocity = targetValue;
kd = gain;
break;
}
case CONTROL_MODE_TORQUE:
{
valid = 0;
break;
}
default:
{
valid = 0;
}
}
}
if (size==8)
{
@@ -353,16 +427,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
valid = 1;
}
if (size==8 && controlMode!=CONTROL_MODE_POSITION_VELOCITY_PD)
{
PyErr_SetString(SpamError, "8 argument call only applicable for control mode CONTROL_MODE_POSITION_VELOCITY_PD");
return NULL;
}
if (controlMode==CONTROL_MODE_POSITION_VELOCITY_PD && size!=8)
{
PyErr_SetString(SpamError, "For CONTROL_MODE_POSITION_VELOCITY_PD please call with explicit targetPosition & targetVelocity");
return NULL;
}
if (valid)
{
@@ -395,25 +460,25 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
case CONTROL_MODE_VELOCITY:
{
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
case CONTROL_MODE_TORQUE:
{
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce);
break;
}
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break;
}
default:
@@ -426,7 +491,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
PyErr_SetString(SpamError, "Error parsing arguments in setJointControl.");
return NULL;
}
@@ -532,7 +597,7 @@ pybullet_setTimeStep(PyObject* self, PyObject* args)
// Internal function used to get the base position and orientation
// Orientation is returned in quaternions
static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
static int pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double basePosition[3],double baseOrientation[3])
{
basePosition[0] = 0.;
basePosition[1] = 0.;
@@ -552,7 +617,11 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
return 0;
}
const double* actualStateQ;
// const double* jointReactionForces[];
int i;
@@ -579,6 +648,7 @@ static void pybullet_internalGetBasePositionAndOrientation(int bodyIndex, double
}
}
return 1;
}
// Get the positions (x,y,z) and orientation (x,y,z,w) in quaternion
@@ -606,7 +676,11 @@ pybullet_getBasePositionAndOrientation(PyObject* self, PyObject* args)
return NULL;
}
pybullet_internalGetBasePositionAndOrientation(bodyIndex,basePosition,baseOrientation);
if (0==pybullet_internalGetBasePositionAndOrientation(bodyIndex, basePosition, baseOrientation))
{
PyErr_SetString(SpamError, "GetBasePositionAndOrientation failed (#joints/links exceeds maximum?).");
return NULL;
}
{
@@ -848,42 +922,44 @@ pybullet_getJointInfo(PyObject* self, PyObject* args)
// printf("body index = %d, joint index =%d\n", bodyIndex, jointIndex);
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
pyListJointInfo = PyTuple_New(jointInfoSize);
b3GetJointInfo(sm, bodyIndex, jointIndex, &info);
// printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
// info.m_jointIndex,
// info.m_jointName,
// info.m_jointType,
// info.m_qIndex,
// info.m_uIndex);
// printf(" flags=%d jointDamping=%f jointFriction=%f\n",
// info.m_flags,
// info.m_jointDamping,
// info.m_jointFriction);
PyTuple_SetItem(pyListJointInfo, 0,
PyInt_FromLong(info.m_jointIndex));
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyTuple_SetItem(pyListJointInfo, 2,
PyInt_FromLong(info.m_jointType));
PyTuple_SetItem(pyListJointInfo, 3,
PyInt_FromLong(info.m_qIndex));
PyTuple_SetItem(pyListJointInfo, 4,
PyInt_FromLong(info.m_uIndex));
PyTuple_SetItem(pyListJointInfo, 5,
PyInt_FromLong(info.m_flags));
PyTuple_SetItem(pyListJointInfo, 6,
PyFloat_FromDouble(info.m_jointDamping));
PyTuple_SetItem(pyListJointInfo, 7,
PyFloat_FromDouble(info.m_jointFriction));
return pyListJointInfo;
if (b3GetJointInfo(sm, bodyIndex, jointIndex, &info))
{
// printf("Joint%d %s, type %d, at q-index %d and u-index %d\n",
// info.m_jointIndex,
// info.m_jointName,
// info.m_jointType,
// info.m_qIndex,
// info.m_uIndex);
// printf(" flags=%d jointDamping=%f jointFriction=%f\n",
// info.m_flags,
// info.m_jointDamping,
// info.m_jointFriction);
PyTuple_SetItem(pyListJointInfo, 0,
PyInt_FromLong(info.m_jointIndex));
PyTuple_SetItem(pyListJointInfo, 1,
PyString_FromString(info.m_jointName));
PyTuple_SetItem(pyListJointInfo, 2,
PyInt_FromLong(info.m_jointType));
PyTuple_SetItem(pyListJointInfo, 3,
PyInt_FromLong(info.m_qIndex));
PyTuple_SetItem(pyListJointInfo, 4,
PyInt_FromLong(info.m_uIndex));
PyTuple_SetItem(pyListJointInfo, 5,
PyInt_FromLong(info.m_flags));
PyTuple_SetItem(pyListJointInfo, 6,
PyFloat_FromDouble(info.m_jointDamping));
PyTuple_SetItem(pyListJointInfo, 7,
PyFloat_FromDouble(info.m_jointFriction));
return pyListJointInfo;
}
else
{
PyErr_SetString(SpamError, "GetJointInfo failed.");
return NULL;
}
}
}
@@ -934,12 +1010,19 @@ pybullet_getJointState(PyObject* self, PyObject* args)
{
if (PyArg_ParseTuple(args, "ii", &bodyIndex, &jointIndex))
{
int status_type = 0;
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
status_type = b3GetStatusType(status_handle);
if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
PyErr_SetString(SpamError, "getBasePositionAndOrientation failed.");
return NULL;
}
pyListJointState = PyTuple_New(sensorStateSize);
pyListJointForceTorque = PyTuple_New(forceTorqueSize);