🤖
マニピュレータ制御講習
Koki Muramoto
2025/9/21
マニピュレータ制御講習
環境構築から始め、マニピュレータの簡単な制御、Depthカメラを使用した物体取得などをできるようになることを目指すコースです。 コマンドに触れ、ROS2環境での開発に慣れるとともに、画像認識から制御の流れを学ぶ事ができます。
環境構築
以下のコマンドで必要なリポジトリをクローンします。
mkdir ~/ros2_ws/src && cd ~/ros2_ws/src
git clone https://github.com/basalte1199/detic_onnx_ros2
git clone https://github.com/AgRoboticsResearch/Lerobot_ros2
git clone https://github.com/IntelRealSense/realsense-ros.git
cd ..
colcon build --symlink-install
source install/setup.bash
ビルドが完了したら、次に自分のパッケージを作ります。
cd src
ros2 pkg create --build-type ament_python my_package
cd ..
colcon build --symlink-install
画像認識
RealSenseから出力される画像データから、簡単な画像認識をしてみましょう。 RealSenseを接続して、以下のコマンドを実行します。
source realsense_ws/install/local_setup.bash ros2 launch realsense2_camera rs_launch.py
別のターミナルで次のコマンドを打ち、addタブからimageを選択します
rviz2
RealSenseからの画像を見れたら成功です。
次に画像認識を行います。
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import cv2
from ultralytics import YOLO
class YoloDetectorNode(Node):
def __init__(self):
super().__init__('yolo_detector')
# YOLOv8モデル読み込み
self.model = YOLO("yolov8.pt")
# OpenCV ⇔ ROS変換
self.bridge = CvBridge()
# /camera/image_raw 購読
self.subscription = self.create_subscription(
Image,
"/camera/image_raw",
self.image_callback,
10
)
self.get_logger().info("YOLO Detector Node Started.")
def image_callback(self, msg):
# ROS Image → OpenCV (RGB)
frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
# YOLOで推論
results = self.model(frame, verbose=False)
# 描画 (YOLOv8 の built-in draw)
annotated_frame = results[0].plot()
# 画面に表示
cv2.imshow("YOLO Detection", annotated_frame)
cv2.waitKey(1)
def main(args=None):
rclpy.init(args=args)
node = YoloDetectorNode()
try:
rclpy.spin(node)
except KeyboardInterrupt:
pass
node.destroy_node()
rclpy.shutdown()
cv2.destroyAllWindows()
if __name__ == "__main__":
main()
順運動学
まずはロボットアームを動かしていきましょう。 以下のプログラムを実行してください。
ros2 run so101_hw_interface so101_calibrate --ros-args -p port:=/dev/ttyACM0
ros2 run so101_hw_interface so101_motor_bridge --ros-args -p port:=/dev/ttyACM0
/so101_follower/joint_statesトピックに現在位置が出力され、
/so101_follower/joint_commandsに値を入力することで目標位置を与える事ができます。
ではここで準運動学を解くプログラムを用意していきましょう。 準運動学の計算方法はこちらをご覧ください。
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
import numpy as np
def dh(a, alpha, d, theta):
return np.array([
[np.cos(theta), -np.sin(theta)*np.cos(alpha), np.sin(theta)*np.sin(alpha), a*np.cos(theta)],
[np.sin(theta), np.cos(theta)*np.cos(alpha), -np.cos(theta)*np.sin(alpha), a*np.sin(theta)],
[0, np.sin(alpha), np.cos(alpha), d],
[0, 0, 0, 1]
])
class FKNode(Node):
def __init__(self):
super().__init__('fk_node')
self.subscription = self.create_subscription(
JointState,
'/so101_follower/joint_states',
self.joint_callback,
10
)
self.L1 = 0.10 # base → shoulder_lift
self.L2 = 0.15 # upper arm
self.L3 = 0.15 # forearm
self.L4 = 0.05 # wrist link
self.L5 = 0.05 # tool length
def joint_callback(self, msg):
try:
q1 = msg.position[msg.name.index('shoulder_pan')]
q2 = msg.position[msg.name.index('shoulder_lift')]
q3 = msg.position[msg.name.index('elbow_flex')]
q4 = msg.position[msg.name.index('wrist_flex')]
q5 = msg.position[msg.name.index('wrist_roll')]
except ValueError:
self.get_logger().warn("Joint names do not match expected list.")
return
# ---- DH パラメータ（例）----
# それぞれの行が a, alpha, d, θ(関節角)
H1 = dh(0, np.pi/2, self.L1, q1)
H2 = dh(self.L2, 0, 0, q2)
H3 = dh(self.L3, 0, 0, q3)
H4 = dh(self.L4, 0, 0, q4)
H5 = dh(self.L5, 0, 0, q5)
# ---- 総合変換行列 ----
T = H1 @ H2 @ H3 @ H4 @ H5
# ---- 先端位置 ----
x = T[0, 3]
y = T[1, 3]
z = T[2, 3]
# ---- yaw の取り出し ----
# yaw = atan2(r21, r11)
yaw = np.arctan2(T[1, 0], T[0, 0])
self.get_logger().info(
f"EndEffector → x:{x:.3f} y:{y:.3f} z:{z:.3f} yaw:{yaw:.3f}"
)
def main(args=None):
rclpy.init(args=args)
node = FKNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
実行して、3次元座標とグリッパーのrollが出力されたら成功です。
逆運動学
同様に、逆運動学も実装していきましょう。 マニピュレータは目標の３次元座標を達成するためにそれぞれのモーターの角度を逆算する事ができます。
同じくこちらの記事を参考にしてください
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray # 例えば joint_commands を Float64MultiArray で送る場合
import math
class IKNode(Node):
def __init__(self):
super().__init__('ik_node')
self.pub = self.create_publisher(Float64MultiArray, '/so101_follower/joint_commands', 10)
self.L1 = 0.10 # base height (肩ベースの高さ)
self.L2 = 0.15 # 上腕
self.L3 = 0.15 # 前腕
self.L4 = 0.05 # 手首リンク
self.L5 = 0.05 # エンドエフェクタ長
def ik(self, x, y, z, roll):
# yaw (a1) — ベース回転
a1 = math.atan2(y, x)
# XY 平面における距離
L_axis = math.hypot(x, y)
# z 修正: base 高さを引く
z_rel = z - self.L1
# 2リンク (upper arm + forearm+手首部分) の距離を求める
c = math.sqrt(L_axis**2 + z_rel**2)
# 可到達性チェック
if c > (self.L2 + self.L3) or c < abs(self.L2 - self.L3):
self.get_logger().warn("Target is out of reach: c=%.3f" % c)
return None
# 余弦定理で肘角 (a3) を求める
# c^2 = L2^2 + L3^2 - 2*L2*L3*cos(pi - a3) => a3 = pi - acos(...)
cos_a3 = (self.L2**2 + self.L3**2 - c**2) / (2 * self.L2 * self.L3)
a3 = math.pi - math.acos(cos_a3)
# 肩ピッチ (a2) を求める
# 三角形の内角と z_rel, L_axis を使って
alpha = math.atan2(z_rel, L_axis)
cos_beta = (c**2 + self.L2**2 - self.L3**2) / (2 * c * self.L2)
beta = math.acos(cos_beta)
a2 = alpha + beta
a4 = - (a2 + a3) # 簡易的な “合わせ” — 実機に合わせて修正が必要
# 手首ロール (wrist_roll): 入力 roll をそのまま代入
wrist_roll = roll
return [a1, a2, a3, a4, wrist_roll]
def send_target(self, x, y, z, roll):
angles = self.ik(x, y, z, roll)
if angles is None:
return
msg = Float64MultiArray()
msg.data = angles + [0.0]
self.pub.publish(msg)
self.get_logger().info(f"IK → joint angles: {msg.data}")
def main(args=None):
rclpy.init(args=args)
node = IKNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
目標の姿勢を入力してロボットアームが動けば成功です！