🤖
Manipulator Controll Tutorial
Koki Muramoto
2025/9/21
Manipulator Control Tutorial
This course guides you from environment setup to basic manipulator control and object acquisition using a depth camera.
You will become familiar with ROS 2 command-line tools, development workflow, and the full pipeline from image recognition to robot control.
Environment Setup
Clone the required repositories:
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
Once the build is complete, create your own package:
cd src
ros2 pkg create --build-type ament_python my_package
cd ..
colcon build --symlink-install
Image Recognition
Let’s start by performing simple image recognition using the RealSense camera.
Connect your RealSense sensor, then run:
source realsense_ws/install/local_setup.bash
ros2 launch realsense2_camera rs_launch.py
In another terminal:
rviz2
Add an “Image” display type and select /camera/color/image_raw.
If you can see the RealSense image stream, you are ready.
YOLO-based Object Detection Example
#!/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')
# Load YOLOv8 model
self.model = YOLO("yolov8.pt")
# OpenCV-ROS bridge
self.bridge = CvBridge()
# Subscribe to camera images
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):
# Convert ROS Image → OpenCV BGR image
frame = self.bridge.imgmsg_to_cv2(msg, desired_encoding='bgr8')
# YOLOv8 inference
results = self.model(frame, verbose=False)
# Annotate output
annotated_frame = results[0].plot()
# Display
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()
Forward Kinematics
Let’s now control the manipulator.
Start the hardware interface:
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
The current joint positions are published to:
/so101_follower/joint_states
The goal joint positions can be sent to:
/so101_follower/joint_commands
Forward kinematics explanation
Please refer to this article:
Forward Kinematics Node
#!/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
)
# Link lengths
self.L1 = 0.10
self.L2 = 0.15
self.L3 = 0.15
self.L4 = 0.05
self.L5 = 0.05
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
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 = 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()
If you can see the 3D position and wrist roll printed, your forward kinematics is working.
Inverse Kinematics
Inverse kinematics computes the joint angles needed to reach a desired 3D position.
Refer again to:
Inverse Kinematics Node
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64MultiArray
import math
class IKNode(Node):
def __init__(self):
super().__init__('ik_node')
self.pub = self.create_publisher(Float64MultiArray, '/so101_follower/joint_commands', 10)
# Link lengths
self.L1 = 0.10
self.L2 = 0.15
self.L3 = 0.15
self.L4 = 0.05
self.L5 = 0.05
def ik(self, x, y, z, roll):
a1 = math.atan2(y, x)
L_axis = math.hypot(x, y)
z_rel = z - self.L1
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
cos_a3 = (self.L2**2 + self.L3**2 - c**2) / (2 * self.L2 * self.L3)
a3 = math.pi - math.acos(cos_a3)
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
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] # gripper
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()
When the manipulator moves to the input pose, your inverse kinematics is working successfully!