ik for UR donation

[TheURMaxer]

About 2 weeks ago, I posted a problem regarding the IK of UR robots. The issue related to recovering rotation information and the IK solution in UR_kin.cpp. I mentioned that the original paper, Analytic Inverse Kinematics for the Universal Robots UR-5/UR-10 Arms by Kelsey P. Hawkins, December 7, 2013, provided the "correct" forward kinematics fomulae, but only a sketch of the inverse kinematics. Keeping the story short, I made modifications to the UR_kin.cpp to provide the matching IK to the paper by Hawkins. I hereby post my modifications in the hope they are useful to anyone else.

Regards,
Max van Rooij

`UR5_inverse_kin_new(const double T[16], double q_sols[8*6], double q6_des)
{
double const d1 = 0.08920f; //Denavit-Hartenberg parameters for the UR5 in [m]
double const d4 = 0.10900f;
double const d5 = 0.09300f;
double const d6 = 0.08200f;
double const a2 = -0.42500f;
double const a3 = -0.39243f;

int num_sols = 0;

////////////////////////////// shoulder rotate joint (q1) //////////////////////////////
double q1[2];
{
	double A = T[3] - d6*T[2];
	double B = T[7] - d6*T[6];
	double R = A*A + B*B;


	double arccos = arc_cosine(d4 / square_root(R)) ;
	double arctan = arc_tangent2(B, A);
	double pos = arctan + arccos;
	double neg = arctan - arccos;
	if(std::fabs(pos) < zero_thr)
	{
		pos = 0.0;
	}
	if(std::fabs(neg) < zero_thr)
	{
		neg = 0.0;
	}
	q1[0] = pos + half_pi;
	q1[1] = neg + half_pi;

}
////////////////////////////////////////////////////////////////////////////////

////////////////////////////// wrist 2 joint (q5) //////////////////////////////
double q5[2][2];
{
	for(int i = 0;i < 2;++i)
	{
		double numer = (T[3]*std::sin(q1[i]) - T[7]*std::cos(q1[i])-d4);
		double arccos = arc_cosine(numer/d6);
		q5[i][0] = +arccos;
		q5[i][1] = -arccos;
	}
}
////////////////////////////////////////////////////////////////////////////////

{
	for (int i = 0;i < 2;++i)
	{
		for (int j = 0;j < 2;++j)
		{
			double c1 = std::cos(q1[i]), s1 = std::sin(q1[i]);
			double c5 = std::cos(q5[i][j]), s5 = std::sin(q5[i][j]);
			double q6;
			////////////////////////////// wrist 3 joint (q6) //////////////////////////////
			if (std::fabs(s5) < zero_thr)
			{
				q6 = q6_des;
			}
			else
			{
				q6 = arc_tangent2(sign_f(s5)*-(T[1]*s1 - T[5]*c1),sign_f(s5)*(T[0]*s1 - T[4]*c1));
				if (std::fabs(q6) < zero_thr)
				{
					q6 = 0.0;
				}
			}
			////////////////////////////////////////////////////////////////////////////////

			double q2[2], q3[2], q4[2];
			///////////////////////////// RRR joints (q2,q3,q4) ////////////////////////////
			double c6 = std::cos(q6), s6 = std::sin(q6);

			double x04x = -s5*(T[2]*c1 + T[6]*s1) - c5*(s6*(T[1]*c1 + T[5]*s1) - c6*(T[0]*c1 + T[4]*s1));
			double x04y = c5*(T[8]*c6 - T[9]*s6) - T[10]*s5;
			double p13x = d5*(s6*(T[0]*c1 + T[4]*s1) + c6*(T[1]*c1 + T[5]*s1)) - d6*(T[2]*c1 + T[6]*s1) + T[3]*c1 + T[7]*s1;
			double p13y = T[11] - d1 - d6*T[10] + d5*(T[9]*c6 + T[8]*s6);


			double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
			double arccos = arc_cosine(c3);
			q3[0] = +arccos;
			q3[1] = -arccos;
			double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
			double s3 = std::sin(arccos);
			double A = (a2 + a3*c3), B = a3*s3;
			q2[0] = arc_tangent2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
			q2[1] = arc_tangent2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
			double c23_0 = std::cos(q2[0]+q3[0]);
			double s23_0 = std::sin(q2[0]+q3[0]);
			double c23_1 = std::cos(q2[1]+q3[1]);
			double s23_1 = std::sin(q2[1]+q3[1]);
			q4[0] = arc_tangent2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
			q4[1] = arc_tangent2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
			////////////////////////////////////////////////////////////////////////////////
			for(int k=0;k<2;k++)
			{
				if(std::fabs(q2[k]) < zero_thr)
				{
					q2[k] = 0.0;
				}
				
				if(std::fabs(q4[k]) < zero_thr)
				{
					q4[k] = 0.0;
				}

				q_sols[num_sols*6+0] = q1[i];    q_sols[num_sols*6+1] = q2[k];
				q_sols[num_sols*6+2] = q3[k];    q_sols[num_sols*6+3] = q4[k];
				q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
				++num_sols;
			}
		}
	}
}
return num_sols;

}`

[atenpas]

You should create a pull request for such things because this can be beneficial to other users.

What does your forward function look like?

[shaun-edwards]

+1 to a pull request.

[atenpas]

The IK solver in UR_kin.cpp works fine.

[gavanderhoorn]

@atenpas: could you elaborate your comment a bit? Are you saying that you don't encounter the problems that @TheURMaxer describes / described in #299?

[atenpas]

@gavanderhoorn: As I've stated in #299: "I've been using the ur_kin module without any problems on our UR5 robot (I use this directly without moveit though)". I also emailed with the developer of that module, and there does not seem to be a real issue here.

[gavanderhoorn]

Ok. Tnx.

[a-price]

I believe the kinematics core functionality is fine; the problem happens in the MoveIt wrapper, and seems to be the root of the joint_limited work-around as well. Take a look at ur_moveit_plugin.cpp, L658 and following: there's a block of code that tries to account for the fact that the joint angles cover more than one rotation. The issue is that, if each joint has two periodic solutions (q[i][j] and q[i][j]+2pi), then each kinematic solution has 2^6=64 joint-space solutions. So, for a typical IK request with 8 kinematically distinct configurations (elbow up and down, wrist up and down, shoulder forward and back, so 2^3) there are 512 total solutions. Since the current function only considers one permutation of the 64, the likelihood of the IK returning the true nearest solution is pretty small, leading to the jumping behavior seen in the MoveIt RViz window.

To test this, I wrote a function that recursively enumerates all 64 possible joint configurations for a given solution, which has solved the erratic behavior in RViz on my machine. Right now it uses the std::vector + std::sort technique that's currently implemented, but it seems like it might be more efficient to return a priority queue, or maybe just the best result. I'll open a pull request for further discussion.

[gavanderhoorn]

Marking this as an issue for wrid19 as it'd be nice to get a final answer to this in addition to @atenpas' answer.

Note: this issue is also part of the Issues in IK project.