Ask Your Question
0

Moveit Inverse Kinematics Solver not working properly

asked 2018-08-08 11:18:57 -0600

shreyasgan gravatar image

updated 2018-08-10 17:40:41 -0600

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 ... (more)

edit retag flag offensive close merge delete

Comments

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?

KothariA gravatar imageKothariA ( 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.

KothariA gravatar imageKothariA ( 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)

KothariA gravatar imageKothariA ( 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].

shreyasgan gravatar imageshreyasgan ( 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

shreyasgan gravatar imageshreyasgan ( 2018-08-10 17:31:10 -0600 )edit

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

KothariA gravatar imageKothariA ( 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.

shreyasgan gravatar imageshreyasgan ( 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.

shreyasgan gravatar imageshreyasgan ( 2018-08-10 17:45:17 -0600 )edit

2 Answers

Sort by ยป oldest newest most voted
1

answered 2018-08-10 16:41:32 -0600

KothariA gravatar image

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

Comments

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

rklutz gravatar imagerklutz ( 2019-12-02 17:30:41 -0600 )edit
0

answered 2018-08-09 12:56:37 -0600

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.

edit flag offensive delete link more

Comments

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.

shreyasgan gravatar imageshreyasgan ( 2018-08-09 16:40:44 -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

2 followers

Stats

Asked: 2018-08-08 11:18:57 -0600

Seen: 595 times

Last updated: Aug 10 '18