ROS Moveit -> gazebo と ３軸ロボット実機 練習【備忘録】

こんにちはRoboTakaoです

前回Moveit Setup Assistantを使ってみました。

今回はさらにgazeboと実機に繋げたので備忘録を書いておきます。

基本、O'REILLYのプログラミングROSを参考にしています。



前回Moveit Setup Assistantでconfigファイルを作りましたが、そこまでは同じです。

ディレクトリ構成

今回は以下のパッケージ構成となります。変更追加するファイルのみ記載しています。

nx16
  +- launch
      +- nx16_simulation.launch
  +- urdf
      +- nx16.urdf
  +- controllers.yaml

nx16_moveit_config
  +- config
      +- controllers.yaml
  +- launch
     +- nx16_all.launch
     +- nx16_moveit_controller_manager.launch.xml

以下で実行
roslaunch nx16_moveit_config nx16_all.launch



以下ソースコード
(M5Atom のスケッチは前回のもの参照）

nx16/launch/nx16_simulation.launch

<?xml version="1.0" ?>
<launch>
  <param name="robot_description" textfile="$(find nx16)/urdf/nx16.urdf" />
  <include file="$(find gazebo_ros)/launch/empty_world.launch" />
  <node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model" args="-param robot_description -urdf -model nx16" />
  <rosparam file="$(find nx16)/controllers.yaml" command="load"/>
  <node name="controller_spawner" pkg="controller_manager" type="spawner" args="arm_controller"/>
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>
</launch>

nx16/urdf/nx16.urdf

<?xml version="1.0" ?>
<robot name="nx16">
    <material name="blue">
        <color rgba="0.0 0.0 1.0 2.0" />
    </material>
    <material name="red">
        <color rgba="1.0 0.0 0.0 2.0" />
    </material>
    <material name="green">
        <color rgba="0.0 1.0 0.0 2.0" />
    </material>
    <material name="yellow">
        <color rgba="1.0 1.0 0.0 2.0" />
    </material>
    <material name="white">
        <color rgba="1.0 1.0 1.0 2.0" />
    </material>
    <link name="base_link" />
    <joint name="base_joint" type="fixed">
        <parent link="base_link" />
        <child link="link1" />
    </joint>
    <link name="link1">
        <visual>
            <geometry>
                <mesh filename="package://nx16/stl/Link1.stl" />
            </geometry>
            <origin xyz="0 0 0" rpy="0 0 0" />
            <material name="blue" />
        </visual>
        <collision>
            <geometry>
                <box size="0.050 0.050 0.031" />
            </geometry>
            <origin xyz="0 0 0.0155" rpy="0 0 0" />
            <material name="blue" />
        </collision>
        <inertial>
            <origin xyz="0 0 0.0155" rpy="0 0 0" />
            <mass value="0.024534" />
            <inertia ixx="0.000007076" ixy="0" ixz="0" iyx="0" iyy="0.000007076" iyz="0" izx="0" izy="0" izz="0.000010223" />
        </inertial>
    </link>
    <joint name="joint1" type="revolute">
        <parent link="link1" />
        <child link="link2" />
        <origin xyz="-0.00515 0 0.034" rpy="0 0 0" />
        <axis xyz="0 0 1" />
        <limit lower="-1.57" upper="1.57" effort="30" velocity="1.571" />
    </joint>
    <link name="link2">
        <visual>
            <geometry>
                <mesh filename="package://nx16/stl/Link2.stl" />
            </geometry>
            <origin xyz="0 0 0.002" rpy="0 0 0" />
            <material name="red" />
        </visual>
        <collision>
            <geometry>
                <box size="0.025 0.0232 0.039" />
            </geometry>
            <origin xyz="0 -0.0232 0.0215" rpy="0 0 0" />
            <material name="red" />
        </collision>
        <inertial>
            <origin xyz="0 -0.0232 0.0215" rpy="0 0 0" />
            <mass value="0.022" />
            <inertia ixx="0.000003775" ixy="0" ixz="0" iyx="0" iyy="0.000003934" iyz="0" izx="0" izy="0" izz="0.000002133" />
        </inertial>
    </link>
    <joint name="joint2" type="revolute">
        <parent link="link2" />
        <child link="link3" />
        <origin xyz="0 -0.0051 0.0293" rpy="0 0.785398163397 0" />
        <axis xyz="0 1 0" />
        <limit lower="-1.57" upper="1.57" effort="30" velocity="1.571" />
    </joint>
    <link name="link3">
        <visual>
            <geometry>
                <mesh filename="package://nx16/stl/Link3.stl" />
            </geometry>
            <origin xyz="0 0.0021 0" rpy="0 0 0" />
            <material name="green" />
        </visual>
        <collision>
            <geometry>
                <box size="0.012 0.005 0.0977" />
            </geometry>
            <origin xyz="0 0.0172 0.04285" rpy="0 0 0" />
            <material name="green" />
        </collision>
        <inertial>
            <origin xyz="0 0.0172 0.04285" rpy="0 0 0" />
            <mass value="0.021116" />
            <inertia ixx="0.000016841" ixy="0" ixz="0" iyx="0" iyy="0.000017050" iyz="0" izx="0" izy="0" izz="0.000000297" />
        </inertial>
    </link>
    <joint name="joint3" type="revolute">
        <parent link="link3" />
        <child link="link4" />
        <origin xyz="0 0.0082 0.080" rpy="0 -1.570796326795 3.1415926535897931" />
        <axis xyz="0 1 0" />
        <limit lower="-1.57" upper="1.57" effort="30" velocity="1.571" />
    </joint>
    <link name="link4">
        <visual>
            <geometry>
                <mesh filename="package://nx16/stl/Link4.stl" />
            </geometry>
            <origin xyz="0 0.0021 0" rpy="0 0 0" />
            <material name="yellow" />
        </visual>
        <collision>
            <geometry>
                <box size="0.012 0.004 0.092" />
            </geometry>
            <origin xyz="0 0.0041 0.040" rpy="0 0 0" />
            <material name="yellow" />
        </collision>
        <inertial>
            <origin xyz="0 0.0041 0.040" rpy="0 0 0" />
            <mass value="0.005476" />
            <inertia ixx="0.000003870" ixy="0" ixz="0" iyx="0" iyy="0.000003928" iyz="0" izx="0" izy="0" izz="0.000000073" />
        </inertial>
    </link>
    <joint name="control_point_joint1" type="revolute">
        <parent link="link4" />
        <child link="control_point1" />
        <origin xyz="0 0.0061 0.080" rpy="0 0.785398163397 0" />
        <axis xyz="0 1 0" />
        <limit lower="-1.57" upper="1.57" effort="30" velocity="1.571" />
    </joint>
    <link name="control_point1">
        <visual>
            <geometry>
                <box size="0.008 0.004 0.004" />
            </geometry>
            <origin xyz="-0.002 0.002 0" rpy="0 0 0" />
            <material name="white" />
        </visual>
        <collision>
            <geometry>
                <box size="0.008 0.004 0.004" />
            </geometry>
            <origin xyz="-0.002 0.002 0" rpy="0 0 0" />
            <material name="white" />
        </collision>
        <inertial>
            <origin xyz="-0.002 0.002 0" rpy="0 0 0" />
            <mass value="0.0001" />
            <inertia ixx="0.000000003" ixy="0" ixz="0" iyx="0" iyy="0.000000007" iyz="0" izx="0" izy="0" izz="0.000000007" />
        </inertial>
    </link>
    <joint name="control_point_joint2" type="revolute">
        <parent link="control_point1" />
        <child link="control_point2" />
        <origin xyz="-0.006 0 0" rpy="0 0 0" />
        <axis xyz="1 0 0" />
        <limit lower="-1.57" upper="1.57" effort="30" velocity="1.571" />
    </joint>
    <link name="control_point2">
        <visual>
            <geometry>
                <box size="0.004 0.008 0.004" />
            </geometry>
            <origin xyz="-0.002 0 0" rpy="0 0 0" />
            <material name="white" />
        </visual>
        <collision>
            <geometry>
                <box size="0.004 0.008 0.004" />
            </geometry>
            <origin xyz="-0.002 0 0" rpy="0 0 0" />
            <material name="white" />
        </collision>
        <inertial>
            <origin xyz="-0.002 0 0" rpy="0 0 0" />
            <mass value="0.0001" />
            <inertia ixx="0.000000007" ixy="0" ixz="0" iyx="0" iyy="0.000000003" iyz="0" izx="0" izy="0" izz="0.000000007" />
        </inertial>
    </link>
    <transmission name="trans_joint1">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint1_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint2">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint2_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_joint3">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="joint3">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="joint3_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_control_point_joint1">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="control_point_joint1">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="control_point_joint1_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <transmission name="trans_control_point_joint2">
        <type>transmission_interface/SimpleTransmission</type>
        <joint name="control_point_joint2">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
        </joint>
        <actuator name="control_point_joint2_motor">
            <hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
            <mechanicalReduction>1</mechanicalReduction>
        </actuator>
    </transmission>
    <gazebo>
        <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
            <robotNamespace>/</robotNamespace>
            <robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
            <legacyModeNS>true</legacyModeNS>
        </plugin>
        <plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
            <robotNamespace>/</robotNamespace>
            <jointName>joint1, joint2, joint3, control_point_joint1, control_point_joint2</jointName>
        </plugin>
    </gazebo>
</robot>

nx16/controllers.yaml

arm_controller:
  type: position_controllers/JointTrajectoryController
  publish_rate: 50
  joints:
    - joint1
    - joint2
    - joint3
    - control_point_joint1
    - control_point_joint2 

nx16_moveit_config/config/controller.yaml

controller_manager_ns: /
controller_list:
  - name: arm_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    joints:
      - joint1
      - joint2
      - joint3
      - control_point_joint1
      - control_point_joint2 

nx16_moveit_config/launch/nx16_all.launch

<!-- Hand-made -->
<launch>
  <include file="$(find nx16)/launch/nx16_simulation.launch"/>
  <include file="$(find nx16_moveit_config)/launch/move_group.launch"/>
  <include file="$(find nx16_moveit_config)/launch/moveit_rviz.launch">
    <arg name="config" value="True"/>
  </include>
  <node pkg="rosserial_python" type="serial_node.py" name="serial_node">
    <param name="port" value="tcp"/>
  </node>
</launch>

nx16_moveit_config/launch/nx16_moveit_controller_manager.launch.xml

<launch>
  <!-- loads moveit_controller_manager on the parameter server which is taken as argument 
    if no argument is passed, moveit_simple_controller_manager will be set -->
  <arg name="moveit_controller_manager" default="moveit_simple_controller_manager/MoveItSimpleControllerManager" />
  <param name="moveit_controller_manager" value="$(arg moveit_controller_manager)"/>
  <param name="contoller_manager_name" value="/" />
  <param name="use_controller_manager" value="true" />
  <!-- loads ros_controllers to the param server -->
  <rosparam file="$(find nx16_moveit_config)/config/controllers.yaml"/>
</launch>