Moveit Inverse Kinematics Solver not working properly
Hello,
I am working with ROS Kinetic on Ubuntu 16.04, and I am trying to send end effector poses to my Staubli TX90 using move group commands. I send an [x,y,z] coordinate through the setPositionTarget function, and a [r,p,y] target through the setRPYTarget function, and the movement in Rviz does not reflect the desired movement. When i send joint angle commands, everything works as expected, and I am able to get the [x,y,z] position (which means forward kinematics is working fine).
So the problem is definitely with the inverse kinematics solver.
The URDF files I am using to create my moveit package are directly from Staubli Experimental, so there should be no problem with that.
I am using Moveit's default kinematics solver and planning library.
Here is the code for the commands I am sending: NOTE: I commented out the changing of the x and y values to see if the end effector will move if I give it the same pose over and over again (which it shouldn't, but it does.) visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");
move_group.setNamedTarget("all-zeros");
// Ask Rviz to visualize and not move
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.execute(my_plan);
double rad = 0.0;
double x = 0.0;
double y = 0.0;
double z = 0.0;
std::vector<double> rpy;
geometry_msgs::PoseStamped current_poseS;
geometry_msgs::Pose current_pose;
geometry_msgs::PoseStamped target_pose;
current_poseS = move_group.getCurrentPose(move_group.getEndEffectorLink().c_str());
rpy = move_group.getCurrentRPY(move_group.getEndEffectorLink().c_str());
cout << "XYZ position :" << "[" << current_poseS.pose.position.x << ", " << current_poseS.pose.position.y << ", " << current_poseS.pose.position.z << ", " << current_poseS.pose.orientation.w << "]" << "\n";
for(double rad = 0.0; rad < 3; rad += 0.1) {
//x = current_poseS.pose.position.x + cos(rad);
//y = current_poseS.pose.position.y + sin(rad);
move_group.setPositionTarget(x, y, z, move_group.getEndEffectorLink().c_str());
move_group.setRPYTarget(rpy[0], rpy[1], rpy[2], move_group.getEndEffectorLink().c_str());
cout << "XYZ desired position : " << "[" << x << ", " << y << ", " << z << "]" << "\n";
success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
move_group.execute(my_plan);
target_pose = move_group.getCurrentPose(move_group.getEndEffectorLink().c_str());
cout << "XYZ actual position: " << "[" << target_pose.pose.position.x << ", " << target_pose.pose.position.y << ", " << target_pose.pose.position.z << "]" << "\n";
}
In this code, I output the position values of what I ask the end effector to do, and what it actually does. Here are the values for that:
XYZ position :[0.149872, 5.96057e-06, 2.11, 1]
XYZ desired position : [0.149872, 5.96057e-06, 2.11]
XYZ actual position: [-0.171006, 0.125541, 1.02336]
XYZ desired position : [0.149872, 5.96057e-06, 2.11]
XYZ actual position: [1.36898, 0.0393462, 0.0825644]
XYZ desired position : [0.149872, 5.96057e-06, 2.11]
XYZ actual position: [0.125503, -1.30601, 0.334007]
XYZ desired position : [0.149872, 5.96057e-06, 2.11]
XYZ actual position: [-0.166515, 0.520085, 1.04025]
XYZ desired position : [0.149872, 5.96057e-06, 2.11]
XYZ actual position: [0.858479, 1.07407, 0.799356]
XYZ desired position : [0.149872, 5.96057e-06, 2 ...
I just want to clarify the issue. When you send a set of joint angles you get out a pose (x,y,z,r,p,y) for the end effector. If you set those values using setPositionTarget and setRPYTarget, the end effector moves to a different location? Have you tried setPoseTarget?
I am pretty sure that the issue is the functions. I tried it with the panda arm. If i set the target pose with set_position_target and set_rpy_target, it goes to a different location than with set_pose_target. I did this in Python and with a different arm, so it may not apply, but worth a shot.
The documentation says that each function clears out any previous targets when set. Depending on the order, only one of the goals is being achieved.(https://tinyurl.com/yaujn43z)
To clarify, I am sending an end effector position and RPY target, say I give it the points [0.149872, 5.96057e-06, 2.11] for X Y and Z, and a set of RPY points. The visualization in Rviz shows the end effector at these XYZ points: [-0.171006, 0.125541, 1.02336].
I have tried using setPoseTarget() commands before I tried using setPosition and setRPY, which result in the same discrepancy between the desired and actual end effector positions. Although I tried setPoseTarget with the TX90, which I created the moveit package for, I didnt try with the provided one
Have you tried setting the pose instead of setting position and orientation separately?
To clarify further what I do in the code, I set a pre defined target pose, plan, execute (to make sure forward kinematics is working correctly), then get the current pose (which is the pre defined target pose), and ask the end effector to visit that same position, plan, execute, and get the pose.
Yes, but I have not tried setting the pose with the robot arm for which the moveit_configuration_package is given to me. I will try that and post my results.
Can you try running it with the
after
I think with the commands switched the position will be correct
I tried setting the pose using setPoseTarget, and that worked, but the orientation of the end effector changed every time I sent the same pose to plan and execute. I also tried switching the commands, and I got this error after every plan/execute :Fail: ABORTED: No motion plan found. No execution at
Although the setPoseTarget command works for the rx160 (moveit config was provided), I think I will need to fix my moveit config for the arm I am actually working with (tx90) and configure the ikfast plugin.
The Error Message (ABORTED:...) is shown when a plan can't be determined to the desired goal. I think its not working because it assumes the EE pose should be the same with setPositionTarget. The tx160 and tx90 look pretty similar so hopefully that is a good guide. Good Luck!
Hello, @KothariA, I was running through the moveit tutorials to see if there was anything wrong with the way I set up moveit itself, and the panda arm from the move_group tutorial isnt doing what the youtube video shows.
I found that the desired position and the actual position were a little different. Is this normal? desired : [0.28, -0.2, 0.5] actual: [0.3595, -2.09821e-12, 0.643499]
I used the move_it tutorial and it seems to work as expected. What is different between the video and what you are seeing? The error in the positions is more than I would expected. Did you use the files from the moveit_tutorial git?
So in the tutorial, they show the trajectory line in lime green, and my trajectory line doesnt look the same as the ones in the tutorial. Also, the cartesian path planner was only able to calculate 47% of the path, so there definitely something wrong. I ran the move_group_tutorial from the tutorial.