mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
Merge pull request #717 from matpalm/pyJointMotorControlArgs
update pybullet_setJointMotorControl with seperate args for targetPosition, targetVelocity, kp and kd
This commit is contained in:
commit
9451923dbd
@ -294,16 +294,16 @@ pybullet_resetSimulation(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 targetPosition=0;
|
||||
double targetVelocity=0;
|
||||
double maxForce=100000;
|
||||
double gains=0.1;
|
||||
double kp=0.1;
|
||||
double kd=1.0;
|
||||
int valid = 0;
|
||||
|
||||
|
||||
if (0==sm)
|
||||
{
|
||||
PyErr_SetString(SpamError, "Not connected to physics server.");
|
||||
@ -311,10 +311,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 +324,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");
|
||||
@ -334,8 +335,17 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
|
||||
}
|
||||
if (size==6)
|
||||
{
|
||||
|
||||
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &gains))
|
||||
if (!PyArg_ParseTuple(args, "iiiddd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce, &kd))
|
||||
{
|
||||
PyErr_SetString(SpamError, "Error parsing arguments");
|
||||
return NULL;
|
||||
}
|
||||
valid = 1;
|
||||
}
|
||||
if (size==8)
|
||||
{
|
||||
// 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 +353,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,10 +395,9 @@ 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);
|
||||
b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
|
||||
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
|
||||
break;
|
||||
}
|
||||
|
||||
@ -389,10 +409,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
|
||||
|
||||
case CONTROL_MODE_POSITION_VELOCITY_PD:
|
||||
{
|
||||
double kp = gains;
|
||||
b3JointControlSetDesiredPosition( commandHandle, info.m_qIndex, targetValue);
|
||||
b3JointControlSetKp(commandHandle,info.m_uIndex,kp);
|
||||
b3JointControlSetMaximumForce(commandHandle,info.m_uIndex,maxForce);
|
||||
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;
|
||||
}
|
||||
default:
|
||||
@ -405,12 +426,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)
|
||||
{
|
||||
|
Loading…
Reference in New Issue
Block a user