From ca71b84913a6284787ee0341ff8b06d901251c78 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 21 Nov 2016 22:33:23 -0800 Subject: [PATCH 1/4] fix uninitialized m_lightColor (see ExampleBrowser/Rendering/TinyRenderer, Software was black. --- examples/TinyRenderer/TinyRenderer.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/examples/TinyRenderer/TinyRenderer.cpp b/examples/TinyRenderer/TinyRenderer.cpp index 76b9e4053..a9f9ed9c3 100644 --- a/examples/TinyRenderer/TinyRenderer.cpp +++ b/examples/TinyRenderer/TinyRenderer.cpp @@ -107,6 +107,7 @@ m_objectIndex(-1) Vec3f center(0,0,0); Vec3f up(0,0,1); m_lightDirWorld.setValue(0,0,0); + m_lightColor.setValue(1, 1, 1); m_localScaling.setValue(1,1,1); m_modelMatrix = Matrix::identity(); @@ -127,6 +128,7 @@ m_objectIndex(objectIndex) Vec3f center(0,0,0); Vec3f up(0,0,1); m_lightDirWorld.setValue(0,0,0); + m_lightColor.setValue(1, 1, 1); m_localScaling.setValue(1,1,1); m_modelMatrix = Matrix::identity(); From 0516d2ecaaceb5ad43d8612413c676961e8e223d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 22 Nov 2016 10:11:04 -0800 Subject: [PATCH 2/4] allow getClosestPoints for btCompoundCollisionAlgorithm and btSphereTriangleCollisionAlgorithm add optional 'lightColor' arg to testrender.py script --- examples/pybullet/testrender.py | 3 ++- .../CollisionDispatch/SphereTriangleDetector.cpp | 2 +- .../CollisionDispatch/btCompoundCollisionAlgorithm.cpp | 3 +++ .../CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp | 2 +- 4 files changed, 7 insertions(+), 3 deletions(-) diff --git a/examples/pybullet/testrender.py b/examples/pybullet/testrender.py index 5935c388f..0d3475c84 100644 --- a/examples/pybullet/testrender.py +++ b/examples/pybullet/testrender.py @@ -19,6 +19,7 @@ pixelHeight = 240 nearPlane = 0.01 farPlane = 1000 lightDirection = [0,1,0] +lightColor = [1,1,1]#optional argument fov = 60 #img_arr = pybullet.renderImage(pixelWidth, pixelHeight) @@ -28,7 +29,7 @@ for pitch in range (0,360,10) : viewMatrix = pybullet.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, yaw, pitch, roll, upAxisIndex) aspect = pixelWidth / pixelHeight; projectionMatrix = pybullet.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane); - img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection) + img_arr = pybullet.getCameraImage(pixelWidth, pixelHeight, viewMatrix,projectionMatrix, lightDirection,lightColor) w=img_arr[0] h=img_arr[1] rgb=img_arr[2] diff --git a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp index 634017809..006cc65a2 100644 --- a/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp +++ b/src/BulletCollision/CollisionDispatch/SphereTriangleDetector.cpp @@ -100,7 +100,7 @@ bool SphereTriangleDetector::collide(const btVector3& sphereCenter,btVector3 &po btScalar radiusWithThreshold = radius + contactBreakingThreshold; btVector3 normal = (vertices[1]-vertices[0]).cross(vertices[2]-vertices[0]); - normal.normalize(); + normal.safeNormalize(); btVector3 p1ToCentre = sphereCenter - vertices[0]; btScalar distanceFromPlane = p1ToCentre.dot(normal); diff --git a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp index 1fc960df5..7f4dea1c6 100644 --- a/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btCompoundCollisionAlgorithm.cpp @@ -292,6 +292,9 @@ void btCompoundCollisionAlgorithm::processCollision (const btCollisionObjectWrap btTransform otherInCompoundSpace; otherInCompoundSpace = colObjWrap->getWorldTransform().inverse() * otherObjWrap->getWorldTransform(); otherObjWrap->getCollisionShape()->getAabb(otherInCompoundSpace,localAabbMin,localAabbMax); + btVector3 extraExtends(resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold, resultOut->m_closestPointDistanceThreshold); + localAabbMin -= extraExtends; + localAabbMax += extraExtends; const ATTRIBUTE_ALIGNED16(btDbvtVolume) bounds=btDbvtVolume::FromMM(localAabbMin,localAabbMax); //process all children, that overlap with the given AABB bounds diff --git a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp index 280a4d355..86d4e7440 100644 --- a/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp +++ b/src/BulletCollision/CollisionDispatch/btSphereTriangleCollisionAlgorithm.cpp @@ -56,7 +56,7 @@ void btSphereTriangleCollisionAlgorithm::processCollision (const btCollisionObje /// report a contact. internally this will be kept persistent, and contact reduction is done resultOut->setPersistentManifold(m_manifoldPtr); - SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()); + SphereTriangleDetector detector(sphere,triangle, m_manifoldPtr->getContactBreakingThreshold()+ resultOut->m_closestPointDistanceThreshold); btDiscreteCollisionDetectorInterface::ClosestPointInput input; input.m_maximumDistanceSquared = btScalar(BT_LARGE_FLOAT);///@todo: tighter bounds From 2e372a525e05443e757041ac95f6823908e565e4 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Nov 2016 12:36:52 -0800 Subject: [PATCH 3/4] remove duplicate 'setTimeStep' in pybullet.c --- examples/pybullet/pybullet.c | 5 ----- 1 file changed, 5 deletions(-) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 1947ec103..61b57c083 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3301,11 +3301,6 @@ static PyMethodDef SpamMethods[] = { "Set the amount of time to proceed at each call to stepSimulation. (unit " "is seconds, typically range is 0.01 or 0.001)"}, - - {"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)"}, - {"setDefaultContactERP", pybullet_setDefaultContactERP, METH_VARARGS, "Set the amount of contact penetration Error Recovery Paramater " "(ERP) in each time step. \ From 2d42c7963a545c1822289ee45ba41c574bc535ea Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 28 Nov 2016 15:11:34 -0800 Subject: [PATCH 4/4] add pybullet getBaseVelocity / resetBaseVelocity C-API b3CreatePoseCommandSetBaseLinearVelocity/b3CreatePoseCommandSetBaseAngularVelocity --- examples/SharedMemory/PhysicsClientC_API.cpp | 36 ++ examples/SharedMemory/PhysicsClientC_API.h | 3 + .../PhysicsServerCommandProcessor.cpp | 26 +- examples/SharedMemory/SharedMemoryCommands.h | 6 +- examples/pybullet/pybullet.c | 420 +++++++++++++----- 5 files changed, 373 insertions(+), 118 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index e60953750..24d7e298c 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -672,6 +672,40 @@ int b3CreatePoseCommandSetBaseOrientation(b3SharedMemoryCommandHandle commandHan return 0; } +int b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_BASE_LINEAR_VELOCITY; + command->m_initPoseArgs.m_hasInitialStateQdot[0] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[1] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[2] = 1; + + command->m_initPoseArgs.m_initialStateQdot[0] = linVel[0]; + command->m_initPoseArgs.m_initialStateQdot[1] = linVel[1]; + command->m_initPoseArgs.m_initialStateQdot[2] = linVel[2]; + + return 0; +} + +int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_INIT_POSE); + command->m_updateFlags |= INIT_POSE_HAS_BASE_ANGULAR_VELOCITY; + command->m_initPoseArgs.m_hasInitialStateQdot[3] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[4] = 1; + command->m_initPoseArgs.m_hasInitialStateQdot[5] = 1; + + command->m_initPoseArgs.m_initialStateQdot[3] = angVel[0]; + command->m_initPoseArgs.m_initialStateQdot[4] = angVel[1]; + command->m_initPoseArgs.m_initialStateQdot[5] = angVel[2]; + + return 0; +} + int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; @@ -686,6 +720,8 @@ int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHand return 0; } + + int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b9da28cd5..f8377c089 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -256,6 +256,9 @@ int b3CreateBoxCommandSetColorRGBA(b3SharedMemoryCommandHandle commandHandle, do 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 b3CreatePoseCommandSetBaseLinearVelocity(b3SharedMemoryCommandHandle commandHandle, double linVel[3]); +int b3CreatePoseCommandSetBaseAngularVelocity(b3SharedMemoryCommandHandle commandHandle, double angVel[3]); + int b3CreatePoseCommandSetJointPositions(b3SharedMemoryCommandHandle commandHandle, int numJointPositions, const double* jointPositions); int b3CreatePoseCommandSetJointPosition(b3PhysicsClientHandle physClient, b3SharedMemoryCommandHandle commandHandle, int jointIndex, double jointPosition); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 18231566f..d0a3dca38 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -2342,6 +2342,28 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; + btVector3 baseLinVel(0, 0, 0); + btVector3 baseAngVel(0, 0, 0); + + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_LINEAR_VELOCITY) + { + baseLinVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[0], + clientCmd.m_initPoseArgs.m_initialStateQdot[1], + clientCmd.m_initPoseArgs.m_initialStateQdot[2]); + mb->setBaseVel(baseLinVel); + + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_BASE_ANGULAR_VELOCITY) + { + baseAngVel.setValue(clientCmd.m_initPoseArgs.m_initialStateQdot[3], + clientCmd.m_initPoseArgs.m_initialStateQdot[4], + clientCmd.m_initPoseArgs.m_initialStateQdot[5]); + mb->setBaseOmega(baseAngVel); + } + + if (clientCmd.m_updateFlags & INIT_POSE_HAS_INITIAL_POSITION) { btVector3 zero(0,0,0); @@ -2349,7 +2371,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[1] && clientCmd.m_initPoseArgs.m_hasInitialStateQ[2]); - mb->setBaseVel(zero); + mb->setBaseVel(baseLinVel); mb->setBasePos(btVector3( clientCmd.m_initPoseArgs.m_initialStateQ[0], clientCmd.m_initPoseArgs.m_initialStateQ[1], @@ -2362,7 +2384,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm clientCmd.m_initPoseArgs.m_hasInitialStateQ[5] && clientCmd.m_initPoseArgs.m_hasInitialStateQ[6]); - mb->setBaseOmega(btVector3(0,0,0)); + mb->setBaseOmega(baseAngVel); btQuaternion invOrn(clientCmd.m_initPoseArgs.m_initialStateQ[3], clientCmd.m_initPoseArgs.m_initialStateQ[4], clientCmd.m_initPoseArgs.m_initialStateQ[5], diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 5b3346433..5d25059c2 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -109,7 +109,9 @@ enum EnumInitPoseFlags { INIT_POSE_HAS_INITIAL_POSITION=1, INIT_POSE_HAS_INITIAL_ORIENTATION=2, - INIT_POSE_HAS_JOINT_STATE=4 + INIT_POSE_HAS_JOINT_STATE=4, + INIT_POSE_HAS_BASE_LINEAR_VELOCITY = 8, + INIT_POSE_HAS_BASE_ANGULAR_VELOCITY = 16, }; @@ -122,6 +124,8 @@ struct InitPoseArgs int m_bodyUniqueId; int m_hasInitialStateQ[MAX_DEGREE_OF_FREEDOM]; double m_initialStateQ[MAX_DEGREE_OF_FREEDOM]; + int m_hasInitialStateQdot[MAX_DEGREE_OF_FREEDOM]; + double m_initialStateQdot[MAX_DEGREE_OF_FREEDOM]; }; diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 61b57c083..7aeaab7e0 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -30,6 +30,123 @@ enum eCONNECT_METHOD { static PyObject* SpamError; static b3PhysicsClientHandle sm = 0; + +static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { + double v = 0.0; + PyObject* item; + + if (PyList_Check(seq)) { + item = PyList_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + else { + item = PyTuple_GET_ITEM(seq, index); + v = PyFloat_AsDouble(item); + } + return v; +} + + +// internal function to set a float matrix[16] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// matrix - float[16] which will be set by values from objMat +static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { + int i, len; + PyObject* seq; + + seq = PySequence_Fast(objMat, "expected a sequence"); + if (seq) + { + len = PySequence_Size(objMat); + if (len == 16) { + for (i = 0; i < len; i++) { + matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// internal function to set a float vector[3] +// used to initialize camera position with +// a view and projection matrix in renderImage() +// +// // Args: +// vector - float[3] which will be set by values from objMat +static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { + int i, len; + PyObject* seq = 0; + if (objVec == NULL) + return 0; + + seq = PySequence_Fast(objVec, "expected a sequence"); + if (seq) + { + + len = PySequence_Size(objVec); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + if (seq) + { + len = PySequence_Size(obVec); + if (len == 3) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + } + return 0; +} + +// vector - double[3] which will be set by values from obVec +static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { + int i, len; + PyObject* seq; + if (obVec == NULL) + return 0; + + seq = PySequence_Fast(obVec, "expected a sequence"); + len = PySequence_Size(obVec); + if (len == 4) { + for (i = 0; i < len; i++) { + vector[i] = pybullet_internalGetFloatFromSequence(seq, i); + } + Py_DECREF(seq); + return 1; + } + Py_DECREF(seq); + return 0; +} + + + // Step through one timestep of the simulation static PyObject* pybullet_stepSimulation(PyObject* self, PyObject* args) { if (0 == sm) { @@ -371,19 +488,6 @@ static PyObject* pybullet_loadURDF(PyObject* self, PyObject* args) { return PyLong_FromLong(bodyIndex); } -static double pybullet_internalGetFloatFromSequence(PyObject* seq, int index) { - double v = 0.0; - PyObject* item; - - if (PyList_Check(seq)) { - item = PyList_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } else { - item = PyTuple_GET_ITEM(seq, index); - v = PyFloat_AsDouble(item); - } - return v; -} @@ -768,11 +872,68 @@ pybullet_setDefaultContactERP(PyObject* self, PyObject* args) return Py_None; } +static int pybullet_internalGetBaseVelocity( + int bodyIndex, double baseLinearVelocity[3], double baseAngularVelocity[3]) { + baseLinearVelocity[0] = 0.; + baseLinearVelocity[1] = 0.; + baseLinearVelocity[2] = 0.; + + baseAngularVelocity[0] = 0.; + baseAngularVelocity[1] = 0.; + baseAngularVelocity[2] = 0.; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return 0; + } + + { + { + b3SharedMemoryCommandHandle cmd_handle = + b3RequestActualStateCommandInit(sm, bodyIndex); + b3SharedMemoryStatusHandle status_handle = + b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + + const int status_type = b3GetStatusType(status_handle); + const double* actualStateQdot; + // const double* jointReactionForces[]; + + + if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) { + PyErr_SetString(SpamError, "getBaseVelocity failed."); + return 0; + } + + b3GetStatusActualState( + status_handle, 0 /* body_unique_id */, + 0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */, + 0 /*root_local_inertial_frame*/, 0, + &actualStateQdot, 0 /* joint_reaction_forces */); + + // printf("joint reaction forces="); + // for (i=0; i < (sizeof(jointReactionForces)/sizeof(double)); i++) { + // printf("%f ", jointReactionForces[i]); + // } + // now, position x,y,z = actualStateQ[0],actualStateQ[1],actualStateQ[2] + // and orientation x,y,z,w = + // actualStateQ[3],actualStateQ[4],actualStateQ[5],actualStateQ[6] + baseLinearVelocity[0] = actualStateQdot[0]; + baseLinearVelocity[1] = actualStateQdot[1]; + baseLinearVelocity[2] = actualStateQdot[2]; + + baseAngularVelocity[0] = actualStateQdot[3]; + baseAngularVelocity[1] = actualStateQdot[4]; + baseAngularVelocity[2] = actualStateQdot[5]; + + } + } + return 1; +} // Internal function used to get the base position and orientation // Orientation is returned in quaternions static int pybullet_internalGetBasePositionAndOrientation( - int bodyIndex, double basePosition[3], double baseOrientation[3]) { + int bodyIndex, double basePosition[3], double baseOrientation[4]) { basePosition[0] = 0.; basePosition[1] = 0.; basePosition[2] = 0.; @@ -855,8 +1016,7 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, if (0 == pybullet_internalGetBasePositionAndOrientation( bodyIndex, basePosition, baseOrientation)) { PyErr_SetString(SpamError, - "GetBasePositionAndOrientation failed (#joints/links " - "exceeds maximum?)."); + "GetBasePositionAndOrientation failed."); return NULL; } @@ -891,6 +1051,62 @@ static PyObject* pybullet_getBasePositionAndOrientation(PyObject* self, } } + +static PyObject* pybullet_getBaseVelocity(PyObject* self, + PyObject* args) { + int bodyIndex = -1; + double baseLinearVelocity[3]; + double baseAngularVelocity[3]; + PyObject* pylistLinVel=0; + PyObject* pylistAngVel=0; + + if (0 == sm) { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + if (!PyArg_ParseTuple(args, "i", &bodyIndex)) { + PyErr_SetString(SpamError, "Expected a body index (integer)."); + return NULL; + } + + if (0 == pybullet_internalGetBaseVelocity( + bodyIndex, baseLinearVelocity, baseAngularVelocity)) { + PyErr_SetString(SpamError, + "getBaseVelocity failed."); + return NULL; + } + + { + PyObject* item; + int i; + int num = 3; + pylistLinVel = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseLinearVelocity[i]); + PyTuple_SetItem(pylistLinVel, i, item); + } + } + + { + PyObject* item; + int i; + int num = 3; + pylistAngVel = PyTuple_New(num); + for (i = 0; i < num; i++) { + item = PyFloat_FromDouble(baseAngularVelocity[i]); + PyTuple_SetItem(pylistAngVel, i, item); + } + } + + { + PyObject* pylist; + pylist = PyTuple_New(2); + PyTuple_SetItem(pylist, 0, pylistLinVel); + PyTuple_SetItem(pylist, 1, pylistAngVel); + return pylist; + } +} static PyObject* pybullet_getNumBodies(PyObject* self, PyObject* args) { if (0 == sm) { @@ -1035,6 +1251,66 @@ static PyObject* pybullet_resetJointState(PyObject* self, PyObject* args) { return NULL; } + + + +static PyObject* pybullet_resetBaseVelocity(PyObject* self, PyObject* args, PyObject *keywds) +{ + static char *kwlist[] = { "objectUniqueId", "linearVelocity", "angularVelocity", NULL }; + + if (0 == sm) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int bodyIndex=0; + PyObject* linVelObj=0; + PyObject* angVelObj=0; + double linVel[3] = { 0, 0, 0 }; + double angVel[3] = { 0, 0, 0 }; + + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|OO", kwlist, &bodyIndex, &linVelObj, &angVelObj)) + { + return NULL; + } + if (linVelObj || angVelObj) + { + + b3SharedMemoryCommandHandle commandHandle; + b3SharedMemoryStatusHandle statusHandle; + + commandHandle = b3CreatePoseCommandInit(sm, bodyIndex); + + if (linVelObj) + { + pybullet_internalSetVectord(linVelObj, linVel); + b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVel); + } + + if (angVelObj) + { + pybullet_internalSetVectord(angVelObj, angVel); + b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVel); + } + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + Py_INCREF(Py_None); + return Py_None; + } + else + { + PyErr_SetString(SpamError, "expected at least linearVelocity and/or angularVelocity."); + return NULL; + } + } + PyErr_SetString(SpamError, "error in resetJointState."); + return NULL; +} + + + // 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, @@ -1366,105 +1642,6 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args) { return Py_None; } -// internal function to set a float matrix[16] -// used to initialize camera position with -// a view and projection matrix in renderImage() -// -// // Args: -// matrix - float[16] which will be set by values from objMat -static int pybullet_internalSetMatrix(PyObject* objMat, float matrix[16]) { - int i, len; - PyObject* seq; - - seq = PySequence_Fast(objMat, "expected a sequence"); - if (seq) - { - len = PySequence_Size(objMat); - if (len == 16) { - for (i = 0; i < len; i++) { - matrix[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// internal function to set a float vector[3] -// used to initialize camera position with -// a view and projection matrix in renderImage() -// -// // Args: -// vector - float[3] which will be set by values from objMat -static int pybullet_internalSetVector(PyObject* objVec, float vector[3]) { - int i, len; - PyObject* seq=0; - if (objVec==NULL) - return 0; - - seq = PySequence_Fast(objVec, "expected a sequence"); - if (seq) - { - - len = PySequence_Size(objVec); - if (len == 3) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVectord(PyObject* obVec, double vector[3]) { - int i, len; - PyObject* seq; - if (obVec==NULL) - return 0; - - seq = PySequence_Fast(obVec, "expected a sequence"); - if (seq) - { - len = PySequence_Size(obVec); - if (len == 3) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - } - return 0; -} - -// vector - double[3] which will be set by values from obVec -static int pybullet_internalSetVector4d(PyObject* obVec, double vector[4]) { - int i, len; - PyObject* seq; - if (obVec==NULL) - return 0; - - seq = PySequence_Fast(obVec, "expected a sequence"); - len = PySequence_Size(obVec); - if (len == 4) { - for (i = 0; i < len; i++) { - vector[i] = pybullet_internalGetFloatFromSequence(seq, i); - } - Py_DECREF(seq); - return 1; - } - Py_DECREF(seq); - return 0; -} - - static PyObject* pybullet_addUserDebugText(PyObject* self, PyObject* args, PyObject *keywds) { @@ -3361,6 +3538,19 @@ static PyMethodDef SpamMethods[] = { "instantaneously, not through physics simulation. (x,y,z) position vector " "and (x,y,z,w) quaternion orientation."}, + { "getBaseVelocity", pybullet_getBaseVelocity, + METH_VARARGS, + "Get the linear and angular velocity of the base of the object " + " in world space coordinates. " + "(x,y,z) linear velocity vector and (x,y,z) angular velocity vector." }, + + { "resetBaseVelocity", (PyCFunction)pybullet_resetBaseVelocity, METH_VARARGS | METH_KEYWORDS, + "Reset the linear and/or angular velocity of the base of the object " + " in world space coordinates. " + "linearVelocity (x,y,z) and angularVelocity (x,y,z)." }, + + + {"getNumJoints", pybullet_getNumJoints, METH_VARARGS, "Get the number of joints for an object."},