sciurus17_examples

Sciurus17のためのパッケージ、 sciurus17 で用いるサンプルをまとめたパッケージです。

システムの起動方法

Sciurus17の頭部カメラ、胸部カメラ、制御信号の各ケーブルを制御用パソコンへ接続します。
本体の電源をONにしカメラが接続されていることを確認します。
Terminalを開き、 sciurus17_bringup の sciurus17_bringup.launch を起動します。このlaunchファイルには次のオプションが用意されています。

• use_rviz (default: true)
  Rvizを使用する/使用しない
• use_head_camera (default: true)
  頭部カメラを使用する/使用しない
• use_chest_camera (default: true)
  胸部カメラを使用する/使用しない

簡易シミュレータを使う場合

実機無しで動作を確認する場合、制御信号のケーブルを接続しない状態で次のコマンドを実行します。

roslaunch sciurus17_bringup sciurus17_bringup.launch

物理演算やセンサを含めたシミュレーションは、 後述の「Gazeboを使う場合」の手順に従ってください。

実機を使う場合

実機で動作を確認する場合、制御信号のケーブルを接続し取り扱い説明書に従いモータパワーをONにした状態で次のコマンドを実行します。

roslaunch sciurus17_bringup sciurus17_bringup.launch

カメラを使用しない場合

次のようにオプションを指定するとカメラを使用しない状態で起動します。

roslaunch sciurus17_bringup sciurus17_bringup.launch use_head_camera:=false use_chest_camera:=false

rvizを使用しない場合

次のようにオプションを指定するとrvizによる画面表示を使用しない状態で起動します。画面表示を省略することで制御用パソコンの負荷を下げることができます。

roslaunch sciurus17_bringup sciurus17_bringup.launch use_rviz:=false

Gazeboを使う場合

次のコマンドで起動します。実機との接続やsciurus17_bringupの実行は必要ありません。

roslaunch sciurus17_gazebo sciurus17_with_table.launch

# rvizを使用しない場合
roslaunch sciurus17_gazebo sciurus17_with_table.launch use_rviz:=false

Run Examples

sciurus17_bringup.launchを実行している状態で各サンプルを実行できます。

• gripper_action_example
• neck_joint_trajectory_example
• waist_joint_trajectory_example
• pick_and_place_demo
• hand_position_publisher
• head_camera_tracking
• chest_camera_tracking
• depth_camera_tracking
• preset_pid_gain_example
• box_stacking_example
• current_control_right_arm
• current_control_left_wrist

gripper_action_example

両腕のハンドを開閉させるコード例です。
次のコマンドで26度まで開いて閉じる動作を実行します。

rosrun sciurus17_examples gripper_action_example.py

<img src= https://rt-net.github.io/images/sciurus17/gazebo_gripper_example.gif width=500px />

Videos

back to example list

neck_joint_trajectory_example

首の角度を変更するコード例です。 次のコマンドで頭を上下左右へ向ける動作を実行します。

rosrun sciurus17_examples neck_joint_trajectory_example.py

<img src = https://rt-net.github.io/images/sciurus17/gazebo_neck_example.gif width = 500px />

Videos

back to example list

waist_joint_trajectory_example

腰の角度を変更するコード例です。 次のコマンドで腰を左右へひねる動作を実行します。

rosrun sciurus17_examples waist_joint_trajectory_example.py

<img src = https://rt-net.github.io/images/sciurus17/gazebo_waist_example.gif width = 500px />

Videos

back to example list

pick_and_place_demo

右手でターゲットを掴んで動かすデモ動作を次のコマンドで実行します。腰の回転も使用します。

rosrun sciurus17_examples pick_and_place_right_arm_demo.py

<img src = https://rt-net.github.io/images/sciurus17/gazebo_pick_and_place_right.gif width = 500px />

左手でターゲットを掴んで動かすデモ動作を次のコマンドで実行します。

rosrun sciurus17_examples pick_and_place_left_arm_demo.py

<img src = https://rt-net.github.io/images/sciurus17/gazebo_pick_and_place_left.gif width = 500px />

両手でターゲットを掴んで動かすデモ動作を次のコマンドで実行します。

rosrun sciurus17_examples pick_and_place_two_arm_demo.py

<img src = https://rt-net.github.io/images/sciurus17/gazebo_pick_and_place_two.gif width = 500px />

Videos

back to example list

hand_position_publisher

tfの機能でリンク位置を求めるノード例です。
l_link7とr_link7について、base_linkを基準とした座標をそれぞれ/sciurus17/hand_pos/leftトピックと /sciurus17/hand_pos/rightトピックへ配信します。
次のコマンドでノードを起動します。

rosrun sciurus17_examples hand_position_publisher_example.py

back to example list

head_camera_tracking

頭のカメラを使うコード例です。 OpenCVを使ってボール追跡と顔追跡をします。

次のコマンドでノードを起動します。

rosrun sciurus17_examples head_camera_tracking.py

ボール追跡をする場合

./scripts/head_camera_tracking.pyを編集します。

def _image_callback(self, ros_image):
    # ~~~ 省略 ~~~

        # オブジェクト(特定色 or 顔) の検出
        output_image = self._detect_orange_object(input_image)
        # output_image = self._detect_blue_object(input_image)
        # output_image = self._detect_face(input_image)

<img src = https://rt-net.github.io/images/sciurus17/gazebo_head_camera.gif width = 500px />

顔追跡をする場合

./scripts/head_camera_tracking.pyを編集します。

顔追跡にはカスケード型分類器を使用します。

カスケードファイルのディレクトリを設定してください。 USER_NAME は環境に合わせて書き換えてください。

class ObjectTracker:
    def __init__(self):
        # ~~~ 省略 ~~~

        # カスケードファイルの読み込み
        # 例
        # self._face_cascade = cv2.CascadeClassifier("/home/USER_NAME/.local/lib/python2.7/site-packages/cv2/data/haarcascade_frontalface_alt2.xml")
        # self._eyes_cascade = cv2.CascadeClassifier("/home/USER_NAME/.local/lib/python2.7/site-packages/cv2/data/haarcascade_eye.xml")
        self._face_cascade = cv2.CascadeClassifier("/home/USER_NAME/.local/lib/python2.7/site-packages/cv2/data/haarcascade_frontalface_alt2.xml")
        self._eyes_cascade = cv2.CascadeClassifier("/home/USER_NAME/.local/lib/python2.7/site-packages/cv2/data/haarcascade_eye.xml")

def _image_callback(self, ros_image):
    # ~~~ 省略 ~~~

        # オブジェクト(特定色 or 顔) の検出
        # output_image = self._detect_orange_object(input_image)
        # output_image = self._detect_blue_object(input_image)
        output_image = self._detect_face(input_image)

Videos

動画で使用しているボールは、アールティショップの こちらのページ で購入できます。

back to example list

chest_camera_tracking

胸のカメラを使うコード例です。 OpenCVを使ってボール追跡をします。

次のコマンドでノードを起動します。

rosrun sciurus17_examples chest_camera_tracking.py

<img src = https://rt-net.github.io/images/sciurus17/gazebo_chest_camera.gif width = 500px />

顔追跡とボール追跡の同時実行

頭カメラと胸のカメラの両方を使って、顔追跡とボール追跡をします。

rosrun sciurus17_examples head_camera_tracking.py

# 別のターミナルで実行
rosrun sciurus17_examples chest_camera_tracking.py

Videos

back to example list

depth_camera_tracking

頭の深度カメラを使うコード例です。 指定深度内の物体を追跡します。

次のコマンドでノードを起動します。

rosrun sciurus17_examples depth_camera_tracking.py

デフォルトでは検出範囲を4段階に分けています。 検出範囲を変更する場合は./scripts/depth_camera_tracking.pyを編集します。

    def _detect_object(self, input_depth_image):
        # 検出するオブジェクトの大きさを制限する
        MIN_OBJECT_SIZE = 10000 # px * px
        MAX_OBJECT_SIZE = 80000 # px * px

        # 検出範囲を4段階設ける
        # 単位はmm
        DETECTION_DEPTH = [
                (500, 700),
                (600, 800),
                (700, 900),
                (800, 1000)]

back to example list

preset_pid_gain_example

sciurus17_controlのpreset_reconfigureを使うコード例です。 サーボモータのPIDゲインを一斉に変更できます。

プリセットはsciurus17_control/scripts/preset_reconfigure.py にて編集できます。

次のコマンドを実行すると、preset_reconfigure.pyとpreset_pid_gain_example.pyのノードを起動します。

roslaunch sciurus17_examples preset_pid_gain_example.launch

back to example list

box_stacking_example

<img src = https://rt-net.github.io/images/sciurus17/gazebo_box_stacking.gif width = 500px />

PointCloudLibraryを用いて箱を検出し、箱を積み重ねるコード例です。

次のコマンドでノードを起動します。

roslaunch sciurus17_examples box_stacking_example.launch

RVizでvisualization_msgs/MarkerArrayの/sciurus17/example/markersを表示すると検出した箱を確認できます。

<img src = https://rt-net.github.io/images/sciurus17/rviz_box_stacking.png width = 500px />

Videos

back to example list

current_control_right_arm

右腕を電流制御モードに変更して動かす方法を紹介します。

電流制御モードは、位置制御モードと異なり、サーボに設定された角度リミットが無効になります。 ユーザーの責任において十分に安全に注意した上でご使用下さい。 当該製品および当ソフトウェアの使用中に生じたいかなる損害も株式会社アールティでは一切の責任を負いかねます。

Ctrl+cでサンプルを終了する前に右腕を支えて下さい。 右腕が脱力したときに、腕が物にぶつかる可能性があります。

Gazeboで動かす場合

右腕のhardware_interfaceを変更するため、オプションを追加してGazeboを起動します。

roslaunch sciurus17_gazebo sciurus17_with_table.launch use_effort_right_arm:=true

実機を動かす場合

実機を動かす前に、 Dynamixel Wizard 2.0 等のアプリケーションを用いて、 右腕のサーボモータ(ID2 ~ ID8)のOperating Modeを位置制御から電流制御に変更します。

※サーボモータの制御モード変更については sciurus17_controlのREADME も参照して下さい。

その後、 sciurus17_control/config/sciurus17_cotrol1.yaml を次のように編集します。

• コントローラの種類をeffort_controllers/JointTrajectoryControllerに変更。

right_arm_controller:
-  type: "position_controllers/JointTrajectoryController"
+  type: "effort_controllers/JointTrajectoryController"
  publish_rate: 500

• 制御モードを3(位置制御)から0(電流制御)に変更。

-    r_arm_joint1: {id: 2, center: 2048, home: 2048, effort_const: 2.79, mode: 3 }
-    r_arm_joint2: {id: 3, center: 2048, home: 1024, effort_const: 2.79, mode: 3 }
-    r_arm_joint3: {id: 4, center: 2048, home: 2048, effort_const: 1.69, mode: 3 }
-    r_arm_joint4: {id: 5, center: 2048, home: 3825, effort_const: 1.79, mode: 3 }
-    r_arm_joint5: {id: 6, center: 2048, home: 2048, effort_const: 1.79, mode: 3 }
-    r_arm_joint6: {id: 7, center: 2048, home:  683, effort_const: 1.79, mode: 3 }
-    r_arm_joint7: {id: 8, center: 2048, home: 2048, effort_const: 1.79, mode: 3 }
     r_hand_joint: {id: 9, center: 2048, home: 2048, effort_const: 1.79, mode: 3 }

+    r_arm_joint1: {id: 2, center: 2048, home: 2048, effort_const: 2.79, mode: 0 }
+    r_arm_joint2: {id: 3, center: 2048, home: 1024, effort_const: 2.79, mode: 0 }
+    r_arm_joint3: {id: 4, center: 2048, home: 2048, effort_const: 1.69, mode: 0 }
+    r_arm_joint4: {id: 5, center: 2048, home: 3825, effort_const: 1.79, mode: 0 }
+    r_arm_joint5: {id: 6, center: 2048, home: 2048, effort_const: 1.79, mode: 0 }
+    r_arm_joint6: {id: 7, center: 2048, home:  683, effort_const: 1.79, mode: 0 }
+    r_arm_joint7: {id: 8, center: 2048, home: 2048, effort_const: 1.79, mode: 0 }
     r_hand_joint: {id: 9, center: 2048, home: 2048, effort_const: 1.79, mode: 3 }

ファイル変更後に下記コマンドを実行し、sciurus17のノードを起動します。

roslaunch sciurus17_bringup sciurus17_bringup.launch

コントローラのPIDゲインは、 sciurus17_control/config/sciurus17_cotrol1.yaml で設定されています。 Sciurus17の個体によっては目標姿勢に到達しなかったり、振動する場合があります。 適宜、PIDゲインを変更して下さい。

  right_arm_controller:
    type: "effort_controllers/JointTrajectoryController"
    # --- 省略 ---

    # for current control
    gains:
      r_arm_joint1: { p: 5.0,  d: 0.1, i: 0.0 }
      r_arm_joint2: { p: 5.0,  d: 0.1, i: 0.0 }
      r_arm_joint3: { p: 5.0,  d: 0.1, i: 0.0 }
      r_arm_joint4: { p: 5.0,  d: 0.1, i: 0.0 }
      r_arm_joint5: { p: 1.0,  d: 0.1, i: 0.0 }
      r_arm_joint6: { p: 1.0,  d: 0.1, i: 0.0 }
      r_arm_joint7: { p: 1.0,  d: 0.1, i: 0.0 }

Videos

back to example list

current_control_left_wrist

左手首を電流制御モードに変更して動かす方法を紹介します。

Gazeboで動かす場合

左手首のhardware_interfaceを変更するため、オプションを追加してGazeboを起動します。

roslaunch sciurus17_gazebo sciurus17_with_table.launch use_effort_left_wrist:=true

実機を動かす場合

実機を動かす前に、 Dynamixel Wizard 2.0 等のアプリケーションを用いて、 左手首のサーボモータ(ID16)のOperating Modeを位置制御から電流制御に変更します。

その後、 sciurus17_control/config/sciurus17_cotrol2.yaml を次のように編集します。

• 手首ジョイントのコントローラを追加

      goal_time: 0.0
      stopped_velocity_tolerance: 1.0

+  left_wrist_controller:
+    type: "effort_controllers/JointEffortController"
+    joint: l_arm_joint7
+    pid: {p: 1.0, d: 0.0, i: 0.0}

  left_hand_controller:

• 制御モードを3(位置制御)から0(電流制御)に変更。

    l_arm_joint6: {id: 15, center: 2048, home: 3413, effort_const: 1.79, mode: 3  }
-   l_arm_joint7: {id: 16, center: 2048, home: 2048, effort_const: 1.79, mode: 3  }
    l_hand_joint: {id: 17, center: 2048, home: 2048, effort_const: 1.79, mode: 3  }

    l_arm_joint6: {id: 15, center: 2048, home: 3413, effort_const: 1.79, mode: 3  }
+   l_arm_joint7: {id: 16, center: 2048, home: 2048, effort_const: 1.79, mode: 0  }
    l_hand_joint: {id: 17, center: 2048, home: 2048, effort_const: 1.79, mode: 3  }

最後に、 sciurus17_control/launch/controller2.launch を編集します。

    <node name="controller_manager"
        pkg="controller_manager"
        type="spawner" respawn="false"
        output="screen"
        args="joint_state_controller
-             left_arm_controller
+             left_wrist_controller
              left_hand_controller"/>

ファイル変更後に下記コマンドを実行し、sciurus17のノードを起動します。

roslaunch sciurus17_bringup sciurus17_bringup.launch

サンプルの実行

次のコマンドを実行すると、 左手首を±90度に変化させるサンプルプログラムが起動します。

rosrun sciurus17_examples control_effort_wrist.py

Videos

back to example list