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.