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

[easUser]

trafficstars

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

[MatthijsBurgh]

@smits @meyerj could you handle this one?

[smits]

I'll take a look when I find some time.

[WangZP10969]

@easUser I think your problem is because you used DH1989 directly to generate the model KDL’s joint is based on DH1955, that is to say, the rotation of the joint is placed at the top of the segment jointposenew = rotz×jointpose But for DH1989, the rotation of the joint is jointposenew = jointpose×rotz

There is a way to solve this problem, which is also shown in the sample program That is to put the rotation of the joint on the next segment, that is, when the model is generated, the first joint is set to none, and then the second joint is set to rotz, and after the last joint , another unit transformation joint is added and set to rotz In this way, all rotations will be pushed back by a joint, and you may get the correct result

//joint 1
chain1.addSegment(KDL::Segment(Joint(Joint::None),
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)));

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

https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/models/kukaLWR_DHnew.cpp

[superseppl]

I have the same issue on the UR5 robot. When i parse the URDF xacro file the calculation of the end effector postion with the fk_solver seems to be incorrect in the zero configuration.

Current:

Desired: from https://bjpcjp.github.io/pdfs/robotics/MR_ch04_fwd_kinematics.pdf

[MatthijsBurgh]

@superseppl Adding more pictures doesn't explain your case.

The picture of rviz, shows indeed different frame orientation compared to the other two images. But how is this library involved in the defined frame orientations?

[superseppl]

@MatthijsBurgh With the library I read my URDF xacro file of the UR5 (https://github.com/ros-industrial/universal_robot/tree/melodic-devel/ur_description/urdf), build the kinematic chain and then calculate the forward kinematics of my end-effector wrt. the world frame.

So it might be either that the issue is because of the base_frame is different as the base_link_frame, but the calculation should from ee to base_link right? Or the calculation with the urdf file has the same issue as above, i.e. the ur_description is made according to a different DH standard and parsing it leads to a different result.

[MatthijsBurgh]

Just start very simple.

Make sure you publish the joint states. Run a robot_state_publisher and check the TF frames and robot model in rviz. Please share the result. Is that what you expect?

[superseppl]

@MatthijsBurgh after some different trials I stumbled accross ROS REP-199, which defines coordinate frames for serial industrial manipulators and might cause the initial confusion. Is there a way to check if the FK solution is right? We use the Matlab Robotics System Toolbox (RST) and RBDL to verify the solution and we see that the orientation of the KDL solution is somehow inverted, i.e. we need to transpose the rotation matrix to have the same results.

[MatthijsBurgh]

Start by my setup mentioned in https://github.com/orocos/orocos_kinematics_dynamics/issues/268#issuecomment-1419048035

This really the most simple check.

[superseppl]

@MatthijsBurgh Alright! Here is the setup of the UR5 in zero configuration using the URDF according to REP-199. The slightly thicker frame at the flange is the forward kinematic solution (flange-base_link_inertia) calculated with KDL.

[MatthijsBurgh]

So the tf frames do match the REP-199? (Remember REP-199 is not an official REP)

But the KDL FK solution does have the correct position, but incorrect orientation?

Can you check the frames in between as well?

[superseppl]

Yes, it has the correct position but incorrect orientation. In the zero configuration, where all joints are 0, the orientation should be [ 0.7071068, 0, 0, 0.7071068 ] in Quaternion [x, y, z, w] according to the Tool but it is [0, 0, 0.707, 0.707]. Which looks like (the one more to left is the desired solution): for the flange:

for the wrist_3_link:

[MatthijsBurgh]

When you flip wrist_3_link to the correct orientation, will the flange also be correct?

[superseppl]

Found the issue, we further used Eigen and I was not aware that KDL::Rotation is row-major, while default Eigen::Matrix is column-major.

[MatthijsBurgh]

@superseppl so no bugs in KDL? So we can close this one?

[superseppl]

Yes, no bugs, thanks for the support :+1: