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

Remove undesired assert, it makes pybullet mistakes fail in c++

Browse Source 
Shared memory api/pybullet: by default, set joint motors in position PD mode with target 0, to maintain 0 joint angle.
pybullet: allow setJointControlMode(body, link, POSITION_CONTROL,targetPos etc.
This commit is contained in:
erwin coumans
2016-08-18 13:10:28 -07:00
parent 6005e54aa1
commit 5b0253ed47
3 changed files with 94 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -81,7 +81,6 @@ int PhysicsClientSharedMemory::getNumJoints(int bodyIndex) const
return bodyJoints->m_jointInfo.size(); return bodyJoints->m_jointInfo.size();
} }
btAssert(0);
return 0; return 0;
} }

2
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -700,6 +700,8 @@ void PhysicsServerCommandProcessor::createJointMotors(btMultiBody* mb)
int dof = 0; int dof = 0;
btScalar desiredVelocity = 0.f; btScalar desiredVelocity = 0.f;
btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse); btMultiBodyJointMotor* motor = new btMultiBodyJointMotor(mb,mbLinkIndex,dof,desiredVelocity,maxMotorImpulse);
motor->setPositionTarget(0, 0.1);
motor->setVelocityTarget(0, 1);
//motor->setMaxAppliedImpulse(0); //motor->setMaxAppliedImpulse(0);
mb->getLink(mbLinkIndex).m_userPtr = motor; mb->getLink(mbLinkIndex).m_userPtr = motor;
m_data->m_dynamicsWorld->addMultiBodyConstraint(motor); m_data->m_dynamicsWorld->addMultiBodyConstraint(motor);

119
examples/pybullet/pybullet.c
View File

@@ -296,10 +296,11 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
int size; int size;
int bodyIndex, jointIndex, controlMode; int bodyIndex, jointIndex, controlMode;
double targetValue=0;
double targetPosition=0; double targetPosition=0;
double targetVelocity=0; double targetVelocity=0;
double maxForce=100000; double maxForce=100000;
double appliedForce = 0;
double kp=0.1; double kp=0.1;
double kd=1.0; double kd=1.0;
int valid = 0; int valid = 0;
@@ -313,34 +314,107 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
size= PySequence_Size(args); size= PySequence_Size(args);
if (size==4) if (size==4)
{ {
// for CONTROL_MODE_VELOCITY targetValue -> velocity double targetValue = 0;
// for CONTROL_MODE_TORQUE targetValue -> force torque // see switch statement below for convertsions dependent on controlMode
if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue)) if (!PyArg_ParseTuple(args, "iiid", &bodyIndex, &jointIndex, &controlMode, &targetValue))
{ {
PyErr_SetString(SpamError, "Error parsing arguments"); PyErr_SetString(SpamError, "Error parsing arguments");
return NULL; 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) if (size==5)
{ {
// for CONTROL_MODE_VELOCITY targetValue -> velocity double targetValue = 0;
// for CONTROL_MODE_TORQUE targetValue -> force torque //See switch statement for conversions
if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce)) if (!PyArg_ParseTuple(args, "iiidd", &bodyIndex, &jointIndex, &controlMode, &targetValue, &maxForce))
{ {
PyErr_SetString(SpamError, "Error parsing arguments"); PyErr_SetString(SpamError, "Error parsing arguments");
return NULL; 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:
{
valid = 0;
break;
}
default:
{
valid = 0;
}
}
} }
if (size==6) 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"); PyErr_SetString(SpamError, "Error parsing arguments");
return NULL; 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) if (size==8)
{ {
@@ -353,16 +427,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
valid = 1; 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) if (valid)
{ {
@@ -395,25 +460,25 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
{ {
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetValue); b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd); b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break; break;
} }
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, targetValue); b3JointControlSetDesiredForceTorque(commandHandle, info.m_uIndex, appliedForce);
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
{ {
b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition);
b3JointControlSetKp(commandHandle, info.m_uIndex, kp); b3JointControlSetKp(commandHandle, info.m_uIndex, kp);
b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity); b3JointControlSetDesiredVelocity(commandHandle, info.m_uIndex, targetVelocity);
b3JointControlSetKd(commandHandle, info.m_uIndex, kd); b3JointControlSetKd(commandHandle, info.m_uIndex, kd);
b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce); b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, maxForce);
break; break;
} }
default: default:
@@ -426,7 +491,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args)
Py_INCREF(Py_None); Py_INCREF(Py_None);
return 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; return NULL;
} }