\text{Joint Trajectory Controller} & 100 \text{ Hz} \ \text{Joint State Controller} & 100 \text{ Hz} \ \text{Cartesian Controller} & 100 \text{ Hz} \end{cases} \rightarrow \text{Hardware Interface} \rightarrow \text{Real-time loop (1 kHz)} $$ The real-time control loop uses a **hardware abstraction layer** with: - `JointStateInterface`: reads encoder positions, velocities, efforts - `PositionJointInterface`: commands position setpoints - `VelocityJointInterface`: commands velocity setpoints - `EffortJointInterface`: commands torque/current setpoints **URDF** (Unified Robot Description Format) for kinematic/dynamic models: An XML format describing link geometries, joint types, limits, and inertial properties. The URDF is parsed into a kinematic chain for forward/inverse kinematics computation. **Timed elastic band (TEB) trajectory optimization**: $$ \min_{\mathbf{q}_k, \Delta T_k} \sum_{k=1}^{N-1} \left( \|\mathbf{q}_{k+1} - \mathbf{q}_k\|^2 + \Delta T_k^2 \right) $$ subject to: joint limits, velocity limits, acceleration limits, collision avoidance. ### Act III -- The Engineering ```python # Example ROS2 control node structure (Python) import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint class JointController(Node): def __init__(self): super().__init__('joint_controller') self.joint_state_sub = self.create_subscription( JointState, '/joint_states', self.joint_state_callback, 10) self.cmd_pub = self.create_publisher( JointTrajectory, '/joint_trajectory_controller/joint_trajectory', 10) self.timer = self.create_timer(0.01, self.control_loop) self.q_meas = [0.0] * 6 self.q_des = [0.0] * 6 def joint_state_callback(self, msg): self.q_meas = msg.position[:6] def control_loop(self): # PID joint control cmd = JointTrajectory() cmd.joint_names = [f'joint_{i+1}' for i in range(6)] point = JointTrajectoryPoint() point.positions = self.q_meas point.velocities = [ (self.q_des[i] - self.q_meas[i]) * 5.0 for i in range(6) ] point.time_from_start.sec = 0 cmd.points = [point] self.cmd_pub.publish(cmd) def set_desired(self, q): self.q_des = q def main(args=None): rclpy.init(args=args) controller = JointController() rclpy.spin(controller) controller.destroy_node() rclpy.shutdown() ``` ```cpp // ROS control hardware interface (C++ for real-time) #include #include #include class MyRobotHW : public hardware_interface::RobotHW { private: double joint_position_[6], joint_velocity_[6], joint_effort_[6]; double joint_position_command_[6], joint_velocity_command_[6], joint_effort_command_[6]; hardware_interface::JointStateInterface jnt_state_interface; hardware_interface::PositionJointInterface jnt_pos_interface; public: MyRobotHW() { for (int i = 0; i < 6; i++) { joint_position_[i] = joint_velocity_[i] = joint_effort_[i] = 0; joint_position_command_[i] = 0; } registerInterface(&jnt_state_interface); registerInterface(&jnt_pos_interface); } void read() { for (int i = 0; i < 6; i++) { joint_position_[i] = readEncoder(i); joint_velocity_[i] = computeVelocity(i); } } void write() { for (int i = 0; i < 6; i++) { setMotorPosition(i, joint_position_command_[i]); } } }; ``` ```xml ```