Skip to content

MoveIt Pick and Place 예제

요구 환경

  • Ubuntu 16.04 및 ROS Kinetic 이상 이 설치된 PC

  • indy-ros 및 indy-ros-examples 소스코드 설치

위 요구 환경의 설치가 완료되지 않은 경우 설치를 먼저 진행하여 주십시오.

실행 방법

1. 터미널 2개 열기

2. ROS 환경설정 확인하기

3. 명령어 실행하기
첫번째 터미널에서 아래 명령어 실행 후 rviz가 완전히 실행될때까지 기다립니다.

1
roslaunch indy7_robotiq_moveit_config demo.launch

rviz 와 moveit plugin이 완전히 실행되면 두번째 터미널에서 아래 명령어를 실행합니다.

1
rosrun indy7_robotiq_moveit_interface pick_n_place.py

잠시 기다리면 아래 사진과 같은 메세지가 출력되며 planning 준비가 완료되었음을 알 수 있습니다.


위 메세지가 출력된 후 엔터를 입력하면 아래 동영상과 같은 픽 앤 플레이스가 시작됩니다.

pick_n_place.py 설명

본 예제는 MoveIt Tutorial을 참조하여 작성되었습니다. 아래에서 설명되지 않는 부분에 대해서는 링크를 참조해주시기 바랍니다.

PickNPlaceTutorial 클래스

init

먼저 moveit commander와 rospy를 초기화합니다. rospy 초기화에서 ros에서 사용되는 노드의 이름을 입력합니다.

1
2
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('pick_n_place_tutorial', anonymous=True)

scene 과 robot_group, hand_group 을 초기화합니다. scene으로 로봇 주변 환경을 추가하거나 제거할 수 있습니다. robot_group과 hand_group을 통해 각각 로봇과 핸드의 모션을 생성할 수 있으며, 로봇을 움직이는 명령을 보낼 수 있습니다.

1
2
3
self.scene = moveit_commander.PlanningSceneInterface()
self.robot_group = moveit_commander.MoveGroupCommander('indy7')
self.hand_group = moveit_commander.MoveGroupCommander('hand')

hold hand, release hand

입력받은 이름의 타겟 물체를 핑거에 부착하고, 떼어내기 위해 사용되는 함수입니다. 프로그램을 간단히 하기 위해 핑거를 움직이지는 않습니다.

1
2
3
4
5
6
def hold_hand(self, target_name):
    touch_links = ['left_inner_finger', 'right_inner_finger']
    self.hand_group.attach_object(target_name, 'robotiq_85_base_link', touch_links=touch_links)

def release_hand(self, target_name):
    self.hand_group.detach_object(target_name)

jmove_to_pose_goal

robot_group의 set_pose_target를 사용하면 pose를 타겟으로 설정할 수 있습니다. ROS 에서 pose 는 물체의 xyz 좌표(위치)와 quaternion 로 표기되는 자세로 구성됩니다. 현재 위치에서 타겟 pose 까지의 경로를 joint space에서 planning 후 로봇을 움직입니다.

1
2
self.robot_group.set_pose_target(pose_goal)
self.robot_group.go(wait=True)

jmove_to_joint_goal

설정하는 목표 자세가 관절각으로 주어질 경우, go를 실행할때 목표 위치를 입력 할 수 있습니다.

1
self.robot_group.go(joint_goal, wait=True)

tmove_to_pose_goal

task move를 실행하기 위해서는 waypoint 를 설정할 필요가 있습니다. tmove_to_pose_goal 함수에서는 로봇의 현재 위치와 목표위치를 waypoint에 추가하여 cartesian path를 계산하여 plan 변수에 입력합니다. 마지막으로 robot_group의 execute함수를 사용하여 plan 을 실행합니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
waypoints = []
waypoints.append(self.robot_group.get_current_pose().pose)
waypoints.append(pose_goal)


plan, _ = self.robot_group.compute_cartesian_path(
                            waypoints,   # waypoints to follow
                            0.01,        # eef_step
                            0.0)         # jump_threshold

self.robot_group.execute(plan)

add_box

scene의 add_box 를 사용하여 박스형 물체를 배치하기 위한 함수입니다. add_box 가 호출된 뒤, scene 업데이트가 반영될때 까지 프로그램의 진행을 멈추기 위하여 wait_for_state_update 를 호출합니다.

1
2
self.scene.add_box(name, pose_stamp, size=size)
self.wait_for_state_update(box_name=name, box_is_known=True)

main 함수

node 가 실행되면 먼저 PickNPlaceTutorial 를 초기화 합니다. raw_input()에서 사용자의 터미널 입력을 기다립니다.

1
2
pnp = PickNPlaceTutorial()
raw_input()

로딩이 끝나고, 사용자의 입력이 이루어지면, scene에 오브젝트를 배치합니다. 오브젝트의 위치와 자세를 입력하기 위해 geometry_msgs.msg.PoseStamped() 를 사용합니다. pose.position(x, y, z)과 pose.orientation에 오브젝트의 위치를 입력합니다. ROS에서 pose.orientation에는 quaternion(x, y, z, w)을 사용합니다. 아래의 경우는 xyz 좌표로 0.5 [m], 0 [m], 0.05 [m] 에 크기가 0.5 [m] x 0.5 [m] x 0.1 [m] 인 박스를 회전없이 배치 하고 있습니다.

1
2
3
4
5
6
7
8
table = geometry_msgs.msg.PoseStamped()
table.header.frame_id = pnp.robot_group.get_planning_frame()
table.pose.position.x =0.5
table.pose.position.y =0
table.pose.position.z =0.05
table.pose.orientation.w = 1.0                            

pnp.add_box(name='table', pose_stamp=table, size=(0.5, 0.5, 0.1))

동일한 방법으로 벽과 픽 대상이 되는 큐브 2개를 추가했습니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
wall = geometry_msgs.msg.PoseStamped()
wall.header.frame_id = pnp.robot_group.get_planning_frame()
wall.pose.position.x =0.5
wall.pose.position.y =0
wall.pose.position.z =0.2
wall.pose.orientation.w = 1.0                            

pnp.add_box(name='wall', pose_stamp=wall, size=(0.01, 0.5, 0.25))

cube_0 = geometry_msgs.msg.PoseStamped()
cube_0.header.frame_id = pnp.robot_group.get_planning_frame()
cube_0.pose.position.x =0.35
cube_0.pose.position.y =-0.10
cube_0.pose.position.z =0.13
cube_0.pose.orientation.w = 1.0                            

pnp.add_box(name='cube_0', pose_stamp=cube_0)

cube_1 = geometry_msgs.msg.PoseStamped()
cube_1.header.frame_id = pnp.robot_group.get_planning_frame()
cube_1.pose.position.x =0.35
cube_1.pose.position.y =0.10
cube_1.pose.position.z =0.13
cube_1.pose.orientation.w = 1.0                            

pnp.add_box(name='cube_1', pose_stamp=cube_1)

main 함수의 나머지 부분에서 로봇의 목표 위치를 설정합니다. quaternion을 직접 입력하는 것은 일반적으로 어렵기 때문에 list_to_pose 함수를 이용하여 리스트로 주어지는 좌표를 pose로 변환하여 move함수에 입력합니다. 리스트의 요소는 순서대로 x좌표, y좌표, z좌표, x축 회전, y축 회전, z축 회전을 나타냅니다. move 함수가 하나씩 실행되며 입력받은 위치로 로봇이 이동합니다.

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0])

pnp.jmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.18, 0, -pi, -pi/2]))
pnp.hold_hand('cube_0')
pnp.tmove_to_pose_goal(list_to_pose([0.35, -0.10, 0.28, 0, -pi, -pi/2]))


pnp.jmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.18, 0, -pi, -pi/2]))
pnp.release_hand('cube_0')
pnp.tmove_to_pose_goal(list_to_pose([0.65, -0.10, 0.28, 0, -pi, -pi/2]))


pnp.jmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.18, 0, -pi, -pi/2]))
pnp.hold_hand('cube_1')
pnp.tmove_to_pose_goal(list_to_pose([0.35, 0.10, 0.28, 0, -pi, -pi/2]))


pnp.jmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.48, 0, -pi, -pi/2]))
pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.18, 0, -pi, -pi/2]))
pnp.release_hand('cube_1')
pnp.tmove_to_pose_goal(list_to_pose([0.65, 0.10, 0.28, 0, -pi, -pi/2]))

pnp.jmove_to_joint_goal([0, 0, -pi/2, 0, -pi/2, 0])