0
$\begingroup$

Rosanswers logo

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

XYZ actual position: [0.377225, 0.798595, 0.652163]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.656954, -0.625306, 1.33333]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.191138, -0.0686085, 1.65773]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.775435, 1.09043, 1.45864]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.514744, 0.337815, 1.06536]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.110696, 0.0264874, 1.55515]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.53965, 0.406767, 1.76621]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.423981, 0.367639, 1.14733]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.449486, -0.530723, 1.87968]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.710675, 0.313392, 1.50893]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.136642, -0.226274, 1.84577]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.0835286, -0.0100949, 1.96986]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.878419, -0.0090857, 1.2062]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-1.03625, 0.787584, 0.811148]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.00873582, 0.187781, 2.05236]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.680327, 0.0804499, 0.899528]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.354603, 0.734311, 1.92727]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.242637, 0.0182263, 1.54077]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-1.21858, -0.124986, 0.94397]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.924168, -0.53434, 0.56409]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.45143, 0.707721, 0.746427]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.00314717, 0.0523088, 1.09032]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [1.04328, -0.469255, 1.37842]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [0.257355, -0.873096, 1.77061]

XYZ desired position : [0.149872, 5.96057e-06, 2.11]

XYZ actual position: [-0.944385, 0.161417, 1.28219]


Originally posted by shreyasgan on ROS Answers with karma: 26 on 2018-08-08

Post score: 0


Original comments

Comment by KothariA on 2018-08-10:
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?

Comment by KothariA on 2018-08-10:
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.

Comment by KothariA on 2018-08-10:
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)

Comment by shreyasgan on 2018-08-10:
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].

Comment by shreyasgan on 2018-08-10:
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

Comment by KothariA on 2018-08-10:
Have you tried setting the pose instead of setting position and orientation separately?

Comment by shreyasgan on 2018-08-10:
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.

Comment by shreyasgan on 2018-08-10:
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.

Comment by KothariA on 2018-08-10:
Can you try running it with the

move_group.setPositionTarget(x, y, z, move_group.getEndEffectorLink().c_str());

after

move_group.setRPYTarget(rpy[0], rpy[1], rpy[2], move_group.getEndEffectorLink().c_str());

I think with the commands switched the position will be correct

Comment by shreyasgan on 2018-08-10:
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

Comment by shreyasgan on 2018-08-10:
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.

Comment by KothariA on 2018-08-10:
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!

Comment by shreyasgan on 2018-08-11:
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.

Comment by shreyasgan on 2018-08-11:
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]

Comment by KothariA on 2018-08-11:
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?

Comment by shreyasgan on 2018-08-13:
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.

Comment by KothariA on 2018-08-13:
Hmm that is weird. I'll try out the c++ code tonight or tomorrow and see if I have the same issues.

Comment by shreyasgan on 2018-08-14:
You probably wont run into the same errors. I dont know exactly whats going wrong here, but I know the code is not wrong for the moveit_tutorials. I believe there is something wrong on a lower level maybe. I redownloaded moveit from source, and still get the same issue.

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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)

Originally posted by KothariA with karma: 26 on 2018-08-10

This answer was ACCEPTED on the original site

Post score: 1


Original comments

Comment by rklutz on 2019-12-02:
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

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.