diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 1bf44c362..a3cb2bdf6 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -6904,7 +6904,8 @@ bool PhysicsServerCommandProcessor::processInitPoseCommand(const struct SharedMe } } - + + btAlignedObjectArray scratch_q; btAlignedObjectArray scratch_m; @@ -8201,201 +8202,295 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; - - if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + btAlignedObjectArray startingPositions; + startingPositions.reserve(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) { - const int numDofs = bodyHandle->m_multiBody->getNumDofs(); - int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; - b3AlignedObjectArray jacobian_linear; - jacobian_linear.resize(3*numDofs); - b3AlignedObjectArray jacobian_angular; - jacobian_angular.resize(3*numDofs); - int jacSize = 0; - - btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); + 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 jacobian_linear; + b3AlignedObjectArray jacobian_angular; + btAlignedObjectArray q_current; + btAlignedObjectArray q_new; + btAlignedObjectArray lower_limit; + btAlignedObjectArray upper_limit; + btAlignedObjectArray joint_range; + btAlignedObjectArray rest_pose; + const int numDofs = bodyHandle->m_multiBody->getNumDofs(); + int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; + btInverseDynamics::vecx nu(numDofs+baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + + for (int i=0;i residualThreshold;i++) + { + BT_PROFILE("InverseKinematics1Step"); + if (ikHelperPtr && (endEffectorLinkIndexm_multiBody->getNumLinks())) + { + + + jacobian_linear.resize(3*numDofs); + jacobian_angular.resize(3*numDofs); + int jacSize = 0; + + btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); - - btAlignedObjectArray q_current; - q_current.resize(numDofs); - - - - if (tree && ((numDofs+ baseDofs) == tree->numDoFs())) - { - jacSize = jacobian_linear.size(); - // Set jacobian value + 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); - qdot[DofIndex+baseDofs] = 0; - nu[DofIndex+baseDofs] = 0; - DofIndex++; - } - } // Set the gravity to correspond to the world gravity - btInverseDynamics::vec3 id_grav(m_data->m_dynamicsWorld->getGravity()); - - if (-1 != tree->setGravityInWorldFrame(id_grav) && - -1 != tree->calculateInverseDynamics(q, qdot, nu, &joint_force)) - { - tree->calculateJacobians(q); - 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); - for (int i = 0; i < 3; ++i) - { - for (int j = 0; j < numDofs; ++j) - { - jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); - jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); - } - } - } - - - - btAlignedObjectArray 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)) - { - //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) - { - if (clientCmd.m_updateFlags & IK_SDLS) + + int DofIndex = 0; + for (int i = 0; i < bodyHandle->m_multiBody->getNumLinks(); ++i) { - ikMethod = IK2_VEL_SDLS_WITH_ORIENTATION; + 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++; + } + } // 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)) + { + tree->calculateJacobians(q); + 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); + + //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) + { + jacobian_linear[i*numDofs+j] = jac_t(i,(baseDofs+j)); + jacobian_angular[i*numDofs+j] = jac_r(i,(baseDofs+j)); + } + } + } + } + + + + + q_new.resize(numDofs); + 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) + { + 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_WITH_ORIENTATION; + if (clientCmd.m_updateFlags & IK_SDLS) + { + ikMethod = IK2_VEL_SDLS; + } + else + { + ikMethod = IK2_VEL_DLS;; + } } - } - 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 - { - if (clientCmd.m_updateFlags & IK_SDLS) - { - ikMethod = IK2_VEL_SDLS; - } - else - { - ikMethod = IK2_VEL_DLS;; - } - } - if (clientCmd.m_updateFlags& IK_HAS_NULL_SPACE_VELOCITY) - { - btAlignedObjectArray lower_limit; - btAlignedObjectArray upper_limit; - btAlignedObjectArray joint_range; - btAlignedObjectArray 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) + 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]; - joint_range[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointRange[i]; - rest_pose[i] = clientCmd.m_calculateInverseKinematicsArguments.m_restPose[i]; + + 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]; + } + { + 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; - btQuaternionDoubleData endEffectorWorldOrientation; + btVector3DoubleData endEffectorWorldPosition; + btQuaternionDoubleData endEffectorWorldOrientation; + + //get the position from the inverse dynamics (based on q) instead of endEffectorTransformWorld + btVector3 endEffectorPosWorldOrg = world_origin; + btQuaternion endEffectorOriWorldOrg; + world_rot.getRotation(endEffectorOriWorldOrg); + + btTransform endEffectorBaseCoord; + endEffectorBaseCoord.setOrigin(endEffectorPosWorldOrg); + endEffectorBaseCoord.setRotation(endEffectorOriWorldOrg); + + //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()); - 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()); + endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); + endEffectorBaseCoord.getRotation().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]); + + + + //diff + currentDiff = (endEffectorBaseCoord.getOrigin()-targetBaseCoord.getOrigin()).length(); - 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); + 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 - // coefficients differently for each joint. The larger - // the damping coefficient is, the less we rely on - // this joint to achieve the IK target. - btAlignedObjectArray joint_damping; - joint_damping.resize(numDofs,0.5); - if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) - { - for (int i = 0; i < numDofs; ++i) + // 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 joint_damping; + joint_damping.resize(numDofs,0.5); + if (clientCmd.m_updateFlags& IK_HAS_JOINT_DAMPING) { - joint_damping[i] = clientCmd.m_calculateInverseKinematicsArguments.m_jointDamping[i]; + 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}; + + { + 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;isetDampingCoeff(numDofs, &joint_damping[0]); - - double targetDampCoeff[6] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 }; - 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;im_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); - btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); + m_data->m_prevCanSleep = multiCol->m_multiBody->getCanSleep(); + multiCol->m_multiBody->setCanSleep(false); - btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); - //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' - //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; - p2p->setMaxAppliedImpulse(2*scaling); + btVector3 pivotInA = multiCol->m_multiBody->worldPosToLocal(multiCol->m_link, pickPos); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(p2p); - m_data->m_pickingMultiBodyPoint2Point =p2p; - } + btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(multiCol->m_multiBody,multiCol->m_link,0,pivotInA,pickPos); + //if you add too much energy to the system, causing high angular velocities, simulation 'explodes' + //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=10; + p2p->setMaxAppliedImpulse(2*scaling); + + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(p2p); + m_data->m_pickingMultiBodyPoint2Point =p2p; } } @@ -9812,4 +9904,4 @@ const btQuaternion& PhysicsServerCommandProcessor::getVRTeleportOrientation() co void PhysicsServerCommandProcessor::setVRTeleportOrientation(const btQuaternion& vrTeleportOrn) { gVRTeleportOrn = vrTeleportOrn; -} +} \ No newline at end of file