AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-13 21:30:09 +00:00
Code Issues Releases Wiki Activity

Merge pull request #673 from erwincoumans/master

Browse Source 
implement a few more pybullet methods:
This commit is contained in:
erwincoumans 2016-06-26 19:09:54 -07:00 committed by GitHub
commit cf1a4b02a1
7 changed files with 547 additions and 149 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

47
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -903,3 +903,50 @@ void b3GetCameraImageData(b3PhysicsClientHandle physClient, struct b3CameraImage
}
}
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_APPLY_EXTERNAL_FORCE;
command->m_updateFlags = 0;
command->m_externalForceArguments.m_numForcesAndTorques = 0;
return (b3SharedMemoryCommandHandle) command;
}
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_FORCE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = force[i];
command->m_externalForceArguments.m_positions[index+i] = position[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flag)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_APPLY_EXTERNAL_FORCE);
int index = command->m_externalForceArguments.m_numForcesAndTorques;
command->m_externalForceArguments.m_bodyUniqueIds[index] = bodyUniqueId;
command->m_externalForceArguments.m_linkIds[index] = linkId;
command->m_externalForceArguments.m_forceFlags[index] = EF_TORQUE+flag;
for (int i = 0; i < 3; ++i) {
command->m_externalForceArguments.m_forcesAndTorques[index+i] = torque[i];
}
command->m_externalForceArguments.m_numForcesAndTorques++;
}

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -159,6 +159,11 @@ b3SharedMemoryCommandHandle b3MovePickedBody(b3PhysicsClientHandle physClient, d
double rayToWorldZ);
b3SharedMemoryCommandHandle b3RemovePickingConstraint(b3PhysicsClientHandle physClient);
/// Apply external force at the body (or link) center of mass, in world space/Cartesian coordinates.
b3SharedMemoryCommandHandle b3ApplyExternalForceCommandInit(b3PhysicsClientHandle physClient);
void b3ApplyExternalForce(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double force[3], const double position[3], int flags);
void b3ApplyExternalTorque(b3SharedMemoryCommandHandle commandHandle, int bodyUniqueId, int linkId, const double torque[3], int flags);
#ifdef __cplusplus
}
#endif

64
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -2056,6 +2056,70 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
hasStatus = true;
break;
}
case CMD_APPLY_EXTERNAL_FORCE:
{
for (int i = 0; i < clientCmd.m_externalForceArguments.m_numForcesAndTorques; ++i)
{
InteralBodyData* body = m_data->getHandle(clientCmd.m_externalForceArguments.m_bodyUniqueIds[i]);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
bool isLinkFrame = ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_LINK_FRAME)!=0);
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_FORCE)!=0)
{
btVector3 forceLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
btVector3 positionLocal(
clientCmd.m_externalForceArguments.m_positions[i*3+0],
clientCmd.m_externalForceArguments.m_positions[i*3+1],
clientCmd.m_externalForceArguments.m_positions[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 forceWorld = isLinkFrame ? forceLocal : mb->getBaseWorldTransform().getBasis()*forceLocal;
btVector3 relPosWorld = isLinkFrame ? positionLocal : mb->getBaseWorldTransform().getBasis()*positionLocal;
mb->addBaseForce(forceWorld);
mb->addBaseTorque(relPosWorld.cross(forceWorld));
//b3Printf("apply base force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2],positionLocal[0],positionLocal[1],positionLocal[2]);
} else
{
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 forceWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*forceLocal;
btVector3 relPosWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*positionLocal;
mb->addLinkForce(link, forceWorld);
mb->addLinkTorque(link,relPosWorld.cross(forceWorld));
//b3Printf("apply link force of %f,%f,%f at %f,%f,%f\n", forceWorld[0],forceWorld[1],forceWorld[2], positionLocal[0],positionLocal[1],positionLocal[2]);
}
}
if ((clientCmd.m_externalForceArguments.m_forceFlags[i] & EF_TORQUE)!=0)
{
btVector3 torqueLocal(clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+0],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+1],
clientCmd.m_externalForceArguments.m_forcesAndTorques[i*3+2]);
if (clientCmd.m_externalForceArguments.m_linkIds[i] == -1)
{
btVector3 torqueWorld = isLinkFrame ? torqueLocal : mb->getBaseWorldTransform().getBasis()*torqueLocal;
mb->addBaseTorque(torqueWorld);
//b3Printf("apply base torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
} else
{
int link = clientCmd.m_externalForceArguments.m_linkIds[i];
btVector3 torqueWorld = mb->getLink(link).m_cachedWorldTransform.getBasis()*torqueLocal;
mb->addLinkTorque(link, torqueWorld);
//b3Printf("apply link torque of %f,%f,%f\n", torqueWorld[0],torqueWorld[1],torqueWorld[2]);
}
}
}
}
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_CLIENT_COMMAND_COMPLETED;
hasStatus = true;
break;
}
default:
{
b3Error("Unknown command encountered");

22
examples/SharedMemory/SharedMemoryCommands.h
View File

@ -319,6 +319,27 @@ struct SdfRequestInfoArgs
int m_bodyUniqueId;
};
///flags for b3ApplyExternalTorque and b3ApplyExternalForce
enum EnumExternalForcePrivateFlags
{
// EF_LINK_FRAME=1,
// EF_WORLD_FRAME=2,
EF_TORQUE=4,
EF_FORCE=8,
};
struct ExternalForceArgs
{
int m_numForcesAndTorques;
int m_bodyUniqueIds[MAX_SDF_BODIES];
int m_linkIds[MAX_SDF_BODIES];
double m_forcesAndTorques[3*MAX_SDF_BODIES];
double m_positions[3*MAX_SDF_BODIES];
int m_forceFlags[MAX_SDF_BODIES];
};
enum EnumSdfRequestInfoFlags
{
SDF_REQUEST_INFO_BODY=1,
@ -350,6 +371,7 @@ struct SharedMemoryCommand
struct RequestDebugLinesArgs m_requestDebugLinesArguments;
struct RequestPixelDataArgs m_requestPixelDataArguments;
struct PickBodyArgs m_pickBodyArguments;
struct ExternalForceArgs m_externalForceArguments;
};
};

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@ -26,6 +26,7 @@ enum EnumSharedMemoryClientCommand
CMD_MOVE_PICKED_BODY,
CMD_REMOVE_PICKING_CONSTRAINT_BODY,
CMD_REQUEST_CAMERA_IMAGE_DATA,
CMD_APPLY_EXTERNAL_FORCE,
CMD_MAX_CLIENT_COMMANDS
};
@ -143,5 +144,11 @@ enum {
CONTROL_MODE_POSITION_VELOCITY_PD,
};
///flags for b3ApplyExternalTorque and b3ApplyExternalForce
enum EnumExternalForceFlags
{
EF_LINK_FRAME=1,
EF_WORLD_FRAME=2,
};
#endif//SHARED_MEMORY_PUBLIC_H

2
examples/SharedMemory/premake4.lua
View File

@ -26,6 +26,8 @@ files {
"PhysicsServer.h",
"main.cpp",
"PhysicsClientC_API.cpp",
"SharedMemoryCommands.h",
"SharedMemoryPublic.h",
"PhysicsServer.cpp",
"PosixSharedMemory.cpp",
"Win32SharedMemory.cpp",

549
examples/pybullet/pybullet.c
View File

@ -437,6 +437,36 @@ pybullet_setGravity(PyObject* self, PyObject* args)
return Py_None;
}
static PyObject *
pybullet_setTimeStep(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
double timeStep=0.001;
int ret;
b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(sm);
b3SharedMemoryStatusHandle statusHandle;
if (!PyArg_ParseTuple(args, "d", &timeStep))
{
PyErr_SetString(SpamError, "setTimeStep expected a single value (double).");
return NULL;
}
ret = b3PhysicsParamSetTimeStep(command, timeStep);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
//ASSERT_EQ(b3GetStatusType(statusHandle), CMD_CLIENT_COMMAND_COMPLETED);
}
Py_INCREF(Py_None);
return Py_None;
}
// Internal function used to get the base position and orientation
@ -632,140 +662,88 @@ pybullet_resetJointState(PyObject* self, PyObject* args)
return NULL;
}
// CURRENTLY NOT SUPPORTED
// Initalize a single joint position for a specific body index
//
// This method skips any physics simulation and
// teleports all joints to the new positions.
// TODO(hellojas): initializing one joint currently not supported
// static PyObject*
// pybullet_initializeJointPosition(PyObject* self, PyObject* args)
// {
// if (0==sm)
// {
// PyErr_SetString(SpamError, "Not connected to physics server.");
// return NULL;
// }
//
// int i;
// int bodyIndex = -1;
// int jointIndex = -1;
// double jointPos = 0.0;
//
// int size= PySequence_Size(args);
//
// if (size==3) // get body index, joint index, and joint position value
// {
// if (PyArg_ParseTuple(args, "iid", &bodyIndex, &jointIndex, &jointPos))
// {
// b3SharedMemoryCommandHandle cmd_handle = b3CreatePoseCommandInit(sm, bodyIndex);
//
// // printf("initializing joint %d at %f\n", jointIndex, jointPos);
// b3CreatePoseCommandSetJointPosition(sm, cmd_handle, jointIndex, jointPos);
//
// b3SharedMemoryStatusHandle status_handle =
// b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
//
// const int status_type = b3GetStatusType(status_handle);
//
// }
// }
//
// Py_INCREF(Py_None);
// return Py_None;
// }
#if 0
static void pybullet_internalGetJointPositions(int bodyIndex, int numJoints, double jointPositions[]) {
int i, j;
int numDegreeQ;
int numDegreeU;
int arrSizeOfPosAndOrn = 7;
for (i =0;i <numJoints; i++){
jointPositions[i] = .5;
}
{
b3SharedMemoryCommandHandle cmd_handle =
b3RequestActualStateCommandInit(sm, bodyIndex);
b3SharedMemoryStatusHandle status_handle =
b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);
const int status_type = b3GetStatusType(status_handle);
const double* actualStateQ;
b3GetStatusActualState(status_handle, 0/* body_unique_id */,
&numDegreeQ ,
&numDegreeU/* num_degree_of_freedom_u */, 0 /*root_local_inertial_frame*/,
&actualStateQ , 0 /* actual_state_q_dot */,
0 /* joint_reaction_forces */);
// printf("actual state Q, size = %lu\n", sizeof(actualStateQ.));
// printf("numjoints = %d\n", numJoints);
// printf("numDegreeQ = %d\n", numDegreeQ);
// printf("numDegreeU = %d\n", numDegreeU);
// printf("actualStateQ[0] = %f\n",actualStateQ[0]);
for (j = arrSizeOfPosAndOrn; j < numJoints + arrSizeOfPosAndOrn; j++) {
jointPositions[j - arrSizeOfPosAndOrn] = actualStateQ[j];
// printf("%d=%f\n", j, jointPositions[j - arrSizeOfPosAndOrn]);
}
}
}
// Get a list of all joint positions for a given body index
//
// Args:
// bodyIndex - integer indicating body in simulation
// Returns:
// pyListJointPos - list of positions for each joint index
//
static PyObject *
pybullet_getJointPositions(PyObject* self, PyObject* args)
// Reset the position and orientation of the base/root link, position [x,y,z] and orientation quaternion [x,y,z,w]
static PyObject*
pybullet_resetBasePositionAndOrientation(PyObject* self, PyObject* args)
{
int size;
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
int bodyIndex = -1;
if (!PyArg_ParseTuple(args, "i", &bodyIndex ))
size= PySequence_Size(args);
if (size==3)
{
PyErr_SetString(SpamError, "Expected a body index (integer).");
return NULL;
}
{
PyObject *item;
PyObject *pyListJointPos;
int i;
int numJoints = b3GetNumJoints(sm,bodyIndex);
double* jointPositions = malloc(numJoints*sizeof(double));
pyListJointPos = PyTuple_New(numJoints);
int bodyIndex;
PyObject* posObj;
PyObject* ornObj;
double pos[3];
double orn[4];//as a quaternion
if (PyArg_ParseTuple(args, "iOO", &bodyIndex, &posObj, &ornObj))
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(posObj, "expected a sequence");
len = PySequence_Size(posObj);
if (len==3)
{
for (i = 0; i < 3; i++)
{
pos[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
}
// printf("joint positions size = %lu\n", sizeof(jointPositions)/sizeof(double));
pybullet_internalGetJointPositions(bodyIndex, numJoints,jointPositions);
for (i =0;i <numJoints; i++){
item = PyFloat_FromDouble(jointPositions[i]);
PyTuple_SetItem(pyListJointPos, i, item);
}
free(jointPositions);
return pyListJointPos;
}
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(ornObj, "expected a sequence");
len = PySequence_Size(ornObj);
if (len==4)
{
for (i = 0; i < 4; i++)
{
orn[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "orientation needs a 4 coordinates, quaternion [x,y,z,w].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
}
Py_INCREF(Py_None);
return Py_None;
commandHandle = b3CreatePoseCommandInit(sm, bodyIndex);
b3CreatePoseCommandSetBasePosition( commandHandle, pos[0],pos[1],pos[2]);
b3CreatePoseCommandSetBaseOrientation( commandHandle, orn[0],orn[1],orn[2],orn[3]);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle);
Py_INCREF(Py_None);
return Py_None;
}
}
PyErr_SetString(SpamError, "error in resetJointState.");
return NULL;
}
#endif
// Get the a single joint info for a specific bodyIndex
//
@ -890,9 +868,6 @@ pybullet_getJointState(PyObject* self, PyObject* args)
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
if (size==2) // get body index and joint index
{
@ -908,28 +883,15 @@ pybullet_getJointState(PyObject* self, PyObject* args)
pyListJointForceTorque = PyTuple_New(forceTorqueSize);
// double m_jointPosition;
// double m_jointVelocity;
// double m_jointForceTorque[6]; /* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
// double m_jointMotorTorque;
b3GetJointState(sm, status_handle, jointIndex, &sensorState);
// printf("Joint%d: position=%f velocity=%f motortorque=%f\n",
// jointIndex,
// sensorState.m_jointPosition,
// sensorState.m_jointVelocity,
// sensorState.m_jointMotorTorque);
PyTuple_SetItem(pyListJointState, 0,
PyFloat_FromDouble(sensorState.m_jointPosition));
PyTuple_SetItem(pyListJointState, 1,
PyFloat_FromDouble(sensorState.m_jointVelocity));
// joint force torque is list of 6
/* note to roboticists: this is NOT the motor torque/force, but the spatial reaction force vector at joint */
// printf(" jointForceTorque = ");
for (j = 0; j < forceTorqueSize; j++) {
// printf("%f ", sensorState.m_jointForceTorque[j]);
PyTuple_SetItem(pyListJointForceTorque, j,
PyTuple_SetItem(pyListJointForceTorque, j,
PyFloat_FromDouble(sensorState.m_jointForceTorque[j]));
}
@ -941,6 +903,10 @@ pybullet_getJointState(PyObject* self, PyObject* args)
return pyListJointState;
}
} else
{
PyErr_SetString(SpamError, "getJointState expects 2 arguments (objectUniqueId and joint index).");
return NULL;
}
Py_INCREF(Py_None);
@ -1091,6 +1057,274 @@ static PyObject* pybullet_renderImage(PyObject* self, PyObject* args)
return Py_None;
}
static PyObject* pybullet_applyExternalForce(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int objectUniqueId, linkIndex, flags;
double force[3];
double position[3]={0,0,0};
PyObject* forceObj, *posObj;
b3SharedMemoryCommandHandle command;
b3SharedMemoryStatusHandle statusHandle;
int size= PySequence_Size(args);
if (size==5)
{
if(!PyArg_ParseTuple(args, "iiOOi", &objectUniqueId, &linkIndex, &forceObj, &posObj, &flags))
{
PyErr_SetString(SpamError, "applyBaseForce couldn't parse arguments");
return NULL;
}
} else
{
PyErr_SetString(SpamError, "applyBaseForce needs 5 arguments: objectUniqueId, linkIndex (-1 for base/root link), force [x,y,z], position [x,y,z], flags");
return NULL;
}
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(forceObj, "expected a sequence");
len = PySequence_Size(forceObj);
if (len==3)
{
for (i = 0; i < 3; i++)
{
force[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "force needs a 3 coordinates [x,y,z].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
}
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(posObj, "expected a sequence");
len = PySequence_Size(posObj);
if (len==3)
{
for (i = 0; i < 3; i++)
{
position[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "position needs a 3 coordinates [x,y,z].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
}
if ((flags !=EF_WORLD_FRAME) && (flags != EF_LINK_FRAME))
{
PyErr_SetString(SpamError, "flag has to be either WORLD_FRAME or LINK_FRAME");
return NULL;
}
command = b3ApplyExternalForceCommandInit(sm);
b3ApplyExternalForce(command,objectUniqueId,-1,force,position, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_applyExternalTorque(PyObject* self, PyObject* args)
{
if (0==sm)
{
PyErr_SetString(SpamError, "Not connected to physics server.");
return NULL;
}
{
int objectUniqueId, linkIndex, flags;
double torque[3];
PyObject* torqueObj;
if (PyArg_ParseTuple(args, "iiOi", &objectUniqueId, &linkIndex, &torqueObj, &flags))
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(torqueObj, "expected a sequence");
len = PySequence_Size(torqueObj);
if (len==3)
{
for (i = 0; i < 3; i++)
{
torque[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "torque needs a 3 coordinates [x,y,z].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
if (linkIndex <-1)
{
PyErr_SetString(SpamError, "Invalid link index, has to be -1 or larger");
return NULL;
}
if ((flags !=EF_WORLD_FRAME) && (flags != EF_LINK_FRAME))
{
PyErr_SetString(SpamError, "flag has to be either WORLD_FRAME or LINK_FRAME");
return NULL;
}
b3SharedMemoryStatusHandle statusHandle;
b3SharedMemoryCommandHandle command = b3ApplyExternalForceCommandInit(sm);
b3ApplyExternalTorque(command,objectUniqueId,-1,torque, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyObject* pybullet_getQuaternionFromEuler(PyObject* self, PyObject* args)
{
double rpy[3];
PyObject* eulerObj;
if (PyArg_ParseTuple(args, "O", &eulerObj))
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(eulerObj, "expected a sequence");
len = PySequence_Size(eulerObj);
if (len==3)
{
for (i = 0; i < 3; i++)
{
rpy[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
} else
{
PyErr_SetString(SpamError, "Euler angles need a 3 coordinates [roll, pitch, yaw].");
return NULL;
}
{
double phi, the, psi;
double roll = rpy[0];
double pitch = rpy[1];
double yaw = rpy[2];
phi = roll / 2.0;
the = pitch / 2.0;
psi = yaw / 2.0;
{
double quat[4] = {
sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
//normalize the quaternion
double len = sqrt(quat[0]*quat[0]+quat[1]*quat[1]+quat[2]*quat[2]+quat[3]*quat[3]);
quat[0] /= len;
quat[1] /= len;
quat[2] /= len;
quat[3] /= len;
{
PyObject *pylist;
int i;
pylist = PyTuple_New(4);
for (i=0;i<4;i++)
PyTuple_SetItem(pylist,i,PyFloat_FromDouble(quat[i]));
return pylist;
}
}
}
Py_INCREF(Py_None);
return Py_None;
}
///quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo
///https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc
static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, PyObject* args)
{
double squ;
double sqx;
double sqy;
double sqz;
double quat[4];
PyObject* quatObj;
if (PyArg_ParseTuple(args, "O", &quatObj))
{
PyObject* seq;
int len,i;
seq = PySequence_Fast(quatObj, "expected a sequence");
len = PySequence_Size(quatObj);
if (len==4)
{
for (i = 0; i < 4; i++)
{
quat[i] = pybullet_internalGetFloatFromSequence(seq,i);
}
} else
{
PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
Py_DECREF(seq);
return NULL;
}
Py_DECREF(seq);
} else
{
PyErr_SetString(SpamError, "Quaternion need a 4 components [x,y,z,w].");
return NULL;
}
{
double rpy[3];
sqx = quat[0] * quat[0];
sqy = quat[1] * quat[1];
sqz = quat[2] * quat[2];
squ = quat[3] * quat[3];
rpy[0] = atan2(2 * (quat[1]*quat[2] + quat[3]*quat[0]), squ - sqx - sqy + sqz);
double sarg = -2 * (quat[0]*quat[2] - quat[3] * quat[1]);
rpy[1] = sarg <= -1.0 ? -0.5*3.141592538 : (sarg >= 1.0 ? 0.5*3.141592538 : asin(sarg));
rpy[2] = atan2(2 * (quat[0]*quat[1] + quat[3]*quat[2]), squ + sqx - sqy - sqz);
{
PyObject *pylist;
int i;
pylist = PyTuple_New(3);
for (i=0;i<3;i++)
PyTuple_SetItem(pylist,i,PyFloat_FromDouble(rpy[i]));
return pylist;
}
}
Py_INCREF(Py_None);
return Py_None;
}
static PyMethodDef SpamMethods[] = {
@ -1109,6 +1343,9 @@ static PyMethodDef SpamMethods[] = {
{"setGravity", pybullet_setGravity, METH_VARARGS,
"Set the gravity acceleration (x,y,z)."},
{"setTimeStep", pybullet_setTimeStep, METH_VARARGS,
"Set the amount of time to proceed at each call to stepSimulation. (unit is seconds, typically range is 0.01 or 0.001)"},
{"loadURDF", pybullet_loadURDF, METH_VARARGS,
"Create a multibody by loading a URDF file."},
@ -1118,8 +1355,8 @@ static PyMethodDef SpamMethods[] = {
{"getBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
"Get the world position and orientation of the base of the object. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
// {"resetBasePositionAndOrientation", pybullet_getBasePositionAndOrientation, METH_VARARGS,
// "Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"resetBasePositionAndOrientation", pybullet_resetBasePositionAndOrientation, METH_VARARGS,
"Reset the world position and orientation of the base of the object instantaneously, not through physics simulation. (x,y,z) position vector and (x,y,z,w) quaternion orientation."},
{"getNumJoints", pybullet_getNumJoints, METH_VARARGS,
"Get the number of joints for an object."},
@ -1136,18 +1373,29 @@ static PyMethodDef SpamMethods[] = {
{"setJointMotorControl", pybullet_setJointMotorControl, METH_VARARGS,
"Set a single joint motor control mode and desired target value. There is no immediate state change, stepSimulation will process the motors."},
{"applyExternalForce", pybullet_applyExternalForce, METH_VARARGS,
"for objectUniqueId, linkIndex (-1 for base/root link), apply a force [x,y,z] at the a position [x,y,z], flag to select FORCE_IN_LINK_FRAME or FORCE_IN_WORLD_FRAME coordinates"},
{"applyExternalTorque", pybullet_applyExternalTorque, METH_VARARGS,
"for objectUniqueId, linkIndex (-1 for base/root link) apply a torque [x,y,z] in Cartesian coordinates, flag to select TORQUE_IN_LINK_FRAME or TORQUE_IN_WORLD_FRAME coordinates"},
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{"getQuaternionFromEuler", pybullet_getQuaternionFromEuler, METH_VARARGS,
"Convert Euler [roll, pitch, yaw] as in URDF/SDF convention, to quaternion [x,y,z,w]"},
{"getEulerFromQuaternion", pybullet_getEulerFromQuaternion, METH_VARARGS,
"Convert quaternion [x,y,z,w] to Euler [roll, pitch, yaw] as in URDF/SDF convention"},
//todo(erwincoumans)
//saveSnapshot
//loadSnapshot
////todo(erwincoumans)
//collision info
//raycast info
//applyBaseForce
//applyLinkForce
{"renderImage", pybullet_renderImage, METH_VARARGS,
"Render an image (given the pixel resolution width & height and camera view & projection matrices), and return the 8-8-8bit RGB pixel data and floating point depth values"},
{NULL, NULL, 0, NULL} /* Sentinel */
};
@ -1155,7 +1403,7 @@ static PyMethodDef SpamMethods[] = {
static struct PyModuleDef moduledef = {
PyModuleDef_HEAD_INIT,
"pybullet", /* m_name */
"Python bindings for Bullet", /* m_doc */
"Python bindings for Bullet Physics Robotics API (also known as Shared Memory API)", /* m_doc */
-1, /* m_size */
SpamMethods, /* m_methods */
NULL, /* m_reload */
@ -1198,6 +1446,9 @@ initpybullet(void)
PyModule_AddIntConstant (m, "VELOCITY_CONTROL", CONTROL_MODE_VELOCITY); // user read
PyModule_AddIntConstant (m, "POSITION_CONTROL", CONTROL_MODE_POSITION_VELOCITY_PD); // user read
PyModule_AddIntConstant (m, "LINK_FRAME", EF_LINK_FRAME);
PyModule_AddIntConstant (m, "WORLD_FRAME", EF_WORLD_FRAME);
SpamError = PyErr_NewException("pybullet.error", NULL, NULL);
Py_INCREF(SpamError);
PyModule_AddObject(m, "error", SpamError);