diff --git a/examples/SharedMemory/IKTrajectoryHelper.cpp b/examples/SharedMemory/IKTrajectoryHelper.cpp index 0c7851fcd..2e7418bbe 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.cpp +++ b/examples/SharedMemory/IKTrajectoryHelper.cpp @@ -46,7 +46,7 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], { 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); + Jacobian ikJacobian(useAngularPart, numQ, 1); ikJacobian.Reset(); @@ -185,6 +185,115 @@ bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3], return true; } + +bool IKTrajectoryHelper::computeIK2( + const double* endEffectorTargetPositions, + const double* endEffectorCurrentPositions, + int numEndEffectors, + const double* q_current, int numQ, + double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6]) +{ + bool useAngularPart = false;//for now (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, numEndEffectors); + + ikJacobian.Reset(); + + bool UseJacobianTargets1 = false; + + if (UseJacobianTargets1) + { + ikJacobian.SetJtargetActive(); + } + else + { + ikJacobian.SetJendActive(); + } + + VectorRn deltaC(numEndEffectors *3); + MatrixRmn completeJacobian(numEndEffectors*3, numQ); + + for (int ne = 0; ne < numEndEffectors; ne++) + { + VectorR3 targets; + targets.Set(endEffectorTargetPositions[ne*3+0], endEffectorTargetPositions[ne * 3 + 1], endEffectorTargetPositions[ne * 3 + 2]); + + // Set one end effector world position from Bullet + VectorRn deltaS(3); + for (int i = 0; i < 3; ++i) + { + deltaS.Set(i, dampIk[i] * (endEffectorTargetPositions[ne*3+i] - endEffectorCurrentPositions[ne*3+i])); + } + { + + for (int i = 0; i < 3; ++i) + { + deltaC.Set(ne*3+i, deltaS[i]); + for (int j = 0; j < numQ; ++j) + { + completeJacobian.Set(ne * 3 + i, j, linear_jacobians[(ne*3+i * numQ) + j]); + } + } + } + } + ikJacobian.SetDeltaS(deltaC); + ikJacobian.SetJendTrans(completeJacobian); + + // Calculate the change in theta values + switch (ikMethod) + { + case IK2_JACOB_TRANS: + ikJacobian.CalcDeltaThetasTranspose(); // Jacobian transpose method + break; + case IK2_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); + break; + case IK2_VEL_DLS_WITH_NULLSPACE: + case IK2_VEL_DLS_WITH_ORIENTATION_NULLSPACE: + assert(m_data->m_nullSpaceVelocity.GetLength() == numQ); + ikJacobian.CalcDeltaThetasDLSwithNullspace(m_data->m_nullSpaceVelocity); + break; + case IK2_DLS_SVD: + ikJacobian.CalcDeltaThetasDLSwithSVD(); + break; + case IK2_PURE_PSEUDO: + 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: + ikJacobian.ZeroDeltaThetas(); + break; + } + + // Use for velocity IK, update theta dot + //ikJacobian.UpdateThetaDot(); + + // Use for position IK, incrementally update theta + //ikJacobian.UpdateThetas(); + + // Apply the change in the theta values + //ikJacobian.UpdatedSClampValue(&targets); + + for (int i = 0; i < numQ; i++) + { + // Use for velocity IK + q_new[i] = ikJacobian.dTheta[i] + q_current[i]; + + // Use for position IK + //q_new[i] = m_data->m_ikNodes[i]->GetTheta(); + } + return true; +} + + bool IKTrajectoryHelper::computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose) { m_data->m_nullSpaceVelocity.SetLength(numQ); diff --git a/examples/SharedMemory/IKTrajectoryHelper.h b/examples/SharedMemory/IKTrajectoryHelper.h index bba324534..cdbe7a230 100644 --- a/examples/SharedMemory/IKTrajectoryHelper.h +++ b/examples/SharedMemory/IKTrajectoryHelper.h @@ -31,6 +31,13 @@ public: const double* q_old, 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 computeIK2( + const double* endEffectorTargetPositions, + const double* endEffectorCurrentPositions, + int numEndEffectors, + const double* q_current, int numQ, + double* q_new, int ikMethod, const double* linear_jacobians, const double dampIk[6]); + bool computeNullspaceVel(int numQ, const double* q_current, const double* lower_limit, const double* upper_limit, const double* joint_range, const double* rest_pose); bool setDampingCoeff(int numQ, const double* coeff); }; diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 24d690713..5f8544e58 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -4905,28 +4905,51 @@ B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMem b3Assert(command); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); command->m_updateFlags |= IK_HAS_TARGET_POSITION; - command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; - - 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_endEffectorLinkIndices[0] = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_targetPositions[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2]; + command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1; 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; } + +B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command); + b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); + command->m_updateFlags |= IK_HAS_TARGET_POSITION; + command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = numEndEffectorLinkIndices; + + for (int i = 0; i < numEndEffectorLinkIndices; i++) + { + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[i] = endEffectorIndices[i]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 0] = targetPositions[i * 3 + 0]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 1] = targetPositions[i * 3 + 1]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[i * 3 + 2] = targetPositions[i * 3 + 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; +} + B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[3], const double targetOrientation[4]) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION; - command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1; - 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_targetPositions[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; @@ -4940,11 +4963,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosWithNullSpaceVel(b3SharedMemor b3Assert(command); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_NULL_SPACE_VELOCITY; - command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1; - 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_targetPositions[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2]; for (int i = 0; i < numDof; ++i) { @@ -4961,11 +4985,12 @@ B3_SHARED_API void b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(b3SharedMe b3Assert(command); b3Assert(command->m_type == CMD_CALCULATE_INVERSE_KINEMATICS); command->m_updateFlags |= IK_HAS_TARGET_POSITION + IK_HAS_TARGET_ORIENTATION + IK_HAS_NULL_SPACE_VELOCITY; - command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0] = endEffectorLinkIndex; + command->m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices = 1; - 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_targetPositions[0] = targetPosition[0]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[1] = targetPosition[1]; + command->m_calculateInverseKinematicsArguments.m_targetPositions[2] = targetPosition[2]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[0] = targetOrientation[0]; command->m_calculateInverseKinematicsArguments.m_targetOrientation[1] = targetOrientation[1]; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 406b949ef..9f765cbd3 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -424,6 +424,7 @@ extern "C" ///compute the joint positions to move the end effector to a desired target using inverse kinematics B3_SHARED_API b3SharedMemoryCommandHandle b3CalculateInverseKinematicsCommandInit(b3PhysicsClientHandle physClient, int bodyUniqueId); B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPurePosition(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/]); + B3_SHARED_API void b3CalculateInverseKinematicsAddTargetsPurePosition(b3SharedMemoryCommandHandle commandHandle, int numEndEffectorLinkIndices, const int* endEffectorIndices, const double* targetPositions); B3_SHARED_API void b3CalculateInverseKinematicsAddTargetPositionWithOrientation(b3SharedMemoryCommandHandle commandHandle, int endEffectorLinkIndex, const double targetPosition[/*3*/], const double targetOrientation[/*4*/]); 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); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 985b9c64e..e75336b5a 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -10219,34 +10219,10 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con ikHelperPtr = tmpHelper; } - int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex; 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) - { - 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; @@ -10282,6 +10258,9 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con } btScalar currentDiff = 1e30f; + b3AlignedObjectArray endEffectorTargetWorldPositions; + b3AlignedObjectArray endEffectorTargetWorldOrientations; + b3AlignedObjectArray endEffectorCurrentWorldPositions; b3AlignedObjectArray jacobian_linear; b3AlignedObjectArray jacobian_angular; btAlignedObjectArray q_current; @@ -10294,13 +10273,64 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con int baseDofs = bodyHandle->m_multiBody->hasFixedBase() ? 0 : 6; btInverseDynamics::vecx nu(numDofs + baseDofs), qdot(numDofs + baseDofs), q(numDofs + baseDofs), joint_force(numDofs + baseDofs); + endEffectorTargetWorldPositions.resize(0); + endEffectorTargetWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3); + endEffectorTargetWorldOrientations.resize(0); + endEffectorTargetWorldOrientations.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 4); + + bool validEndEffectorLinkIndices = true; + + for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++) + { + + int endEffectorLinkIndex = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne]; + validEndEffectorLinkIndices = validEndEffectorLinkIndices && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks()); + + btVector3 targetPosWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne*3+0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetPositions[ne * 3 + 2]); + + btQuaternion targetOrnWorld(clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 0], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 1], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 2], + clientCmd.m_calculateInverseKinematicsArguments.m_targetOrientation[ne * 4 + 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; + } + + btVector3DoubleData targetPosBaseCoord; + btQuaternionDoubleData targetOrnBaseCoord; + targetBaseCoord.getOrigin().serializeDouble(targetPosBaseCoord); + targetBaseCoord.getRotation().serializeDouble(targetOrnBaseCoord); + + endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[0]); + endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[1]); + endEffectorTargetWorldPositions.push_back(targetPosBaseCoord.m_floats[2]); + + endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[0]); + endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[1]); + endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[2]); + endEffectorTargetWorldOrientations.push_back(targetOrnBaseCoord.m_floats[3]); + } for (int i = 0; i < numIterations && currentDiff > residualThreshold; i++) { BT_PROFILE("InverseKinematics1Step"); - if (ikHelperPtr && (endEffectorLinkIndex < bodyHandle->m_multiBody->getNumLinks())) + if (ikHelperPtr && validEndEffectorLinkIndices) { - jacobian_linear.resize(3 * numDofs); - jacobian_angular.resize(3 * numDofs); + jacobian_linear.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices*3 * numDofs); + jacobian_angular.resize(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices*3 * numDofs); int jacSize = 0; btInverseDynamics::MultiBodyTree* tree = m_data->findOrCreateTree(bodyHandle->m_multiBody); @@ -10321,7 +10351,6 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con 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; @@ -10340,21 +10369,44 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con 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) + currentDiff = 0; + + endEffectorCurrentWorldPositions.resize(0); + endEffectorCurrentWorldPositions.reserve(clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices * 3); + + for (int ne = 0; ne < clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices; ne++) { - for (int j = 0; j < numDofs; ++j) + + int endEffectorLinkIndex2 = clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[ne]; + + // Note that inverse dynamics uses zero-based indexing of bodies, not starting from -1 for the base link. + tree->getBodyJacobianTrans(endEffectorLinkIndex2 + 1, &jac_t); + tree->getBodyJacobianRot(endEffectorLinkIndex2 + 1, &jac_r); + + //calculatePositionKinematics is already done inside calculateInverseDynamics + + tree->getBodyOrigin(endEffectorLinkIndex2 + 1, &world_origin); + tree->getBodyTransform(endEffectorLinkIndex2 + 1, &world_rot); + + for (int i = 0; i < 3; ++i) { - jacobian_linear[i * numDofs + j] = jac_t(i, (baseDofs + j)); - jacobian_angular[i * numDofs + j] = jac_r(i, (baseDofs + j)); + for (int j = 0; j < numDofs; ++j) + { + jacobian_linear[(ne*3+i) * numDofs + j] = jac_t(i, (baseDofs + j)); + jacobian_angular[(ne * 3 + i) * numDofs + j] = jac_r(i, (baseDofs + j)); + } } + + endEffectorCurrentWorldPositions.push_back(world_origin[0]); + endEffectorCurrentWorldPositions.push_back(world_origin[1]); + endEffectorCurrentWorldPositions.push_back(world_origin[2]); + + btInverseDynamics::vec3 targetPos(btVector3(endEffectorTargetWorldPositions[ne*3+0], + endEffectorTargetWorldPositions[ne * 3 + 1], + endEffectorTargetWorldPositions[ne * 3 + 2])); + //diff + currentDiff = btMax(currentDiff, (world_origin - targetPos).length()); + } } } @@ -10443,13 +10495,8 @@ bool PhysicsServerCommandProcessor::processCalculateInverseKinematicsCommand(con endEffectorBaseCoord.getOrigin().serializeDouble(endEffectorWorldPosition); endEffectorBaseCoord.getRotation().serializeDouble(endEffectorWorldOrientation); - //diff - currentDiff = (endEffectorBaseCoord.getOrigin() - targetBaseCoord.getOrigin()).length(); - 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 @@ -10469,25 +10516,48 @@ 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}; + bool performedIK = false; + if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices==1) { + BT_PROFILE("computeIK"); - ikHelperPtr->computeIK(targetPosBaseCoord.m_floats, targetOrnBaseCoord.m_floats, + ikHelperPtr->computeIK(&endEffectorTargetWorldPositions[0], + &endEffectorTargetWorldOrientations[0], endEffectorWorldPosition.m_floats, endEffectorWorldOrientation.m_floats, &q_current[0], - numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndex, + numDofs, clientCmd.m_calculateInverseKinematicsArguments.m_endEffectorLinkIndices[0], &q_new[0], ikMethod, &jacobian_linear[0], &jacobian_angular[0], jacSize * 2, targetDampCoeff); + performedIK = true; } - serverCmd.m_inverseKinematicsResultArgs.m_bodyUniqueId = clientCmd.m_calculateInverseDynamicsArguments.m_bodyUniqueId; - for (int i = 0; i < numDofs; i++) + else { - serverCmd.m_inverseKinematicsResultArgs.m_jointPositions[i] = q_new[i]; + + if (clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices>1) + { + ikHelperPtr->computeIK2(&endEffectorTargetWorldPositions[0], + &endEffectorCurrentWorldPositions[0], + clientCmd.m_calculateInverseKinematicsArguments.m_numEndEffectorLinkIndices, + //endEffectorWorldOrientation.m_floats, + &q_current[0], + numDofs, + &q_new[0], ikMethod, &jacobian_linear[0], targetDampCoeff); + performedIK = true; + } } - serverCmd.m_inverseKinematicsResultArgs.m_dofCount = numDofs; - serverCmd.m_type = CMD_CALCULATE_INVERSE_KINEMATICS_COMPLETED; - for (int i = 0; i < numDofs; i++) + if (performedIK) { - startingPositions[i] = q_new[i]; + 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; + for (int i = 0; i < numDofs; i++) + { + startingPositions[i] = q_new[i]; + } } } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index ed0bfaca7..2691790ef 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -30,7 +30,7 @@ typedef unsigned long long int smUint64_t; #endif #define SHARED_MEMORY_SERVER_TEST_C -#define MAX_DEGREE_OF_FREEDOM 128 +#define MAX_DEGREE_OF_FREEDOM 128 #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_SDF_FILENAME_LENGTH 1024 @@ -740,9 +740,10 @@ struct CalculateInverseKinematicsArgs { int m_bodyUniqueId; // double m_jointPositionsQ[MAX_DEGREE_OF_FREEDOM]; - double m_targetPosition[3]; + double m_targetPositions[MAX_DEGREE_OF_FREEDOM*3]; + int m_numEndEffectorLinkIndices; double m_targetOrientation[4]; //orientation represented as quaternion, x,y,z,w - int m_endEffectorLinkIndex; + int m_endEffectorLinkIndices[MAX_DEGREE_OF_FREEDOM]; double m_lowerLimit[MAX_DEGREE_OF_FREEDOM]; double m_upperLimit[MAX_DEGREE_OF_FREEDOM]; double m_jointRange[MAX_DEGREE_OF_FREEDOM]; diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp index 3dfff6f7b..f91a5e397 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.cpp +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.cpp @@ -84,10 +84,10 @@ Jacobian::Jacobian(Tree* tree) Reset(); } -Jacobian::Jacobian(bool useAngularJacobian, int nDof) +Jacobian::Jacobian(bool useAngularJacobian, int nDof, int numEndEffectors) { m_tree = 0; - m_nEffector = 1; + m_nEffector = numEndEffectors; if (useAngularJacobian) { diff --git a/examples/ThirdPartyLibs/BussIK/Jacobian.h b/examples/ThirdPartyLibs/BussIK/Jacobian.h index 6e11513bc..d7dc81bcd 100644 --- a/examples/ThirdPartyLibs/BussIK/Jacobian.h +++ b/examples/ThirdPartyLibs/BussIK/Jacobian.h @@ -57,7 +57,7 @@ class Jacobian { public: Jacobian(Tree*); - Jacobian(bool useAngularJacobian, int nDof); + Jacobian(bool useAngularJacobian, int nDof, int numEndEffectors); void ComputeJacobian(VectorR3* targets); const MatrixRmn& ActiveJacobian() const { return *Jactive; } diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index a86e3f3c9..00d722ee6 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -51,7 +51,7 @@ #endif static PyObject* SpamError; - +#define B3_MAX_NUM_END_EFFECTORS 128 #define MAX_PHYSICS_CLIENTS 1024 static b3PhysicsClientHandle sPhysicsClients1[MAX_PHYSICS_CLIENTS] = {0}; static int sPhysicsClientsGUI[MAX_PHYSICS_CLIENTS] = {0}; @@ -10009,6 +10009,253 @@ static PyObject* pybullet_calculateInverseKinematics(PyObject* self, return Py_None; } + +///Inverse Kinematics binding +static PyObject* pybullet_calculateInverseKinematics2(PyObject* self, + PyObject* args, PyObject* keywds) + +{ + int bodyUniqueId; + int endEffectorLinkIndex=-1; + + PyObject* targetPosObj = 0; + //PyObject* targetOrnObj = 0; + + int solver = 0; // the default IK solver is DLS + int physicsClientId = 0; + b3PhysicsClientHandle sm = 0; + PyObject* endEffectorLinkIndicesObj = 0; + PyObject* lowerLimitsObj = 0; + PyObject* upperLimitsObj = 0; + PyObject* jointRangesObj = 0; + PyObject* restPosesObj = 0; + PyObject* jointDampingObj = 0; + PyObject* currentPositionsObj = 0; + int maxNumIterations = -1; + double residualThreshold = -1; + + static char* kwlist[] = { "bodyUniqueId", "endEffectorLinkIndices", "targetPositions", "lowerLimits", "upperLimits", "jointRanges", "restPoses", "jointDamping", "solver", "currentPositions", "maxNumIterations", "residualThreshold", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "iOO|OOOOOOiOidi", kwlist, &bodyUniqueId, &endEffectorLinkIndicesObj, &targetPosObj, &lowerLimitsObj, &upperLimitsObj, &jointRangesObj, &restPosesObj, &jointDampingObj, &solver, ¤tPositionsObj, &maxNumIterations, &residualThreshold, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + { + int numEndEffectorPositions = extractVertices(targetPosObj, 0, B3_MAX_NUM_END_EFFECTORS); + int numIndices = extractIndices(endEffectorLinkIndicesObj, 0, B3_MAX_NUM_END_EFFECTORS); + double* positions = numEndEffectorPositions ? malloc(numEndEffectorPositions * 3 * sizeof(double)) : 0; + int* indices = numIndices ? malloc(numIndices * sizeof(int)) : 0; + + numEndEffectorPositions = extractVertices(targetPosObj, positions, B3_MAX_NUM_VERTICES); + + if (endEffectorLinkIndicesObj) + { + numIndices = extractIndices(endEffectorLinkIndicesObj, indices, B3_MAX_NUM_INDICES); + } + + double pos[3] = { 0, 0, 0 }; + double ori[4] = { 0, 0, 0, 1 }; + int hasPos = numEndEffectorPositions > 0; + int hasOrn = 0;// pybullet_internalSetVector4d(targetOrnObj, ori); + + int szLowerLimits = lowerLimitsObj ? PySequence_Size(lowerLimitsObj) : 0; + int szUpperLimits = upperLimitsObj ? PySequence_Size(upperLimitsObj) : 0; + int szJointRanges = jointRangesObj ? PySequence_Size(jointRangesObj) : 0; + int szRestPoses = restPosesObj ? PySequence_Size(restPosesObj) : 0; + int szJointDamping = jointDampingObj ? PySequence_Size(jointDampingObj) : 0; + + int szCurrentPositions = currentPositionsObj ? PySequence_Size(currentPositionsObj) : 0; + + int numJoints = b3GetNumJoints(sm, bodyUniqueId); + int dofCount = b3ComputeDofCount(sm, bodyUniqueId); + + int hasNullSpace = 0; + int hasJointDamping = 0; + int hasCurrentPositions = 0; + double* lowerLimits = 0; + double* upperLimits = 0; + double* jointRanges = 0; + double* restPoses = 0; + double* jointDamping = 0; + double* currentPositions = 0; + + if (dofCount && (szLowerLimits == dofCount) && (szUpperLimits == dofCount) && + (szJointRanges == dofCount) && (szRestPoses == dofCount)) + { + int szInBytes = sizeof(double) * dofCount; + int i; + lowerLimits = (double*)malloc(szInBytes); + upperLimits = (double*)malloc(szInBytes); + jointRanges = (double*)malloc(szInBytes); + restPoses = (double*)malloc(szInBytes); + + for (i = 0; i < dofCount; i++) + { + lowerLimits[i] = pybullet_internalGetFloatFromSequence(lowerLimitsObj, i); + upperLimits[i] = pybullet_internalGetFloatFromSequence(upperLimitsObj, i); + jointRanges[i] = pybullet_internalGetFloatFromSequence(jointRangesObj, i); + restPoses[i] = pybullet_internalGetFloatFromSequence(restPosesObj, i); + } + hasNullSpace = 1; + } + + if (szCurrentPositions > 0) + { + if (szCurrentPositions != dofCount) + { + PyErr_SetString(SpamError, + "calculateInverseKinematics the size of input current positions needs to be equal to the number of degrees of freedom."); + free(lowerLimits); + free(upperLimits); + free(jointRanges); + free(restPoses); + return NULL; + } + else + { + int szInBytes = sizeof(double) * szCurrentPositions; + int i; + currentPositions = (double*)malloc(szInBytes); + for (i = 0; i < szCurrentPositions; i++) + { + currentPositions[i] = pybullet_internalGetFloatFromSequence(currentPositionsObj, i); + } + hasCurrentPositions = 1; + } + } + + if (szJointDamping > 0) + { + if (szJointDamping < dofCount) + { + printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, not using joint damping."); + } + else + { + int szInBytes = sizeof(double) * szJointDamping; + int i; + //if (szJointDamping != dofCount) + //{ + // printf("calculateInverseKinematics: the size of input joint damping values should be equal to the number of degrees of freedom, ignoring the additonal values."); + //} + jointDamping = (double*)malloc(szInBytes); + for (i = 0; i < szJointDamping; i++) + { + jointDamping[i] = pybullet_internalGetFloatFromSequence(jointDampingObj, i); + } + hasJointDamping = 1; + } + } + + if (hasPos) + { + b3SharedMemoryStatusHandle statusHandle; + int numPos = 0; + int resultBodyIndex; + int result; + + b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(sm, bodyUniqueId); + b3CalculateInverseKinematicsSelectSolver(command, solver); + + if (hasCurrentPositions) + { + b3CalculateInverseKinematicsSetCurrentPositions(command, dofCount, currentPositions); + } + if (maxNumIterations > 0) + { + b3CalculateInverseKinematicsSetMaxNumIterations(command, maxNumIterations); + } + if (residualThreshold >= 0) + { + b3CalculateInverseKinematicsSetResidualThreshold(command, residualThreshold); + } + + if (hasNullSpace) + { + if (hasOrn) + { + b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, ori, lowerLimits, upperLimits, jointRanges, restPoses); + } + else + { + b3CalculateInverseKinematicsPosWithNullSpaceVel(command, dofCount, endEffectorLinkIndex, pos, lowerLimits, upperLimits, jointRanges, restPoses); + } + } + else + { + if (hasOrn) + { + b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, endEffectorLinkIndex, pos, ori); + } + else + { + //b3CalculateInverseKinematicsAddTargetPurePosition(command, endEffectorLinkIndex, pos); + b3CalculateInverseKinematicsAddTargetsPurePosition(command, numEndEffectorPositions, indices, positions); + } + } + + if (hasJointDamping) + { + b3CalculateInverseKinematicsSetJointDamping(command, dofCount, jointDamping); + } + free(currentPositions); + free(jointDamping); + + free(lowerLimits); + free(upperLimits); + free(jointRanges); + free(restPoses); + + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); + + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &resultBodyIndex, + &numPos, + 0); + if (result && numPos) + { + int i; + PyObject* pylist; + double* ikOutPutJointPos = (double*)malloc(numPos * sizeof(double)); + result = b3GetStatusInverseKinematicsJointPositions(statusHandle, + &resultBodyIndex, + &numPos, + ikOutPutJointPos); + pylist = PyTuple_New(numPos); + for (i = 0; i < numPos; i++) + { + PyTuple_SetItem(pylist, i, + PyFloat_FromDouble(ikOutPutJointPos[i])); + } + + free(ikOutPutJointPos); + return pylist; + } + else + { + PyErr_SetString(SpamError, + "Error in calculateInverseKinematics"); + return NULL; + } + } + else + { + PyErr_SetString(SpamError, + "calculateInverseKinematics couldn't extract position vector3"); + return NULL; + } + } + + Py_INCREF(Py_None); + return Py_None; +} + + /// Given an object id, joint positions, joint velocities and joint /// accelerations, /// compute the joint forces using Inverse Dynamics @@ -10878,6 +11125,12 @@ static PyMethodDef SpamMethods[] = { "current joint positions and target position" " for the end effector," "compute the inverse kinematics and return the new joint state"}, + { "calculateInverseKinematics2", (PyCFunction)pybullet_calculateInverseKinematics2, + METH_VARARGS | METH_KEYWORDS, + "Inverse Kinematics bindings: Given an object id, " + "current joint positions and target positions" + " for the end effectors," + "compute the inverse kinematics and return the new joint state" }, {"getVREvents", (PyCFunction)pybullet_getVREvents, METH_VARARGS | METH_KEYWORDS, "Get Virtual Reality events, for example to track VR controllers position/buttons"},