ROS의 RViz로 세계적인 수술용 로봇 daVinci 다빈치를 만나보자

제가 요즘 ROS와 Unity에 빠져지내는 것과 달리 저의 진로는 조금 있으면 로봇업계를 떠나게 됩니다. 아주 아쉬운 일이죠ㅠㅠ. 뭐 아무튼 저는 ROS의 학습이 RViz를 한 번 구동하고 따라하는걸 수행했지요[바로가기] RViz는 URDF라는 것을 이용해서 로봇의 링크 구조를 잘 보여주는 것 같네요. 저는 여기서 한가지 기억이 났죠~~^^ RVIz 극 초보인 저에게는 살짝 괜찮은 수준의 예제라고 생각했거든요^^

일단... 예전에 저는 다빈치(daVinci)라는 어마무시한 세계 유일.. 독점적 지위를 누리는 복강경 수술 로봇을 소개[바로가기]했었지요. 그 다빈치의 첫 번째 모델을 다빈치 제작사인 Intuitive Surgical에서 연구용으로 배포하고 있는데요. 존스홉킨스 대학의 논문하나를 소개하면서 그 이야기[바로가기]도 했었습니다. 그러면서 다빈치의 연구용 모델이 dVRK(daVinci Research Kit)이라고 소개했었죠^^. 오픈 소스로 ROS를 이용하고 있구요. 그들이 오픈한 소스는 GitHub에 공개[바로가기]되어 있구요^^ 

위 사진이 바로 IS(Intuitive Surgical)이 배포하는 다빈치의 첫 모델입니다.^^. 그리고 존스홉킨스 대학을 위시한 dVRK 연구진이 아주아주 멋~지게 모델까지 소스코드를 공개했다는 거죠^^ 그래서 저도 그 소스코드를 보고 살짝꿍 팔의 일부만 RViz 연습차원에서 한 번 표현해 보았습니다. 아참 이 글은 직전 RViz관련글[바로가기]과 이어서 읽으셔야 합니다.^^ 절차는 모두 거기랑 똑 같거든요^^

<?xml version="1.0" encoding="utf-8"?>

<robot name="daVinci">

  <!-- link 0: psm base link -->
  <link name="psm_base_link">
    <visual>
      <origin rpy="1.5708 0 3.1416" xyz="0.039 -0.03888 -0.07879"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/psm_base.dae"/>      
      </geometry>
    </visual>
  </link>
  
  <!-- joint 1 -->
  <joint name="outer_yaw_joint" type="revolute">
    <parent link="psm_base_link"/>
    <child link="outer_yaw_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="1.5708 -1.5708 0" xyz="0.0 0.0 0.0"/>
    <limit lower="-1.5707" upper="1.5707" velocity=".4" effort="1000"/>
  </joint> 
  
  
  <!-- link 1: outer_yaw -->
  <link name="outer_yaw_link">
    <visual>
      <!-- <origin rpy="0 0 -1.5708" xyz="0.0125 0 0.1575"/> -->
      <origin rpy="3.1416 0 1.5708" xyz="0.0125 0 0.1575"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/outer_yaw.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 2-1: outer_pitch_1 -->
  <joint name="outer_pitch_joint_1" type="revolute">
    <parent link="outer_yaw_link"/>
    <child link="outer_pitch_back_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="-1.5708 -1.5708 0" xyz="0 0.0295 0.1495"/>
    <limit lower="-0.7854" upper="0.7854" velocity=".4" effort="1000"/>
  </joint> 
  
  <!-- link 2: outer_pitch_back -->
  <link name="outer_pitch_back_link">
    <visual>
      <origin rpy="0 0 -0.27129" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/outer_pitch_back.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 2-2: outer_pitch_2 -->
  <joint name="outer_pitch_joint_2" type="continuous">
    <parent link="outer_yaw_link"/>
    <child link="outer_pitch_front_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="-1.5708 -1.5708 0" xyz="0 0.0295 0.0595"/>
    <mimic joint="outer_pitch_joint_1" multiplier="1"/>
  </joint>

  <!-- link 2-2: outer_pitch_front -->
  <link name="outer_pitch_front_link">
    <visual>
      <origin rpy="0 0 -0.27129" xyz="0 0 0"/>
      <!-- <origin rpy="0 0 0" xyz="0 0 0"/> -->
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/outer_pitch_front.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 2-3: outer_pitch_3 -->
  <joint name="outer_pitch_joint_3" type="continuous">
    <parent link="outer_pitch_back_link"/>
    <child link="outer_pitch_bottom_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.04178 0.15007 -0.0137"/>
    <mimic joint="outer_pitch_joint_1" multiplier="-1"/>
  </joint>

  <!-- link 2-3: outer_pitch_bottom -->
  <link name="outer_pitch_bottom_link">
    <visual>
      <origin rpy="0 -1.5708 0" xyz="0.009 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/outer_pitch_bottom.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 2-4: outer_pitch_4 -->
  <joint name="outer_pitch_joint_4" type="continuous">
    <parent link="outer_pitch_back_link"/>
    <child link="outer_pitch_top_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0.04209 0.18695 -0.02412"/>
    <mimic joint="outer_pitch_joint_1" multiplier="-1"/>
  </joint>

  <!-- link 2-4: outer_pitch_top -->
  <link name="outer_pitch_top_link">
    <visual>
      <origin rpy="0 -1.5708 0" xyz="0.009 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/outer_pitch_top.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 2-5: outer_pitch_5 -->
  <joint name="outer_pitch_joint_5" type="continuous">
    <parent link="outer_pitch_bottom_link"/>
    <child link="outer_insertion_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="-0.520 0 -0.0175"/>
    <mimic joint="outer_pitch_joint_1" multiplier="1"/>
  </joint>

  <!-- link 2-5: outer_insertion -->
  <link name="outer_insertion_link">
    <visual>
      <origin rpy="0 -1.5708 1.5708" xyz="0.02528 0.429 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/outer_insertion.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 3: insertion -->
  <joint name="outer_insertion_joint" type="prismatic">
    <parent link="outer_insertion_link"/>
    <child link="tool_main_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="1.5708 0 0" xyz="-0.0403 0.23622 0"/>
    <limit lower="0" upper="0.240" velocity=".4" effort="1000"/>
  </joint>

  <!-- link 3: tool_main_link -->
  <link name="tool_main_link">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_main.dae"/>      
      </geometry>
    </visual>
  </link>
  
  <!-- joint 4-1: Outer Roll -->
  <joint name="outer_roll_joint" type="revolute">
    <parent link="tool_main_link"/>
    <child link="tool_wrist_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0.36680"/>
    <limit lower="-2.2689" upper="2.2689" velocity=".4" effort="1000"/>
  </joint>

  <!-- link 4-1: tool_main_link -->
  <link name="tool_wrist_link">
    <visual>
      <origin rpy="0 0 1.5708" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_wrist_link.dae"/>      
      </geometry>
    </visual>
  </link>

  <!-- joint 4-1: Outer Roll shaft -->
  <joint name="outer_roll_shaft_joint" type="fixed">
    <parent link="tool_wrist_link"/>
    <child link="tool_wrist_shaft_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0.0091"/>
  </joint>

  <!-- link 4-1: tool_wrist_shaft_link -->
  <link name="tool_wrist_shaft_link">
    <visual>
      <origin rpy="1.5708 0 0" xyz="0 0.00401 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_wrist_shaft_link.dae"/> 
      </geometry>
    </visual>
  </link>

  <!-- joint 5: Outer wrist pitch joint -->
  <joint name="outer_wrist_pitch_joint" type="revolute">
    <parent link="tool_wrist_shaft_link"/>
    <child link="tool_wrist_sca_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="-1.5708 -1.5708 0" xyz="0 0 0"/>
    <limit lower="-1.5707" upper="1.5707" velocity=".4" effort="1000"/>
  </joint>

  <!-- link 5: tool_wrist_sca_link -->
  <link name="tool_wrist_sca_link">
    <visual>
      <!-- <origin rpy="0 3.1416 0" xyz="0 0 0"/> -->
      <origin rpy="1.5708 3.1416 0" xyz="0.0051 0.0032 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_wrist_sca_link.dae"/> 
      </geometry>
    </visual>
  </link>

  <!-- joint 5-2: Outer wristyaw joint -->
  <joint name="outer_wrist_yaw_joint" type="revolute">
    <parent link="tool_wrist_sca_link"/>
    <child link="tool_wrist_sca_shaft_link"/>
    <axis xyz="0 0 1"/>
    <origin rpy="-1.5708 -1.5708 0" xyz="0.0089 0 0"/>
    <!-- +/- 80 deg -->
    <limit lower="-1.3963" upper="1.3963" velocity=".4" effort="1000"/>
  </joint>

  <!-- link 5-2: tool_wrist_sca_shaft_link -->
  <link name="tool_wrist_sca_shaft_link">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 -0.0025"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_wrist_sca_shaft_link.dae"/> 
      </geometry>
    </visual>
  </link>
  
  <!-- joint 6-1: Outer open angle 1  -->
  <joint name="outer_wrist_open_angle_joint_1" type="revolute">
    <parent link="tool_wrist_sca_shaft_link"/>
    <child link="tool_wrist_sca_ee_link_1"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <!-- <origin rpy="-1.5708 -1.5708 0" xyz="0 0 0"/> -->
    <limit lower="0" upper="1.5707" velocity=".4" effort="1000"/>
  </joint>

  <!-- link 6-1: Outer open angle link 1 -->
  <link name="tool_wrist_sca_ee_link_1">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_wrist_sca_link_2.dae"/>
      </geometry>
    </visual>
  </link>

  <!-- joint 6-2: Outer open angle 2   -->
  <joint name="outer_wrist_open_angle_joint_2" type="revolute">
    <parent link="tool_wrist_sca_shaft_link"/>
    <child link="tool_wrist_sca_ee_link_2"/>
    <axis xyz="0 0 1"/>
    <origin rpy="0 0 0" xyz="0 0 0"/>
    <limit lower="-1.5707" upper="1.5707" velocity=".4" effort="1000"/>
    <mimic joint="outer_wrist_open_angle_joint_1" multiplier="-1"/>
  </joint>

  <!-- link 6-2: Outer open angle link 2 -->
  <link name="tool_wrist_sca_ee_link_2">
    <visual>
      <origin rpy="0 3.1516 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="package://dvrk_demo/meshes/psm/tool_wrist_sca_link_2.dae"/>
      </geometry>
    </visual>
  </link>


  
</robot>

URDF 파일은 [바로가기]에 있는 내용에서 하나를 잡고 경로만 바꾸었을 뿐입니다.^^.

<launch>
  <arg name="model" default="$(find dvrk_demo)/urdf/ecm_demo.urdf" />
  <arg name="gui" default="true" />

  <param name="robot_description" textfile="$(arg model)" />
  <param name="use_gui" value="$(arg gui)" />


  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />
</launch>

그리고 launch 파일은 이전 RViz글[바로가기]랑 같게 만들었습니다.

그러면 이렇게 두둥~~~ 현재 전 세계를 석권하고 있는 다빈치라는 수술용 로봇의 첫 번째 상용화 모델을 이렇게 만나볼 수 있습니다. ^^.

아주~ 교과서적인 구조로 이미 전세계에서 학습되고 있죠... 그게 또 고스란히 이 제조사로 흡수될 거구요^^

지금은 다빈치 로봇은 그 외형이 아~~주 많이 바뀌었습니다.^^

당연하지만~~ 각 링크의 각도를 조절해 볼 수도 있습니다.

insertion joint를 내려보고 있는데요^^ 제 귀에는 로봇이 움직이는 소리도 같이 들리는 듯 합니다.^^

아~ 이 부분은 instrument라고 하는 사람 몸 속에 들어가는 부분입니다. 아주 정밀한 제조 기술을 가져야 하지요^^

초짜 ROS 학습인(^^)으로서 처음으로 책에서 벗어나서 한 번 RViz로 그냥 단순히 모델을 표현하는 것 뿐이지만.. 그래도 뭐 하나 해보았네요^^

dvrk_demo.zip

이제 또... 즐거운 학습의 시간을 가져야죠^^ 아~~ 노파심에서 이야기를 미리 해둡니다. 쓸데없는 걱정을 하는 분들 때문에~.. 

이 포스팅은 어떠한 기관... 그 어떤 기관의 기밀 혹은 내부 문서와는 관계가 없습니다. 단지... 존스홉킨스 대학에 공개한 자료를 바탕으로 스스로 학습한 ROS를 이용한 것 뿐입니다.ㅠㅠ.