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

undo git merge conflict mess-up with IK

Browse Source
This commit is contained in:
erwincoumans
2018-06-01 22:50:06 -07:00
parent a450600eb8
commit 68ea22bfd0
1 changed files with 279 additions and 187 deletions
Download Patch File Download Diff File Expand all files Collapse all files

182
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6905,6 +6905,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
} }
} }
btAlignedObjectArray<btQuaternion> scratch_q; btAlignedObjectArray<btQuaternion> scratch_q;
btAlignedObjectArray<btVector3> scratch_m; btAlignedObjectArray<btVector3> scratch_m;
@@ -8201,39 +8202,115 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex;
btAlignedObjectArray<double> startingPositions;
startingPositions.reserve(bodyHandle->m_multiBody->getNumLinks());
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks())) 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 targetBaseCoord;
if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS)
{ {
targetBaseCoord.setOrigin(targetPosWorld);
targetBaseCoord.setRotation(targetOrnWorld);
} else
{
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
targetBaseCoord = tr.inverse()*targetWorld;
}
{
int DofIndex = 0;
for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
{
if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
{
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = 0;
if (clientCmd.m_updateFlags& IK_HAS_CURRENT_JOINT_POSITIONS)
{
curPos = clientCmd.m_calculateInverseKinematicsArguments.m_currentPositions[DofIndex];
} else
{
curPos = bodyHandle->m_multiBody->getJointPos(i);
}
startingPositions.push_back(curPos);
DofIndex++;
}
}
}
int numIterations = 20;
if (clientCmd.m_updateFlags& IK_HAS_MAX_ITERATIONS)
{
numIterations = clientCmd.m_calculateInverseKinematicsArguments.m_maxNumIterations;
}
double residualThreshold = 1e-4;
if (clientCmd.m_updateFlags& IK_HAS_RESIDUAL_THRESHOLD)
{
residualThreshold = clientCmd.m_calculateInverseKinematicsArguments.m_residualThreshold;
}
btScalar currentDiff = 1e30f;
b3AlignedObjectArray<double> jacobian_linear;
b3AlignedObjectArray<double> jacobian_angular;
btAlignedObjectArray<double> q_current;
btAlignedObjectArray<double> q_new;
btAlignedObjectArray<double> lower_limit;
btAlignedObjectArray<double> upper_limit;
btAlignedObjectArray<double> joint_range;
btAlignedObjectArray<double> rest_pose;
const int numDofs = bodyHandle->m_multiBody->getNumDofs(); const int numDofs = bodyHandle->m_multiBody->getNumDofs();
int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6;
b3AlignedObjectArray<double> jacobian_linear; btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
for (int i=0;i<numIterations && currentDiff > residualThreshold;i++)
{
BT_PROFILE("InverseKinematics1Step");
if (ikHelperPtr && (endEffectorLinkIndex<bodyHandle->m_multiBody->getNumLinks()))
{
jacobian_linear.resize(3*numDofs); jacobian_linear.resize(3*numDofs);
b3AlignedObjectArray<double> jacobian_angular;
jacobian_angular.resize(3*numDofs); jacobian_angular.resize(3*numDofs);
int jacSize = 0; int jacSize = 0;
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
btAlignedObjectArray<double> q_current;
q_current.resize(numDofs); q_current.resize(numDofs);
if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) if (tree && ((numDofs+ baseDofs) == tree->numDoFs()))
{ {
btInverseDynamics::vec3 world_origin;
btInverseDynamics::mat33 world_rot;
jacSize = jacobian_linear.size(); jacSize = jacobian_linear.size();
// Set jacobian value // Set jacobian value
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
int DofIndex = 0; int DofIndex = 0;
for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i)
if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2) { // 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints. {
q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i); if (bodyHandle->m_multiBody->getLink(i).m_jointType >= 0 && bodyHandle->m_multiBody->getLink(i).m_jointType <= 2)
q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(i); {
// 0, 1, 2 represent revolute, prismatic, and spherical joint types respectively. Skip the fixed joints.
double curPos = startingPositions[DofIndex];
q_current[DofIndex] = curPos;
q[DofIndex+baseDofs] = curPos;
qdot[DofIndex+baseDofs] = 0; qdot[DofIndex+baseDofs] = 0;
nu[DofIndex+baseDofs] = 0; nu[DofIndex+baseDofs] = 0;
DofIndex++; DofIndex++;
@@ -8241,6 +8318,8 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
} // Set the gravity to correspond to the world gravity } // Set the gravity to correspond to the world gravity
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
{
BT_PROFILE("calculateInverseDynamics");
if (-1 != tree->setGravityInWorldFrame(id_grav) && if (-1 != tree->setGravityInWorldFrame(id_grav) &&
-1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force))
{ {
@@ -8250,6 +8329,11 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
// 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->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r); tree->getBodyJacobianRot(endEffectorLinkIndex+1, &jac_r);
//calculatePositionKinematics is already done inside calculateInverseDynamics
tree->getBodyOrigin(endEffectorLinkIndex+1,&world_origin);
tree->getBodyTransform(endEffectorLinkIndex+1,&world_rot);
for (int i = 0; i < 3; ++i) for (int i = 0; i < 3; ++i)
{ {
for (int j = 0; j < numDofs; ++j) for (int j = 0; j < numDofs; ++j)
@@ -8259,10 +8343,11 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
} }
} }
} }
}
btAlignedObjectArray<double> q_new;
q_new.resize(numDofs); q_new.resize(numDofs);
int ikMethod = 0; int ikMethod = 0;
if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY)) if ((clientCmd.m_updateFlags& IK_HAS_TARGET_ORIENTATION)&&(clientCmd.m_updateFlags&IK_HAS_NULL_SPACE_VELOCITY))
@@ -8300,10 +8385,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) 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); lower_limit.resize(numDofs);
upper_limit.resize(numDofs); upper_limit.resize(numDofs);
joint_range.resize(numDofs); joint_range.resize(numDofs);
@@ -8315,44 +8397,48 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i];
rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i];
} }
{
BT_PROFILE("computeNullspaceVel");
ikHelperPtr->computeNullspaceVel(numDofs, &q_current[0], &lower_limit[0], &upper_limit[0], &joint_range[0], &rest_pose[0]); 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();
//btTransform endEffectorTransformWorld = bodyHandle->m_multiBody->getLink(endEffectorLinkIndex).m_cachedWorldTransform * bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
btVector3DoubleData endEffectorWorldPosition; btVector3DoubleData endEffectorWorldPosition;
btQuaternionDoubleData endEffectorWorldOrientation; btQuaternionDoubleData endEffectorWorldOrientation;
btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin(); //get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld
btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation(); btVector3 endEffectorPosWorldOrg = world_origin;
btTransform endEffectorWorld; btQuaternion endEffectorOriWorldOrg;
endEffectorWorld.setOrigin(endEffectorPosWorldOrg); world_rot.getRotation(endEffectorOriWorldOrg);
endEffectorWorld.setRotation(endEffectorOriWorldOrg);
btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform(); btTransform endEffectorBaseCoord;
endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg);
endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg);
btTransform endEffectorBaseCoord = tr.inverse()*endEffectorWorld; //don't need the next two lines
//btTransform linkInertiaInv = bodyHandle->m_linkLocalInertialFrames[endEffectorLinkIndex].inverse();
//endEffectorBaseCoord = endEffectorBaseCoord * linkInertiaInv;
//btTransform tr = bodyHandle->m_multiBody->getBaseWorldTransform();
//endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
//endEffectorBaseCoord = tr.inverse()*endEffectorTransformWorld;
btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation(); btQuaternion endEffectorOriBaseCoord= endEffectorBaseCoord.getRotation();
btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w()); //btVector4 endEffectorOri(endEffectorOriBaseCoord.x(), endEffectorOriBaseCoord.y(), endEffectorOriBaseCoord.z(), endEffectorOriBaseCoord.w());
endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition);
endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); 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], //diff
clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[3]); currentDiff = (endEffectorBaseCoord.getOrigin()-targetBaseCoord.getOrigin()).length();
btTransform targetWorld;
targetWorld.setOrigin(targetPosWorld);
targetWorld.setRotation(targetOrnWorld);
btTransform targetBaseCoord;
targetBaseCoord = tr.inverse()*targetWorld;
btVector3DoubleData targetPosBaseCoord; btVector3DoubleData targetPosBaseCoord;
btQuaternionDoubleData targetOrnBaseCoord; btQuaternionDoubleData targetOrnBaseCoord;
@@ -8376,13 +8462,16 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
} }
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};
{
BT_PROFILE("computeIK");
ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats,
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++)
{ {
@@ -8390,12 +8479,18 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
} }
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;
for (int i=0;i<numDofs;i++)
{
startingPositions[i] = q_new[i];
}
}
} }
} }
} }
return hasStatus; return hasStatus;
} }
// PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE); // PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
// PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX); // PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
// PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER); // PyModule_AddIntConstant(m, "GEOM_CYLINDER", GEOM_CYLINDER);
@@ -9479,9 +9574,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject); btMultiBodyLinkCollider* multiCol = (btMultiBodyLinkCollider*)btMultiBodyLinkCollider::upcast(rayCallback.m_collisionObject);
if (multiCol && multiCol->m_multiBody) if (multiCol && multiCol->m_multiBody)
{ {
//todo: don't activate 'static' colliders/objects
if (!(multiCol->m_multiBody->getNumLinks()==0 && multiCol->m_multiBody->getBaseMass()==0))
{
m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep();
multiCol->m_multiBody->setCanSleep(false); multiCol->m_multiBody->setCanSleep(false);
@@ -9492,7 +9585,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
//see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949 //see also http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=4&t=949
//so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply //so we try to avoid it by clamping the maximum impulse (force) that the mouse pick can apply
//it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?) //it is not satisfying, hopefully we find a better solution (higher order integrator, using joint friction using a zero-velocity target motor with limited force etc?)
btScalar scaling=1; btScalar scaling=10;
p2p->setMaxAppliedImpulse(2*scaling); p2p->setMaxAppliedImpulse(2*scaling);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
@@ -9500,7 +9593,6 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
m_data->m_pickingMultiBodyPoint2Point =p2p; m_data->m_pickingMultiBodyPoint2Point =p2p;
} }
} }
}