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

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

Browse Source
This commit is contained in:
erwincoumans
2017-10-25 08:15:14 -07:00
parent ed8de36ffa c1ba04a580
commit 7524e37124
30 changed files with 759 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
examples/SharedMemory/IKTrajectoryHelper.cpp
View File

@@ -47,7 +47,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
const double* q_current, int numQ,int endEffectorIndex,
double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE) ? true : false;
bool useAngularPart = (ikMethod==IK2_VEL_DLS_WITH_ORIENTATION || ikMethod==IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE
|| ikMethod==IK2_VEL_SDLS_WITH_ORIENTATION) ? true : false;
Jacobian ikJacobian(useAngularPart,numQ);
@@ -136,8 +137,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method
break;
case IK2_DLS:
case IK2_VEL_DLS:
case IK2_VEL_DLS_WITH_ORIENTATION:
case IK2_VEL_DLS:
//ikJacobian.CalcDeltaThetasDLS(); // Damped least squares method
assert(m_data->m_dampingCoeff.GetLength()==numQ);
ikJacobian.CalcDeltaThetasDLS2(m_data->m_dampingCoeff);
@@ -154,6 +155,8 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
ikJacobian.CalcDeltaThetasPseudoinverse(); // Pure pseudoinverse method
break;
case IK2_SDLS:
case IK2_VEL_SDLS:
case IK2_VEL_SDLS_WITH_ORIENTATION:
ikJacobian.CalcDeltaThetasSDLS(); // Selectively damped least squares method
break;
default:

2
examples/SharedMemory/IKTrajectoryHelper.h
View File

@@ -12,6 +12,8 @@ enum IK2_Method
IK2_VEL_DLS_WITH_ORIENTATION,
IK2_VEL_DLS_WITH_NULLSPACE,
IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE,
IK2_VEL_SDLS,
IK2_VEL_SDLS_WITH_ORIENTATION,
};

56
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -2128,7 +2128,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO);
command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId;
command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping;
command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping;
command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING;
return 0;
}
@@ -2292,10 +2292,47 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient*)physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
b3Assert(command);
command->m_type = CMD_USER_CONSTRAINT;
command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE;
command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId;
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
if (status)
{
btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED);
if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED)
{
int i = 0;
constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs;
for (i = 0; i < constraintState->m_numDofs; i++)
{
constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i];
}
for (; i < 6; i++)
{
constraintState->m_appliedConstraintForces[i] = 0;
}
return 1;
}
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId)
{
PhysicsClient* cl = (PhysicsClient* ) physClient;
PhysicsClient* cl = (PhysicsClient* ) physClient;
b3Assert(cl);
b3Assert(cl->canSubmitCommand());
struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand();
@@ -2341,6 +2378,7 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s
}
B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX,
double rayFromWorldY, double rayFromWorldZ,
double rayToWorldX, double rayToWorldY, double rayToWorldZ)
@@ -3612,6 +3650,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem
command->m_calculateInverseKinematicsArguments.m_targetPosition[0] = targetPosition[0];
command->m_calculateInverseKinematicsArguments.m_targetPosition[1] = targetPosition[1];
command->m_calculateInverseKinematicsArguments.m_targetPosition[2] = targetPosition[2];
command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[2] = 0;
command->m_calculateInverseKinematicsArguments.m_targetOrientation[3] = 1;
}
@@ -3695,6 +3739,14 @@ B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCom
}
}
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle;
b3Assert(command);
b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS);
command->m_updateFlags |= solver;
}
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

6
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -143,6 +143,11 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c
B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId);
B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId);
B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState);
B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info);
/// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() )
B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex);
@@ -335,6 +340,7 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(
B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMemoryCommandHandle commandHandle, int numDof, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/], const double* lowerLimit, const double* upperLimit, const double* jointRange, const double* restPose);
B3_SHARED_API void b3CalculateInverseKinematicsSetJointDamping(b3SharedMemoryCommandHandle commandHandle, int numDof, const double* jointDampingCoeff);
B3_SHARED_API void b3CalculateInverseKinematicsSelectSolver(b3SharedMemoryCommandHandle commandHandle, int solver);
B3_SHARED_API int b3GetStatusInverseKinematicsJointPositions(b3SharedMemoryStatusHandle statusHandle,
int* bodyUniqueId,
int* dofCount,

20
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -601,6 +601,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
break;
}
case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
{
break;
}
case CMD_USER_CONSTRAINT_INFO_COMPLETED:
{
B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED");
@@ -649,6 +653,22 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() {
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
userConstraintPtr->m_erp = serverConstraint->m_erp;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink;
}
}
break;
}

21
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -780,10 +780,29 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
{
userConstraintPtr->m_maxAppliedForce = serverConstraint->m_maxAppliedForce;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
userConstraintPtr->m_gearRatio = serverConstraint->m_gearRatio;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
userConstraintPtr->m_relativePositionTarget = serverConstraint->m_relativePositionTarget;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
userConstraintPtr->m_erp = serverConstraint->m_erp;
}
if (serverCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
userConstraintPtr->m_gearAuxLink = serverConstraint->m_gearAuxLink;
}
}
break;
}
case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED:
{
break;
}
case CMD_SYNC_BODY_INFO_COMPLETED:
case CMD_MJCF_LOADING_COMPLETED:
case CMD_SDF_LOADING_COMPLETED:

127
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -5629,13 +5629,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction;
double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution;
btAssert(bodyUniqueId >= 0);
btAssert(linkIndex >= -1);
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (linkIndex == -1)
{
if (mb->getBaseCollider())
@@ -5644,14 +5653,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
mb->getBaseCollider()->setRestitution(restitution);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING)
{
mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping);
}
if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING)
{
@@ -7172,6 +7174,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED;
hasStatus = true;
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE)
{
int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid);
if (userConstraintPtr)
{
serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0;
for (int i = 0; i < 6; i++)
{
serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0;
}
if (userConstraintPtr->m_mbConstraint)
{
serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows();
for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++)
{
serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep;
}
serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED;
}
}
};
if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO)
{
int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId;
@@ -7179,6 +7204,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (userConstraintPtr)
{
serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData;
serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED;
}
}
@@ -7544,19 +7570,23 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_RATIO)
{
userConstraintPtr->m_mbConstraint->setGearRatio(clientCmd.m_userConstraintArguments.m_gearRatio);
userConstraintPtr->m_userConstraintData.m_gearRatio = clientCmd.m_userConstraintArguments.m_gearRatio;
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET)
{
userConstraintPtr->m_mbConstraint->setRelativePositionTarget(clientCmd.m_userConstraintArguments.m_relativePositionTarget);
userConstraintPtr->m_userConstraintData.m_relativePositionTarget = clientCmd.m_userConstraintArguments.m_relativePositionTarget;
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_ERP)
{
userConstraintPtr->m_mbConstraint->setErp(clientCmd.m_userConstraintArguments.m_erp);
userConstraintPtr->m_userConstraintData.m_erp = clientCmd.m_userConstraintArguments.m_erp;
}
if (clientCmd.m_updateFlags & USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK)
{
userConstraintPtr->m_mbConstraint->setGearAuxLink(clientCmd.m_userConstraintArguments.m_gearAuxLink);
userConstraintPtr->m_userConstraintData.m_gearAuxLink = clientCmd.m_userConstraintArguments.m_gearAuxLink;
}
}
@@ -7644,7 +7674,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
const int numDofs = bodyHandle->m_multiBody->getNumDofs();
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
b3AlignedObjectArray<double> jacobian_linear;
jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
@@ -7658,11 +7688,13 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs);
if (tree && (numDofs == tree->numDoFs()))
if (tree && ((numDofs+ baseDofs) == tree->numDoFs()))
{
jacSize = jacobian_linear.size();
// Set jacobian value
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
@@ -7682,8 +7714,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{
tree->calculateJacobians(q);
btInverseDynamics::mat3x jac_t(3, numDofs);
btInverseDynamics::mat3x jac_r(3,numDofs);
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(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
@@ -7691,8 +7723,8 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{
for (int j = 0; j < numDofs; ++j)
{
jacobian_linear[i*numDofs+j] = jac_t(i,j);
jacobian_angular[i*numDofs+j] = jac_r(i,j);
jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j));
jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j));
}
}
}
@@ -7704,19 +7736,35 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE;
}
else if (clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION;
}
else
{
ikMethod = IK2_VEL_DLS_WITH_ORIENTATION;
}
}
else if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
{
//Nullspace task only works with DLS now. TODO: add nullspace task to SDLS.
ikMethod = IK2_VEL_DLS_WITH_NULLSPACE;
}
else
{
ikMethod = IK2_VEL_DLS;
if (clientCmd.m_updateFlags & IK_SDLS)
{
ikMethod = IK2_VEL_SDLS;
}
else
{
ikMethod = IK2_VEL_DLS;;
}
}
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY)
@@ -7742,15 +7790,44 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition;
btVector3DoubleData endEffectorWorldOrientation;
btQuaternionDoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorld = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorld = endEffectorTransformWorld.getRotation();
btVector4 endEffectorOri(endEffectorOriWorld.x(),endEffectorOriWorld.y(),endEffectorOriWorld.z(),endEffectorOriWorld.w());
btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin();
btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation();
btTransform endEffectorWorld;
endEffectorWorld.setOrigin(endEffectorPosWorldOrg);
endEffectorWorld.setRotation(endEffectorOriWorldOrg);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld;
btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation();
btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
endEffectorPosWorld.serializeDouble(endEffectorWorldPosition);
endEffectorOri.serializeDouble(endEffectorWorldOrientation);
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation);
btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetPosition[2]);
btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[0],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[1],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[2],
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]);
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform targetBaseCoord;
targetBaseCoord = tr.inverse()*targetWorld;
btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord;
targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord);
targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord);
// Set joint damping coefficents. A small default
// damping constant is added to prevent singularity
// with pseudo inverse. The user can set joint damping
@@ -7769,7 +7846,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
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,
ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats,
&q_current[0],
numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex,

12
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -633,15 +633,6 @@ struct CalculateMassMatrixResultArgs
int m_dofCount;
};
enum EnumCalculateInverseKinematicsFlags
{
IK_HAS_TARGET_POSITION=1,
IK_HAS_TARGET_ORIENTATION=2,
IK_HAS_NULL_SPACE_VELOCITY=4,
IK_HAS_JOINT_DAMPING=8,
//IK_HAS_CURRENT_JOINT_POSITIONS=16,//not used yet
};
struct CalculateInverseKinematicsArgs
{
int m_bodyUniqueId;
@@ -676,7 +667,7 @@ enum EnumUserConstraintFlags
USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256,
USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512,
USER_CONSTRAINT_CHANGE_ERP=1024,
USER_CONSTRAINT_REQUEST_STATE=2048,
};
enum EnumBodyChangeFlags
@@ -1039,6 +1030,7 @@ struct SharedMemoryStatus
struct SendVisualShapeDataArgs m_sendVisualShapeArgs;
struct UserDebugDrawResultArgs m_userDebugDrawArgs;
struct b3UserConstraint m_userConstraintResultArgs;
struct b3UserConstraintState m_userConstraintStateResultArgs;
struct SendVREvents m_sendVREvents;
struct SendKeyboardEvents m_sendKeyboardEvents;
struct SendRaycastHits m_raycastHits;

23
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -5,7 +5,8 @@
///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures
///my convention is year/month/day/rev
#define SHARED_MEMORY_MAGIC_NUMBER 201710050
#define SHARED_MEMORY_MAGIC_NUMBER 201710180
//#define SHARED_MEMORY_MAGIC_NUMBER 201710050
//#define SHARED_MEMORY_MAGIC_NUMBER 201708270
//#define SHARED_MEMORY_MAGIC_NUMBER 201707140
//#define SHARED_MEMORY_MAGIC_NUMBER 201706015
@@ -143,6 +144,7 @@ enum EnumSharedMemoryServerStatus
CMD_USER_DEBUG_DRAW_FAILED,
CMD_USER_CONSTRAINT_COMPLETED,
CMD_USER_CONSTRAINT_INFO_COMPLETED,
CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_COMPLETED,
CMD_CHANGE_USER_CONSTRAINT_COMPLETED,
CMD_REMOVE_USER_CONSTRAINT_FAILED,
@@ -253,7 +255,6 @@ struct b3UserConstraint
int m_gearAuxLink;
double m_relativePositionTarget;
double m_erp;
};
struct b3BodyInfo
@@ -331,6 +332,12 @@ struct b3OpenGLVisualizerCameraInfo
float m_target[3];
};
struct b3UserConstraintState
{
double m_appliedConstraintForces[6];
int m_numDofs;
};
enum b3VREventType
{
VR_CONTROLLER_MOVE_EVENT=1,
@@ -565,6 +572,18 @@ enum EnumRenderer
//ER_FIRE_RAYS=(1<<18),
};
///flags to pick the IK solver and other options
enum EnumCalculateInverseKinematicsFlags
{
IK_DLS=0,
IK_SDLS=1, //TODO: can add other IK solvers
IK_HAS_TARGET_POSITION=16,
IK_HAS_TARGET_ORIENTATION=32,
IK_HAS_NULL_SPACE_VELOCITY=64,
IK_HAS_JOINT_DAMPING=128,
//IK_HAS_CURRENT_JOINT_POSITIONS=256,//not used yet
};
enum b3ConfigureDebugVisualizerEnum
{
COV_ENABLE_GUI=1,