MoveIt으로 만든 매니퓰레이터 Gazebo와 연동하기

1. Xacro 파일을 수정한다.

<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="dyros_manipulator3r">

<xacro:property name="width" value="0.05"/>
<xacro:property name="height" value="0.2"/>
<xacro:property name="mass" value="0.1"/>

    <link name="world"/>

    <joint name="base" type="fixed">
        <origin xyz="0.0 0.0 0.0" rpy="0.0 0.0 0.0"/>
        <parent link="world"/>
        <child link="base_link"/>
        <axis xyz="0.0 0.0 0.0"/>
        <limit lower="0.0" upper="0.0" effort="0.0" velocity="0.0"/>
    </joint>

    <link name="base_link">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="base_link">
                <color rgba="1.0 0.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint1" type="revolute">
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <parent link="base_link"/>
        <child link="Link1"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link1">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="link1">
                <color rgba="0.0 1.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>
    
    <joint name="Joint2" type="revolute">
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <parent link="Link1"/>
        <child link="Link2"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link2">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="link2">
                <color rgba="0.0 0.0 1.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint3" type="revolute">
        <origin xyz="0.0 0.0 0.25" rpy="0.0 0.0 0.0"/>
        <parent link="Link2"/>
        <child link="Link3"/>
        <axis xyz="1.0 0.0 0.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link3">
        <inertial>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
            <material name="link3">
                <color rgba="0.0 1.0 1.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 ${height/2}" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="${width} ${width} ${height}"/>
            </geometry>
        </collision>
    </link>

    <joint name="Joint4" type="fixed">
        <origin xyz="0.0 0.0 0.2" rpy="0.0 0.0 0.0"/>
        <parent link="Link3"/>
        <child link="Link4"/>
        <axis xyz="0.0 0.0 1.0"/>
        <limit lower="-3.14" upper="3.14" effort="30" velocity="1"/>
    </joint>

    <link name="Link4">
        <inertial>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <mass value="${mass}"/>
            <inertia ixx="${mass/12*0.16*(width*width+width*width)}" ixy="0.0" ixz="0.0" 
                iyy="${mass/12*0.16*(width*width+width*width)}" iyz="0.0" 
                izz="${mass/12*0.16*(height*height+height*height)}"/>
        </inertial>
        <visual>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.02 0.02 0.02"/>
            </geometry>
            <material name="link4">
                <color rgba="1.0 1.0 0.0 1.0"/>
            </material>
        </visual>
        <collision>
            <origin xyz="0.0 0.0 0.01" rpy="0.0 0.0 0.0"/>
            <geometry>
                <box size="0.02 0.02 0.02"/>
            </geometry>
        </collision>
    </link>

    <transmission name="transmission1">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="actuator1">
            <mechanicalReduction>1.0</mechanicalReduction>
            <!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
        </actuator>
    </transmission>

    <transmission name="transmission2">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="actuator2">
            <mechanicalReduction>1.0</mechanicalReduction>
            <!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
        </actuator>
    </transmission>

    <transmission name="transmission3">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="Joint3">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="actuator3">
            <mechanicalReduction>1.0</mechanicalReduction>
            <!-- <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface> -->
        </actuator>
    </transmission>

  <!-- Gazebo Reference macro -->
  <xacro:macro name="RefLink" params="ref">
    <gazebo reference="${ref}">
      <kp>1000000.0</kp>
      <kd>100.0</kd>
      <mu1>30.0</mu1>
      <mu2>30.0</mu2>
      <maxVel>1.0</maxVel>
      <minDepth>0.001</minDepth>
    </gazebo>
  </xacro:macro>

  <gazebo reference="world"/>
  <RefLink ref="Link1"/>
  <RefLink ref="Link2"/>
  <RefLink ref="Link3"/>

  <!-- ros_control plugin -->
  <gazebo>
    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
      <controlPeriod>0.001</controlPeriod>
      <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
      <legacyModeNS>true</legacyModeNS>
    </plugin>
  </gazebo>

</robot>

변경내용

1-1. World 링크를 만들고 base_link와 fixed 조인트 형태로 연결한다. (rviz로만 실행할 때는 moveit assistant에 있는 passive joints에서 설정하면 되지만 gazebo로 실행했을 때는 바닥과 고정되지 않아서 따로 추가해주었다.)

 

1-2. transmission을 추가해준다. 움직이는 모든 조인트에 transmission를 적용한다.

 

1-3. gazebo reference를 추가해준다. 이 작업을 하지 않으면 gazebo에서 매니퓰레이터를 불러왔을 때 조인트에 힘이 들어가지 않고 흐물흐물하게 주저 앉아버린다.

 

1-4.ros_control_plugin을 추가해준다. 이 작업을 추가해야 MoveIt과 Gazebo 사이의 통신이 가능해진다.

 

※ xacro 변경 후 MoveIt assistant를 이용할 때 end-effector나 hand 설정은 따로 하지 않고 모두 arm으로 지정했다.

 

※MoveIt assistant 를 실행한 뒤

Planning Groups에서는 base~Joint4까지 다 넣고

ROS Contrillers에서는 Joint1,2,3만 넣는다.

---

2. launch/gazebo.launch 파일을 수정한다.

  <!-- send robot urdf to param server -->
  <param name="robot_description" command="$(find xacro)/xacro --inorder '$(arg urdf_path)'"/>

2-1. 기존의 textfile="... 내용은 urdf로만 작성했을 때 사용가능한 형태이고, 우리는 xacro형태로 작성했으므로 다음과 같이 변경해준다.

---

3. launch/ros_controllers.launch 파일을 수정한다.

  <!-- Load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn joint_state_controller"/>
  <node name="arm_controller_spawner" pkg="controller_manager" type="controller_manager" respawn="false"
    output="screen" args="spawn arm_controller"/>

3-1. 컨트롤러 노드를 다음과 같이 수정 및 추가해준다.

---

4. config/ros_controllers.yaml 파일을 수정한다.

# Simulation settings for using moveit_sim_controllers
moveit_sim_hw_interface:
  joint_model_group: arm
  joint_model_group_pose: zero
# Settings for ros_control_boilerplate control loop
generic_hw_control_loop:
  loop_hz: 300
  cycle_time_error_threshold: 0.01
# Settings for ros_control hardware interface
hardware_interface:
  joints:
    - Joint1
    - Joint2
    - Joint3
 
  sim_control_mode: 1  # 0: position, 1: velocity
# Publish all joint states
# Creates the /joint_states topic necessary in ROS
joint_state_controller:
  type: joint_state_controller/JointStateController
  publish_rate: 50
arm_controller:
  type: position_controllers/JointTrajectoryController
  joints:
    - Joint1
    - Joint2
    - Joint3
 
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    default: True
    type: FollowJointTrajectory
    joints:
      - Joint1
      - Joint2
      - Joint3

4-1. arm_controller를 추가해주고, controller_list를 수정해준다.

---

5. config/kinematics.yaml 파일을 수정한다.

  position_only_ik: True

5-1. 맨 밑에 다음 내용을 추가해준다. 이 내용을 추가해야 MoveIt에서 로봇을 자유롭게 컨트롤할 수 있다.