# 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 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;

cout << "XYZ position       :" << "[" << current_poseS.pose.position.x << ", " << current_poseS.pose.position.y << ", " << current_poseS.pose.position.z << ", " << current_poseS.pose.orientation.w << "]" << "\n";


cout << "XYZ desired position : " << "[" << x << ", " << y << ", " << z << "]" << "\n";

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

move_group.execute(my_plan);

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 ...

edit retag close merge delete

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?

( 2018-08-10 13:28:07 -0600 )edit
1

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.

( 2018-08-10 14:01:19 -0600 )edit
1

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)

( 2018-08-10 14:03:59 -0600 )edit

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].

( 2018-08-10 17:28:18 -0600 )edit

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

( 2018-08-10 17:31:10 -0600 )edit

Have you tried setting the pose instead of setting position and orientation separately?

( 2018-08-10 17:31:13 -0600 )edit

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.

( 2018-08-10 17:44:08 -0600 )edit

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.

( 2018-08-10 17:45:17 -0600 )edit

Sort by » oldest newest most voted

The problem is which command you are using to set the end effector pose. Use setPoseTarget (or set_pose_target in python). This should fix the issue. The other two commands only set the position or orientation. Everytime one is called, all other goals are removed.

Below is the script that I put together to show it all. The visualization is not super clear, but I think it is sufficient.

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list
import pdb

def moveToVertical():
# We can get the joint values from the group and adjust some of the values:
joint_goal = group.get_current_joint_values()
joint_goal[0] = 0
joint_goal[1] = 0
joint_goal[2] = 0
joint_goal[3] = 0
joint_goal[4] = 0
joint_goal[5] = 0

# The go command can be called with joint values, poses, or without any
# parameters if you have already set the pose or joint target for the group
group.go(joint_goal, wait=True)

# Calling stop() ensures that there is no residual movement
group.stop()

def moveToEEPose():
print("Moving with set_pose_target")
pose_goal = geometry_msgs.msg.Pose()
pose_goal.orientation.w = 1.0
pose_goal.position.x = 0.6
pose_goal.position.y = -0.05
pose_goal.position.z = 1.9
# position:
#   x: 0.572411276237
#   y: -0.0429406637445
#   z: 1.86210526056
# orientation:
#   x: 0.407142030919
#   y: -4.85856506125e-05
#   z: 2.89045501013e-05
#   w: 0.9133648578
group.set_pose_target(pose_goal)
plan = group.go(wait=True)
# Calling stop() ensures that there is no residual movement
group.stop()
# It is always good to clear your targets after planning with poses.
# Note: there is no equivalent function for clear_joint_value_targets()
group.clear_pose_targets()
printPose()

return pose_goal

def moveToEEPose_Incorrect_Position(pose):
print("Moving with set_position_target")
group.set_rpy_target([pose.orientation.x, pose.orientation.y, pose.orientation.z])
group.set_position_target([pose.position.x, pose.position.y, pose.position.z])
plan = group.go(wait=True)
group.stop()
group.clear_pose_targets()
printPose()

def moveToEEPose_Incorrect_Orientation(pose):
print("Moving with set_orientation_target")
group.set_position_target([pose.position.x, pose.position.y, pose.position.z])
group.set_rpy_target([pose.orientation.x, pose.orientation.y, pose.orientation.z])
plan = group.go(wait=True)
group.stop()
group.clear_pose_targets()
printPose()

def printPose():
print("Final Pose:")
print(group.get_current_pose())

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('move_group_python_interface_tutorial',
anonymous=True)

robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group_name = "manipulator"
group = moveit_commander.MoveGroupCommander(group_name)

display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
moveit_msgs.msg.DisplayTrajectory,
queue_size=20)

moveToVertical()
pose = moveToEEPose()

moveToVertical()
moveToEEPose_Incorrect_Position(pose)

moveToVertical()
moveToEEPose_Incorrect_Orientation(pose)

more

I'm confused by your example. You only set pose.orientation.w to 1 yet you log all of the other components set. I'm struggling to get it to respect my requested orientations, here

( 2019-12-02 17:30:41 -0600 )edit

Rviz does not include an IK solver, that's done in your MoveIt! node. RVIZ just looks at the joint states topic which is just a set of joint angles to visualize.

I'd look at MoveIt! You probably have configured something incorrectly.

more

I corrected that on the question, but to see if there was something wrong with my Moveit configuration, I decided to try sending end effector poses to a different robot arm, which has the moveit configuration package on github @ https://github.com/ros-industrial/sta... , I get the same problem.

( 2018-08-09 16:40:44 -0600 )edit