IntPrim 프레임워크에 kuka iiwa 넣기

튜토리얼에서 제공되는 로봇 두대 중 프레임워크에 의해 컨트롤되는 controlled agent (UR5c) 대신 iiwa 로봇을 사용하고자 한다.

아래 튜토리얼을 따라서 새 device를 추가하기 위한 device driver를 만들어줬다. 

 

github.com/ir-lab/intprim_framework_ros/blob/master/docs/tutorials/4_creating_new_device.md

ir-lab/intprim_framework_ros

 

튜토리얼에서는 Robot1이라는 이름의 디바이스를 추가하는데 나는 iiwa7이라는 이름으로 진행했다. 디바이스는 아래 상속 구조를 가지므로, 두 클래스를 이용한다.

 

• DeviceInterface -> RobotInterface -> IIWA7Interface
• DeviceState -> IIWA7 State

 

아래 경로에 iiwa7.h를 만들어주고 튜토리얼을 참조해서 lbr4.h와 비슷하게 만든 뒤, 위에서 이야기한 두 클래스의 이름만 iiwa7으로 변경했다.

 

아래 위치에 iiwa7.cpp 파일을 만들고, 헤더 파일과 마찬가지로 lbr4.cpp 코드와 비슷하게 작성했다.

 

아래 interaction_application.h 파일을 열어서 DeviceInterfaceTypes에 iiwa7을 추가한다.

 

interaction_application.cpp 에서 개별 device에 대한 코드가 있는 곳마다 iiwa7 관련 코드를 넣어준다.

 

cmakelist.txt에 iiwa7.cpp를 추가한다.

 

여기까지 빌드를 해서 오류 없이 빌드되는 것을 확인한 후, interaction_application2.launch 파일에서 사용하는 param 파일들과 include 하는 또 다른 런치 파일을 수정한다.

 

param 파일로는 interaction.yaml, experiments.yaml, intprim_param.yaml 이 있는데 이 세 파일 모두 iiwa가 포함되도록 수정한다.

 

include 하는 또 다른 런치 파일은 coppelia_controller.launch인데, 여기도 iiwa7이 포함되도록 수정한다.

 

iiwa7을 추가하는 것뿐 아니라, joint 개수가 6으로 fix 되어있는 것도 해결해야 한다.

 

joint 개수가 6으로 정해져 있는 부분은 robot_sim.h였다. 여기서 const static int NUM_JOINTS = 6 으로 정의되어있는데, 이걸 robot_sim.cpp에서도 참조하고, coppelia_controller.cpp에서도 참조해서 사용했다.

 

const static int 값으로 지정하고 사용하는 이유는, joint state를 array로 기록하기 때문이다. cpp에서 array를 선언할 때 꼭 array크기를 지정해야 하기 때문이다. 그래서 생각한 해결 방법은 아래 두 가지였다.

1) array 대신 vector 사용하기

2) robot_sim_7.h를 만들어 NUM_JOINT=6인 소스코드를 만들기

 

결과적으로 방법 1)로 해결하게 되었는데, 방법 2)의 경우, robot_sim에서 정의된 클래스를 로봇 별로 인스턴스로 만들고, robot_sim인스턴스를 벡터에 넣어 인터랙션 agent로 사용하기 때문에 별도의 클래스를 만들 수 없었다.

 

방법 1)을 통해 DoF와 상관없이 쓸 수 있는 드라이버로 만들어주었고, 그 결과 아래 영상처럼 coppelia sim에서 동작도 하고 그 path를 기록하여 intprimd에서도 쓸 수 있게 되었다!

 

 

이제 트레이닝 데이터를 잔뜩 뽑아 모델 만들어서 테스트를 해봐야겠다~