[ROS] Universal Robot manipulator with Moveit!

개요

지난 포스팅에서 Universal robot의 manipulator ROS package를 사용해보았습니다.

이번에는 Moveit!을 활용하여 manipulator를 제어해 보겠습니다.

개발환경 구축

개발 환경은 Kinetic, Melodic 상관없구요, UR ROS package를 설치해야합니다.

물론 설치 하신 분들은 건너뛰시구요.

UR 로봇의 공식 ROS git에서 다음과 같이 설치 방법 및 실행 방법을 설명하고 있습니다.

- Kinetic
sudo apt-get install ros-kinetic-universal-robot

- Melodic
sudo apt-get install ros-melodic-universal-robot

- 공통사항
rospack profile && rosstack profile

그리고 나서 Moveit! 설치를 진행합니다.

- Kinetic
rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get install ros-kinetic-catkin python-catkin-tools
sudo apt install ros-kinetic-moveit

- Melodic
rosdep update
sudo apt-get update
sudo apt-get dist-upgrade
sudo apt-get install ros-melodic-catkin python-catkin-tools
sudo apt install ros-melodic-moveit

- 공통사항
rospack profile && rosstack profile

제 깃허브에 python으로 작성한 테스트 파일이 있습니다.
깃허브 제목은 'ur3-moveit-test' 입니다만, 사실 ur3, 5, 10 모두 사용 가능합니다.
ROS version도 무관합니다.

- git clone
$ cd ~/catkin_ws/src && git clone https://github.com/MrLacquer/ur3-moveit-test.git
$ cd ~/catkin_ws && catkin_make
$ rospack profile && rosstack profile

- Registration
$ cd ~/catkin_ws/src/ur3-moveit-test/ur3_moveit/script
$ chmod +x ur3_move.py
$ chmod +x ur3_demo.py 

Gazebo 시뮬레이터 실행

이제 실제로 동작시켜보기 위해 ur 로봇의 Gazebo 시뮬레이터를 불러옵니다.

- Bring up Gazebo and the URx manipulator:
$ roslaunch ur_gazebo ur3.launch
$ roslaunch ur3_moveit_config ur3_moveit_planning_execution.launch sim:=true
$ roslaunch ur3_moveit_config moveit_rviz.launch config:=true

- Moveit! python code 실행
$ rosrun ur3_moveit ur3_move.py 
or
$ rosrun ur3_moveit ur3_demo.py 

우선, ur3_demo.py 코드는,

import sys
import rospy
import copy, math
from math import pi

from moveit_commander import RobotCommander, MoveGroupCommander
from moveit_commander import PlanningSceneInterface, roscpp_initialize, roscpp_shutdown
from geometry_msgs.msg import PoseStamped
from moveit_msgs.msg import Grasp, GripperTranslation, PlaceLocation, MoveItErrorCodes
from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint
from tf.transformations import euler_from_quaternion, quaternion_from_euler
import random

GROUP_NAME_ARM = "manipulator"        # moveit setup assistant에서 설정한 arm group 이름
FIXED_FRAME = 'world'
#GROUP_NAME_GRIPPER = "NAME OF GRIPPER"

class TestMove():

    def __init__(self):
        roscpp_initialize(sys.argv)        
        rospy.init_node('ur3_move',anonymous=True)

        self.scene = PlanningSceneInterface()
        self.robot_cmd = RobotCommander()

        self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM)
        #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER)
        self.robot_arm.set_goal_orientation_tolerance(0.005)
        self.robot_arm.set_planning_time(5)
        self.robot_arm.set_num_planning_attempts(5)

        rospy.sleep(2)
        # Allow replanning to increase the odds of a solution
        self.robot_arm.allow_replanning(True)        

    def move_code(self):

        self.robot_arm.set_named_target("home")  # moveit setup assistant에서 설정한 동작
        self.robot_arm.go(wait=True)
        print("====== move plan go to home 1 ======")        
        rospy.sleep(1)        

        self.robot_arm.set_named_target("zeros")  # moveit setup assistant에서 설정한 동작
        self.robot_arm.go(wait=True)
        print("====== move plan go to zeros ======")        
        rospy.sleep(1)       

        robot_state = self.robot_arm.get_current_pose();
        robot_angle = self.robot_arm.get_current_joint_values();

        print(robot_state)


if __name__=='__main__':
    tm = TestMove()
    tm.__init__()
    tm.move_code()

    rospy.spin()
    roscpp_shutdown()

• 동작사진 추가 예정
  이와 같이 moveit setup assistant에서 설정한 자세인 'home'과 'zero' 모션을 순차적으로 동작합니다.
  물론 tm.move_code()를 반복문안에 넣으면 반복동작합니다.

---

ur3_demo.py코드는,

import sys
import rospy
import tf
import moveit_commander  # https://answers.ros.org/question/285216/importerror-no-module-named-moveit_commander/
import random
from geometry_msgs.msg import Pose, Point, Quaternion
from math import pi

pose_goal = Pose()
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('ur3_move',anonymous=True)
group = [moveit_commander.MoveGroupCommander("manipulator")]  # ur3 moveit group name: manipulator

xx = 1

while not rospy.is_shutdown():  
  if(xx%2 == 1):        
    pose_goal.orientation.w = 0.0
    pose_goal.position.x = 0.4 # red line      0.2   0.2
    pose_goal.position.y = 0.15  # green line  0.15   0.15
    pose_goal.position.z = 0.2  # blue line   # 0.35   0.6
    group[0].set_pose_target(pose_goal)
    group[0].go(True)

    #print(xx)
    rospy.sleep(2)

  else:
    pose_goal.orientation.w = 0.0
    pose_goal.position.x = 0.2 # red line      0.2   0.2
    pose_goal.position.y = 0.15  # green line  0.15   0.15
    pose_goal.position.z = 0.2  # blue line   # 0.35   0.6
    group[0].set_pose_target(pose_goal)
    group[0].go(True)

    #print(xx)
    rospy.sleep(2) 

  xx = xx + 1   

moveit_commander.roscpp_shutdown()

• 동작사진 추가예정

본 코드는 UR3에서만 제대로 작동합니다.
왜냐면 while문 안에 설정한 좌표들은 UR3의 resion of motion 에 맞춘 좌표이기 때문이지요.

저는 해당 좌표들을 Rviz에서 interactive marker를 조절하여 excute 한 뒤에, rosrun tf tf_echo /base_link /eef_link 로 end-effector의 좌표를 읽어서 설정하거나, Moveit! interface의 get_current_pose()함수를 활용했습니다.