Method computeIK

examples/SharedMemory/IKTrajectoryHelper.cpp:40–187

}

bool IKTrajectoryHelper::computeIK(const double endEffectorTargetPosition[3],
								   const double endEffectorTargetOrientation[4],
								   const double endEffectorWorldPosition[3],
								   const double endEffectorWorldOrientation[4],
								   const double* q_current, int numQ, int endEffectorIndex,
								   double* q_new, int ikMethod, const double* linear_jacobian, const double* angular_jacobian, int jacobian_size, const double dampIk[6])
{
	MatrixRmn AugMat;
	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, 1);

	ikJacobian.Reset();

	bool UseJacobianTargets1 = false;

	if (UseJacobianTargets1)
	{
		ikJacobian.SetJtargetActive();
	}
	else
	{
		ikJacobian.SetJendActive();
	}
	VectorR3 targets;
	targets.Set(endEffectorTargetPosition[0], endEffectorTargetPosition[1], endEffectorTargetPosition[2]);
	ikJacobian.ComputeJacobian(&targets);  // Set up Jacobian and deltaS vectors

	// Set one end effector world position from Bullet
	VectorRn deltaS(3);
	for (int i = 0; i < 3; ++i)
	{
		deltaS.Set(i, dampIk[i] * (endEffectorTargetPosition[i] - endEffectorWorldPosition[i]));
	}

	// Set one end effector world orientation from Bullet
	VectorRn deltaR(3);
	if (useAngularPart)
	{
		btQuaternion startQ(endEffectorWorldOrientation[0], endEffectorWorldOrientation[1], endEffectorWorldOrientation[2], endEffectorWorldOrientation[3]);
		btQuaternion endQ(endEffectorTargetOrientation[0], endEffectorTargetOrientation[1], endEffectorTargetOrientation[2], endEffectorTargetOrientation[3]);
		btQuaternion deltaQ = endQ * startQ.inverse();
		float angle = deltaQ.getAngle();
		btVector3 axis = deltaQ.getAxis();
		if (angle > PI)
		{
			angle -= 2.0 * PI;
		}
		else if (angle < -PI)
		{
			angle += 2.0 * PI;
		}
		float angleDot = angle;
		btVector3 angularVel = angleDot * axis.normalize();
		for (int i = 0; i < 3; ++i)
		{
			deltaR.Set(i, dampIk[i + 3] * angularVel[i]);
		}