AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2024-12-15 14:10:11 +00:00
Code Issues Releases Wiki Activity

Merge branch 'master' of https://github.com/erwincoumans/bullet3

Browse Source
This commit is contained in:
erwincoumans 2017-09-24 11:41:46 -07:00
commit 4ff22a710c
9 changed files with 248 additions and 66 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
CMakeLists.txt
View File

@ -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)

3
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@ -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;

1
examples/SharedMemory/CMakeLists.txt
View File

@ -1,5 +1,4 @@
SET(SharedMemory_SRCS
IKTrajectoryHelper.cpp
IKTrajectoryHelper.h

6
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@ -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++)

5
examples/SharedMemory/PhysicsClientC_API.h
View File

@ -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);

29
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@ -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)

260
examples/pybullet/pybullet.c
View File

@ -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, "

2
src/BulletInverseDynamics/IDMath.cpp
View File

@ -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<int>(b.cols()), static_cast<int>(result->cols()));
abort();
}

2
src/BulletInverseDynamics/details/MultiBodyTreeImpl.cpp
View File

@ -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;
}