الالتقاط والوضع بالرؤية الحاسوبية: ذراعك الروبوتية تجد الأجسام وتمسكها وتنقلها (الجزء 4)

يحتوي هذا المقال على روابط تسويق بالعمولة مُعلَّمة بعلامة *. بصفتي شريكًا في برنامج Amazon Associates، أربح من عمليات الشراء المؤهلة، دون أي تكلفة إضافية عليك. يساعد ذلك على إبقاء الشروحات مجانية.

سلسلة OmArm Zero

الجزء 1: البناء · الجزء 2: URDF + RViz + Gazebo · الجزء 3: MoveIt 2 · الجزء 4: الالتقاط والوضع بالرؤية الحاسوبية (هذا المقال)

▶

يُحمَّل الفيديو من YouTube (Google)، وقد يضع ملفات تعريف الارتباط (Cookies). سياسة الخصوصية

نظرة سريعة

ما ستبنيه: سلسلة معالجة للالتقاط والوضع موجّهة بالرؤية للذراع OmArm Zero، في Gazebo وعلى الذراع الحقيقية.

تمتلك OmArm Zero خمسة مفاصل في الذراع إضافةً إلى القابض؛ ويعدّها الجزء 1 ذراعًا بست درجات حرية 6-DOF (5+1). لكن مفاصل الذراع الخمسة وحدها هي التي تحدد موضع القابض، لذلك يصفها هذا المقال بأنها ذراع بخمس درجات حرية 5-DOF (DOF = Degrees of Freedom أي درجات الحرية، وهي هنا عدد المفاصل المستقلة).

المتطلبات المسبقة: إتمام الأجزاء من 1 إلى 3: ذراع مُجمّعة مع البرنامج الثابت (Firmware) للاتصال التسلسلي مع ROS 2، وملف URDF يعمل، وإعدادات MoveIt، وجسر العتاد (Hardware Bridge). يجب أن تكون قادرًا على تشغيل real_moveit.launch.py وتحريك الذراع الحقيقية من RViz.

العتاد: أي كاميرا ويب USB (بدقة 720 p أو أعلى) مثبتة على حامل علوي متين، على ارتفاع 0.5 m تقريبًا فوق الطاولة وموجّهة إلى الأسفل مباشرة. طابعة لطباعة علامات ArUco. كتلة صغيرة (كتلتي بأبعاد 3 × 3 × 10 cm وعليها علامة بقياس 31 mm ملصقة على وجهها العلوي؛ يسميها الكود وهذا المقال «المكعب»).

البرمجيات: Ubuntu 24.04، وROS 2 Jazzy، وGazebo Harmonic، وMoveIt 2، وOpenCV (مع ArUco)، وNumPy، وSciPy.

الوقت: عطلة نهاية أسبوع، تشمل المعايرتين كلتيهما وضبط الروبوت الحقيقي.

مستوى الصعوبة: متقدم.

احصل على الحزمة وشغّلها

يعرض الكود في هذه الصفحة المقتطفات الأساسية. أما الكود المصدري الكامل للأجزاء من 2 إلى 4، بكل حزم ROS 2 وملفات التشغيل وأدوات المعايرة وملفات المعاملات، فهو موجود في حزمة مساحة عمل ROS 2 الخاصة بـ OmArm Zero (29 €). البرنامج الثابت (Firmware) لـ ESP32 متاح مجانًا في مستودع OmArm Zero على GitHub؛ أما مساحة عمل ROS 2 فهي متوفرة في الحزمة فقط.

فكّ ضغط الأرشيف ونفّذ البناء مرة واحدة:

cd omarm_zero_ws
source /opt/ros/jazzy/setup.bash
rosdep install --from-paths src --ignore-src -y   # pulls OpenCV, MoveIt, etc.
colcon build --symlink-install
source install/setup.bash

بعد ذلك، تتكون سلسلة معالجة الجزء 4 من خمسة أوامر بهذا الترتيب:

# 1. Print the five ArUco markers (section 3)
ros2 run computer_vision_aruco generate_markers \
    --marker-ids 0,1,2,3,4 --size 600 --dictionary 4X4_50

# 2. Intrinsic calibration — once per camera (section 4)
ros2 launch computer_vision_aruco camera_calibration.launch.py \
    camera_id:=0 checkerboard_width:=9 checkerboard_height:=6 square_size:=0.025

# 3. Extrinsic calibration — after mounting the camera (section 5)
ros2 run computer_vision_aruco calibrate_extrinsics --ros-args \
    --params-file src/computer_vision_aruco/config/aruco_params.yaml \
    -p output_file:=src/computer_vision_aruco/config/camera_extrinsics.yaml

# 4. Pick-and-place in SIMULATION, perception real (section 10)
ros2 launch omarm_zero_moveit_config cube_gazebo.launch.py \
    camera_info_url:=/absolute/path/to/your_calibration.yaml run_demo:=true

# 5. REAL robot: MoveIt, bridge and vision; home the arm in RViz, then start the task (section 11)
ros2 launch omarm_zero_moveit_config cube_real.launch.py \
    transport:=serial camera_info_url:=/absolute/path/to/your_calibration.yaml

قبل الخطوة 3، ثبّت علامات الزوايا الأربع على طاولتك بشريط لاصق. ثم أدخل مواضعها المقيسة في table_markers_expected داخل src/computer_vision_aruco/config/aruco_params.yaml (القسم 5).

1. من تخطيط الحركة إلى الإدراك

في الجزء 3، خطّط MoveIt مسارات خالية من التصادم، وأرسل الجسر النقطة الهدف لكل مسار منها إلى محركات السيرفو الحقيقية عبر ESP32. كان بإمكانك تحريك الذراع إلى أي وضعية يمكن الوصول إليها بسحب علامة تفاعلية (Interactive Marker) في RViz أو بكتابة زوايا المفاصل في سكربت Python. لكن الأهداف كانت دائمًا أرقامًا تُدخلها أنت.

يضيف الجزء 4 كاميرا. تعثر الذراع على مكعب يحمل علامة فوق الطاولة وتلتقطه؛ ولم تعد بحاجة إلى إدخال الهدف بنفسك.

إليك الخطة. تنظر كاميرا USB إلى الطاولة من الأعلى مباشرة، وتكتشف المكعب من خلال العلامة المطبوعة على وجهه العلوي. يتعرّف كاشف ArUco على العلامة ويقدّر وضعيتها بست درجات حرية 6-DOF (الموضع والاتجاه) بالنسبة إلى الكاميرا. ثم تُحوَّل هذه الوضعية عبر خطوة معايرة إلى إطار الإحداثيات الخاص بالروبوت. والنتيجة موضع بالأمتار يُنشر على موضوع (Topic) في ROS 2 تشترك فيه مهمة الالتقاط والوضع. ويحسب حلّ مخصص للحركيات العكسية (IK) زوايا المفاصل التي توصل القابض إلى ذلك الموضع للإمساك العمودي. تنزل الذراع إلى الأسفل مباشرة، وتمسك المكعب، وترفعه، وتضعه عند هدف مُعلَّم. بعد ذلك تعود الذراع إلى وضعية البداية (Home) وتنتظر الدورة التالية.

علامات بدلًا من التعلّم العميق. تعطيك علامة ArUco مطبوعة وضعية كاملة بست درجات حرية 6-DOF من كاميرا ويب واحدة. ولا تحتاج إلى بيانات تدريب ولا إلى معالج رسومي (GPU)؛ إذ يعمل الكشف على المعالج المركزي (CPU). في إعدادي، يبلغ خطأ الموضع بعد المعايرة نحو 1 cm (القسم 5). وهذا كافٍ لذراع مكتبية عليها أن تجد جسمًا واحدًا معروفًا على طاولة. ويمكنك استبدال الكاشف لاحقًا، لأن الواجهة مع بقية النظام هي موضوع واحد في ROS 2 هو /cup_pose. بقي هذا الاسم من كوب كنت أخطط لالتقاطه أولًا؛ ثم انتقلت إلى المكعب وأبقيت اسم الموضوع كما هو. وأي كاشف ينشر رسالة PoseStamped (رسالة ROS تحتوي على موضع واتجاه وطابع زمني واسم الإطار) على هذا الموضوع سيعمل مع النظام مباشرة.

المحاكاة أولًا، مع الكاميرا الحقيقية. يطابق مشهد Gazebo الطاولة الحقيقية، ويظهر المكعب في Gazebo عند الموضع الذي تُبلغ عنه الكاميرا الحقيقية للمكعب الحقيقي. وهكذا تصحّح أخطاء التخطيط والإمساك في المحاكاة، بينما يكون نصف الإدراك حقيقيًا بالفعل. وللانتقال إلى الذراع الحقيقية، تشغّل cube_real.launch.py بدلًا من cube_gazebo.launch.py. وعلى الذراع الحقيقية، تبدأ المهمة بنفسك بعد حركة واحدة إلى وضعية البداية (Home) في RViz، باستخدام ملف المعاملات الخاص بالروبوت الحقيقي pick_place_params_real.yaml. ويسرد القسم 11 ما يختلف على العتاد الحقيقي.

2. مشكلة الإدراك: من البكسلات إلى إحداثيات العالم

تقيس الكاميرا البكسلات، أما الروبوت فيحتاج إلى الأمتار في إطاره الأساسي (Base Frame). والانتقال من هذا إلى ذاك هو المشكلة الجوهرية في رؤية الروبوتات، ويتطلب ثلاثة مكونات.

المعاملات الداخلية (Intrinsics) تصف كيف تُسقط كاميرتك تحديدًا الأشعة ثلاثية الأبعاد (3-D) على بكسلات ثنائية الأبعاد (2-D). يخبرك البعدان البؤريان (fx, fy) بعدد البكسلات التي يمتد عليها راديان واحد، أفقيًا وعموديًا. ويخبرك المركز البصري (cx, cy) بالبكسل الذي يصيبه المحور المركزي للكاميرا. أما معاملات التشوّه فتصف كيف تُحني العدسة الصورة، خاصةً قرب الحواف. تختلف كل وحدة كاميرا قليلًا عن غيرها، حتى بين نسختين من الطراز نفسه، بسبب تفاوتات التصنيع في العدسة وفي موضع المستشعر. لذلك عليك قياس هذه الأرقام لكاميرتك. وتُسمى هذه العملية المعايرة الداخلية (القسم 4).

هندسة العلامة وPnP تعطيانك وضعية العلامة بالنسبة إلى الكاميرا. PnP اختصار لـ Perspective-n-Point. إذا كنت تعرف الحجم الفعلي للعلامة (طول ضلعها بالأمتار)، وكانت الكاميرا قد كشفت الزوايا الأربع بالبكسلات، فلديك أربع تقابلات من 3-D إلى 2-D. وهذا يكفي لحساب الدوران والإزاحة من العلامة إلى الكاميرا. تؤدي الدالة solvePnP في OpenCV ذلك عندما تمرّر إليها المعاملات الداخلية وبكسلات الزوايا وحجم العلامة. والناتج متجه دوران ومتجه إزاحة يصفان معًا وضعية العلامة الكاملة بست درجات حرية (6-DOF) في الإطار البصري للكاميرا.

المعاملات الخارجية (Extrinsics) تخبرك بمكان الكاميرا بالنسبة إلى الروبوت. وهي وضعية ثابتة واحدة للإطار camera_optical_frame بالنسبة إلى base_link. إذا كنت تعرف مكان الكاميرا، وأخبرك PnP بمكان العلامة بالنسبة إلى الكاميرا، فإن ربط الاثنين معًا يعطيك موضع العلامة بالنسبة إلى الروبوت. تتولى TF2 (أو TF اختصارًا)، وهي مكتبة ROS 2 التي تتتبع جميع أطر الإحداثيات والوضعيات بينها، عملية الربط هذه؛ وقد أعددتها في الجزء 2. تبدو السلسلة هكذا:

world == base_link --> camera_optical_frame --> aruco_marker_0 (the cube)
        (extrinsics, static)    (detector, live)

لكل مكوّن نمط فشل خاص به. المعاملات الداخلية الخاطئة تشوّه العالم بحيث تنجرف المواضع نحو حواف الصورة. وحجم العلامة الخاطئ يغيّر مقياس كل شيء (خطأ بنسبة 5 % في عرض العلامة المطبوعة يعني خطأ بنسبة 5 % في المسافة المقدّرة). أما المعاملات الخارجية الخاطئة فتزيح المشهد كله أو تدوّره. والعَرَض الكلاسيكي لمعاملات خارجية سيئة هو أن يتحرك الروبوت إلى موضع يبعد باستمرار من 5 إلى 10 cm عن الجسم.

3. علامات ArUco: وضعيات بست درجات حرية (6-DOF) بتكلفة زهيدة

علامات ArUco هي علامات مرجعية (Fiducials) مربعة ثنائية الترميز (علامات مرجعية مطبوعة يمكن للكاميرا تحديد موقعها بدقة). كل علامة مربع أسود يحتوي على شبكة من الخلايا السوداء والبيضاء تُرمّز معرّفًا (ID) فريدًا على شكل عدد صحيح. وقد صُممت لتقدير الوضعية بالكاميرا في مجال الروبوتات. تكشف وحدة ArUco في OpenCV زواياها الأربع بدقة أقل من البكسل (Sub-pixel)، وتتعرّف عليها من خلال المعرّف، لذا يمكنك التمييز بين العلامات عندما تظهر عدة علامات معًا. ثم تحلّ مسألة PnP باستخدام طول الضلع الفعلي لتحصل على وضعية كاملة بست درجات حرية (6-DOF).

تعمل سلسلة الكشف على النحو التالي. تُحوَّل الصورة إلى تدرّج الرمادي. ثم يطبّق OpenCV العتبة التكيفية (Adaptive Thresholding) لإيجاد جميع المربعات السوداء المرشّحة. ولكل مرشّح، يتحقق مما إذا كان نمط الخلايا الداخلية يطابق معرّفًا صالحًا في القاموس المختار. فإن طابقه، تُحسَّن مواضع الزوايا الأربع إلى دقة أقل من البكسل، ويُفك ترميز معرّف العلامة. ومن هذه الزوايا الأربع مع الحجم الفعلي المعروف، تحسب solvePnP (باستخدام خوارزمية SOLVEPNP_IPPE_SQUARE المحسّنة للعلامات المربعة) دوران العلامة وإزاحتها في إطار الكاميرا. ويعمل الكشف على المعالج المركزي (CPU) دون الحاجة إلى GPU.

أستخدم القاموس DICT_4X4_50، الذي يحتوي على 50 علامة بشبكة داخلية 4×4. الشبكة الأصغر تعني عددًا أقل من البتات في كل علامة، فتكون كل خلية أكبر فعليًا وأسهل في الكشف من مسافات أبعد أو بكاميرات أقل دقة. ولإعداد مكتبي بخمس علامات، فإن 50 علامة أكثر من كافية.

قائمة علاماتي:

المعرّف	الموقع	الحجم	الغرض
0	على الوجه العلوي للمكعب	31 mm	الجسم المراد التقاطه
1	زاوية الطاولة، الأمامية اليمنى (+X, -Y)	31 mm	تجهيزة المعايرة الخارجية
2	زاوية الطاولة، الخلفية اليمنى (-X, -Y)	31 mm	تجهيزة المعايرة الخارجية
3	زاوية الطاولة، الخلفية اليسرى (-X, +Y)	31 mm	تجهيزة المعايرة الخارجية
4	زاوية الطاولة، الأمامية اليسرى (+X, +Y)	31 mm	تجهيزة المعايرة الخارجية / هدف الوضع

الأمام والخلف واليسار واليمين من منظور الروبوت: يشير +X إلى الأمام، ويشير +Y إلى يسار الروبوت.

أنشئها واطبعها باستخدام:

ros2 run computer_vision_aruco generate_markers \
    --marker-ids 0,1,2,3,4 --size 600 --dictionary 4X4_50

نصائح للطباعة:

قِس المربع الأسود المطبوع بالقدمة (Caliper) واستخدم هذه القيمة في ملف الإعدادات، لا القيمة التي طلبتها من الطابعة. فالطابعات تغيّر المقياس. إذا صغّرت طابعتك العلامة بنسبة 5 %، فسيخطئ كل تقدير للمسافة من تلك العلامة بنسبة 5 %. ضبطتُ العلامات الخمس كلها على 31 mm (marker_size: 0.031 في ملف الإعدادات)، لكن طابعتك قد تُخرجها بقياس 30 mm أو 32 mm. وعلى مكعبي، تكون العلامة المطبوعة مع حافتها البيضاء أعرض من الوجه العلوي البالغ 3 × 3 cm، فتبرز خارج حوافه (الصور في القسم 11).

استخدم ورقًا غير لامع. الورق اللامع تحت إضاءة السقف يُحدث انعكاسات تمحو الخلايا السوداء. فيرى الكاشف بقعة بيضاء بدلًا من العلامة ويستبعد هذا الكشف. أما ورق الطابعات النافثة للحبر غير اللامع فمناسب تمامًا.

اترك حافة بيضاء («المنطقة الهادئة» أو Quiet Zone) حول المربع الأسود. يحتاج الكاشف إلى تباين بين حافة العلامة وما يحيط بها ليجد محيط المربع.

تحقق من دقة الكاميرا. تغطي علامة بقياس 31 mm على مسافة 0.5 m نحو 45 بكسلًا عند دقة 1280×720. هذا يعمل، لكنه الحد الأدنى للكشف الموثوق. وعند 640×480، تتقلص العلامة إلى نحو 23 بكسلًا ويصبح الكشف غير موثوق. استخدم دقة 720 p أو أعلى، وأضف مزيدًا من الإضاءة.

4. إعداد الكاميرا والمعايرة الداخلية

ثبّت كاميرا الويب بإحكام، موجّهةً إلى الأسفل مباشرة، وفي منتصف مساحة العمل تقريبًا، على ارتفاع 0.5 m تقريبًا. فكل مليمتر تتحركه الكاميرا بعد المعايرة يعني مليمترًا من الخطأ في الإمساك. استخدمتُ حاملًا صغيرًا للتصوير من الألومنيوم مثبتًا بمشبك على المكتب، وربطتُ الكاميرا بالعارضة الأفقية بأربطة بلاستيكية (Zip Ties). وأي حامل لا يهتز عندما تلمس الطاولة سيفي بالغرض.

ما الذي تفعله المعايرة الداخلية

عندما توجّه الكاميرا نحو خط مستقيم في العالم الحقيقي، يظهر الخط منحنيًا قليلًا في الصورة، خاصةً قرب الحواف. هذا هو تشوّه العدسة، وهو موجود في كل عدسة. يفترض نموذج الكاميرا ذات الثقب (Pinhole Camera Model) أن أشعة الضوء تسير في خطوط مستقيمة عبر نقطة واحدة، لكن العدسات الحقيقية قطع من الزجاج المنحني، وهي تُحني الأشعة.

تقيس المعايرة الداخلية كيف تُحني عدستك تلك الأشعة. والناتج مجموعة من الأرقام: البعدان البؤريان (fx وfy)، والمركز البصري (cx وcy)، وخمسة معاملات تشوّه. وبهذه الأرقام يستطيع OpenCV «إزالة تشوّه» الصورة (Undistort) بحيث تبدو الخطوط المستقيمة في العالم مستقيمة في الصورة من جديد. ويحتاج PnP أيضًا إلى هذه الأرقام لحساب وضعيات دقيقة ثلاثية الأبعاد (3-D) من مواضع البكسلات ثنائية الأبعاد (2-D).

إجراء المعايرة

تُجرى المعايرة باستخدام نمط رقعة شطرنج مطبوع. تحتوي الرقعة على شبكة معروفة من مربعات سوداء وبيضاء متناوبة. يكشف OpenCV الزوايا الداخلية للشبكة (حيث تلتقي أربعة مربعات) ويستخدمها نقاطًا للمعايرة. ومن خلال رؤية الرقعة نفسها بزوايا ومواضع كثيرة في أنحاء الصورة، تحسب الخوارزمية (طريقة Zhang) جميع المعاملات الداخلية دفعة واحدة.

اطبع رقعة شطرنج (المعيار الشائع 9 أعمدة في 6 صفوف من الزوايا الداخلية) وألصقها على سطح مستوٍ وصلب، مثل قطعة من الكرتون. ثم شغّل:

ros2 launch computer_vision_aruco camera_calibration.launch.py \
    camera_id:=0 checkerboard_width:=9 checkerboard_height:=6 square_size:=0.025

square_size هو طول ضلع مربع واحد من رقعة الشطرنج المطبوعة بالأمتار (25 mm هنا). قِس ورقتك المطبوعة وأدخل تلك القيمة.

تُفتح نافذة المعايرة وتعرض البث المباشر من الكاميرا. أمسك رقعة الشطرنج أمام الكاميرا وأمِلها في اتجاهات مختلفة. اضغط SPACE لالتقاط كل وضعية. اجمع 20 وضعية على الأقل. غطِّ مجال الرؤية بأكمله، خاصةً الزوايا والحواف، لأن التشوّه يكون أسوأ هناك. انتهت معايرتي عند دقة 1280×720 بخطأ إعادة إسقاط (Reprojection Error) من نوع RMS (الجذر التربيعي لمتوسط المربعات) قدره 0.21 بكسل. أي أن الزوايا التي يتنبأ بها نموذج الكاميرا المُوائَم تبعد في المتوسط 0.21 بكسل عن الزوايا التي كشفها OpenCV.

قاعدتان:

عايِر بالدقة نفسها التي ستعمل بها. المعاملات الداخلية خاصة بدقة الصورة. فإذا عايرت عند 1920×1080 ثم شغّلت الكاشف عند 1280×720، فستكون الأبعاد البؤرية خاطئة وستنجرف الوضعيات. تعمل إعداداتي عند 1280×720 (frame_width: 1280, frame_height: 720)، لذلك أعاير عند 1280×720.

غطِّ مجال الرؤية بأكمله بوضعيات الرقعة. إذا كانت كل لقطاتك في وسط الصورة، فلن تتوفر للخوارزمية بيانات لتقدير التشوّه عند الحواف. وعاجلًا أم آجلًا سيقع المكعب قرب حافة الصورة، وضعف تصحيح التشوّه هناك يعني تقديرات ضعيفة للوضعية في ذلك الموضع.

اضغط c لتنفيذ المعايرة، و s للحفظ. والنتيجة ملف YAML في ./calibration/camera_calibration_<timestamp>.yaml. يصبح هذا الملف قيمة camera_info_url في جميع عمليات التشغيل اللاحقة.

تحقق من أن عقدة الكاميرا حمّلت ملف المعايرة الخاص بك:

ros2 topic echo /camera/camera_info --once

يجب أن تُظهر المخرجات أبعادك البؤرية، لا أصفارًا أو قيمًا افتراضية. إذا ظهرت مصفوفة وحدة 3×3 بوصفها مصفوفة الكاميرا، فهذا يعني أن ملف المعايرة لم يُحمَّل. تحقق من المسار.

5. المعايرة الخارجية: تجهيزة الطاولة ذات العلامات الأربع

فشلت محاولتي الأولى لتحديد المعاملات الخارجية.

قستُ وضعية الكاميرا يدويًا. كانت على ارتفاع 50 cm تقريبًا، في المنتصف تقريبًا، وموجّهة إلى الأسفل مباشرة. أدخلتُ زوايا الدوران الثلاث للكاميرا في ناشر TF ثابت (Static TF Publisher، وهو عقدة تبث وضعية ثابتة واحدة بين إطارين) ثم شغّلت سلسلة المعالجة. النتيجة: كان موضع المكعب المكتشف خاطئًا، واتجهت الذراع في كل تشغيل إلى نقطة تبعد 10 cm عن المكعب. كان الخطأ ثابتًا، وهذه هي العلامة المميزة لمشكلة في المعايرة، لا لضوضاء عشوائية.

لماذا يفشل قياس زاوية الكاميرا بالعين: ميلان الكاميرا بدرجة واحدة على ارتفاع 0.5 m يُنتج خطأً في الموضع يبلغ نحو 9 mm على سطح الطاولة. ولا يمكنك أن ترى ميلانًا بدرجة واحدة. فالمحور البصري للكاميرا غير مرئي، لذا حتى اتجاه الميلان مجرد تخمين. أما الإزاحة (إزاحة الكاميرا في المستوى XY عن قاعدة الروبوت) فقياسها بالمسطرة أسهل، بخلاف الدوران.

الحل: المعايرة باستخدام أربع علامات مقيسة على الطاولة

لعلامات زوايا الطاولة الأربع مواضع معروفة في إطار الروبوت. تقيس بالمسطرة بُعد مركز كل علامة عن قاعدة الروبوت. وتُدخل هذه القيم في aruco_params.yaml بوصفها المعامل table_markers_expected:

table_markers_expected: '{"1":[0.272,-0.1645],"2":[-0.272,-0.1645],
                          "3":[-0.272,0.1345],"4":[0.272,0.1345]}'

كل إدخال هو موضع مركز العلامة في المستوى XY ضمن الإطار base_link للروبوت، بالأمتار. يقع X الموجب أمام الروبوت، ويقع Y الموجب إلى يساره. والإحداثي Z يساوي صفرًا لأن العلامات مستوية على الطاولة، ولأن base_link يقع على سطح الطاولة.

ترى الكاميرا العلامات نفسها في إطارها البصري الخاص. وبذلك يصبح لديك أربعة أزواج متناظرة من النقاط ثلاثية الأبعاد (3-D): أربع نقاط تعرف مواضعها في إطار الكاميرا وفي إطار الروبوت معًا. وإيجاد الدوران والإزاحة اللذين ينقلان إحدى مجموعتي النقاط إلى الأخرى هو ما تحله خوارزمية Kabsch، المعروفة أيضًا بمسألة Procrustes المتعامدة.

كيف تعمل خوارزمية Kabsch/SVD (للمبتدئين)

لديك مجموعتان من النقاط: P_cam (مواضع العلامات في إطار الكاميرا، محسوبة كمتوسط لنحو 200 إطار صورة لتقليل الضوضاء) و P_base (المواضع التي قستها بالمسطرة). والمطلوب هو الدوران R والإزاحة t بحيث P_base = R * P_cam + t، مع تقليل مجموع مربعات المسافات بين المواضع المتوقعة والمواضع المقيسة إلى أدنى حد.

تعمل الخوارزمية في ثلاث خطوات. أولًا، مركِز مجموعتي النقاط، أي اطرح من كل مجموعة متوسطها. ثانيًا، احسب مصفوفة التغاير المتقاطع H بين مجموعتي النقاط بعد التمركز (مصفوفة 3 × 3 تجمع مقدار تغيّر إحداثيات النقاط المتناظرة معًا). ثم خذ تحليلها إلى القيم المفردة (Singular Value Decomposition أو SVD)، وهو تفكيك قياسي للمصفوفة H إلى مصفوفتين متعامدتين (دورانات، قد يصحبها انعكاس) ومصفوفة قطرية من معاملات التحجيم. ثالثًا، كوّن الدوران من عوامل SVD، مع تصحيح للإشارة يضمن الحصول على دوران حقيقي (لا انعكاس). وتُحسب الإزاحة بعد ذلك من مركزي الثقل والدوران.

التنفيذ في الملف calibrate_extrinsics.py:

@staticmethod
def _kabsch(P_src, P_dst):
    """Rigid transform mapping P_src -> P_dst: dst = R @ src + t."""
    c_s = P_src.mean(axis=0)
    c_d = P_dst.mean(axis=0)
    H = (P_src - c_s).T @ (P_dst - c_d)
    U, _, Vt = np.linalg.svd(H)
    d = np.sign(np.linalg.det(Vt.T @ U.T))
    D = np.diag([1.0, 1.0, d])
    R = Vt.T @ D @ U.T
    t = c_d - R @ c_s
    return R, t

السطر D = np.diag([1.0, 1.0, d]) هو تصحيح الانعكاس. إذا كانت قيمة det(V * U^T) سالبة، فهذا يعني أن SVD أعطى انعكاسًا بدلًا من دوران. وعكس إشارة العمود الثالث يصلح ذلك. ومن دون هذا التصحيح سينعكس المشهد كما في المرآة، وستمتد الذراع نحو مواضع على الجانب الخطأ.

تشغيل المعايرة

تأكد من أن الكاميرا مثبتة، وأن علامات الطاولة الأربع كلها ظاهرة، وأن كاشف ArUco يعمل. ثم:

ros2 run computer_vision_aruco calibrate_extrinsics --ros-args \
    --params-file src/computer_vision_aruco/config/aruco_params.yaml \
    -p output_file:=src/computer_vision_aruco/config/camera_extrinsics.yaml

تجمع العقدة 200 عينة TF لكل علامة (المعامل samples: 200 في aruco_params.yaml)، وتحسب متوسطها لتقليل الارتجاف، ثم تنفذ حل Kabsch، وتكتب النتيجة في camera_extrinsics.yaml. وتعرض قيمة RMS للبواقي (الخطأ النموذجي عبر جميع العلامات). وتخبرك هذه القيمة بمدى دقة وضعية الكاميرا المحسوبة في إعادة إنتاج مواضع العلامات المعروفة:

Marker 1: Residuum 0.78 cm
Marker 2: Residuum 1.12 cm
Marker 3: Residuum 0.85 cm
Marker 4: Residuum 1.03 cm
RMS = 0.95 cm   (Marker: 1,2,3,4)

تبلغ قيمة RMS للبواقي 0.95 cm؛ وأسوأ علامة (ID 2) تنحرف بمقدار 1.12 cm. وبالنسبة إلى مكعب عرضه 3 cm، فهذا كافٍ لإمساك موثوق.

وتتحقق المعايرة أيضًا من معقولية النتيجة. فإذا كانت قيمة Z للكاميرا سالبة (أي أن الكاميرا ستكون تحت الطاولة)، أو إذا لم يكن المحور البصري متجهًا إلى الأسفل، أو إذا تجاوزت قيمة RMS حد 2 cm، فإنها تسجّل تحذيرًا. وتكشف هذه الفحوص أخطاء الإشارة في مواضع العلامات المقيسة، وهي الخطأ الأكثر شيوعًا.

التحقق المستمر

بعد المعايرة، تواصل العقدة cube_pose_publisher مقارنة علامات الطاولة الأربع بمواضعها المتوقعة، وتسجّل قيمة RMS كل ثانيتين. هذا المقطع منسوخ من الطرفية (Terminal) لدي (لقطة الشاشة في القسم 10):

Table-marker verification (base_link X/Y):
  ID 1: detected (+0.278,-0.163) expected (+0.272,-0.165) err=0.6 cm
  ID 2: detected (-0.279,-0.161) expected (-0.272,-0.165) err=0.7 cm
  ID 3: detected (-0.280,+0.128) expected (-0.272,+0.135) err=1.0 cm
  ID 4: detected (+0.280,+0.127) expected (+0.272,+0.135) err=1.1 cm
  RMS error = 0.9 cm

إذا اصطدم أحد بحامل الكاميرا، ترتفع قيمة RMS فورًا. وهناك قاعدة واحدة: علامات الزوايا هي منظومتك المرجعية. لا تحركها أبدًا، ولا تطبع نسخة ثانية من أي علامة زاوية (القسم 9).

6. سلسلة معالجة الرؤية: ثلاث عقد وملف تشغيل واحد

يتكون نظام الرؤية من ثلاث عقد ROS 2 تعمل معًا من ملف إطلاق (Launch File) واحد. لكل عقدة مهمة واحدة، وتتواصل فيما بينها عبر آليات ROS 2 القياسية: المواضيع (Topics) للبيانات، و TF للوضعيات بين الإطارات.

camera_node --image--> aruco_detector_node --TF--> cube_pose_publisher --> /cup_pose
                              |                            |
                       /aruco/debug_image        table collision object,
                                                 RViz markers, RMS log

شغّل خط معالجة الرؤية كاملًا باستخدام:

ros2 launch computer_vision_aruco vision_bringup.launch.py \
    camera_info_url:=/absolute/path/to/your_calibration.yaml

العقدة 1: camera_node

تفتح عقدة الكاميرا كاميرا الويب USB وتلتقط الإطارات بمعدل 30 fps. وتنشرها على شكل رسائل Image في ROS 2 مع رسائل CameraInfo (التي تحتوي على المعايرة الداخلية). وتحدد الإعدادات في aruco_params.yaml جهاز الكاميرا:

camera_node:
  ros__parameters:
    camera_device: "/dev/v4l/by-id/usb-HD_Web_Camera_HD_Web_Camera_Ucamera001-video-index0"
    camera_id: 0                     # fallback only
    frame_rate: 30.0
    frame_width: 1280
    frame_height: 720
    camera_frame: "camera_optical_frame"
    camera_info_url: ""              # path to calibrated intrinsics (override at launch)

نصيحة عملية: استخدم مسار camera_device (الرابط الرمزي /dev/v4l/by-id/…) بدلًا من camera_id. فالحواسيب المحمولة تحتوي على كاميرات ويب مدمجة تحجز هي الأخرى أرقام فهارس الفيديو، وقد تتبادل هذه الفهارس في كل مرة تعيد فيها توصيل كاميرا USB. أما مسار by-id فثابت لأنه مبني على واصف USB الخاص بالكاميرا، لا على ترتيب تعدادها. وعلى حاسوبي المحمول تبادلت الكاميرا المدمجة وكاميرا USB المواقع بين video0 و video2 بعد إعادة التوصيل أو إعادة التشغيل، لذلك تستخدم إعداداتي مسار by-id.

العقدة 2: aruco_detector_node

تشترك هذه العقدة في صور الكاميرا، وتكشف علامات ArUco، وتقدّر وضعياتها، وتبث إطارات TF. وهذا هو الإعداد من الكود المصدري:

class ArucoDetectorNode(Node):
    def __init__(self):
        super().__init__('aruco_detector_node')

        self.declare_parameter('marker_size', 0.05)
        self.declare_parameter('marker_sizes', '')       # per-ID JSON
        self.declare_parameter('dictionary', '4X4_50')
        self.declare_parameter('camera_frame', 'camera_link')
        self.declare_parameter('publish_tf', True)
        self.declare_parameter('tf_prefix', 'aruco_marker_')

        # ... parameter loading ...

        # Initialize ArUco detector (compatible with OpenCV 4.7+)
        self.aruco_dict = self.get_aruco_dictionary(self.dictionary_name)
        self.aruco_params = self.get_detector_parameters()
        cv_version = tuple(map(int, cv2.__version__.split('.')[:2]))
        if cv_version >= (4, 7):
            self.aruco_detector = cv2.aruco.ArucoDetector(
                self.aruco_dict, self.aruco_params)
            self.use_new_api = True

تدعم الإعدادات تحديد حجم كل علامة حسب ID على شكل سلسلة JSON، فيمكنك استخدام أحجام فعلية مختلفة لعلامات مختلفة. وفي إعدادي، جميع العلامات الخمس بحجم 31 mm:

aruco_detector_node:
  ros__parameters:
    marker_size: 0.031
    marker_sizes: '{"0": 0.031, "1": 0.031, "2": 0.031, "3": 0.031, "4": 0.031}'
    dictionary: "4X4_50"
    camera_frame: "camera_optical_frame"
    publish_tf: true
    tf_prefix: "aruco_marker_"

لكل علامة مكتشفة، تبث العقدة إطار TF باسم aruco_marker_<id> نسبةً إلى camera_optical_frame. كما تنشر صورة تصحيح (Debug) على /aruco/debug_image تُظهر العلامات المكتشفة مع مربعاتها المحيطة وأرقام ID مرسومة فوقها. افتح هذه الصورة في rqt_image_view (أداة ROS صغيرة تعرض موضوع صورة) للتحقق من أن الكشف يعمل.

العقدة 3: cube_pose_publisher

تحوّل هذه العقدة نتائج الكشف الخام إلى بيانات تستطيع مهمة الالتقاط والوضع استخدامها. فالاستعلام المباشر عن TF يعطيك موضع العلامة، لكن هذا الموضع يرتجف من إطار إلى آخر، وقد تظهر فيه قيم شاذة في إطار منفرد. كما أنه لا يخبرك إن كانت العلامة لا تزال مرئية أو إن كانت البيانات قديمة. وتضيف عقدة نشر وضعية المكعب أربع وظائف فوق الكشف الخام.

التنعيم بمرشح EWMA. ترتجف علامة بحجم 31 mm على مسافة 0.5 m بضعة مليمترات من إطار إلى آخر بسبب ضوضاء الصورة، وعدم اليقين في كشف الزوايا، والاهتزاز الطفيف للكاميرا. ويعمل مرشح المتوسط المتحرك الأسي الموزون (Exponentially Weighted Moving Average أو EWMA) على تثبيت الموضع. و EWMA مرشح يُمزج فيه كل موضع جديد مع الموضع المنعَّم السابق باستخدام وزن alpha:

smoothed = alpha * new + (1 - alpha) * smoothed_previous

مع smoothing_alpha: 0.3، يساهم كل كشف جديد بنسبة 30 % ويساهم السجل السابق بنسبة 70 %. وينتج عن ذلك موضع سلس يستجيب للحركة الحقيقية خلال بضعة إطارات، لكنه يرشّح الضوضاء التي تظهر في إطار منفرد. وهذا منطق الترشيح من الكود المصدري:

if self.smoothed is not None and self._reject_count < 5:
    jump = math.dist((x, y, z), self.smoothed)
    if jump > self.min_jump:
        self._reject_count += 1
        self.get_logger().warn(
            f'Cube pose jump {jump*100:.1f} cm > '
            f'{self.min_jump*100:.1f} cm — ignoring outlier '
            f'({self._reject_count}/5)')
    else:
        self._reject_count = 0
        x = self.alpha * x + (1 - self.alpha) * self.smoothed[0]
        y = self.alpha * y + (1 - self.alpha) * self.smoothed[1]
        z = self.alpha * z + (1 - self.alpha) * self.smoothed[2]
        self.smoothed = (x, y, z)

رفض القيم الشاذة مع التعافي الذاتي. إذا قفز كشف جديد عن الموضع المنعَّم بأكثر من min_position_jump (القيمة الافتراضية 5 cm)، فإنه يُرفض بوصفه قيمة شاذة. ويعالج هذا أخطاء الكشف العابرة في إطار منفرد، حين يخطئ تحديد موضع زوايا العلامة. أما إذا نُقل المكعب إلى موضع جديد، فإن «القيم الشاذة» المتتالية ستتوافق فيما بينها. وبعد 5 حالات رفض متتالية، يفترض المرشح أن المكعب تحرك فعلًا، ويعيد تهيئة نفسه على الموضع الجديد.

حداثة البيانات. لا يُنشر الموضوع /cup_pose إلا ما دامت العلامة مرئية وبيانات TF حديثة (عمرها أقل من pose_timeout_sec ثانية). وإذا خرج المكعب من مجال الرؤية أو حُجبت العلامة، يتوقف تحديث /cup_pose. وتتحقق مهمة الالتقاط من ذلك، ولا تتصرف أبدًا بناءً على وضعية قديمة.

كائن التصادم للطاولة. تضيف العقدة الناشرة أيضًا كائن تصادم على شكل صندوق يمثل الطاولة إلى مشهد التخطيط في MoveIt. ويمنع هذا MoveIt من تخطيط مسارات تمرّ بالذراع عبر سطح الطاولة. وتأتي أبعاد الطاولة وموضعها من الإعدادات:

cube_pose_publisher_node:
  ros__parameters:
    marker_frame: "aruco_marker_0"
    target_frame: "world"
    cube_size_x: 0.03
    cube_size_y: 0.03
    cube_size_z: 0.10
    cube_center_z: 0.05
    publish_rate: 10.0
    pose_timeout_sec: 3.0
    smoothing_alpha: 0.3
    min_position_jump: 0.05
    enable_planning_scene: false     # cube collision OFF (blocks grasp plans)
    enable_table: true
    table_size_x: 0.60
    table_size_y: 0.35
    table_size_z: 0.02

لاحظ القيمة enable_planning_scene: false للمكعب نفسه. جرّبت إضافة المكعب ككائن تصادم، لكنه منع خطة الإمساك. فعندما يجب أن تلتف الأصابع حول المكعب، يمنع وجود كائن تصادم في موضع المكعب MoveIt من تخطيط الاقتراب. وبدلًا من ذلك، يُعرض المكعب كعلامة RViz زرقاء (للعرض فقط، دون تصادم)، وتبقى الطاولة كائن تصادم لحماية الذراع.

7. حل مخصص للحركيات العكسية (IK) بخمس درجات حرية (5-DOF) للإمساك من الأعلى

تخبرنا الكاميرا أن المكعب في الموضع (x, y). فما زوايا المفاصل التي تضع القابض هناك، والأصابع متجهة مباشرة إلى الأسفل؟

في الجزء 3، ضبطت MoveIt على position_only_ik: true، لأن ذراعًا بخمس درجات حرية (5-DOF) لا تستطيع تلبية طلب وضعية اعتباطية بست درجات حرية (6-DOF). فمحلّل IK الافتراضي في MoveIt، وهو KDL (Kinematics and Dynamics Library)، سيفشل مع معظم الأهداف إذا طلبت الموضع والاتجاه معًا. نجح وضع «الموضع فقط» مع طلبات مثل «اذهب إلى هذه النقطة XYZ»، لكن الإمساك يحتاج إلى أكثر من الموضع. فيجب أن تقترب الأصابع عموديًا، متجهة مباشرة إلى الأسفل. وإذا اقتربت بزاوية مائلة، فإنها تُسقط المكعب بدلًا من الإمساك به.

احسب درجات الحرية. يحتاج الإمساك من الأعلى إلى الأسفل إلى:

• موضع الأداة: 3 قيود (X, Y, Z).
• محور الاقتراب المتجه مباشرة إلى الأسفل: 2 من القيود (إذ يتحدد اتجاه المحور بزاويتين).

وهذا يعني 5 قيود إجمالًا. أما الدوران حول المحور العمودي فيبقى حرًا: فالذراع ذات درجات الحرية الخمس (5-DOF) لا تستطيع اختياره، لذلك يُغلق القابض عند أي دوران يعطيه الحل. خمسة قيود لخمسة مفاصل: المسألة محددة تمامًا ولا تترك أي هامش. ولا يستطيع محلّل KDL حل هذا الهدف المختلط من الموضع ومحور الاقتراب، لذلك كتبت محللي الخاص.

المحلّل arm_ik

تنفّذ الوحدة البرمجية arm_ik.py ذلك مباشرة. فهي تحلل ملف URDF لبناء سلسلة الحركيات الأمامية (FK) من base_link عبر المفاصل الدوّارة الخمسة جميعها حتى الإطار tcp (الحركيات الأمامية: حساب وضعية الأداة من زوايا مفاصل معطاة). ويُعرَّف الإطار tcp (نقطة مركز الأداة، Tool Center Point) في URDF عند منتصف المسافة بين أطراف الأصابع، مدوَّرًا بحيث يكون محوره +Z هو محور الاقتراب/الأصابع. ثم يجد مُحسِّن المربعات الصغرى المقيَّد في SciPy زوايا المفاصل التي تقلل نوعين من البواقي (Residuals) في آن واحد: خطأ الموضع (3 مركّبات) وخطأ محاذاة المحور (2 من المركّبات).

إعداد المحلّل:

JOINT_CHAIN = ['Revolute 1', 'Revolute 2', 'Revolute 3', 'Revolute 4',
               'Revolute 5', 'gripper_base_to_tcp']

class ArmIK:
    def __init__(self, xacro_path=None):
        # Parse the URDF chain base_link -> tcp
        if xacro_path is None:
            xacro_path = os.path.join(
                os.path.dirname(__file__), '..', '..', '..', 
                'omarm_zero_description', 'urdf', 'omarm_zero.xacro')
        # ... parse joint origins, axes, limits from the URDF ...
        self.limits = np.array(self.limits)   # (5,2) lower/upper bounds

    def fk(self, q):
        """Forward kinematics base_link -> tcp. Returns 4x4 transform."""
        T = np.eye(4)
        qi = 0
        for e in self.chain:
            T = T @ _T(e['R0'], e['xyz'])
            if e['type'] == 'revolute':
                T = T @ _T(_axis_R(e['axis'], q[qi]), np.zeros(3))
                qi += 1
        return T

تُبنى سلسلة الحركيات الأمامية من أوصاف المفاصل في URDF، لذلك ينتقل أي تغيير في URDF (أطوال وصلات مختلفة، اتجاهات محاور مفاصل مختلفة) تلقائيًا. ولا يحتوي المحلّل على أطوال وصلات أو محاور مفاصل مثبتة في الكود.

يعتمد المحلّل استراتيجية نقاط البداية المتعددة (Multi-seed). فنقطة البداية الواحدة (التخمين الأولي) قد تتقارب نحو قيمة صغرى محلية أو نحو فرع حلول سيئ. لذلك يولّد المحلّل بدلًا منها العديد من التكوينات الابتدائية المرشحة، ويختار أفضل نتيجة:

def ik_solve(self, target_xyz, approach_dir=(0, 0, -1), seed=None,
             align_axis=2, w_orient=0.1, pos_tol=0.01, ang_tol=15.0,
             quick=False):
    """Solve for [Rev1..Rev5] via bounded least-squares from many seeds.

    Places the tcp at target_xyz AND aligns its axis 'align_axis' 
    (2=Z = finger/approach axis) with 'approach_dir' (default straight 
    down). Returns list[5] joints, or None if no seed converges."""
    from scipy.optimize import least_squares
    # ... 1500 random samples scored by FK, best become solver seeds ...
    # ... structured sweeps of the joint space ...
    # ... current arm state as a seed for continuity ...

توليد نقاط البداية بأخذ العينات ثم التحسين: يُقيَّم 1500 تكوين عشوائي للمفاصل تقييمًا منخفض الكلفة بالحركيات الأمامية (فحص للموضع واتجاه المحور). وتصبح أكثر التكوينات المرشحة واعدًا نقاطَ بداية للمحلّل، إلى جانب عمليات مسح منظَّمة وحالة المفاصل الحالية. وبهذا يجد المحلّل فروع حلول كانت ستفوتها نقطة بداية واحدة.

الوضع quick=True: في عمليات مسح قابلية الوصول («هل لهذا الارتفاع حل أصلًا؟»)، يجيب حلّ مختصر بعدد أقل من العينات بـ«لا» خلال 1 إلى 2 ثانية بدلًا من 15 ثانية. وتستخدم المهمة المسح السريع أولًا لعمليات البحث الواسعة، ثم تعود إلى المحلّل الكامل قبل الوثوق بأي «لا».

تقييم الحلول: من بين جميع الحلول الصالحة (التي تستوفي pos_tol و ang_tol معًا)، يفضّل المحلّل الحلول القريبة من حالة المفاصل الحالية (لتكون الحركات بين نقاط المسار صغيرة) وذات وضعية معصم سليمة. ويحظى المعيار الثاني بقسم خاص به، لأنه أصلح أعند خلل واجهته.

8. عائلتا المعصم

على الروبوت الحقيقي، ظل الإمساك يقع على بعد سنتيمتر واحد بجانب المكعب، ودائمًا على الجانب نفسه. وكان العَرَض الظاهر أن المعصم بدا ملتويًا، وهذا الالتواء هو ما سبب الإزاحة. كانت معايرة الكاميرا سليمة (RMS بقيمة 1 cm)، وخطأ الموضع في IK بين 0 و 2 mm، والكاشف يجد علامة المكعب. أما السبب فكان هندسة المعصم.

عند محور اقتراب عمودي (الأصابع متجهة مباشرة إلى الأسفل)، يملك هذا المعصم عائلتين من الحلول، ولا شيء بينهما.

العائلة	دوران الساعد (J4، أي Revolute 4 في URDF)	ميل المعصم (J5، أي Revolute 5 في URDF)	كيف يبدو
«ملتوية»	حوالي -180 درجة (عند حد التوقف النهائي)	حوالي -18 درجة	المعصم مُدار بالكامل، والإزاحة متجهة إلى الجانب
«محايدة»	حوالي 0 درجة	حوالي -162 درجة	المعصم مستقيم، والإزاحة متجهة بشكل صحيح

كان المحلّل يجد العائلة الملتوية أولًا دائمًا، لأن نقاط البداية المنظَّمة لديه كانت تبدأ قرب حدود المفاصل، حيث يقع التكوين الملتوي. ولأن للإطار tcp إزاحة جانبية صغيرة عن محور المعصم (فمنتصف أطراف الأصابع لا يقع تمامًا على مركز الدوران)، فإن العائلتين تعكسان هذه الإزاحة كما في المرآة. اختر العائلة الخطأ، وستتجه الإزاحة المعايَرة في الاتجاه الخطأ. فتهبط الأصابع بجانب المكعب بدلًا من أن تحيط به.

الحل

حلّ تعديلان في arm_ik.py المشكلة:

أضف نقاط بداية للعائلة المحايدة صراحةً. لا يستطيع المحلّل تفضيل فرع لا يجده أصلًا. وقد أُضيفت نقاط بداية نموذجية لتكوين المعصم المحايد (J4 قرب الصفر، و J5 قرب -162 درجة) إلى مجموعة نقاط البداية، فيستكشف المحلّل العائلتين دائمًا.

تفضيل الوضعية في التقييم. تجعل عقوبة صغيرة على القيمة المطلقة لـ J4 العائلةَ المحايدة تفوز كلما كانت موجودة. ولا يزال المحلّل يجد العائلة الملتوية كحل صالح، لكنها تأتي في مرتبة أدنى من المحايدة.

تنبيه واحد: العائلة المحايدة ليست موجودة في كل مكان. فحدود المفاصل تلغيها في المنطقة الخلفية اليمنى خلف الروبوت (x < 0 و y < 0، باتجاه العلامة 2). أما في المنطقة الأمامية/الوسطى (حيث تضع المكعب عادةً)، فتحصل على عمليات إمساك دقيقة بمعصم محايد. وعند الأطراف، تحذّرك المهمة («الحل هنا تقريبي، وقد ينحرف الإمساك») بدلًا من أن تفشل بصمت. وهذا قصور في ذراع بخمس درجات حرية (5-DOF) ذات إزاحة في tcp.

9. مهمة الالتقاط والوضع

يشغّل الملف cup_pick_and_place.py دورة الالتقاط والحمل والوضع كاملة. ومثل /cup_pose، سُمّي الملف على اسم الكوب (Cup) الذي خططت لالتقاطه في البداية.

التسلسل الكامل

open gripper --> fly over (adaptive height) --> descend linearly --> close gripper
  --> lift linearly --> carry --> fly over place target --> settle linearly
  --> open gripper --> retreat linearly --> home

لماذا يهم الهبوط الخطي

كانت نسختي الأولى ترسل موضع ما قبل الإمساك وموضع الإمساك إلى MoveIt كهدفين منفصلين في فضاء المفاصل (Joint Space). وهذا هو النهج البديهي: خطط للوصول إلى ما فوق المكعب، ثم خطط للوصول إلى موضع الإمساك. المشكلة: هدف فضاء المفاصل يجري الاستكمال (Interpolation) على زوايا المفاصل، لا على موضع القابض. لذلك يتحرك القابض في الفضاء الديكارتي (إحداثيات X, Y, Z للطاولة) على قوس بدلًا من خط مستقيم. وفي أثناء النزول، انجرفت الأصابع جانبيًا ودفعت المكعب بعيدًا قبل أن تُغلق أصلًا. اكتشفت ذلك في التوأم الرقمي على Gazebo، قبل أن تنفّذ الذراع الحقيقية المهمة.

يتكون الحل من شقين.

التحليق التكيفي. تبحث المهمة نزولًا من grasp_z + approach_height عن أعلى ارتفاع فوق المكعب يمكن بلوغه عموديًا. وتستخدم لذلك عمليات مسح IK سريعة (الوضع quick=True)، مع الرجوع إلى المحلّل الكامل لأي ارتفاعات يرفضها الوضع السريع. ثم تخطط حركة MoveIt عادية واحدة في فضاء المفاصل إلى نقطة التحليق هذه. ولأن نقطة التحليق عالية فوق المكعب، فلا يوجد ما يمكن الاصطدام به، ولا يهم شكل القوس.

الهبوط الخطي. من نقطة التحليق، تأخذ المهمة قيم Z بخطوات قدرها 1 cm من ارتفاع التحليق نزولًا إلى ارتفاع الإمساك. وفي كل خطوة، تحل IK مستخدمة حل الخطوة السابقة نقطةَ بداية. ويُبقي هذا المحلّل في فرع الحلول نفسه (فلا تنقلب عائلة المعصم في منتصف الهبوط)، وينتج مسارًا متصلًا للمفاصل. وتُرسل كل خطوة كأمر مسار مستقل من نقطة واحدة إلى متحكم مسارات المفاصل (Joint Trajectory Controller). فينزل tcp على خط عمودي فوق المكعب بخطوات قدرها 1 cm.

وتعمل الطريقة نفسها بالعكس عند الرفع. فالحركة على قوس أثناء الرفع ستجرّ المكعب الممسوك جانبيًا.

إعداد العقدة

class CupPickAndPlace(Node):
    def __init__(self):
        super().__init__('cup_pick_and_place')

        self.declare_parameter('grasp_z', 0.04)
        self.declare_parameter('approach_height', 0.10)
        self.declare_parameter('lift', 0.06)
        self.declare_parameter('approach_dir', [0.0, 0.0, -1.0])
        self.declare_parameter('place_x', 0.15)
        self.declare_parameter('place_y', 0.10)
        self.declare_parameter('place_z', 0.10)
        self.declare_parameter('gripper_open', [-1.2, -1.2])
        self.declare_parameter('gripper_closed', [0.9, 0.9])
        self.declare_parameter('home_joints', [0.0, 0.0, 0.0, 0.0, 0.0])

        # 5-DOF IK solver (loads the URDF chain base_link -> tcp)
        self.ik = arm_ik.ArmIK()

        # MoveGroup action client for arm planning
        self._mg_client = None

        # Gripper: direct FollowJointTrajectory action
        self._gripper_client = ActionClient(
            self, FollowJointTrajectory,
            '/joint_trajectory_controller/follow_joint_trajectory')

        # Direct arm-trajectory publisher for LINEAR descent/ascent
        self._arm_traj_pub = self.create_publisher(
            JointTrajectory,
            '/joint_trajectory_controller/joint_trajectory', 10)

شرح الإعدادات: pick_place_params.yaml (المحاكاة)

معاملات المحاكاة:

cup_pick_and_place:
  ros__parameters:
    grasp_z: 0.07            # fingertip height at grasp
    approach_height: 0.10    # fly-over height ABOVE grasp_z
    lift: 0.06               # linear lift after closing
    approach_dir: [0.0, 0.0, -1.0]   # tcp +Z points straight DOWN
    place_x: 0.15
    place_y: 0.10
    place_z: 0.07
    gripper_open: [-1.2, -1.2]
    gripper_closed: [0.16, 0.16]
    gripper_time_sec: 2.0
    gripper_joints: ['Revolute 6', 'Revolute 7']

القيمة grasp_z: 0.07 هي ارتفاع أطراف الأصابع (الإطار tcp) لحظة الإمساك. يبلغ ارتفاع المكعب 10 cm، لذا يقع سطحه العلوي عند Z = 0.10. ومع grasp_z بقيمة 0.07، تنزل أطراف الأصابع إلى 3 cm تحت قمة المكعب، فتتمركز القبضة على الجزء العلوي منه.

تُضاف القيمة approach_height: 0.10 إلى grasp_z للحصول على ارتفاع التحليق الابتدائي. وتبحث المهمة نزولًا من هذا الارتفاع، لذا فاختيار قيمة كبيرة آمن. وتنتقل الذراع إلى أعلى نقطة يمكن بلوغها ضمن هذا المدى.

approach_dir: [0.0, 0.0, -1.0] يحدد أن المحور +Z للإطار tcp (محور الأصابع/محور الاقتراب) يتجه مباشرة إلى الأسفل في إطار العالم (world frame). وهذا يعطي إمساكًا عموديًا من الأعلى إلى الأسفل.

gripper_open و gripper_closed هما قيم مفصلَي القابض (Revolute 6 و Revolute 7 في ملف URDF). القيم السالبة تفتح الأصابع، والقيم الموجبة تغلقها. وتعتمد هذه القيم على هندسة القابض لديك وعلى معايرة السيرفو (القسم 11).

الوضع عند علامة

يمكن أن يكون هدف الوضع موضعًا ثابتًا أو الوضعية المكتشفة لحظيًا لعلامة ArUco أخرى. تستخدم إعدادات الروبوت الحقيقي العلامة 4 (إحدى علامات المعايرة في زوايا الطاولة):

place_marker_frame: 'aruco_marker_4'

تقرأ المهمة الموضع اللحظي للعلامة من TF وتستخدمه هدفًا للوضع. تقع العلامة 4 في زاوية الطاولة، خارج مدى وصول الذراع. وبدلًا من أن تفشل، تُزيح المهمة الهدف نحو الداخل على امتداد الخط الواصل بين القاعدة والعلامة حتى تجد الحركيات العكسية (IK) حلًا للإمساك العمودي:

# Slide the target radially inward on the base->marker line
# until BOTH the fly-over AND the settle height are solvable.
r = math.hypot(mx, my)
ux, uy = (mx / r, my / r) if r > 1e-6 else (1.0, 0.0)
rr = min(r, 0.22)   # beyond ~22 cm nothing is reachable
while rr >= 0.10:
    tx, ty = ux * rr, uy * rr
    if (self.ik.ik_solve((tx, ty, pz + 0.05), ...) is not None
            and self.ik.ik_solve((tx, ty, pz), ...) is not None):
        found = (tx, ty)
        break
    rr -= 0.01

تسجّل المهمة هذا التعديل والهدف الجديد (انظر السجل في القسم 11).

لوضع المكعب في مكان آخر، اطبع علامة بمعرّف (ID) خاص بها (مثلًا ID 5، وبمقاس 31 mm أيضًا) واضبط place_marker_frame: ‘aruco_marker_5’. يبث الكاشف إطار TF لكل معرّف يراه، ويستخدم marker_size (31 mm) لأي معرّف غير موجود في marker_sizes. لا تطبع علامة ثانية بمعرّف إحدى علامات الزوايا: فعلامتان بالمعرّف نفسه تعطيان موضعين متعارضين لإطار TF واحد، فيتعطل فحص علامات الزوايا المستمر الموصوف في القسم 5.

انتظر الاستقرار قبل الفتح

عندما كنت أُفلت المكعب على ارتفاع التحويم، كان يسقط وينقلب. لذلك تُنزل المهمة الذراع حتى يقف المكعب على الطاولة. في الروبوت الحقيقي يكون المكعب أعلى داخل القبضة، لذا خفّضت place_z إلى 0.01. تواصل الذراع النزول بعد أن يلامس المكعب الطاولة، فتنزلق الأصابع على جانبيه في السنتيمتر الأخير وتضغطه ليستقر مستويًا. تنتهي أطراف الأصابع على ارتفاع 1 cm تقريبًا فوق الطاولة. وعندها فقط تنفتح الأصابع. ثم تتراجع الذراع إلى الأعلى مباشرة حتى لا تُسقط الأصابع المكعب بعد وضعه.

الفشل بأمان

تُفحص كل نقطة طريق (Waypoint) بالحركيات العكسية (IK) قبل أن تبدأ أي حركة. وإذا كان موضع المكعب خارج مدى الوصول، تتوقف المهمة مع تلميح واضح («حرّك المكعب بضعة سنتيمترات نحو منتصف الطاولة»). يظهر خطأ START_STATE_INVALID العابر في Gazebo (رمز خطأ MoveIt هو -26) عندما يتجاوز متحكم المفاصل المحاكى (حلقة تغذية راجعة تقود كل مفصل نحو هدفه) حدَّ أحد المفاصل لفترة وجيزة بعد حركة ما. عندها تنتظر المهمة حتى يستقر الوضع وتعيد المحاولة تلقائيًا بدلًا من إنهاء التشغيل.

10. المحاكاة أولًا: التوأم الرقمي في Gazebo مع كاميرا حقيقية

أمر واحد يشغّل سلسلة المحاكاة كاملة:

ros2 launch omarm_zero_moveit_config cube_gazebo.launch.py \
    camera_info_url:=/absolute/path/to/your_calibration.yaml run_demo:=true

يشغّل ملف الإطلاق (launch file) هذا وحده ما يلي:

• Gazebo مع نموذج الروبوت وطاولة عليها علامات في زواياها.
• MoveIt و RViz لتخطيط الحركة والعرض المرئي.
• سلسلة معالجة الكاميرا الحقيقية (عقدة الكاميرا، وكاشف ArUco، وناشر وضعية المكعب).
• مكعب يُنشأ في Gazebo عند الموضع الذي تُبلغ عنه الكاميرا الحقيقية.
• العقدة المساعدة لربط المكعب عند الإمساك (للإمساك في المحاكاة).
• مهمة الالتقاط والوضع نفسها.

ضع مكعبك الحقيقي على الطاولة الحقيقية. يظهر توأمه في Gazebo عند الإحداثيات نفسها، وتلتقطه الذراع المحاكاة. الذراع وحدها هي المحاكاة. وبذلك تختبر سلسلة الإدراك كاملة (الكاميرا، والمعايرة، والكشف، والترشيح، وسلسلة TF) على بيانات كاميرا حقيقية دون تعريض العتاد للخطر.

ثلاث حيل للمحاكاة

الإمساك دون فيزياء الإمساك. الإمساك الواقعي بالاحتكاك لجسم خفيف غير موثوق في Gazebo عندما يكون التحكم في المفاصل مضبوطًا لمحركات سيرفو الهواة. فإما أن تضغط الأصابع المحاكاة بقوة زائدة فتقذف المكعب، أو بقوة غير كافية فيسقط من بينها. تتجنب عقدة مساعدة فيزياء الإمساك: فهي تنقل نموذج المكعب فوريًا إلى الإطار tcp بمعدل 50 Hz ما دام القابض قد تلقى أمر الإغلاق. فيبقى المكعب حيث توجد أطراف الأصابع، دون أي فيزياء.

معدل التحديث 50 Hz مهم هنا. استخدمت في البداية أمر سطر الأوامر gz service لضبط وضعية المكعب عند كل تحديث، لكن كل استدعاء عبر سطر الأوامر (CLI) استغرق من 150 إلى 300 ms. وبهذا المعدل كان المكعب يتأخر بوضوح عن الذراع ويبدو كأنه يسقط منها في كل حركة. وكان الحل جسر خدمات بين ROS و Gazebo (يربط /world/empty/set_pose بالخدمة SetEntityPose) مع عميل ROS غير متزامن (async). فانخفض زمن التأخير إلى بضع ميلي ثوانٍ فقط، والتصق المكعب بالقابض بسلاسة.

مكعب بلا جاذبية وبلا تصادمات. المكعب الذي يُنقل فوريًا لا يحتاج إلى فيزياء. فعندما كانت التصادمات مفعّلة، كانت الأصابع المغلقة تدفع المكعب في كل دورة تحديث (tick)، فتتراكم سرعته. وعندما أفلتّه (أوقفت النقل الفوري)، قذفت السرعة المتراكمة المكعب عبر مشهد Gazebo. الحل: عطّل الجاذبية على نموذج المكعب (<gravity>0</gravity> في ملف SDF، أي Simulation Description Format، وهو ملف النموذج في Gazebo) واحذف عناصر التصادم منه. أثناء الإمساك يكون المكعب عنصرًا مرئيًا فقط. وعند وضعه يبقى حيث تضعه المهمة.

المحاكاة تدقق ملف URDF الخاص بك. لم يُغلق القابض لديّ إغلاقًا كاملًا في المحاكاة أبدًا. كان ملف URDF ينص على upper=0.3 rad و velocity=0.1 rad/s لمفصلَي القابض، بينما كانت وضعية الإغلاق المطلوبة 0.9 rad. فرض محرك الفيزياء في Gazebo حدود المفاصل بصرامة، فقيّد الموضع المطلوب عند 0.3 rad. أما السيرفو الحقيقي فكان قد تقبّل هذا الأمر الخارج عن النطاق لأسابيع. كما جعل حد السرعة 0.1 rad/s الإغلاق بطيئًا أيضًا: فبهذه السرعة يستغرق قطع مسافة 2.1 rad من وضع الفتح (-1.2 rad) إلى القيمة المطلوبة 0.9 rad مدة 21 ثانية. أصبح ملف URDF وملف joint_limits.yaml لديّ يسمحان الآن بمدى من -1.6 إلى +1.6 rad وبسرعة تصل إلى 4.0 rad/s لكلا مفصلَي القابض، وهذا يغطي المدى الكامل للسيرفو البالغ ±1.57 rad. اضبط حدودك بحيث تغطي قيمتَي gripper_open و gripper_closed الخاصتين بك، ثم أعد البناء باستخدام colcon build وأعد التشغيل. وفي Gazebo خفّضت أيضًا قيمة الإغلاق خطوة بخطوة: فكلٌّ من 0.9 و 0.54 و 0.38 و 0.27 rad أغلق الأصابع المحاكاة أكثر من اللازم. ويستخدم ملف المحاكاة الآن 0.16 rad.

مشاهدة دورة المحاكاة

عند ضبط run_demo:=true، تنفّذ المهمة التسلسل الموصوف في القسم 9 تلقائيًا، ويمكنك متابعته في RViz و Gazebo.

تُسجَّل كل خطوة في الطرفية مع توقيتها ومقاييس الحركيات العكسية (IK). وإذا فشل شيء ما، يخبرك السجل بالخطوة التي فشلت وبسبب فشلها.

11. الانتقال إلى الواقع: سلسلة المعالجة كاملة على العتاد الحقيقي

ثلاث نوافذ طرفية (أو ملف cube_real.launch.py المرفق):

# Terminal 1: robot (MoveIt + ESP32 bridge)
ros2 launch omarm_zero_moveit_config real_moveit.launch.py \
    transport:=serial use_rviz:=true home_on_start:=true

# Terminal 2: vision (same pipeline as in simulation)
ros2 launch computer_vision_aruco vision_bringup.launch.py \
    camera_info_url:=/absolute/path/to/your_calibration.yaml

# Terminal 3: the task, with the REAL parameter file
ros2 run omarm_zero_moveit_config cup_pick_and_place.py --ros-args \
    --params-file src/omarm_zero_moveit_config/config/pick_place_params_real.yaml

لخيار البدء في الطرفية 1 أهمية كبيرة على العتاد الحقيقي. فالخيار home_on_start:=true (القيمة الافتراضية) يرسل أمر HOME إلى البرنامج الثابت (Firmware) عند البدء وينتظر 1.5 s (home_wait_sec). بعدها يحرّك omarm_zero_ros2_serial.ino من الجزء 3 كل محركات السيرفو إلى 90°، وهي منتصف مدى كل مفصل؛ أما البرنامج الثابت من الجزء 1 فيعود إلى 0° بدلًا من ذلك. لا تستخدم start_at_ros_zero:=true: فهو يرسل 0 rad إلى كل مفصل، مما يضع J1 عند 0° و J2 حتى J5 عند 180°، وهي وضعية نهاية المشوار (End Stop) المذكورة في تحذير home_joints أدناه.

لا تعطي محركات السيرفو أي تغذية راجعة عن موضعها، لذلك يظل الجسر (Bridge) بعد أمر HOME يفترض 0 rad لكل مفصل. تبدأ المهمة بفتح القابض. ويملأ الجسر قيم مفاصل الذراع في هذا الأمر بقيم 0 rad المخزّنة، فستندفع الذراع عندها إلى وضعية نهاية المشوار بسرعة القابض. قبل أن تشغّل الطرفية 3، اختر حالة الهدف home لمجموعة arm في RViz واضغط Plan وExecute مرة واحدة. وإذا استخدمت cube_real.launch.py بدلًا من ذلك، فهو يشغّل عُقد الطرفيتين 1 و 2 بإعداد HOME الافتراضي نفسه. اترك run_demo فيه على false (القيمة الافتراضية)، حتى لا تبدأ المهمة قبل حركة العودة إلى home هذه.

ما الذي يختلف على العتاد الحقيقي

إعدادات الروبوت الحقيقي مدمجة في الحزمة على شكل تهيئات منفصلة.

ملفا معاملات، لا يُخلطان أبدًا. توجد معاملات الروبوت الحقيقي في pick_place_params_real.yaml، منفصلةً عن معاملات المحاكاة:

# pick_place_params_real.yaml (real robot)
cup_pick_and_place:
  ros__parameters:
    grasp_z: 0.07
    approach_height: 0.10
    lift: 0.06
    approach_dir: [0.0, 0.0, -1.0]
    place_marker_frame: 'aruco_marker_4'
    place_z: 0.01             # settle until cube stands on table
    gripper_open: [-1.57, -1.57]    # full servo range: 180 degrees
    gripper_closed: [1.57, 1.57]    # full servo range: 0 degrees
    gripper_time_sec: 1.0
    home_joints: [0.8, -0.8, -1.2, -0.8, -2.6]   # safe real home

قارن قيم القابض: تستخدم المحاكاة [-1.2, -1.2] للفتح / [0.16, 0.16] للإغلاق. وقيمة الإغلاق الأصغر ناتجة عن الضبط في Gazebo الموصوف في القسم 10. أما الروبوت الحقيقي فيستخدم [-1.57, -1.57] / [1.57, 1.57] (المدى الكامل للسيرفو من 0 إلى 180 درجة، بعد تحويله عبر جسر العتاد).

الفرق في home_joints هو الأخطر. فالقيمة الافتراضية في المحاكاة [0, 0, 0, 0, 0] غير ضارة في Gazebo (فهي وضعية الصفر في URDF). أما على الروبوت الحقيقي، فتمر هذه الأصفار عبر معادلة تحويل السيرفو deg = 90 + direction * (rad – zero_rad) * 57.3. والنتيجة أن كل سيرفو يُدفع إلى نهاية مشواره عند 0 أو 180 درجة. تتوقف محركات السيرفو عند نهايات المشوار (Stall)، وينخفض جهد خط 5 V (خط التغذية المشترك بجهد 5 V لجميع محركات السيرفو)، فتضطرب كل محركات السيرفو في الوقت نفسه. وتستخدم إعدادات الروبوت الحقيقي الوضعية الآمنة المجرَّبة [0.8, -0.8, -1.2, -0.8, -2.6]، وهي وضعية home نفسها المستخدمة في الجزء 3.

السرعة تُضبط في البرنامج الثابت، لا في MoveIt. يمرّر الجسر إلى ESP32 النقطة الأخيرة فقط من المسار (Trajectory). ثم يحرّك البرنامج الثابت في ESP32 محركات السيرفو تدريجيًا نحو تلك النقطة وفق إعداد السرعة الخاص به. ولا يغيّر تحجيم السرعة (velocity scaling) في MoveIt سرعة السيرفو على العتاد الحقيقي. ويبدّل الجسر سرعة البرنامج الثابت مع كل أمر: حركات الذراع تستخدم السرعة 5 (بطيئة وسلسة)، وحركات القابض وحده تستخدم السرعة 20 (تنغلق الأصابع بسرعة).

عاير القابض بصريًا. حرّك سيرفو القابض ببطء من 180 إلى 0 درجة وسجّل زاويتين: زاوية الفتح الكامل، وزاوية الإغلاق المحكم حول المكعب. يحوّل الجسر قيم القابض باستخدام zero_rad 0 و direction -1 (في hardware.yaml)، لذا فإن قيمة المفصل = (90 – زاوية السيرفو) / 57.3. وهذا يعطي -1.57 عند 180° و +1.57 عند 0°؛ أدخل قيمتيك على أنهما gripper_open و gripper_closed. في النهاية صار القابض لديّ يستخدم المدى الكامل للسيرفو بعد أن أعدت تركيب الإصبع على ذراع السيرفو (Horn)، وهي الذراع البلاستيكية المثبتة على عمود خرج السيرفو، مُزاحًا بسنٍّ واحدة نحو الاتساع.

تصحيح عائلة حلول المعصم يخص الروبوت الحقيقي. في المحاكاة، تنجح كلتا عائلتَي حلول المعصم في الإمساك، لأن القابض المحاكى متماثل، ولأن الإمساك القائم على النقل الفوري يتجاهل الإزاحة الجانبية. أما على العتاد الحقيقي وحده، حيث للأصابع هندسة حقيقية وتؤثر إزاحة الإطار tcp، فتُخطئ العائلة الملتوية المكعب.

مشاهدة الدورة الحقيقية

ضع المكعب بحيث تكون علامته مرئية للكاميرا، داخل حلقة مساحة العمل على بعد 15 إلى 19 cm تقريبًا من القاعدة. شغّل الطرفيتين 1 و 2، وحرّك الذراع إلى home في RViz، ثم شغّل الطرفية 3. تنتظر المهمة /cup_pose (حتى 15 ثانية)، ثم تبدأ.

سجل تشغيل ناجح:

Cup pose received. Planning a vertical top-down grasp (5-DOF)...
   /cup_pose: Marker 0 @ (+0.165, -0.042, +0.050) [world==base_link]
1. Opening gripper (start state)...
2. Pre-grasp (vertically above the cube, z=0.17 = 7 cm above the marker)...
   IK ok: pos_err=1mm, tcp-Z aligned 0 deg from vertical
3. Grasp (LINEAR vertical descent)...
   Descent: 10 steps, z=0.17 -> z=0.07
4. Closing gripper...
5. Lifting (LINEAR up to z=0.13)...
6. Flying OVER the place position (z=0.14)...
   Marker "aruco_marker_4" (+0.272,+0.135) out of reach
   — placing 9 cm short of it: (+0.195,+0.097)
7. Opening gripper (placing)...
8. Returning to the home position...
   Home position reached
=== Pick-and-place demo DONE! ===

نزول عمودي والأصابع مفتوحة.

الإمساك: تنغلق الأصابع حول المكعب.

الرفع والنقل (النقل حركة عادية في فضاء المفاصل عبر MoveIt).

الاستقرار على الطاولة (place_z: 0.01)، ثم تنفتح الأصابع وتتراجع الذراع وتعود إلى home.

12. استكشاف الأخطاء وإصلاحها

يتحرك الروبوت إلى المكان الخطأ باستمرار.

تكاد تكون هذه دائمًا مشكلة في المعاملات الخارجية (Extrinsics). افحص قيمة RMS لعلامات الطاولة في سجل سلسلة معالجة الرؤية. بعد المعايرة، يطلب منك calibrate_extrinsics التحقق من أن هذه القيمة أقل من 1 cm، ويُصدر تحذيرًا عندما يتجاوز الخطأ المتبقي الخاص به 2 cm. إذا ارتفعت قيمة RMS فوق 2 cm، فشغّل calibrate_extrinsics مرة أخرى. وأعد أيضًا قياس قيم table_markers_expected. فخطأ في الإشارة في زاوية واحدة مقيسة (Y موجبة بدلًا من سالبة مثلًا) يُفسد حل Kabsch بأكمله.

لا يُحدَّث /cup_pose رغم أن المكعب تحرّك.

قارن الكشف الخام بالخرج بعد الترشيح. شغّل ros2 run tf2_ros tf2_echo world aruco_marker_0 لرؤية TF الخام، و ros2 topic echo /cup_pose لرؤية الموضع بعد الترشيح. إذا كان TF الخام محدّثًا بينما /cup_pose عالق، فمرشح القيم الشاذة (Outlier Filter) ما زال يعيد تهيئة قيمته المرجعية. انتظر نحو ثانية واحدة (فهو يحتاج إلى 5 «قيم شاذة» متتالية قبل أن يعيد التهيئة)، أو أعد تشغيل سلسلة معالجة الرؤية. وإذا كان TF الخام قديمًا أيضًا، فالعلامة لا تُكتشف. افحص /aruco/debug_image بحثًا عن مشكلات في الإضاءة أو عن مكعب مقلوب (المكعب الملقى على جانبه يُخفي العلامة الموجودة على وجهه العلوي).

يتعذّر فتح الكاميرا أو تُفتح كاميرا خاطئة.

أُعيد ترقيم الكاميرا بعد فصلها وتوصيلها من جديد. شغّل ls /dev/video* لرؤية الأجهزة المتاحة. الحواسيب المحمولة ذات كاميرات الويب المدمجة تحجز هي أيضًا أرقام video. وبعد إعادة التوصيل قد تنتقل كاميرا USB لديك من video0 إلى video2. اضبط camera_id وفقًا لذلك، أو استخدم مسار camera_device الثابت من aruco_params.yaml.

الروبوت الحقيقي لا يستجيب إطلاقًا. والجسر يُبلغ أنه انتهى دون أخطاء بعد ثانية واحدة.

هناك برنامج آخر يحجز المنفذ التسلسلي. والسبب الأكثر شيوعًا هو نافذة الشاشة التسلسلية (Serial Monitor) مفتوحة في Arduino IDE. شغّل fuser /dev/ttyUSB0 لمعرفة العملية التي تحجز المنفذ، ثم أغلقها وأعد التشغيل. وتحقق أيضًا من أن ESP32 موصول بالطاقة وأن كابل USB يدعم نقل البيانات (بعض كابلات USB مخصصة للشحن فقط).

«No vertically reachable height» (لا يوجد ارتفاع يمكن الوصول إليه عموديًا) أو تتوقف المهمة قبل أن تتحرك.

عند موضع المكعب هذا، لا يوجد اقتراب عمودي ممكن لهذه الذراع ذات خمس درجات حرية (5-DOF). مساحة العمل للإمساك العمودي حلقة تقع على بعد 15 إلى 19 cm تقريبًا من القاعدة. المواضع الأقرب تقع دون الحد الأدنى لمدى وصول الذراع، والمواضع الأبعد تتجاوز الحد الأقصى لمدى الوصول في الاقتراب العمودي. تفحص المهمة المنطقة المجاورة وتطبع اقتراحًا محددًا: «push the cube 3 cm toward the marker-2 corner (reachable at (-0.03, -0.18))»، أي ادفع المكعب نحو تلك الزاوية إلى موضع يمكن الوصول إليه. العلامة 2 هي الزاوية الخلفية اليمنى. وداخل حلقة الإمساك لا ينبغي أن تظهر هذه الرسالة.

القابض بالكاد ينغلق أو يستغرق إغلاقه وقتًا طويلًا جدًا في المحاكاة.

حدود مفاصل القابض في ملف URDF لديك لا تغطي وضعية الإغلاق المطلوبة، فيقيّد Gazebo أمر الإغلاق، ويجعل حد السرعة المنخفض الحركة بطيئة. صحّح الحدود كما هو موضح في القسم 10 (في URDF و joint_limits.yaml)، ثم أعد البناء باستخدام colcon build وأعد التشغيل.

الإمساك يقع دائمًا بجانب المكعب على الروبوت الحقيقي.

هذه مشكلة عائلة حلول المعصم الموصوفة في القسم 8. افحص دوران الساعد (J4، ويظهر في السجل باسم Revolute 4) في السجل أو في RViz. إذا كان قريبًا من -180 درجة، فأنت في العائلة الملتوية. الحركيات العكسية (IK) في هذه الحزمة مصممة لتفضيل الوضعية المحايدة، فتختار العائلة المحايدة حيثما وُجدت. أبقِ المكعب في المنطقة الأمامية/الوسطى (X موجبة، و Y صغيرة) حيث تتوفر العائلة المحايدة.

انقطاع الطاقة عن جانب الروبوت أثناء التشغيل.

أعد تشغيل real_moveit.launch.py كما في القسم 11، وحرّك الذراع إلى home في RViz مرة واحدة قبل أن تشغّل المهمة من جديد. هذا يعيد مزامنة الذراع الفعلية مع الحالة في البرمجيات. ويعيد الجسر تطبيق إعداد السرعة الخاص به. وإذا انخفض جهد خط 5 V أثناء انقطاع الطاقة، فتحقق من أن جميع محركات السيرفو ما زالت تستجيب قبل تشغيل المهمة مرة أخرى.

يعرض RViz المكعب في موضع مختلف عن موضع المكعب الحقيقي.

افحص معايرة الكاميرا أولًا: يجب أن يعرض الأمر ros2 topic echo /camera/camera_info –once الأطوال البؤرية الخاصة بكاميرتك، لا القيم الافتراضية. ثم افحص مقاس العلامة في ملف الإعدادات. إذا كانت العلامة المطبوعة 30 mm بينما تقول الإعدادات 31 mm، فستنحرف كل المسافات بنحو 3 %.

13. الأسئلة الشائعة

هل يمكنني تخطي المحاكاة والانتقال مباشرة إلى العتاد الحقيقي؟ يمكنك ذلك، لكن توأم Gazebo كشف أربعة أخطاء قبل أن يتعرض أي سيرفو للخطر (القسمان 9 و 10)، كما أن المحاكاة والروبوت الحقيقي يشغّلان كود المهمة نفسه.

هل يعمل هذا مع ارتفاع كاميرا مختلف أو طاولة مختلفة؟ نعم. فهذا بالضبط ما تستوعبه المعايرتان. أعد المعايرة الداخلية (Intrinsic) إذا تغيّر طراز الكاميرا أو دقة الصورة. وأعد المعايرة الخارجية (أربع علامات وأمر واحد) إذا تحرّك حامل الكاميرا. وأعد قياس مواضع علامات الزوايا إذا تغيّرت الطاولة.

هندسة القابض لديّ مختلفة. ما الذي يجب أن أعيد ضبطه؟ ثلاث قيم: أصل الإطار tcp في ملف URDF (اضبطه على نقطة المنتصف بين طرفي أصابع القابض)، وقيمتا gripper_open و gripper_closed (بمسح بصري واحد للسيرفو، القسم 11)، وقيمة grasp_z بحسب ارتفاع الجسم الذي تمسكه. أما كل ما عدا ذلك فيتكيّف عبر سلسلة الحركيات الأمامية (FK) المبنية على URDF.

ماذا يحدث إذا حُرّك المكعب أثناء الإمساك؟ تقرأ المهمة موضع المكعب مرة واحدة في البداية وتخطط التسلسل كله بناءً على تلك اللقطة. ولا تتتبع المكعب أثناء النزول. فإذا حرّك أحدهم المكعب بعد أن تبدأ الذراع بالنزول، ينغلق القابض عند الموضع القديم ويُخطئ المكعب. وهذا مقبول في عرض توضيحي على سطح المكتب، أما النظام الإنتاجي فسيحتاج إلى تتبع مستمر وإعادة تخطيط.

هل يمكنني استخدام هذا مع كاميرا عمق بدلًا من علامات ArUco؟ من حيث المبدأ، نعم. تعطيك كاميرا العمق (مثل RealSense) سُحبًا نقطية (Point Clouds)، ويمكنك اكتشاف الأجسام من خلال شكلها بدلًا من العلامة. ستختلف سلسلة معالجة الإدراك (تقسيم السحابة النقطية بدلًا من كشف ArUco)، لكن حلّال الحركيات العكسية (IK Solver) ومهمة الالتقاط والوضع ومنطق النزول الخطي تبقى كما هي. فالواجهة هي الموضوع (Topic) /cup_pose، لذلك يمكنك لاحقًا أيضًا وضع كاشف YOLO (شبكة عصبية تعثر على الأجسام في الصور) أو سلسلة معالجة للسحب النقطية مكان الكاشف الحالي. وأي عقدة تنشر رسالة PoseStamped هناك ستعمل.

14. خلاصة السلسلة

الجزء 1: الميكانيكا والإلكترونيات والبرنامج الثابت لـ ESP32 والتحكم عبر الويب. ذراع مكتبية تكلّف قطعها نحو $48، وتتحرك عندما تسحب أشرطة التمرير على هاتفك. اقرأ الجزء 1

الجزء 2: URDF/Xacro، والعرض المرئي في RViz، وتوأم رقمي في Gazebo مع محاكاة فيزيائية. للمرة الأولى صار للذراع وجود في البرمجيات. اقرأ الجزء 2

الجزء 3: تخطيط الحركة باستخدام MoveIt 2، من العتاد الوهمي (Mock Hardware) إلى محركات السيرفو الحقيقية عبر جسر تسلسلي. صارت الذراع تذهب حيث تأمرها؛ وعلى محركات السيرفو الحقيقية يقودها الجسر مباشرة إلى هدف كل مسار مخطط. اقرأ الجزء 3

الجزء 4: معايرة الكاميرا، والإدراك باستخدام ArUco، وحلّ مخصص للحركيات العكسية (IK) لإمساك الأجسام بذراع ذات 5-DOF، وسلسلة التقاط ووضع مُختبرة على الذراع الحقيقية. صارت الذراع الآن تأخذ هدفها من الكاميرا بدلًا منك.

تعمل السلسلة كاملة الآن على الذراع الحقيقية. تعثر الكاميرا على العلامة، وتحوّل المعايرة البكسلات إلى إحداثيات الروبوت، ويحسب حلّال الحركيات العكسية (IK) زوايا المفاصل. ثم يخطط MoveIt مسارًا خاليًا من التصادمات، ويقود الجسر التسلسلي محركات السيرفو.

المصادر

• الجزء 1: بناء ذراع روبوتية بست درجات حرية (6-DOF، أي 5+1 للقابض) مع تحكم عبر الويب باستخدام ESP32: الميكانيكا والإلكترونيات والبرنامج الثابت.
• الجزء 2: شرح URDF في ROS 2: RViz و Gazebo: نموذج الروبوت.
• الجزء 3: شرح MoveIt 2: تخطيط الحركة والتحكم في الروبوت الحقيقي: التخطيط والجسر التسلسلي.
• مساحة عمل ROS 2 لـ OmArm Zero (المتجر): مساحة العمل الكاملة للأجزاء من 2 إلى 4.
• OmArm Zero على YouTube: سلسلة الفيديو.
• OmArm Zero على GitHub: المستودع.