Ask Your Question

How to check if the robot is in correct position (Pose)?

asked 2020-02-11 08:14:46 -0600

lbajlo gravatar image

updated 2020-03-03 16:08:18 -0600

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":

  //set EEF link

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;


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 ...
edit retag flag offensive close merge delete


You should definitely provide more data for this to be solved, or explain how you solved it yourself.

fvd gravatar image fvd  ( 2020-02-29 11:51:35 -0600 )edit

Hi @fvd Check edited question, thanks in advance

lbajlo gravatar image lbajlo  ( 2020-02-29 16:17:39 -0600 )edit

1 Answer

Sort by ยป oldest newest most voted

answered 2020-03-01 04:51:57 -0600

fvd gravatar image

updated 2020-03-01 05:48:47 -0600

gvdhoorn gravatar image

Some comments:

  1. Have you tried planning with setPoseTarget?
  2. Is the position you request feasible/reachable when you plan and execute the motion in Rviz?
  3. 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.

Please update your post with the results if this doesn't answer your question.

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);
edit flag offensive delete link more


Seeing as the OP is using setJointValueTarget(..), but then supplies it with a PoseStamped, could this be a problem with the IK solver? According to the docs, setJointValueTarget(..):

Set[s] the joint state goal for a particular joint by computing IK.

If IK does something unexpected, this could lead to the robot being directed to the "wrong" joint pose.

gvdhoorn gravatar image gvdhoorn  ( 2020-03-01 05:51:35 -0600 )edit

Possible, I've never used setJointValueTarget. I would expect setPoseTarget to call the IK somewhere along the pipeline as well, but it can't hurt to check with both.

fvd gravatar image fvd  ( 2020-03-01 11:29:46 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools



Asked: 2020-02-11 08:14:46 -0600

Seen: 630 times

Last updated: Mar 03 '20