Hi everyone, I have following situation, I have set robots EEF and sent the robot to some desired pose, but when I check Pose of EEf link in RVIZ, after movement is successfully accomplished,Pose of given EEF (position and orientation) does not match given Pose.

From my understanding, after movement is successfully finished I should see that EEf link is wanted position, is that correct?

If somebody needs more data I can provide it later because now I am not on same computer.

Thanks in advance, any help would be appreciated.

EDIT 29.2.2020

Here are more details about my problem. I am using ros melodic with moveit, fanuc robot and gazebo 9. I have made small program in which I have generated one PoseStamped object, set some values for pose and orientation of the axis and sent robot to the that position using method "move_group.setJointValueTarget(pose);". When trajectory is finished I get message in terminal that it is succeeded successfully but when I check postion of EEF link in RVIZ it is not in the same pose in which I have sent robot.

Before in the program EEF is set to "fanuc1tool0" and reference Frame to "world":

  move_group.setPoseReferenceFrame("world");
  move_group.setPlanningTime(10);
  move_group.setNumPlanningAttempts(2);
  move_group.allowReplanning(true);
  //set EEF link
  move_group.setEndEffectorLink("fanuc1tool0");

Here is my code:

geometry_msgs::PoseStamped currentStamped_ToolPose1 = move_group.getCurrentPose("fanuc1tool0");
cout << currentStamped_ToolPose1;

geometry_msgs::PoseStamped pose;
pose.pose.position.x = 0.8;
pose.pose.position.y = 0.8;
pose.pose.position.z = 1.3;
pose.pose.orientation.x = 1;
pose.pose.orientation.y = 0;
pose.pose.orientation.z = 0;
pose.pose.orientation.w = 1;

bool success = move_group.setJointValueTarget(pose);

if (success)
{
  cout << "Wanted Pose: ";
  cout << pose;
  geometry_msgs::PoseStamped currPose = move_group.getCurrentPose("fanuc1tool0");
  cout << "Current fanuc1tool0 pose is:";
  cout << currPose;
}

move_group.move();

Here is text from terminal after execution is done:

[ INFO] [1583012945.141493385, 10.622000000]: RRTConnect: Starting planning with 1 states already in datastructure 
[INFO] [1583012945.141751197, 10.622000000]: RRTConnect: Starting planning with 1 states already in datastructure 
[INFO] [1583012946.718190689, 11.071000000]: RRTConnect: Created 4 states (2 start + 2 goal) 
[ INFO] [1583012949.177107989, 11.693000000]: RRTConnect: Created 4 states (2 start + 2 goal)
[ INFO] [1583012949.178592165, 11.694000000]: ParallelPlan::solve(): Solution found by one or more threads in 4.037833 seconds 
[ WARN] [1583012949.323915108, 11.716000000]: Solution path may slightly touch on an invalid region of the state space 
[ INFO] [1583012949.326566742, 11.716000000]: SimpleSetup: Path simplification took 0.147820 seconds and changed from 3 to 2 states
[ INFO] [1583012957.876403955, 14.222000000]: Controller /fanuc1/arm_controller successfully finished 
[ INFO] [1583012957.878555998, 14.222000000]: Completed trajectory execution with status SUCCEEDED ...

So as explained before when trajectory execution is successfully completed my robot is not in wanted position. When I check position of link "fanuc1tool0 " in Rviz then I can see that pose is different from wanted, these are values from Rviz:

pose.position.x = 0.800068
pose.position.y = 0.799962
pose.position.z = 1.299772
pose.orientation.x = 0.85345
pose.orientation.y = 0.27073
pose.orientation.z = 0.2299
pose.orientation.w = 0.3814

Seems like position part of the object is correct but orientation is not.

If it helps I tried to do it with several different positions and also tried to plan some random position in Rviz and copy values to the code and try to reach same position through the program but it was not successful.

I hope that my explanation will help to solve this problem.

EDIT 03.03.2020

Have you tried planning with setPoseTarget? Yes I have, initially I tried with setPoseTarget but I final position of the robot in the end differentiate even more than with setJointState.

Is the position you request feasible/reachable when you plan and execute the motion in Rviz? yes,position is reachable. I have planed some random valid position in Rviz and then I copied these values to my program so I can be sure that position is reachable.

If neither of these help, does the problem persist if you add another link "E2" to the current EEF link "E1", use either setEndEffectorLink or the moveit_config/setup assistant to set the EEF link to "E2", and then plan and execute the motion? If it is related to this, there might be a bug. I haven't tried this one because it requires to slightly change urdf file and I can do it for the weekend.

But this one helped me

Unrelated: Your quaternions should be normalized. It is also convenient to set them from RPY using TF Quaternions.

pose.pose.orientation = tf::quaternionTFToMsg( tf::createQuaternionFromRPY(roll, pitch, yaw), pose.pose.orientation);

I have created a quaternion and gave it a x,y,z,w rotation values, created tf message from it and then send robot to that pose using setJointState()and with it I get correct position. When I tried to use setPoseTarget() I got robot in almost same position as I wanted but still the robot position was slightly different from the wanted one. This is my code so far (notice change from PoseStamped to Pose):

geometry_msgs::Pose pose;
pose.position.x = -0.26;
pose.position.y = -0.32;
pose.position.z = 2.245;

tf::Quaternion poseQuat(-0.62, 0.22, 0.74, -0.11);
poseQuat.normalize();
geometry_msgs::Quaternion poseQuat_msg;
tf::quaternionTFToMsg(poseQuat, poseQuat_msg);
pose.orientation=poseQuat_msg;
cout << poseQuat_msg;

bool success = move_group.setJointValueTarget(pose);
//bool success = move_group.setPoseTarget(pose, "fanuc1tool0");

move_group.move();

pose.pose.orientation = tf::quaternionTFToMsg( tf::createQuaternionFromRPY(roll, pitch, yaw), pose.pose.orientation);