[Kinetic] UR3 overshooting goal using Moveit python api

edit

HI,

For some reason when using the moveit API on python, when executing the code the UR3 overshoots. I am not sure if this is an issue with the code or something else.

Doing some research, I found other users having the same issue but with different robots example: link text

I have attached some information I thought would be useful.

Using: Ros Kinetic Ubuntu 18.04 UR3 Moveit installed by apt-get UR3 Moveit packages installed via apt-get ur_modern_driver installed using catkin make(Using the kinetic branch) My teach pendent speed is set to 100% I am using a tool that weighs 1.2kg. (I set the weight in the teach pendent) Used both the joint goal and tpc goals.

I have also attached my code below.

I believe it is an issue with the planning itself and if it is, I was wondering if there is a way to use the Moveit api to just move to the specified goal and wait until the robot has reached that goal?

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import rospy, sys
import moveit_commander
from geometry_msgs.msg import Pose
from copy import deepcopy
from math import pi 
from moveit_commander.conversions import pose_to_list
import moveit_msgs.msg
import time

moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('moveit_ur3', anonymous=False)


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)

pose_a = [-1.4873150030719202, -1.5659659544574183, -2.1959059874164026, -0.9785626570331019, 1.5747051239013672, 0.08589393645524979]
pose_b = [-1.5720184485064905, -1.5519183317767542, -2.28664476076235, -0.8552258650409144, -1.5674932638751429, 0.1681944876909256]
pose_c = [-1.5748141447650355, -1.5515945593463343, -2.289560143147604, -0.851447884236471, -3.107366387044088, 0.17082464694976807]
#pose_d = [-1.5748141447650355, -1.5516184012042444, -2.2895482222186487, -0.8514960447894495, -3.1073783079730433, -3.207879130040304]
#pose_e = [-1.5748260656939905, -1.5515826384173792, -2.289499823247091, -0.8514598051654261, 0.00015579492901451886, -6.283077074647075]
#pose_f = [-1.574789826069967, -1.5515945593463343, -2.289572302495138, -0.8514121214496058, -0.005062405263082326, -3.1386335531817835]

def go_to_joint_state():
    joint_goal = group.get_current_joint_values()
    print joint_goal
    #joint_goal[0] = pose_a[0]
    #joint_goal[1] = pose_a[1]
    #joint_goal[2] = pose_a[2]
    #joint_goal[3] = pose_a[3]
    #joint_goal[4] = pose_a[4]
    #joint_goal[5] = pose_a[5]

    group.go(pose_a, wait=True)
    time.sleep(7)
    group.go(pose_b,wait=True)
    time.sleep(7)
    group.stop()

def go_to_pose_goal():
    pose_goal = geometry_msgs.msg.Pose()
    #pose_goal.orientation.w = 0.4900
    pose_goal.position.x = -0.4900
    pose_goal.position.y = 0.4900
    pose_goal.position.z = 0.4900    
    group.set_pose_target(pose_goal)

    group.go(joint_goal, wait=True)
    time.sleep(3)
    group.stop()
    group.clear_pose_targets()

def main():
    go_to_joint_state()

if __name__ == '__main__':
    main()