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

add targetPosition, targetValue, kp & kd to pybullet_setJointMotorControl

Browse Source
This commit is contained in:
Mat Kelcey
2016-07-26 11:09:12 -07:00
parent a6e5c6a273
commit b221d2ad18
1 changed files with 42 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

42
examples/pybullet/pybullet.c
View File

@@ -294,16 +294,18 @@ pybullet_resetSimulation(PyObject* self, PyObject* args)
static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
//todo(erwincoumans): set max forces, kp, kd
//TODO(matkelcey): should this just be three methods?
int size;
int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double targetPosition=0;
double targetVelocity=0;
double maxForce=100000;
double gains=0.1;
double kp=0.1;
double kd=0.1;
int valid = 0;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
@@ -311,10 +313,10 @@ 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
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@@ -324,7 +326,8 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
if (size==5)
{
// for CONTROL_MODE_VELOCITY targetValue -> velocity
// for CONTROL_MODE_TORQUE targetValue -> force torque
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{
PyErr_SetString(SpamError, "Error parsing arguments");
@@ -332,10 +335,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
}
valid = 1;
}
if (size==6)
if (size==8)
{
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
// only applicable for CONTROL_MODE_POSITION_VELOCITY_PD.
if (!PyArg_ParseTuple(args, "iiiddddd", &bodyIndex, &jointIndex, &controlMode, &targetPosition, &targetVelocity, &maxForce, &kp, &kd))
{
PyErr_SetString(SpamError, "Error parsing arguments");
return NULL;
@@ -343,9 +346,20 @@ 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)
{
int numJoints;
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
@@ -374,7 +388,6 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{
case CONTROL_MODE_VELOCITY:
{
double kd = gains;
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
@@ -389,9 +402,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
case CONTROL_MODE_POSITION_VELOCITY_PD:
{
double kp = gains;
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
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);
break;
}
@@ -405,12 +419,10 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
Py_INCREF(Py_None);
return Py_None;
}
PyErr_SetString(SpamError, "error in setJointControl.");
PyErr_SetString(SpamError, "Invalid number of args passed to setJointControl.");
return NULL;
}
static PyObject *
pybullet_setRealTimeSimulation(PyObject* self, PyObject* args)
{