scene 과 robot_group, hand_group 을 초기화합니다.
scene으로 로봇 주변 환경을 추가하거나 제거할 수 있습니다.
robot_group과 hand_group을 통해 각각 로봇과 핸드의 모션을 생성할 수 있으며,
로봇을 움직이는 명령을 보낼 수 있습니다.
robot_group의 set_pose_target를 사용하면 pose를 타겟으로 설정할 수 있습니다.
ROS 에서 pose 는 물체의 xyz 좌표(위치)와 quaternion 로 표기되는 자세로 구성됩니다.
현재 위치에서 타겟 pose 까지의 경로를 joint space에서 planning 후 로봇을 움직입니다.
설정하는 목표 자세가 관절각으로 주어질 경우, 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 91011
waypoints=[]waypoints.append(self.robot_group.get_current_pose().pose)waypoints.append(pose_goal)plan,_=self.robot_group.compute_cartesian_path(waypoints,# waypoints to follow0.01,# eef_step0.0)# jump_thresholdself.robot_group.execute(plan)
add_box
scene의 add_box 를 사용하여 박스형 물체를 배치하기 위한 함수입니다.
add_box 가 호출된 뒤, scene 업데이트가 반영될때 까지 프로그램의 진행을 멈추기 위하여
wait_for_state_update 를 호출합니다.
node 가 실행되면 먼저 PickNPlaceTutorial 를 초기화 합니다.
raw_input()에서 사용자의 터미널 입력을 기다립니다.
12
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] 인
박스를 회전없이 배치 하고 있습니다.
main 함수의 나머지 부분에서 로봇의 목표 위치를 설정합니다.
quaternion을 직접 입력하는 것은 일반적으로 어렵기 때문에 list_to_pose 함수를 이용하여
리스트로 주어지는 좌표를 pose로 변환하여 move함수에 입력합니다.
리스트의 요소는 순서대로 x좌표, y좌표, z좌표, x축 회전, y축 회전, z축 회전을 나타냅니다.
move 함수가 하나씩 실행되며 입력받은 위치로 로봇이 이동합니다.