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

unlock thread at exitPhysics

Browse Source 
pybullet: don't crash in inverse kinematic if #dofs don't match due to free base
C-API: don't crash if status/statusHandle = 0
This commit is contained in:
erwincoumans
2017-10-10 11:10:42 -07:00
parent 7bddc7706d
commit c155e105a5
4 changed files with 128 additions and 95 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/ExampleBrowser/InProcessExampleBrowser.cpp
View File

@@ -391,7 +391,7 @@ void btShutDownExampleBrowser(btInProcessExampleBrowserInternalData* data)
} else } else
{ {
// printf("polling.."); // printf("polling..");
b3Clock::usleep(1000); b3Clock::usleep(0);
} }
}; };

63
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -1654,6 +1654,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3RequestCollisionInfoCommandInit(b3Ph
B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3]) B3_SHARED_API int b3GetStatusAABB(b3SharedMemoryStatusHandle statusHandle, int linkIndex, double aabbMin[3], double aabbMax[3])
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs; const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs;
btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED); btAssert(status->m_type == CMD_REQUEST_COLLISION_INFO_COMPLETED);
if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED) if (status->m_type != CMD_REQUEST_COLLISION_INFO_COMPLETED)
@@ -1695,6 +1698,9 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle
const double* actualStateQdot[], const double* actualStateQdot[],
const double* jointReactionForces[]) { const double* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const SendActualStateArgs &args = status->m_sendActualStateArgs; const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED); btAssert(status->m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED);
if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) if (status->m_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
@@ -1910,6 +1916,8 @@ B3_SHARED_API int b3GetStatusPluginCommandResult(b3SharedMemoryStatusHandle stat
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status); b3Assert(status);
if (status==0)
return statusUniqueId;
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED) if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{ {
@@ -1924,10 +1932,13 @@ B3_SHARED_API int b3GetStatusPluginUniqueId(b3SharedMemoryStatusHandle statusHan
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status); b3Assert(status);
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED); if (status)
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{ {
statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId; b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
{
statusUniqueId = status->m_customCommandResultArgs.m_pluginUniqueId;
}
} }
return statusUniqueId; return statusUniqueId;
} }
@@ -2317,12 +2328,14 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status); b3Assert(status);
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED); if (status)
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
{ {
return status->m_userConstraintResultArgs.m_userConstraintUniqueId; b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
{
return status->m_userConstraintResultArgs.m_userConstraintUniqueId;
}
} }
return -1; return -1;
} }
@@ -2635,11 +2648,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics
B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue) B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED); if (status)
if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
{ {
*paramValue = status->m_userDebugDrawArgs.m_parameterValue; btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
return 1; if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
{
*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
return 1;
}
} }
return 0; return 0;
} }
@@ -3280,10 +3296,13 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa
{ {
int uid = -1; int uid = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED); if (status)
if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
{ {
uid = status->m_loadTextureResultArguments.m_textureUniqueId; btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);
if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
{
uid = status->m_loadTextureResultArguments.m_textureUniqueId;
}
} }
return uid; return uid;
} }
@@ -3420,6 +3439,9 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
double* jointForces) double* jointForces)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED); btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED) if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
return false; return false;
@@ -3475,6 +3497,9 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateJacobianCommandInit(b3Physi
B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian) B3_SHARED_API int b3GetStatusJacobian(b3SharedMemoryStatusHandle statusHandle, int* dofCount, double* linearJacobian, double* angularJacobian)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED); btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED) if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
return false; return false;
@@ -3528,6 +3553,9 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
b3Assert(cl); b3Assert(cl);
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED); btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED) if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
return false; return false;
@@ -3661,9 +3689,12 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
double* jointPositions) double* jointPositions)
{ {
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status);
if (status==0)
return 0;
btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED); btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED) if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
return false; return 0;
if (dofCount) if (dofCount)
@@ -3682,7 +3713,7 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
} }
} }
return true; return 1;
} }
B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient) B3_SHARED_API b3SharedMemoryCommandHandle b3RequestVREventsCommandInit(b3PhysicsClientHandle physClient)

155
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -7659,7 +7659,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> q_current; btAlignedObjectArray<double> q_current;
q_current.resize(numDofs); q_current.resize(numDofs);
if (tree) if (tree && (numDofs == tree->numDoFs()))
{ {
jacSize = jacobian_linear.size(); jacSize = jacobian_linear.size();
// Set jacobian value // Set jacobian value
@@ -7697,92 +7697,93 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
} }
} }
} }
}
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs);
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) btAlignedObjectArray<double> q_new;
{ q_new.resize(numDofs);
btAlignedObjectArray<double> lower_limit; int ikMethod = 0;
btAlignedObjectArray<double> upper_limit; if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
btAlignedObjectArray<double> joint_range; {
btAlignedObjectArray<double> rest_pose; ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
lower_limit.resize(numDofs); }
upper_limit.resize(numDofs); else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
joint_range.resize(numDofs); {
rest_pose.resize(numDofs); ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
for (int i = 0; i < numDofs; ++i) }
{ else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i]; {
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i]; ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; }
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; else
} {
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); ikMethod = IK2_VEL_DLS;
} }
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse(); if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
lower_limit.resize(numDofs);
upper_limit.resize(numDofs);
joint_range.resize(numDofs);
rest_pose.resize(numDofs);
for (int i = 0; i < numDofs; ++i)
{
lower_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_lowerLimit[i];
upper_limit[i] = clientCmd.m_calculateInverseKinematicsArguments.m_upperLimit[i];
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
}
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]);
}
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation; btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin(); btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation(); btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w()); btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition); endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation); endEffectorOri.serializeDouble(endEffectorWorldOrientation);
// Set joint damping coefficents. A small default // Set joint damping coefficents. A small default
// damping constant is added to prevent singularity // damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping // with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger // coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on // the damping coefficient is, the less we rely on
// this joint to achieve the IK target. // this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping; btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5); joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{ {
for (int i = 0; i < numDofs; ++i) for (int i = 0; i < numDofs; ++i)
{ {
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
} }
} }
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]); ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation, ikHelperPtr->computeIK(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition, clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0], &q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff); &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++) for (int i=0;i<numDofs;i++)
{ {
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i]; serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
} }
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs; serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED; serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
} }
} }
hasStatus = true; hasStatus = true;

3
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -1789,6 +1789,7 @@ void PhysicsServerExample::exitPhysics()
{ {
for (int i=0;i<MAX_MOTION_NUM_THREADS;i++) for (int i=0;i<MAX_MOTION_NUM_THREADS;i++)
{ {
m_args[i].m_cs2->unlock();
m_args[i].m_cs->lock(); m_args[i].m_cs->lock();
m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion); m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
m_args[i].m_cs->unlock(); m_args[i].m_cs->unlock();
@@ -1805,7 +1806,7 @@ void PhysicsServerExample::exitPhysics()
} else } else
{ {
b3Clock::usleep(1000); b3Clock::usleep(0);
} }
//we need to call 'stepSimulation' to make sure that //we need to call 'stepSimulation' to make sure that
//other threads get out of blocking state (workerThreadWait) //other threads get out of blocking state (workerThreadWait)