mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-19 05:20:06 +00:00
undo git merge conflict mess-up with IK
This commit is contained in:
erwincoumans
parent
a450600eb8
commit
68ea22bfd0
@ -6905,6 +6905,7 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
btAlignedObjectArray<btQuaternion> scratch_q;
|
||||
btAlignedObjectArray<btVector3> scratch_m;
|
||||
|
||||
@ -8201,39 +8202,115 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
|
||||
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();
|
||||
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);
|
||||
b3AlignedObjectArray<double> jacobian_angular;
|
||||
jacobian_angular.resize(3*numDofs);
|
||||
int jacSize = 0;
|
||||
|
||||
btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody);
|
||||
|
||||
|
||||
|
||||
btAlignedObjectArray<double> q_current;
|
||||
q_current.resize(numDofs);
|
||||
|
||||
|
||||
|
||||
if (tree && ((numDofs+ baseDofs) == tree->numDoFs()))
|
||||
{
|
||||
btInverseDynamics::vec3 world_origin;
|
||||
btInverseDynamics::mat33 world_rot;
|
||||
|
||||
jacSize = jacobian_linear.size();
|
||||
// Set jacobian value
|
||||
|
||||
|
||||
|
||||
btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs);
|
||||
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.
|
||||
q_current[DofIndex] = bodyHandle->m_multiBody->getJointPos(i);
|
||||
q[DofIndex+baseDofs] = bodyHandle->m_multiBody->getJointPos(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.
|
||||
|
||||
double curPos = startingPositions[DofIndex];
|
||||
q_current[DofIndex] = curPos;
|
||||
q[DofIndex+baseDofs] = curPos;
|
||||
qdot[DofIndex+baseDofs] = 0;
|
||||
nu[DofIndex+baseDofs] = 0;
|
||||
DofIndex++;
|
||||
@ -8241,6 +8318,8 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
} // Set the gravity to correspond to the world gravity
|
||||
btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity());
|
||||
|
||||
{
|
||||
BT_PROFILE("calculateInverseDynamics");
|
||||
if (-1 != tree->setGravityInWorldFrame(id_grav) &&
|
||||
-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.
|
||||
tree->getBodyJacobianTrans(endEffectorLinkIndex+1, &jac_t);
|
||||
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 j = 0; j < numDofs; ++j)
|
||||
@ -8259,10 +8343,11 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
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))
|
||||
@ -8300,10 +8385,7 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
|
||||
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);
|
||||
@ -8315,44 +8397,48 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[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]);
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
btQuaternionDoubleData endEffectorWorldOrientation;
|
||||
|
||||
btVector3 endEffectorPosWorldOrg = endEffectorTransformWorld.getOrigin();
|
||||
btQuaternion endEffectorOriWorldOrg = endEffectorTransformWorld.getRotation();
|
||||
btTransform endEffectorWorld;
|
||||
endEffectorWorld.setOrigin(endEffectorPosWorldOrg);
|
||||
endEffectorWorld.setRotation(endEffectorOriWorldOrg);
|
||||
//get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld
|
||||
btVector3 endEffectorPosWorldOrg = world_origin;
|
||||
btQuaternion endEffectorOriWorldOrg;
|
||||
world_rot.getRotation(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();
|
||||
|
||||
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.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;
|
||||
|
||||
|
||||
//diff
|
||||
currentDiff = (endEffectorBaseCoord.getOrigin()-targetBaseCoord.getOrigin()).length();
|
||||
|
||||
|
||||
btVector3DoubleData targetPosBaseCoord;
|
||||
btQuaternionDoubleData targetOrnBaseCoord;
|
||||
@ -8377,12 +8463,15 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
ikHelperPtr->setDampingCoeff(numDofs, &joint_damping[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,
|
||||
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++)
|
||||
{
|
||||
@ -8390,12 +8479,18 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con
|
||||
}
|
||||
serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs;
|
||||
serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED;
|
||||
for (int i=0;i<numDofs;i++)
|
||||
{
|
||||
startingPositions[i] = q_new[i];
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return hasStatus;
|
||||
}
|
||||
|
||||
|
||||
// PyModule_AddIntConstant(m, "GEOM_SPHERE", GEOM_SPHERE);
|
||||
// PyModule_AddIntConstant(m, "GEOM_BOX", GEOM_BOX);
|
||||
// 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);
|
||||
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();
|
||||
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
|
||||
//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?)
|
||||
btScalar scaling=1;
|
||||
btScalar scaling=10;
|
||||
p2p->setMaxAppliedImpulse(2*scaling);
|
||||
|
||||
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
|
||||
@ -9500,7 +9593,6 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
|
||||
m_data->m_pickingMultiBodyPoint2Point =p2p;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user