diff --git a/CMakeLists.txt b/CMakeLists.txt index 8e5f009df..f1d17027b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -449,7 +449,11 @@ configure_file ( ${CMAKE_CURRENT_SOURCE_DIR}/BulletConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake @ONLY ESCAPE_QUOTES ) -install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/UseBullet.cmake +OPTION(INSTALL_CMAKE_FILES "Install generated CMake files" ON) + +IF (INSTALL_CMAKE_FILES) + install ( FILES ${CMAKE_CURRENT_SOURCE_DIR}/UseBullet.cmake ${CMAKE_CURRENT_BINARY_DIR}/BulletConfig.cmake DESTINATION ${BULLET_CONFIG_CMAKE_PATH} ) +ENDIF (INSTALL_CMAKE_FILES) diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 172a8dfb9..f5556bb60 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -988,7 +988,8 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED) { - b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian); + int dofCount; + b3GetStatusJacobian(statusHandle, &dofCount, linearJacobian, angularJacobian); return true; } return false; diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 5883ce1fa..c479f7db3 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -1,5 +1,4 @@ - SET(SharedMemory_SRCS IKTrajectoryHelper.cpp IKTrajectoryHelper.h diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index c82cd247f..7ec282897 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -3310,13 +3310,17 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi } -B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian) +B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian) { const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED); if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED) return false; + if (dofCount) + { + *dofCount = status->m_jacobianResultArgs.m_dofCount; + } if (linearJacobian) { for (int i = 0; i < status->m_jacobianResultArgs.m_dofCount*3; i++) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index f1e4056f1..3aa31e3d8 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -316,7 +316,10 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3PhysicsClientHandle physClient, int bodyIndex, int linkIndex, const double* localPosition, const double* jointPositionsQ, const double* jointVelocitiesQdot, const double* jointAccelerations); -B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, double* linearJacobian, double* angularJacobian); +B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, + int* dofCount, + double* linearJacobian, + double* angularJacobian); ///compute the joint positions to move the end effector to a desired target using inverse kinematics B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyIndex); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 8fe802ce7..47dff31b3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6618,30 +6618,37 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (tree) { int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - const int num_dofs = bodyHandle->m_multiBody->getNumDofs(); - btInverseDynamics::vecx nu(num_dofs+baseDofs), qdot(num_dofs + baseDofs), q(num_dofs + baseDofs), joint_force(num_dofs + baseDofs); - for (int i = 0; i < num_dofs; i++) + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + btInverseDynamics::vecx q(numDofs + baseDofs); + btInverseDynamics::vecx qdot(numDofs + baseDofs); + btInverseDynamics::vecx nu(numDofs + baseDofs); + btInverseDynamics::vecx joint_force(numDofs + baseDofs); + for (int i = 0; i < numDofs; i++) { q[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointPositionsQ[i]; qdot[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointVelocitiesQdot[i]; - nu[i+baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; + nu[i + baseDofs] = clientCmd.m_calculateJacobianArguments.m_jointAccelerations[i]; } // Set the gravity to correspond to the world gravity btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - if (-1 != tree->setGravityInWorldFrame(id_grav) && -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) { - serverCmd.m_jacobianResultArgs.m_dofCount = num_dofs; + serverCmd.m_jacobianResultArgs.m_dofCount = numDofs + baseDofs; // Set jacobian value tree->calculateJacobians(q); - btInverseDynamics::mat3x jac_t(3, num_dofs); - tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex+1, &jac_t); + btInverseDynamics::mat3x jac_t(3, numDofs + baseDofs); + btInverseDynamics::mat3x jac_r(3, numDofs + baseDofs); + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_t); + tree->getBodyJacobianRot(clientCmd.m_calculateJacobianArguments.m_linkIndex + 1, &jac_r); for (int i = 0; i < 3; ++i) { - for (int j = 0; j < num_dofs; ++j) + for (int j = 0; j < (numDofs + baseDofs); ++j) { - serverCmd.m_jacobianResultArgs.m_linearJacobian[i*num_dofs+j] = jac_t(i,j); + int element = (numDofs + baseDofs) * i + j; + serverCmd.m_jacobianResultArgs.m_linearJacobian[element] = jac_t(i,j); + serverCmd.m_jacobianResultArgs.m_angularJacobian[element] = jac_r(i,j); } } serverCmd.m_type = CMD_CALCULATED_JACOBIAN_COMPLETED; @@ -7365,7 +7372,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm tree->calculateJacobians(q); btInverseDynamics::mat3x jac_t(3, numDofs); btInverseDynamics::mat3x jac_r(3,numDofs); - // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t); tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); for (int i = 0; i < 3; ++i) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index ed4fc453b..dea5e306a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -445,10 +445,10 @@ static PyObject* pybullet_connectPhysicsServer(PyObject* self, PyObject* args, P { printf("Connection terminated, couldn't get body info\n"); b3DisconnectSharedMemory(sm); - sm = 0; + sm = 0; sPhysicsClients1[freeIndex] = 0; - sPhysicsClientsGUI[freeIndex] = 0; - sNumPhysicsClients++; + sPhysicsClientsGUI[freeIndex] = 0; + sNumPhysicsClients++; return PyInt_FromLong(-1); } } @@ -2835,7 +2835,7 @@ static PyObject* pybullet_getJointInfo(PyObject* self, PyObject* args, PyObject* if (info.m_jointName) { PyTuple_SetItem(pyListJointInfo, 1, - PyString_FromString(info.m_jointName)); + PyString_FromString(info.m_jointName)); } else { PyTuple_SetItem(pyListJointInfo, 1, @@ -4754,21 +4754,21 @@ static PyObject* pybullet_loadTexture(PyObject* self, PyObject* args, PyObject* static PyObject* MyConvertContactPoint(struct b3ContactInformation* contactPointPtr) { /* - 0 int m_contactFlags; - 1 int m_bodyUniqueIdA; - 2 int m_bodyUniqueIdB; - 3 int m_linkIndexA; - 4 int m_linkIndexB; - 5 double m_positionOnAInWS[3];//contact point location on object A, - in world space coordinates - 6 double m_positionOnBInWS[3];//contact point location on object - A, in world space coordinates - 7 double m_contactNormalOnBInWS[3];//the separating contact - normal, pointing from object B towards object A - 8 double m_contactDistance;//negative number is penetration, positive - is distance. - 9 double m_normalForce; - */ + 0 int m_contactFlags; + 1 int m_bodyUniqueIdA; + 2 int m_bodyUniqueIdB; + 3 int m_linkIndexA; + 4 int m_linkIndexB; + 5 double m_positionOnAInWS[3];//contact point location on object A, + in world space coordinates + 6 double m_positionOnBInWS[3];//contact point location on object + A, in world space coordinates + 7 double m_contactNormalOnBInWS[3];//the separating contact + normal, pointing from object B towards object A + 8 double m_contactDistance;//negative number is penetration, positive + is distance. + 9 double m_normalForce; + */ int i; @@ -5559,9 +5559,9 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py { int bodyUniqueIdA = -1; int bodyUniqueIdB = -1; - int linkIndexA = -2; - int linkIndexB = -2; - + int linkIndexA = -2; + int linkIndexB = -2; + b3SharedMemoryCommandHandle commandHandle; struct b3ContactInformation contactPointData; b3SharedMemoryStatusHandle statusHandle; @@ -5583,24 +5583,24 @@ static PyObject* pybullet_getContactPointData(PyObject* self, PyObject* args, Py } commandHandle = b3InitRequestContactPointInformation(sm); - if (bodyUniqueIdA>=0) - { - b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); - } - if (bodyUniqueIdB>=0) - { - b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); - } + if (bodyUniqueIdA>=0) + { + b3SetContactFilterBodyA(commandHandle, bodyUniqueIdA); + } + if (bodyUniqueIdB>=0) + { + b3SetContactFilterBodyB(commandHandle, bodyUniqueIdB); + } - if (linkIndexA>=-1) - { - b3SetContactFilterLinkA( commandHandle, linkIndexA); - } - if (linkIndexB >=-1) - { - b3SetContactFilterLinkB( commandHandle, linkIndexB); - } - + if (linkIndexA>=-1) + { + b3SetContactFilterLinkA( commandHandle, linkIndexA); + } + if (linkIndexB >=-1) + { + b3SetContactFilterLinkB( commandHandle, linkIndexB); + } + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); statusType = b3GetStatusType(statusHandle); if (statusType == CMD_CONTACT_POINT_INFORMATION_COMPLETED) @@ -6574,7 +6574,7 @@ static PyObject* pybullet_invertTransform(PyObject* self, PyErr_SetString(SpamError, "Invalid input: expected position [x,y,z] and orientation [x,y,z,w]."); return NULL; } - + /// quaternion <-> euler yaw/pitch/roll convention from URDF/SDF, see Gazebo /// https://github.com/arpg/Gazebo/blob/master/gazebo/math/Quaternion.cc @@ -6956,8 +6956,7 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, /// Given an object id, joint positions, joint velocities and joint /// accelerations, /// compute the joint forces using Inverse Dynamics -static PyObject* pybullet_calculateInverseDynamics(PyObject* self, - PyObject* args, PyObject* keywds) +static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* args, PyObject* keywds) { { int bodyUniqueId; @@ -6966,14 +6965,21 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, PyObject* objAccelerations; int physicsClientId = 0; b3PhysicsClientHandle sm = 0; - static char* kwlist[] = {"bodyUniqueId", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, &bodyUniqueId, &objPositionsQ, - &objVelocitiesQdot, &objAccelerations, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "objPositions", + "objVelocities", "objAccelerations", + "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist, + &bodyUniqueId, &objPositionsQ, + &objVelocitiesQdot, &objAccelerations, + &physicsClientId)) { - static char* kwlist2[] = {"bodyIndex", "objPositions", "objVelocities", "objAccelerations", "physicsClientId", NULL}; + static char* kwlist2[] = {"bodyIndex", "objPositions", + "objVelocities", "objAccelerations", + "physicsClientId", NULL}; PyErr_Clear(); - if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, &bodyUniqueId, &objPositionsQ, - &objVelocitiesQdot, &objAccelerations, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOOO|i", kwlist2, + &bodyUniqueId, &objPositionsQ, &objVelocitiesQdot, + &objAccelerations, &physicsClientId)) { return NULL; } @@ -7072,6 +7078,149 @@ static PyObject* pybullet_calculateInverseDynamics(PyObject* self, return Py_None; } +/// Given an object id, joint positions, joint velocities and joint +/// accelerations, compute the Jacobian +static PyObject* pybullet_calculateJacobian(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int bodyUniqueId; + int linkIndex; + PyObject* localPosition; + PyObject* objPositions; + PyObject* objVelocities; + PyObject* objAccelerations; + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "localPosition", + "objPositions", "objVelocities", + "objAccelerations", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iiOOOO|i", kwlist, + &bodyUniqueId, &linkIndex, &localPosition, &objPositions, + &objVelocities, &objAccelerations, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + int szLoPos = PySequence_Size(localPosition); + int szObPos = PySequence_Size(objPositions); + int szObVel = PySequence_Size(objVelocities); + int szObAcc = PySequence_Size(objAccelerations); + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + if (numJoints && (szLoPos == 3) && (szObPos == numJoints) && + (szObVel == numJoints) && (szObAcc == numJoints)) + { + int byteSizeJoints = sizeof(double) * numJoints; + int byteSizeVec3 = sizeof(double) * 3; + int i; + PyObject* pyResultList = PyTuple_New(2); + double* localPoint = (double*)malloc(byteSizeVec3); + double* jointPositions = (double*)malloc(byteSizeJoints); + double* jointVelocities = (double*)malloc(byteSizeJoints); + double* jointAccelerations = (double*)malloc(byteSizeJoints); + double* linearJacobian = (double*)malloc(3 * byteSizeJoints); + double* angularJacobian = (double*)malloc(3 * byteSizeJoints); + + pybullet_internalSetVectord(localPosition, localPoint); + for (i = 0; i < numJoints; i++) + { + jointPositions[i] = + pybullet_internalGetFloatFromSequence(objPositions, i); + jointVelocities[i] = + pybullet_internalGetFloatFromSequence(objVelocities, i); + jointAccelerations[i] = + pybullet_internalGetFloatFromSequence(objAccelerations, i); + } + { + b3SharedMemoryStatusHandle statusHandle; + b3SharedMemoryCommandHandle commandHandle = + b3CalculateJacobianCommandInit(sm, bodyUniqueId, + linkIndex, localPoint, jointPositions, + jointVelocities, jointAccelerations); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, commandHandle); + int statusType = b3GetStatusType(statusHandle); + if (statusType == CMD_CALCULATED_JACOBIAN_COMPLETED) + { + int dofCount; + b3GetStatusJacobian(statusHandle, &dofCount, NULL, NULL); + if (dofCount) + { + int byteSizeDofCount = sizeof(double) * dofCount; + double* linearJacobian = (double*)malloc(3 * byteSizeDofCount); + double* angularJacobian = (double*)malloc(3 * byteSizeDofCount); + b3GetStatusJacobian(statusHandle, + NULL, + linearJacobian, + angularJacobian); + if (linearJacobian) + { + int r; + PyObject* pymat = PyTuple_New(3); + for (r = 0; r < 3; ++r) { + int c; + PyObject* pyrow = PyTuple_New(dofCount); + for (c = 0; c < dofCount; ++c) { + int element = r * dofCount + c; + PyTuple_SetItem(pyrow, c, + PyFloat_FromDouble(linearJacobian[element])); + } + PyTuple_SetItem(pymat, r, pyrow); + } + PyTuple_SetItem(pyResultList, 0, pymat); + } + if (angularJacobian) + { + int r; + PyObject* pymat = PyTuple_New(3); + for (r = 0; r < 3; ++r) { + int c; + PyObject* pyrow = PyTuple_New(dofCount); + for (c = 0; c < dofCount; ++c) { + int element = r * dofCount + c; + PyTuple_SetItem(pyrow, c, + PyFloat_FromDouble(linearJacobian[element])); + } + PyTuple_SetItem(pymat, r, pyrow); + } + PyTuple_SetItem(pyResultList, 1, pymat); + } + } + } + else + { + PyErr_SetString(SpamError, + "Internal error in calculateJacobian"); + } + } + free(localPoint); + free(jointPositions); + free(jointVelocities); + free(jointAccelerations); + free(linearJacobian); + free(angularJacobian); + if (pyResultList) return pyResultList; + } + else + { + PyErr_SetString(SpamError, + "calculateJacobian [numJoints] needs to be " + "positive, [local position] needs to be of " + "size 3 and [joint positions], " + "[joint velocities], [joint accelerations] " + "need to match the number of joints."); + return NULL; + } + } + } + Py_INCREF(Py_None); + return Py_None; +} static PyMethodDef SpamMethods[] = { @@ -7111,6 +7260,8 @@ static PyMethodDef SpamMethods[] = { "This is for experimental purposes, use at own risk, magic may or not happen"}, {"loadURDF", (PyCFunction)pybullet_loadURDF, METH_VARARGS | METH_KEYWORDS, + "bodyUniqueId = loadURDF(fileName, basePosition=[0.,0.,0.], baseOrientation=[0.,0.,0.,1.], " + "useMaximalCoordinates=0, useFixedBase=0, flags=0, globalScaling=1.0, physicsClientId=0)\n" "Create a multibody by loading a URDF file."}, {"loadSDF", (PyCFunction)pybullet_loadSDF, METH_VARARGS | METH_KEYWORDS, @@ -7359,6 +7510,19 @@ static PyMethodDef SpamMethods[] = { "Given an object id, joint positions, joint velocities and joint " "accelerations, compute the joint forces using Inverse Dynamics"}, + {"calculateJacobian", (PyCFunction)pybullet_calculateJacobian, METH_VARARGS | METH_KEYWORDS, + "Compute the jacobian for a specified local position on a body and its kinematics.\n" + "Args:\n" + " bodyIndex - a scalar defining the unique object id.\n" + " linkIndex - a scalar identifying the link containing the local point.\n" + " localPosition - a list of [x, y, z] of the coordinates of the local point.\n" + " objPositions - a list of the joint positions.\n" + " objVelocities - a list of the joint velocities.\n" + " objAccelerations - a list of the joint accelerations.\n" + "Returns:\n" + " linearJacobian - a list of the partial linear velocities of the jacobian.\n" + " angularJacobian - a list of the partial angular velocities of the jacobian.\n"}, + {"calculateInverseKinematics", (PyCFunction)pybullet_calculateInverseKinematics, METH_VARARGS | METH_KEYWORDS, "Inverse Kinematics bindings: Given an object id, " diff --git a/src/BulletInverseDynamics/IDMath.cpp b/src/BulletInverseDynamics/IDMath.cpp index 021217a17..ecd62f76d 100644 --- a/src/BulletInverseDynamics/IDMath.cpp +++ b/src/BulletInverseDynamics/IDMath.cpp @@ -69,7 +69,7 @@ idScalar maxAbsMat3x(const mat3x &m) { void mul(const mat33 &a, const mat3x &b, mat3x *result) { if (b.cols() != result->cols()) { - error_message("size missmatch. a.cols()= %d, b.cols()= %d\n", + error_message("size missmatch. b.cols()= %d, result->cols()= %d\n", static_cast(b.cols()), static_cast(result->cols())); abort(); } diff --git a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp index 847e5c6c7..b35c55df6 100644 --- a/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp +++ b/src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp @@ -1013,7 +1013,7 @@ int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x* world_jac_trans) const{ CHECK_IF_BODY_INDEX_IS_VALID(body_index); const RigidBody &body = m_body_list[body_index]; - mul(body.m_body_T_world.transpose(), body.m_body_Jac_T,world_jac_trans); + mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans); return 0; }