UR3 hardware cannot move with Moveit Python interface

[Qifan94]

Hi, I tested the ur_modern_driver with the hardware. The test_move.py in ur_modern_driver pkg did work and the plan and execute function in Rivz also could work with the hardware. But my code cannot set a target and move it to the target. I tried my code in a simulation in Rivz with UR3 robot fake controller. But When I change to control the real robot it does not work anymore and the error is shown as following.

[ERROR]: Unable to construct goal representation [INFO]: ABORTED: Catastrophic failure

My python code is here.

#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import numpy as np
import scipy.io as sio

from std_msgs.msg import String

def move_group_python_tutorial():

  print "============ Starting tutorial setup"
  moveit_commander.roscpp_initialize(sys.argv)
  rospy.init_node('move_group_python_tutorial',
                  anonymous=True)

  robot = moveit_commander.RobotCommander()

  scene = moveit_commander.PlanningSceneInterface()

  group = moveit_commander.MoveGroupCommander("arm")

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

  print "============ Waiting for RVIZ..."
  rospy.sleep(3)
  print "============ Starting tutorial "

  print "============ Reference frame: %s" % group.get_planning_frame()

  print "============ Reference frame: %s" % group.get_end_effector_link()

  print "============ Robot Groups:"
  print robot.get_group_names()

  print "============ Printing robot state"
  print robot.get_current_state()
  print "============"

  pose_ready = group.get_current_pose().pose
  print pose_ready

  print 'Move to Ready Position'
  print "============ Generating plan 1"

  n = 36
  R = 0.1
  i = 0
  sio.savemat('test_data_python.mat',{ 'a': 0, 'b': 9, 'c':18, 'd' : 27 })
  data = sio.loadmat('test_data_python.mat')
  print data
  x1 = int(data['a'])
  x2 = int(data['b'])
  x3 = int(data['c'])
  x4 = int(data['d'])
  line = [x1,x2,x3,x4]
  while True:
    ang = line[i] *2 * np.pi / n

    pose_target = group.get_current_pose().pose
    print "=============Print pose_target now:"
        print line[i]
    print pose_target
    pose_target.orientation.w += 0.0
    pose_target.position.x -= R*np.sin(ang)
    pose_target.position.y -= 0
    pose_target.position.z += R*np.cos(ang)
    print pose_target
    group.set_pose_target(pose_target)

    group.plan()
    rospy.sleep(3)
    group.go(wait=True)
    rospy.sleep(3)
    i = i+1

    print "============ Waiting while RVIZ displays plan1..."
    rospy.sleep(5)
    group.set_pose_target(pose_ready)
    group.plan()
    rospy.sleep(3)
    group.go(wait=True)
    rospy.sleep(3)
        if i == 4:
        break

  print "============ STOPPING"

if __name__=='__main__':
  try:
    move_group_python_tutorial()
  except rospy.ROSInterruptException:
    pass

[gavanderhoorn]

If this is still a problem, please re-open over at ur_modern_driver/issues, seeing as you're trying to use ur_modern_driver.

[gavanderhoorn]

pose_target.orientation.w += 0.0

are you sure this is ok?

orientation is a quaternion, not an RPY triple.

[gavanderhoorn]

I'm closing this as in any case this is not an issue with the pkgs in this repository.