Method computeIK2

examples/SharedMemory/IKTrajectoryHelper.cpp:190–296

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])
{
	MatrixRmn AugMat;
	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: