How to check if the robot is in correct position (Pose)?
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 ...
You should definitely provide more data for this to be solved, or explain how you solved it yourself.
Hi @fvd Check edited question, thanks in advance