OpenManipulator-Pro having issues with MoveIt!

asked 2019-10-29 08:07:06 -0600

updated 2019-10-31 08:10:27 -0600

Hello developers!

First my workspace is implemented in Ubuntu 18.04 and ROS Melodic with MoveIt! 1.0. I'm having issues to do a simple task in Python:

  • Move to poseA
  • Move to poseB

The function that I use to command the robot:

def go_to_pose(self, pose_name):
    self.group.set_named_target(pose_name)
    self.group.go(wait=True)
    self.group.stop()
    self.group.clear_pose_targets()

Look that wait=True is set!

if __name__ == '__main__':
    try:
        manip = openManipulator()
        raw_input("Press Enter to start!")
        manip.go_to_pose('pSearch')
        #rospy.sleep(12)
        manip.go_to_pose('pHome')
    except rospy.ROSInterruptException:
        pass
    except KeyboardInterrupt:
        pass

The problem: The OpenManipulator-Pro doesn't respect the sequence: move to position A and then to position B, it tries to do both at the same time, even with the Wait = True argument. However, this does not occur in the Gazebo.

As a temporary solution, rospy.sleep () can make the situation sequential, but it is not a permanent solution. I wondered what is going on.

EDIT - Whole code

#!/usr/bin/env python
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 sensor_msgs.msg import JointState
from apriltag_ros.msg import AprilTagDetectionArray
from moveit_commander.conversions import pose_to_list

class openManipulator:
    def __init__(self):
        # ROS NODE INIT
        rospy.init_node('moveit_routine_fix', anonymous=True)        
        # MOVEIT INIT
        moveit_commander.roscpp_initialize(sys.argv)
        self.robot = moveit_commander.RobotCommander()
        self.scene = moveit_commander.PlanningSceneInterface()
        self.group_name = "arm"
        self.group = moveit_commander.MoveGroupCommander(self.group_name)

        # DEFINE PARAMETERS
        self.group.set_goal_position_tolerance(0.1)          # GOAL TOLERANCE
        self.group.set_planning_time(5)                      # TIME TO PLANNING

    def go_to_pose_controlled(self, pose_name):
        self.group.set_named_target(pose_name)
        self.group.go(wait=True)
        self.group.stop()
        self.group.clear_pose_targets()


    def rotate_joint1_90(self,j):
        joint_goal = self.group.get_current_joint_values()
        joint_goal[0] = j
        joint_goal[1] = j/2
        self.group.go(joints=joint_goal, wait=True)
        self.group.stop()
        self.group.clear_pose_targets()

if __name__ == '__main__':
    try:
        manip = openManipulator()
        raw_input("Press Enter to start!")
        print 255
        manip.go_to_pose_controlled('pHome')
        print 1
        manip.go_to_pose_controlled('pSearch')
        print 2
        manip.rotate_joint1_90(0)     
        print 3
        manip.rotate_joint1_90(-pi/2)
        print 4



    except rospy.ROSInterruptException:
        pass
    except KeyboardInterrupt:
        pass
edit retag flag offensive close merge delete

Comments

Can you post the whole program in here?

A_YIng gravatar imageA_YIng ( 2019-10-31 04:15:39 -0600 )edit

I will for sure. @A_

kaike_wesley_reis gravatar imagekaike_wesley_reis ( 2019-10-31 08:08:27 -0600 )edit

I posted my code after Editing

kaike_wesley_reis gravatar imagekaike_wesley_reis ( 2019-10-31 08:11:03 -0600 )edit