Kol ROS 2'de

#!/usr/bin/env python3
"""so101_bridge.py — SO-101'i LeRobot olmadan ROS 2'ye bağlayan en kısa yol.

Feetech STS3215'leri sync read ile 50 Hz okur → /joint_states
/arm/joint_command (sensor_msgs/JointState, radyan) → sync write ile hedef konum.
  pip install feetech-servo-sdk     # scservo_sdk modülü
Kalibrasyon: LeRobot kalibrasyon dosyandaki homing_offset / aralıkları buraya taşı.
"""
import math

import rclpy
from rclpy.node import Node
from sensor_msgs.msg import JointState
from scservo_sdk import COMM_SUCCESS, GroupSyncRead, GroupSyncWrite, PacketHandler, PortHandler

ADDR_GOAL, ADDR_PRESENT, ADDR_TORQUE = 42, 56, 40     # STS3215 kontrol tablosu
JOINTS = ["shoulder_pan", "shoulder_lift", "elbow_flex", "wrist_flex", "wrist_roll", "gripper"]
IDS = [1, 2, 3, 4, 5, 6]
CENTER = [2048] * 6              # her eklemin "sıfır" sayımı — kalibrasyondan doldur
SIGN = [1, 1, 1, 1, 1, 1]        # URDF yönüyle servo yönü ters ise -1
LIMITS = [(-1.9, 1.9), (-1.75, 1.75), (-1.7, 1.7), (-1.65, 1.65), (-2.8, 2.8), (-0.2, 1.7)]


def to_rad(i, ticks):
    return SIGN[i] * (ticks - CENTER[i]) * 2 * math.pi / 4096


def to_ticks(i, rad):
    lo, hi = LIMITS[i]
    rad = min(hi, max(lo, rad))
    return int(round(CENTER[i] + SIGN[i] * rad * 4096 / (2 * math.pi))) & 0xFFFF


class So101Bridge(Node):
    def __init__(self):
        super().__init__("so101_bridge")
        port = self.declare_parameter("port", "/dev/so101_follower").value
        self.port = PortHandler(port)
        self.pkt = PacketHandler(0)                       # STS: protocol_end = 0
        if not (self.port.openPort() and self.port.setBaudRate(1_000_000)):
            raise RuntimeError("port açılamadı: " + port)
        self.reader = GroupSyncRead(self.port, self.pkt, ADDR_PRESENT, 2)
        for i in IDS:
            self.reader.addParam(i)
        self.writer = GroupSyncWrite(self.port, self.pkt, ADDR_GOAL, 2)
        for i in IDS:
            self.pkt.write1ByteTxRx(self.port, i, ADDR_TORQUE, 1)   # torku aç
        self.pub = self.create_publisher(JointState, "joint_states", 10)
        self.create_subscription(JointState, "arm/joint_command", self.on_cmd, 10)
        self.create_timer(0.02, self.tick)

    def tick(self):
        if self.reader.txRxPacket() != COMM_SUCCESS:
            self.get_logger().warn("sync read başarısız", throttle_duration_sec=2.0)
            return
        js = JointState()
        js.header.stamp = self.get_clock().now().to_msg()
        js.name = JOINTS
        js.position = [to_rad(k, self.reader.getData(i, ADDR_PRESENT, 2)) for k, i in enumerate(IDS)]
        self.pub.publish(js)

    def on_cmd(self, m: JointState):
        targets = dict(zip(m.name, m.position))
        self.writer.clearParam()
        for k, (name, i) in enumerate(zip(JOINTS, IDS)):
            if name in targets:
                t = to_ticks(k, targets[name])
                self.writer.addParam(i, [t & 0xFF, (t >> 8) & 0xFF])
        self.writer.txPacket()

    def destroy_node(self):
        for i in IDS:
            self.pkt.write1ByteTxRx(self.port, i, ADDR_TORQUE, 0)   # kapatırken torku bırak
        self.port.closePort()
        super().destroy_node()


def main():
    rclpy.init()
    node = So101Bridge()
    try:
        rclpy.spin(node)
    finally:
        node.destroy_node()
        rclpy.try_shutdown()


if __name__ == "__main__":
    main()

#!/usr/bin/env python3
"""ik.py — SO-101'in yan düzlemi için kapalı form IK + DLS, simülasyondakiyle aynı.

Kullan: ros2 topic pub ile /arm/target (geometry_msgs/Point) ver, kol oraya uzansın.
Uzunluklar yaklaşık; kendi URDF'inden kesin değerleri al.
"""
import math

import numpy as np

L1, L2, L3 = 0.116, 0.135, 0.100


def analytic(x, z, phi, elbow_up=True):
    wx, wz = x - L3 * math.cos(phi), z - L3 * math.sin(phi)
    c2 = (wx**2 + wz**2 - L1**2 - L2**2) / (2 * L1 * L2)
    if not -1 <= c2 <= 1:
        return None                                    # ulaşılamaz
    q2 = (-1 if elbow_up else 1) * math.acos(c2)
    q1 = math.atan2(wz, wx) - math.atan2(L2 * math.sin(q2), L1 + L2 * math.cos(q2))
    return np.array([q1, q2, phi - q1 - q2])


def fk(q):
    a = np.cumsum(q)
    p = np.array([L1 * math.cos(a[0]) + L2 * math.cos(a[1]) + L3 * math.cos(a[2]),
                  L1 * math.sin(a[0]) + L2 * math.sin(a[1]) + L3 * math.sin(a[2])])
    return p, a[2]


def jacobian(q):
    a = np.cumsum(q)
    s, c = np.sin(a), np.cos(a)
    return np.array([
        [-L1 * s[0] - L2 * s[1] - L3 * s[2], -L2 * s[1] - L3 * s[2], -L3 * s[2]],
        [L1 * c[0] + L2 * c[1] + L3 * c[2], L2 * c[1] + L3 * c[2], L3 * c[2]],
        [1.0, 1.0, 1.0],
    ])


def dls(q, x, z, phi, lam=0.02, iters=100, w_phi=0.1):
    W = np.diag([1, 1, w_phi])
    for _ in range(iters):
        p, a = fk(q)
        e = np.array([x - p[0], z - p[1], math.atan2(math.sin(phi - a), math.cos(phi - a))])
        if np.linalg.norm(e[:2]) < 1e-4:
            break
        J = W @ jacobian(q)
        q = q + J.T @ np.linalg.solve(J @ J.T + lam**2 * np.eye(3), W @ e)
    return q


if __name__ == "__main__":
    q = analytic(0.20, 0.05, math.radians(-60))
    print("kapalı form:", np.degrees(q).round(1))
    print("DLS        :", np.degrees(dls(np.array([1.2, -1.6, -0.6]), 0.20, 0.05, math.radians(-60))).round(1))
    # shoulder_pan: hedefin üstten açısı; bu düzlem çözümü o açıyla döndürülmüş düzlemde geçerli

# MoveIt 2: Setup Assistant ile SO-101 konfigürasyonu
sudo apt install -y ros-jazzy-moveit ros-jazzy-moveit-setup-assistant
ros2 launch moveit_setup_assistant setup_assistant.launch.py
#  1) URDF'i yükle (so101_description)       2) Self-collision matrisi: "Generate"
#  3) Planning group "arm": shoulder_pan … wrist_roll   4) "gripper" grubu
#  5) Poses: "home", "ready"     6) ros2_control: joint_trajectory_controller (position)
#  7) Generate → so101_moveit_config paketi

ros2 launch so101_moveit_config demo.launch.py          # mock donanımla RViz'de planla
# Gerçek kol: ros2_control etiketinde mock yerine kendi hardware eklentin (5. haftadaki yapı)
# ya da hızlı yol: joint_trajectory_controller yerine bu köprüye trajectory → joint_command çeviren küçük bir node.

# Kamera kalibrasyonu + AprilTag
sudo apt install -y ros-jazzy-usb-cam ros-jazzy-camera-calibration ros-jazzy-apriltag-ros
ros2 run usb_cam usb_cam_node_exe --ros-args -p video_device:=/dev/video0 -p image_width:=1280 -p image_height:=720

# 9x6 iç köşeli, 25 mm kareli dama tahtası bas (A4'e sığar), kamerayı kalibre et:
ros2 run camera_calibration cameracalibrator --size 9x6 --square 0.025 \
  --ros-args -r image:=/image_raw -r camera:=/
# "Calibrate" → "Save" → usb_cam'e camera_info_url olarak ver.

# tag36h11 ailesinden 4 cm'lik etiket bas (kenar uzunluğunu kumpasla ölç)
ros2 run apriltag_ros apriltag_node --ros-args -r image_rect:=/image_raw -r camera_info:=/camera_info \
  -p family:=36h11 -p size:=0.04
ros2 run tf2_ros tf2_echo camera tag36h11:0