Moveit Inverse Kinematics Solver not working properly

edit

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)