Problems with FKSolver if the transformation matrices for the kinematic structure is constructed by DH_CRAIG1989

[easUser]

Hello community,

a few days ago i found your repo and tested the FW and IK solver with the iiwa model you've provided with the c++ code - everything seems to work.

After i tried to test the solvers with some other roboters (f.e. kuka kr15-2, ...) i recognized that the initial positions of the tcp weren't correct. So, in short, if i describe the kinematics with dh parameter by craig the fw kinematics solver doesn't work as expected.

I provided you an example below - i'am not sure if i am missunderstanding something ...

I took an example robot from craig's "introduction to robotics" book.

The dh parameters are shown in the picture below (Notice: The final offset for the end effector isn't considered; All joints are rotational; the z-axis are pointing out of the paper).

In the following code you will see how the robot kinematics are described with dh parameters. After i initialized the FW-KIN Solver i set initial joint values and try to solve the forward kinematics.

	using namespace KDL;
	using namespace std;

	double PI_Const = 3.14159265359;

	double l1 = 1;
	double l2 = 2;

	Chain chain1;

	//joint 1
	chain1.addSegment(KDL::Segment(Joint(Joint::RotZ),
		Frame::DH_Craig1989(0.0, 0.0, 0.0, 0.0)));

	//joint 2
	chain1.addSegment(KDL::Segment(Joint(Joint::RotZ),
		Frame::DH_Craig1989(l1, 0, 0.0, 0.0)));

	//joint 3
	chain1.addSegment(KDL::Segment(Joint(Joint::RotZ),
		Frame::DH_Craig1989(l2, 0.0, 0.0, 0.0)));

	int n = chain1.getNrOfJoints();

	ChainFkSolverPos_recursive fwdkin(chain1);

	JntArray q_init(n);
	JntArray q_sol(n);

// 1. attempt: Sets a position for the three joints in rad
	q_init(0) = 0;
	q_init(1) = 0;
	q_init(2) = 0;

	Frame pos_goal, pos_reached;
	fwdkin.JntToCart(q_init, pos_goal);
	**// TCP - Analytic calculated (target) pos: TCP_X = 2 + 1 = 3
	// TCP - CurrentPos: TCP_X = 2 + 1 = 3 -> CORRECT**

// 2. attempt: Sets a position for the three joints in rad
	q_init(0) = PI_Const * 0.5;
	q_init(1) = 0;
	q_init(2) = 0;

	fwdkin.JntToCart(q_init, pos_goal);
	**// TCP - Analytic calculated (target) pos: TCP_X = 0; TCP_Y = 2 + 1 = 3
	// TCP - CurrentPos: TCP_X = 0; TCP_Y = 3 -> CORRECT**

// 3. attepmt: Sets a position for the three joints in rad
	q_init(0) = PI_Const * 0.5;
	q_init(1) = -PI_Const * 0.5;
	q_init(2) = 0;

	fwdkin.JntToCart(q_init, pos_goal);
	**// TCP - Analytic calculated (target) pos: TCP_X = 1; TCP_Y = 2
	// TCP - CurrentPos: TCP_X = 3; TCP_Y = 0 -> INCORRECT**

Why is the last tcp position not correct?

Thanks in advance, Jimmy