AuroraMiddleware/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3 synced 2025-01-18 21:10:05 +00:00
Code Issues 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
Show Stats 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
{
// 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])
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const b3SendCollisionInfoArgs &args = status->m_sendCollisionInfoArgs;
btAssert(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* jointReactionForces[]) {
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
btAssert(status);
if (status==0)
return 0;
const SendActualStateArgs &args = status->m_sendActualStateArgs;
btAssert(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;
b3Assert(status);
if (status==0)
return statusUniqueId;
b3Assert(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;
b3Assert(status);
b3Assert(status->m_type == CMD_CUSTOM_COMMAND_COMPLETED);
if (status->m_type == CMD_CUSTOM_COMMAND_COMPLETED)
if (status)
{
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;
}
@ -2317,12 +2328,14 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
b3Assert(status->m_type == CMD_USER_CONSTRAINT_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_COMPLETED)
if (status)
{
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;
}
@ -2635,11 +2648,14 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitUserDebugReadParameter(b3Physics
B3_SHARED_API int b3GetStatusDebugParameterValue(b3SharedMemoryStatusHandle statusHandle, double* paramValue)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
if (status)
{
*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
return 1;
btAssert(status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED);
if (paramValue && (status->m_type == CMD_USER_DEBUG_DRAW_PARAMETER_COMPLETED))
{
*paramValue = status->m_userDebugDrawArgs.m_parameterValue;
return 1;
}
}
return 0;
}
@ -3280,10 +3296,13 @@ B3_SHARED_API int b3GetStatusTextureUniqueId(b3SharedMemoryStatusHandle statusHa
{
int uid = -1;
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status->m_type == CMD_LOAD_TEXTURE_COMPLETED);
if (status->m_type == CMD_LOAD_TEXTURE_COMPLETED)
if (status)
{
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;
}
@ -3420,6 +3439,9 @@ B3_SHARED_API int b3GetStatusInverseDynamicsJointForces(b3SharedMemoryStatusHand
double* jointForces)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED);
if (status->m_type != CMD_CALCULATED_INVERSE_DYNAMICS_COMPLETED)
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)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_JACOBIAN_COMPLETED);
if (status->m_type != CMD_CALCULATED_JACOBIAN_COMPLETED)
return false;
@ -3528,6 +3553,9 @@ B3_SHARED_API int b3GetStatusMassMatrix(b3PhysicsClientHandle physClient, b3Shar
b3Assert(cl);
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status==0)
return false;
btAssert(status->m_type == CMD_CALCULATED_MASS_MATRIX_COMPLETED);
if (status->m_type != CMD_CALCULATED_MASS_MATRIX_COMPLETED)
return false;
@ -3661,9 +3689,12 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
double* jointPositions)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
btAssert(status);
if (status==0)
return 0;
btAssert(status->m_type == CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED);
if (status->m_type != CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED)
return false;
return 0;
if (dofCount)
@ -3682,7 +3713,7 @@ B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatu
}
}
return true;
return 1;
}
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;
q_current.resize(numDofs);
if (tree)
if (tree && (numDofs == tree->numDoFs()))
{
jacSize = jacobian_linear.size();
// 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> 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]);
}
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;
}
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 endEffectorWorldOrientation;
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on
// this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{
for (int i = 0; i < numDofs; ++i)
{
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
}
}
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[0]);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
// coefficients differently for each joint. The larger
// the damping coefficient is, the less we rely on
// this joint to achieve the IK target.
btAlignedObjectArray<double> joint_damping;
joint_damping.resize(numDofs,0.5);
if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING)
{
for (int i = 0; i < numDofs; ++i)
{
joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i];
}
}
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[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,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
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,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,
&q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize*2, targetDampCoeff);
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId =clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId;
for (int i=0;i<numDofs;i++)
{
serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i];
}
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
}
}
}
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++)
{
m_args[i].m_cs2->unlock();
m_args[i].m_cs->lock();
m_args[i].m_cs->setSharedParam(0,eRequestTerminateMotion);
m_args[i].m_cs->unlock();
@ -1805,7 +1806,7 @@ void PhysicsServerExample::exitPhysics()
} else
{
b3Clock::usleep(1000);
b3Clock::usleep(0);
}
//we need to call 'stepSimulation' to make sure that
//other threads get out of blocking state (workerThreadWait)