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

a few pybullet tweaks to set desired joint motor targets (pos/vel/torque)

Browse Source
This commit is contained in:
Erwin Coumans
2016-06-22 23:21:47 -07:00
parent f5ffb11bc5
commit 8b96e2de3c
3 changed files with 67 additions and 30 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -218,6 +218,7 @@ int b3JointControlSetDesiredForceTorque(b3SharedMemoryCommandHandle commandHandl
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command); b3Assert(command);
command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value; command->m_sendDesiredStateCommandArgument.m_desiredStateForceTorque[dofIndex] = value;
command->m_updateFlags |= SIM_DESIRED_STATE_HAS_MAX_FORCE;
return 0; return 0;
} }

6
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -1389,6 +1389,9 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
mb->clearForcesAndTorques(); mb->clearForcesAndTorques();
int torqueIndex = 0; int torqueIndex = 0;
#if 0
//it is inconsistent to allow application of base force/torque, there is no 'joint' motor. Use a separate API for this.
btVector3 f(0,0,0); btVector3 f(0,0,0);
btVector3 t(0,0,0); btVector3 t(0,0,0);
@@ -1404,7 +1407,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
torqueIndex+=6; torqueIndex+=6;
mb->addBaseForce(f); mb->addBaseForce(f);
mb->addBaseTorque(t); mb->addBaseTorque(t);
for (int link=0;link<mb->getNumLinks();link++) #endif
for (int link=0;link<mb->getNumLinks();link++)
{ {
for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++) for (int dof=0;dof<mb->getLink(link).m_dofCount;dof++)

90
examples/pybullet/pybullet.c
View File

@@ -222,7 +222,7 @@ pybullet_loadSDF(PyObject* self, PyObject* args)
PyObject* pylist=0; PyObject* pylist=0;
b3SharedMemoryStatusHandle statusHandle; b3SharedMemoryStatusHandle statusHandle;
int statusType; int statusType;
b3SharedMemoryCommandHandle command; b3SharedMemoryCommandHandle commandHandle;
if (0==sm) if (0==sm)
{ {
@@ -236,8 +236,8 @@ pybullet_loadSDF(PyObject* self, PyObject* args)
return NULL; return NULL;
} }
command = b3LoadSdfCommandInit(sm, sdfFileName); commandHandle = b3LoadSdfCommandInit(sm, sdfFileName);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
statusType = b3GetStatusType(statusHandle); statusType = b3GetStatusType(statusHandle);
if (statusType!=CMD_SDF_LOADING_COMPLETED) if (statusType!=CMD_SDF_LOADING_COMPLETED)
{ {
@@ -308,10 +308,11 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
len = PySequence_Size(targetValues); len = PySequence_Size(targetValues);
numJoints = b3GetNumJoints(sm,bodyIndex); numJoints = b3GetNumJoints(sm,bodyIndex);
b3SharedMemoryCommandHandle commandHandle; b3SharedMemoryCommandHandle commandHandle;
int qIndex; b3SharedMemoryStatusHandle statusHandle;
int qIndex;
if (len!=numJoints) if (len!=numJoints)
{ {
PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints."); PyErr_SetString(SpamError, "Number of control target values doesn't match the number of joints.");
Py_DECREF(seq); Py_DECREF(seq);
return NULL; return NULL;
@@ -336,20 +337,24 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
{ {
case CONTROL_MODE_VELOCITY: case CONTROL_MODE_VELOCITY:
{ {
b3JointControlSetDesiredVelocity(commandHandle, qIndex, value); b3JointControlSetDesiredVelocity(commandHandle, 6+qIndex, value);
break; b3JointControlSetKd(commandHandle,6+qIndex,1);
b3JointControlSetMaximumForce(commandHandle,6+qIndex,10000);
break;
} }
case CONTROL_MODE_TORQUE: case CONTROL_MODE_TORQUE:
{ {
b3JointControlSetDesiredForceTorque(commandHandle, qIndex, value); b3JointControlSetDesiredForceTorque(commandHandle, 6+qIndex, value);
break; break;
} }
case CONTROL_MODE_POSITION_VELOCITY_PD: case CONTROL_MODE_POSITION_VELOCITY_PD:
{ {
b3JointControlSetDesiredPosition( commandHandle, qIndex, value); b3JointControlSetDesiredPosition( commandHandle, 7+qIndex, value);
break; b3JointControlSetKp(commandHandle,6+qIndex,1);
b3JointControlSetMaximumForce(commandHandle,6+qIndex,10000);
break;
} }
default: default:
{ {
@@ -357,6 +362,9 @@ static PyObject* pybullet_setJointControl(PyObject* self, PyObject* args)
} }
}; };
} }
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_DECREF(seq); Py_DECREF(seq);
Py_INCREF(Py_None); Py_INCREF(Py_None);
return Py_None; return Py_None;
@@ -554,35 +562,59 @@ pybullet_getNumJoints(PyObject* self, PyObject* args)
static PyObject* static PyObject*
pybullet_initializeJointPositions(PyObject* self, PyObject* args) pybullet_initializeJointPositions(PyObject* self, PyObject* args)
{ {
int size;
if (0==sm) if (0==sm)
{ {
PyErr_SetString(SpamError, "Not connected to physics server."); PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL; 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);
*/ size= PySequence_Size(args);
//
// if (size==2)
{
Py_INCREF(Py_None); int bodyIndex;
return Py_None; PyObject* targetValues;
if (PyArg_ParseTuple(args, "iO", &bodyIndex, &targetValues))
{
PyObject* seq;
int numJoints, len;
seq = PySequence_Fast(targetValues, "expected a sequence");
len = PySequence_Size(targetValues);
numJoints = b3GetNumJoints(sm,bodyIndex);
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
int qIndex;
if (len!=numJoints)
{
PyErr_SetString(SpamError, "Number of joint position values doesn't match the number of joints.");
Py_DECREF(seq);
return NULL;
}
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
for (qIndex=0;qIndex<numJoints;qIndex++)
{
float value = pybullet_internalGetFloatFromSequence(seq,qIndex);
b3CreatePoseCommandSetJointPosition(sm, commandHandle, qIndex, value);
}
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_DECREF(seq);
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "error in initializeJointPositions.");
return NULL;
} }
// CURRENTLY NOT SUPPORTED // CURRENTLY NOT SUPPORTED
// Initalize a single joint position for a specific body index // Initalize a single joint position for a specific body index
// //