OpenManipulator-Pro having issues with MoveIt!
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
Can you post the whole program in here?
I will for sure. @A_
I posted my code after Editing