Robot Control Software — Ch.06: Manipulator Control [EN]

STATERA Library · Series 08 · Volume 04 · Chapter 06 · EN
Source: https://github.com/Statera-Library/08_Vol_04_Robot_Control_Software/blob/main/Chapter_06_Manipulator_Control/Chapter_06_Manipulator_Control_EN.md

Volume 04 Robot Control Software

06. Manipulator Control

06.01 Manipulator Forward Kinematics SW Implementation [w/Code]

Forward kinematics software computes the pose of a robot manipulator's end effector from the measured or commanded joint configuration. In a control system, it provides the geometric bridge between joint-space variables and Cartesian-space quantities. The implementation must therefore represent the robot structure accurately while remaining deterministic, numerically stable, and efficient enough for real-time execution.

A manipulator is normally modeled as an ordered kinematic chain consisting of links connected by revolute, prismatic, or fixed joints. Each joint defines a transformation between adjacent coordinate frames. The software stores joint axes, frame offsets, link relationships, joint types, and configuration conventions so that the complete chain can be reconstructed consistently from the base frame to any selected link or tool frame.

A common implementation represents each relationship using a homogeneous transformation matrix containing a 3 × 3 rotation matrix and a three-dimensional translation vector. The transformation combines orientation and position within a single mathematical representation. Sequential multiplication of these matrices propagates the coordinate frame through the manipulator and produces the final transformation from the robot base to the end effector.

For a revolute joint, the variable component of the transformation is determined by the joint angle, while a prismatic joint changes translation along its defined axis. Fixed geometric parameters describe link lengths, offsets, and mounting orientations. Software should clearly separate these constant parameters from runtime joint states because this improves maintainability, calibration management, and computational efficiency.

Denavit--Hartenberg parameters provide one traditional method for constructing serial manipulator transformations. Each link is represented through a compact set of geometric parameters describing rotation and translation between successive frames. Although convenient for analytical implementations, the selected standard or modified DH convention must be documented explicitly because mixing conventions can produce geometrically plausible but incorrect end-effector poses.

Modern robotics software frequently uses direct frame definitions, URDF-based models, or rigid-body transformation libraries instead of manually coded DH equations. In these implementations, the robot description defines the kinematic tree while the forward kinematics engine traverses the relevant parent-child relationships. This approach supports manipulators containing branches, auxiliary sensors, grippers, and additional tool frames more naturally.

The software input is normally a joint-state vector associated with uniquely identified joints. Correct ordering is essential because a mismatch between the numerical vector and the model joint sequence can generate severe pose errors without producing an obvious software exception. Robust implementations therefore maintain explicit mappings between joint names, indices, units, limits, and the corresponding model objects.

At runtime, the forward kinematics calculation begins with the base transformation and successively applies the transformation associated with each joint and link. Intermediate transformations can be retained when poses of multiple links are required by collision checking, visualization, force control, or whole-body coordination. Caching invariant transforms can reduce repeated calculations, particularly when the control loop operates at high frequency.

The resulting end-effector pose may be exposed as a transformation matrix, position plus quaternion, position plus rotation matrix, or another orientation representation required by downstream modules. Internally, rotation matrices and quaternions are often preferred because Euler-angle representations contain singularities and convention ambiguities. Conversion functions should specify axis order, frame convention, and quaternion component ordering explicitly.

Frame semantics are as important as numerical computation. Software must distinguish the world frame, robot base frame, manipulator mounting frame, individual link frames, flange frame, and tool center point. An otherwise correct transformation becomes unusable when its source and destination frames are ambiguous. Frame identifiers should therefore be treated as part of the interface contract rather than informal metadata.

Tool geometry requires particular attention because the manipulator's mechanical flange is rarely identical to the operational tool center point. A gripper, welding torch, camera, probe, or other end-effector device introduces an additional fixed or configurable transformation. Forward kinematics should support this tool transformation independently so that tools can be exchanged without modifying the fundamental robot kinematic model.

Calibration parameters must also be separated from nominal mechanical definitions. Manufacturing tolerances, assembly errors, encoder offsets, and tool installation errors can create measurable discrepancies between theoretical and physical poses. A production implementation may therefore apply calibrated joint zero offsets and corrected geometric parameters while preserving the nominal model for configuration management, simulation, and diagnostic comparison.

Joint measurements entering the calculation require consistent units and sign conventions. Angular quantities should normally use radians internally, while linear quantities should use a defined metric unit such as meters. Encoder direction, mechanical zero position, gear relationships, and software-positive rotation must agree with the robot model. Unit conversion should occur at clearly defined boundaries instead of being scattered throughout the kinematics code.

Numerical validation is essential even though forward kinematics itself does not normally require iterative optimization. Rotation matrices should remain orthonormal within acceptable floating-point tolerance, quaternions should be normalized when required, and transformation matrices should retain valid homogeneous structure. Invalid joint values, NaN data, infinities, missing states, and out-of-range inputs should be detected before they propagate into motion-control functions.

Software architecture should separate the mathematical kinematics engine from communication and hardware interfaces. Encoder acquisition, fieldbus processing, robot model loading, transformation computation, and publication of Cartesian states are distinct responsibilities. This separation allows the same forward kinematics implementation to operate with simulation data, recorded logs, hardware-in-the-loop environments, and physical manipulators without rewriting the mathematical core.

Real-time control introduces additional implementation constraints. Dynamic memory allocation, unpredictable locking, unnecessary string processing, and repeated model parsing should be avoided inside deterministic control loops. The robot model and transformation structures can be initialized before execution, while runtime processing updates only joint-dependent terms. This architecture reduces latency variation and makes execution time easier to verify.

Forward kinematics also supports Jacobian computation because link positions, orientations, and joint axes expressed in common coordinate frames are required to construct the manipulator Jacobian. Maintaining intermediate transformations during the forward pass can therefore reduce duplicated computation. The resulting data can support Cartesian velocity control, force control, singularity monitoring, inverse kinematics, and operational-space control.

Integration with inverse kinematics requires strict agreement on frames and joint conventions. The inverse solver produces a candidate joint configuration, and forward kinematics can immediately reconstruct the resulting Cartesian pose. Comparing that pose with the requested target provides a fundamental consistency check. Position and orientation residuals can be evaluated independently to identify convergence, modeling, or convention errors.

Forward kinematics is also closely coupled with collision detection. Once the transformations of individual links are known, their collision geometries can be positioned in the robot base or world frame. The collision subsystem can then evaluate self-collision and environmental interaction. For this reason, efficient computation of all link poses may be more useful in practical software than calculating only the final end-effector pose.

Simulation provides an important verification environment for the implementation. Known joint configurations can be evaluated independently by the kinematics software and a trusted simulator, and the resulting link and tool poses can be compared numerically. Test configurations should include the home position, joint limits, asymmetric poses, combinations of positive and negative angles, and configurations that expose frame-axis or sign errors.

Unit testing should validate individual joint transformations as well as complete chains. Tests can compare calculated transformations against analytically derived reference values and verify translation, rotation, quaternion normalization, and frame composition independently. Regression tests are particularly valuable after robot-description, calibration, tool, or software-library changes because small geometric modifications can affect every downstream Cartesian function.

Diagnostic software should expose enough information to trace unexpected pose results. Useful runtime information includes the received joint vector, mapped joint names, active model version, calibration set, base transform, tool transform, and selected intermediate link poses. These signals allow developers to distinguish mathematical defects from stale robot descriptions, incorrect encoder offsets, frame mismatches, or communication problems.

For manipulators integrated into mobile robots, the arm transformation is only one part of a larger transform hierarchy. The final world-referenced tool pose may combine world-to-mobile-base localization, mobile-base-to-manipulator mounting geometry, manipulator forward kinematics, and flange-to-tool calibration. Keeping these transformations modular allows navigation and manipulation subsystems to evolve independently while preserving a coherent spatial model.

In ROS 2-based systems, robot descriptions, joint states, and transform frameworks can provide standardized interfaces around the kinematics implementation. However, middleware integration should not replace mathematical verification. Frame names, timestamps, transform direction, joint ordering, and synchronization remain application responsibilities, especially when manipulator states are combined with cameras, mobile platforms, or external localization systems.

Time consistency becomes increasingly important when forward kinematics supports perception or dynamic manipulation. A Cartesian pose calculated from joint measurements at one timestamp should not be combined blindly with sensor data captured at another. Joint-state timestamps, interpolation policies, sensor synchronization, and transform buffering should therefore be considered when software reconstructs the manipulator pose associated with a particular observation.

Performance optimization should focus on measurable bottlenecks rather than premature mathematical simplification. Fixed-size matrix operations, precomputed constant transforms, efficient trigonometric evaluation, cached frame paths, and reduced copying can improve execution time. Profiling should verify both average and worst-case latency, particularly when forward kinematics shares a real-time processor with trajectory generation and actuator control.

Safety-related software should avoid treating forward kinematics output as inherently trustworthy. The calculated pose depends on the validity of the robot model, calibration, joint sensors, communication path, and tool configuration. Independent limit checks, plausibility monitoring, redundant sensing where required, and safety-certified mechanisms may therefore be necessary when Cartesian position contributes to protective functions or restricted workspaces.

A well-designed implementation exposes a stable interface while allowing the underlying robot configuration to evolve. Different manipulators, additional joints, interchangeable tools, and updated calibration sets should be accommodated primarily through model data rather than extensive source-code changes. This model-driven approach reduces platform-specific duplication and supports reuse across industrial arms, collaborative robots, mobile manipulators, and humanoid systems.

Ultimately, manipulator forward kinematics software is more than a direct implementation of transformation equations. It forms a foundational spatial service connecting mechanical geometry, joint sensing, Cartesian control, perception, simulation, collision checking, diagnostics, and safety supervision. Accuracy in frame definitions, disciplined software architecture, systematic validation, and deterministic execution are therefore as important as the underlying kinematic mathematics.

06.02 Inverse Kinematics (IK) Numerical Methods [w/Code]

Inverse kinematics (IK) computes the joint configuration required for a manipulator to achieve a desired end-effector pose. Unlike forward kinematics, which directly maps joint variables to Cartesian pose, IK reverses this relationship. For many robot geometries, a closed-form solution is unavailable or impractical, making numerical IK a fundamental software component for general-purpose manipulation.

A numerical IK solver begins with a target pose expressed relative to a clearly defined reference frame and an initial joint configuration used as the starting estimate. Forward kinematics evaluates the current end-effector pose, and the difference between the current and desired poses forms a Cartesian error. The solver repeatedly converts this error into joint corrections until a convergence criterion is satisfied.

Position error can be represented directly as a three-dimensional displacement between the current and target tool positions. Orientation error requires greater care because orientation does not belong to a simple Euclidean vector space. Rotation matrices, quaternions, or Lie-group representations can be used to construct a three-dimensional rotational error that remains consistent with the coordinate convention adopted by the controller.

The manipulator Jacobian provides the local relationship between joint motion and Cartesian motion. For a joint vector q and end-effector velocity ẋ, the differential relationship is commonly expressed as ẋ = J(q)q̇. Numerical IK uses this local mapping in reverse, estimating a joint displacement that reduces the Cartesian pose error. The Jacobian must therefore use the same frames and conventions as the pose-error calculation.

The Jacobian inverse method provides a straightforward solution when the Jacobian is square and nonsingular. A Cartesian correction can be transformed directly into a joint correction through the inverse matrix. However, many practical manipulators are redundant, underactuated, or close to singular configurations, so a conventional matrix inverse cannot provide a robust general-purpose implementation.

The Jacobian pseudoinverse method extends numerical IK to non-square Jacobians and redundant manipulators. Using the Moore--Penrose pseudoinverse, the solver obtains a least-squares joint update that attempts to minimize Cartesian error. For redundant robots, the solution generally corresponds to a minimum-norm joint motion unless additional null-space objectives are introduced to influence the configuration.

Near kinematic singularities, pseudoinverse solutions can generate excessively large joint velocities or configuration changes. Small Cartesian corrections may require large joint motion because one or more singular values of the Jacobian approach zero. A production IK solver must therefore monitor numerical conditioning and apply stabilization rather than relying on an unconstrained pseudoinverse throughout the workspace.

Damped least squares (DLS) is widely used to improve numerical stability near singularities. A damping term regularizes the inverse calculation and limits extreme joint corrections when the Jacobian becomes poorly conditioned. Larger damping improves robustness but may reduce tracking accuracy, while smaller damping provides more precise motion away from singularities. Adaptive damping can balance these requirements according to the current configuration.

Singular value decomposition (SVD) provides a useful foundation for analyzing and solving Jacobian-based IK. Decomposing the Jacobian exposes singular values associated with available Cartesian motion directions. Very small singular values identify poorly controllable directions and can be truncated or regularized. SVD therefore supports pseudoinverse computation, singularity detection, condition monitoring, and adaptive damping strategies.

The Jacobian transpose method offers another numerical approach that avoids explicit matrix inversion. Joint corrections are generated by projecting Cartesian error through the transpose of the Jacobian, typically combined with an appropriate gain or step size. Although convergence can be slower than pseudoinverse-based methods, the method is computationally simple and can be useful when robustness and implementation simplicity are more important than rapid convergence.

Numerical IK can also be formulated as an optimization problem. The desired pose becomes an objective function, while joint limits, collision avoidance, posture preferences, velocity limits, and other requirements can be represented as constraints or additional costs. Gradient-based optimization, sequential quadratic programming, nonlinear least squares, and related techniques allow IK to incorporate requirements that are difficult to represent using a single Jacobian inversion.

Redundant manipulators contain more joint degrees of freedom than are required for the primary Cartesian task. This redundancy allows the robot to achieve the same tool pose using multiple joint configurations. Null-space projection can preserve the primary end-effector objective while using the remaining freedom for secondary goals such as joint-centering, obstacle avoidance, manipulability improvement, posture optimization, or energy reduction.

Joint limits should be considered during the iterative solution rather than checked only after convergence. A mathematically valid Cartesian solution may require a joint to exceed its physical range. Numerical implementations can clamp candidate updates, introduce joint-limit penalties, apply weighted pseudoinverses, or formulate explicit inequality constraints. Smooth limit avoidance is preferable to abrupt clipping because discontinuous corrections can degrade convergence.

The initial configuration strongly influences numerical IK because the solution process is generally local. Different initial guesses may converge to different valid configurations, fail to converge, or enter undesirable regions of the workspace. For trajectory execution, the previous control-cycle solution is often an effective initial guess because it preserves configuration continuity and reduces the number of iterations required for nearby targets.

Step-size management is another important aspect of solver stability. Applying the full calculated joint correction can overshoot the target or destabilize iterations when the local Jacobian approximation is inaccurate. Scaling the update, limiting individual joint increments, or using line-search techniques can improve convergence. Maximum angular and linear increments should also be consistent with actuator and control-loop constraints.

Convergence criteria normally evaluate both translational and rotational errors. Because these quantities use different physical units, their relative weighting must be explicitly defined. A solver may terminate when position and orientation errors fall below independent tolerances, when the total weighted error becomes sufficiently small, or when successive iterations produce negligible improvement. A maximum iteration count prevents uncontrolled computation.

Failure detection is as important as successful convergence. A target may be outside the reachable workspace, blocked by joint constraints, incompatible with the available degrees of freedom, or numerically difficult because of a singularity. The solver should distinguish convergence, timeout, joint-limit conflict, invalid input, numerical failure, and unreachable-target conditions rather than returning only a joint vector with an ambiguous success flag.

Pose weighting allows applications to prioritize selected Cartesian dimensions. Some tasks require accurate tool position but permit orientation freedom, while others constrain only a tool axis or require precise six-degree-of-freedom alignment. A weighted error vector or task Jacobian can represent these differences. This enables the same numerical IK framework to support grasping, inspection, welding, insertion, camera positioning, and mobile manipulation.

Whole-body and mobile-manipulator systems extend the IK variable vector beyond arm joints. Torso joints, mobile-base motion, redundant wrist joints, or other controllable degrees of freedom can participate in solving the Cartesian task. Weighting and constraints can determine how motion is distributed among subsystems, allowing the software to favor arm movement for small corrections while introducing base motion when targets approach the arm workspace boundary.

Collision avoidance can be integrated into numerical IK through constraints, penalty functions, or secondary objectives. Distances between robot links and environmental obstacles can generate repulsive gradients that modify the joint update while preserving the primary task. Self-collision constraints require similar treatment. These mechanisms transform IK from a purely geometric solver into a component capable of producing practically executable configurations.

Manipulability measures can help the solver avoid configurations where Cartesian motion capability deteriorates. Metrics derived from the Jacobian indicate how effectively joint motion can generate motion in different Cartesian directions. A redundant solver can optimize such measures in the null space while maintaining the primary task, improving configuration quality and reducing the likelihood of approaching singular regions during subsequent motion.

Software implementation should separate the numerical solver from the robot model, forward kinematics engine, Jacobian computation, and application interface. The solver requests poses and derivatives through stable APIs rather than embedding robot-specific equations. This modular architecture allows different algorithms, such as pseudoinverse, DLS, or optimization-based IK, to operate on the same manipulator description and be compared under identical conditions.

Real-time implementations require predictable computational behavior. Matrix dimensions should preferably be known in advance, memory allocation should be minimized inside the control loop, and iteration counts should be bounded. Expensive decomposition or optimization procedures must be evaluated against timing requirements. When IK runs within a high-frequency controller, worst-case execution time is often more important than average solver speed.

Trajectory-level IK differs from solving isolated target poses. Consecutive Cartesian commands should generate smooth joint trajectories without sudden branch changes or discontinuities. Using the previous solution as the next initial state, penalizing large joint changes, maintaining null-space continuity, and respecting velocity and acceleration limits can prevent configuration jumps that would otherwise be mathematically valid but physically undesirable.

Numerical validation should compare the solved configuration through independent forward kinematics. After convergence, the resulting joint vector is passed through the forward model and the reconstructed tool pose is compared with the requested target. Position and orientation residuals, joint-limit margins, Jacobian conditioning, and collision status can then be recorded as quantitative indicators of solution quality.

Testing should cover more than nominal reachable poses. Test sets should include workspace boundaries, singular and near-singular configurations, joint-limit regions, redundant solutions, unreachable targets, orientation-dominant tasks, and small incremental trajectories. Randomized target generation and regression testing can reveal numerical weaknesses that remain hidden when only a small collection of manually selected configurations is evaluated.

Simulation provides a safe environment for verifying solver behavior before deployment. The calculated joint configurations can be applied to a digital robot model while monitoring pose accuracy, convergence time, joint continuity, collisions, and singularity proximity. Hardware-in-the-loop testing can subsequently introduce realistic controller timing and communication behavior before the numerical IK software commands physical actuators.

Diagnostics should expose the information required to understand solver decisions. Useful values include iteration count, initial and final error, damping coefficient, Jacobian condition indicators, active constraints, joint-limit margins, termination reason, and computation time. Recording these quantities enables developers to distinguish unreachable geometry from poor initialization, excessive damping, incorrect frame definitions, or numerical instability.

In ROS 2 or comparable robotic software frameworks, IK may operate as a planning service, motion-control component, or reusable kinematics library. Regardless of middleware, frame identifiers, timestamps, joint ordering, tool definitions, and robot-model versions must remain consistent. A solver that is mathematically correct can still generate invalid commands when its coordinate conventions differ from those of the surrounding motion stack.

Safety supervision should remain independent of numerical IK convergence. A converged solution does not guarantee that the resulting motion is dynamically feasible, collision-free, safe for nearby humans, or acceptable to the actuator system. Joint position, velocity, acceleration, torque, workspace, and collision constraints should therefore be checked by appropriate control and safety layers before commands reach the physical robot.

A robust numerical IK implementation combines geometric modeling, differential kinematics, numerical linear algebra, optimization, constraints, and real-time software engineering. Its objective is not merely to minimize Cartesian error, but to produce stable, continuous, physically valid joint configurations under practical robot limitations. This makes numerical IK a central software layer connecting task-level manipulation commands with executable joint-space motion.

06.03 Jacobian-Based Velocity Control [w/Code]

Jacobian-based velocity control provides a differential mapping between manipulator joint motion and end-effector motion in Cartesian space. Instead of solving directly for a complete joint configuration, the controller determines joint velocities that produce a desired linear and angular tool velocity. This approach is fundamental to Cartesian servoing, trajectory tracking, teleoperation, and reactive robot manipulation.

For a manipulator with joint vector q, the differential kinematic relationship is commonly written as ẋ = J(q)q̇, where q̇ represents joint velocities, ẋ represents the Cartesian twist of the end effector, and J(q) is the manipulator Jacobian. The twist normally combines three linear velocity components and three angular velocity components, providing a compact six-dimensional representation of instantaneous tool motion.

The Jacobian depends on the current joint configuration and therefore changes continuously as the manipulator moves. Each Jacobian column describes how motion of a particular joint contributes to the instantaneous Cartesian motion of the selected tool frame. Revolute joints contribute both linear and angular components, while prismatic joints primarily contribute translation along their corresponding joint axes.

A velocity controller normally receives a desired Cartesian velocity from a trajectory generator, teleoperation interface, visual-servoing module, or higher-level manipulation controller. The current joint state is used to calculate the Jacobian, and an inverse differential mapping converts the desired Cartesian velocity into joint velocity commands. These commands are then constrained and transmitted to lower-level joint controllers.

When the Jacobian is square and sufficiently far from singularity, direct inversion can produce q̇ = J⁻¹ẋ. This formulation is conceptually simple but has limited applicability because practical manipulators may contain redundant degrees of freedom or operate near singular configurations. Numerical conditioning must therefore be considered before direct inversion is used in a production motion-control system.

The Moore--Penrose pseudoinverse provides a general solution for non-square Jacobians and is widely used for redundant manipulators. The relationship q̇ = J†ẋ produces a least-squares velocity solution and, under common conditions, minimizes the norm of the joint velocity vector. This enables a robot with more joints than task dimensions to execute the desired Cartesian velocity without requiring a unique joint-space solution.

Singularities represent a major challenge in Jacobian-based control. Near a singular configuration, one or more Cartesian motion directions become difficult or impossible to generate, and the pseudoinverse may request extremely large joint velocities. Such commands can violate actuator limits and destabilize the control system. Singularity detection and regularization are therefore essential parts of practical velocity-control software.

Damped least squares provides a robust alternative near singularities by introducing a regularization parameter into the inverse calculation. The damping term prevents very small singular values from producing excessively large joint velocities. Adaptive damping can increase stabilization as the robot approaches a singular region and reduce damping in well-conditioned configurations, preserving both robustness and Cartesian tracking accuracy.

Singular value decomposition can be used to evaluate the numerical condition of the Jacobian. The singular values reveal how effectively joint motion can generate Cartesian motion along different directions. The smallest singular value, condition number, or related manipulability measures can be monitored in real time. These indicators allow the controller to modify damping, velocity limits, or trajectory behavior before severe singularity effects occur.

Cartesian velocity commands frequently contain both linear and angular components with different units and practical significance. A controller should therefore define appropriate scaling or weighting between translation and rotation. Task-dependent weighting can emphasize position motion, tool-axis orientation, or selected Cartesian dimensions. Without explicit weighting, the numerical solution may not reflect the physical priorities of the manipulation task.

Velocity control can also incorporate Cartesian feedback rather than relying only on feedforward trajectory commands. A desired velocity may be combined with a pose-error correction term, producing a command such as ẋcmd = ẋff + K e. Here, the feedforward component follows the reference trajectory while the feedback component reduces position and orientation errors caused by modeling uncertainty, disturbances, or finite controller bandwidth.

The Cartesian pose error used for feedback must follow the same frame convention as the Jacobian. Position error is relatively straightforward, but orientation error requires a consistent representation based on rotation matrices, quaternions, or rotation vectors. Mixing body-frame and spatial-frame quantities can produce incorrect motion even when each individual calculation appears mathematically valid.

Reference frames must therefore be defined explicitly throughout the velocity-control pipeline. Cartesian twists may be expressed in the world frame, robot base frame, tool frame, or another task frame. The Jacobian must correspond to the same representation, or an appropriate adjoint transformation must convert velocities between frames. Clear frame semantics prevent subtle direction and rotation errors during integrated operation.

Redundant manipulators allow additional motion to be generated without disturbing the primary Cartesian task. A common formulation adds a null-space term to the pseudoinverse solution. The primary component generates the required end-effector velocity, while the null-space component can optimize secondary objectives such as joint centering, collision avoidance, manipulability, posture preference, or reduced actuator effort.

Joint-limit avoidance is particularly important during continuous velocity control because a locally valid command can gradually drive a joint toward its mechanical boundary. Gradient-based secondary objectives can generate corrective null-space motion before a limit is reached. Weighted pseudoinverse methods can also reduce the use of joints approaching restricted regions, providing smoother behavior than abrupt saturation at the joint boundary.

Collision avoidance can be integrated through velocity constraints or repulsive secondary objectives. Distances between robot links and obstacles can be converted into joint-space gradients or Cartesian inequality constraints. As an obstacle approaches, the allowable velocity space can be modified to prevent further penetration while retaining as much of the commanded task motion as possible. Similar methods can address self-collision.

Joint velocity limits must always be enforced before commands reach actuator controllers. Scaling the entire joint velocity vector can preserve the commanded motion direction while satisfying the most restrictive joint limit. Independent clipping is simpler but can distort the resulting Cartesian direction because different joints are modified by different amounts. The selected saturation strategy should therefore match the task requirements.

Acceleration and jerk constraints are also relevant even though the Jacobian relationship is formulated at the velocity level. Large changes in calculated joint velocity between control cycles can exceed actuator capabilities or excite mechanical vibration. Rate limiting, trajectory filtering, command smoothing, or acceleration-aware optimization can ensure that differential kinematic commands remain compatible with the physical robot dynamics.

The Jacobian itself can be derived analytically from the robot geometry or computed using a rigid-body kinematics library. Analytical expressions may provide high computational efficiency for fixed robot structures, while model-based implementations improve portability and maintainability. Regardless of method, the calculated Jacobian should be validated against numerical differentiation of forward kinematics during software verification.

Numerical differentiation provides a useful independent test by perturbing each joint slightly and observing the resulting change in end-effector pose. The finite-difference result can be compared with the corresponding analytical Jacobian column. Appropriate perturbation size is important because excessively large steps introduce approximation error, while extremely small steps can amplify floating-point noise.

Software architecture should isolate robot modeling, Jacobian calculation, Cartesian command generation, inverse differential mapping, constraint handling, and joint command output. Such separation allows the same controller framework to support different manipulators and inverse algorithms. It also simplifies unit testing because each stage can be validated independently before the complete velocity-control loop is executed.

A typical real-time execution cycle reads synchronized joint positions, calculates the current Jacobian, receives or generates the desired Cartesian twist, applies feedback corrections, computes joint velocities, enforces constraints, and sends the final command to the actuator layer. Diagnostic values should be generated during the same cycle without introducing unpredictable blocking or memory allocation into the real-time path.

Control frequency has a direct effect on Cartesian tracking performance. High-frequency updates improve responsiveness and reduce the distance traveled between Jacobian evaluations, but they also increase computational requirements. The controller must complete Jacobian calculation, matrix operations, constraint processing, and communication within a bounded execution time that remains below the available control-cycle period.

Sensor and command timestamps become important when the manipulator moves rapidly. A Jacobian calculated from delayed joint positions may no longer represent the configuration associated with the desired command. State estimation, interpolation, synchronized sampling, and timestamp validation can reduce this inconsistency. Communication latency should also be monitored when Cartesian commands originate from remote planning or teleoperation systems.

Jacobian-based velocity control is particularly useful for visual servoing, where image-based or pose-based perception continuously generates Cartesian corrections. Rather than repeatedly solving complete inverse-kinematics problems, the controller can convert small Cartesian corrections directly into joint velocities. This supports smooth reactive behavior when tracking objects, aligning cameras, approaching grasp targets, or compensating for moving targets.

Force and interaction control also rely heavily on Jacobian relationships. The transpose relationship between Cartesian wrench and joint torque allows forces measured or commanded at the tool to be related to actuator torques. Although velocity and force control have different objectives, maintaining consistent Jacobian definitions enables them to coexist in hybrid position-force control, impedance control, and compliant manipulation architectures.

For mobile manipulators and humanoid robots, the generalized Jacobian can include arm joints, torso motion, mobile-base velocity, or other controllable degrees of freedom. The velocity solver can distribute a Cartesian task across these subsystems according to weights and constraints. This enables coordinated whole-body motion while maintaining a common mathematical framework for task-space control.

Testing should include nominal trajectories, near-singular motion, workspace boundaries, joint-limit approaches, redundant configurations, abrupt command changes, and intentionally infeasible Cartesian velocities. Tests should verify not only tracking error but also joint velocity magnitude, numerical conditioning, constraint activation, continuity, computation time, and the controller's response to invalid or stale state data.

Simulation allows developers to visualize Cartesian and joint-space behavior before commanding physical hardware. Recorded trajectories can be replayed while comparing desired and achieved twists, singularity metrics, joint-limit margins, and collision distances. Hardware-in-the-loop testing can subsequently verify communication timing, actuator interfaces, and command saturation under realistic execution conditions.

Safety supervision must remain separate from the mathematical velocity solver. Even a numerically valid joint velocity may be unsafe because of unexpected obstacles, sensor faults, excessive Cartesian speed, actuator limitations, or incorrect robot calibration. Independent monitoring should enforce joint, Cartesian, collision, workspace, and emergency-stop constraints before or while commands are executed by the physical robot.

Diagnostics and logging are essential for tuning and troubleshooting. Useful signals include desired and measured Cartesian velocities, calculated joint velocities, Jacobian condition metrics, damping values, active constraints, pose errors, saturation ratios, computation time, and control-cycle overruns. These records help distinguish kinematic limitations from controller tuning problems, timing faults, and model inconsistencies.

A robust Jacobian-based velocity controller therefore combines differential kinematics with numerical stabilization, Cartesian feedback, redundancy resolution, constraint management, timing discipline, and safety supervision. Its purpose is not merely to invert a matrix, but to transform task-space motion commands into smooth, feasible, and predictable joint motion suitable for real-time robotic manipulation.

06.04 Torque-Based Joint Space Control [w/Code]

Torque-based joint-space control directly determines actuator torque commands required to regulate manipulator joint motion. Unlike position or velocity interfaces that rely heavily on embedded servo loops, torque control exposes the robot dynamics to the control software. This enables precise dynamic compensation, compliant behavior, force interaction, and high-performance trajectory tracking when the actuator system supports reliable torque command execution.

The dynamic behavior of an n-degree-of-freedom manipulator is commonly represented by M(q)q̈ + C(q,q̇)q̇ + g(q) + τf = τ + τext. The inertia matrix M(q), Coriolis and centrifugal effects C(q,q̇)q̇, gravity vector g(q), friction torque τf, commanded actuator torque τ, and external torque τext together describe how joint forces generate manipulator acceleration.

A basic joint-space torque controller begins with desired joint position, velocity, and possibly acceleration references generated by a trajectory planner. Measured joint positions and velocities are compared with these references to form tracking errors. The controller then converts the errors and available model information into torque commands that are applied to individual actuators through motor drives or embedded joint-control electronics.

The simplest implementation can use proportional-derivative torque feedback, expressed conceptually as τ = Kp(qd − q) + Kd(q̇d − q̇). The proportional term generates restoring torque according to position error, while the derivative term provides damping based on velocity error. Although simple and effective for moderate motion, this controller does not explicitly compensate for configuration-dependent manipulator dynamics.

Gravity compensation is often added because the torque required to maintain a stationary manipulator can vary substantially with configuration and payload. The model calculates g(q) from the current joint state and adds it to the feedback command. Accurate gravity compensation reduces steady-state position error and allows lower feedback gains, which can improve compliance and reduce unnecessary actuator effort.

Computed torque control extends this approach by explicitly compensating for manipulator dynamics. Desired joint acceleration and feedback terms are transformed through the inertia matrix, while Coriolis, centrifugal, gravity, and possibly friction effects are added as feedforward components. With an accurate model, the nonlinear robot dynamics can be approximately transformed into decoupled linear error dynamics for controller design.

A representative computed-torque structure can be written as τ = M(q)[q̈d + Kd(q̇d − q̇) + Kp(qd − q)] + C(q,q̇)q̇ + g(q). This formulation combines feedforward dynamics with feedback stabilization. Its practical performance depends strongly on the accuracy of inertial parameters, payload estimates, friction models, sensor measurements, and actuator torque calibration.

The inertia matrix describes how difficult it is to accelerate each joint and how motion at one joint dynamically affects other joints. Unlike independent joint controllers, model-based torque control can account for these coupling effects. This becomes increasingly important for high-speed manipulators, heavy payloads, long links, and configurations where the effective reflected inertia changes significantly throughout the workspace.

Coriolis and centrifugal terms become important as joint velocities increase. Ignoring these terms may be acceptable during slow motion but can cause tracking errors and additional feedback effort during aggressive trajectories. Model-based compensation predicts these velocity-dependent torques and applies them before large errors develop, improving trajectory tracking without requiring excessively high feedback gains.

Friction introduces another source of uncertainty. Joint transmissions may exhibit viscous friction, Coulomb friction, stiction, gearbox hysteresis, or nonlinear effects that vary with temperature and wear. A friction model can provide feedforward compensation, but aggressive compensation may amplify noise or produce undesirable behavior near zero velocity. Practical implementations therefore combine modeling with robust feedback and carefully tuned transition behavior.

Payload changes directly affect the manipulator dynamics. A robot carrying an unknown object can experience different gravity torques, inertia, and coupling forces from those predicted by the nominal model. Payload mass, center of mass, and inertia can be configured from known tool data or estimated online. Accurate payload modeling is particularly important when manipulating heavy objects or executing high-acceleration trajectories.

Trajectory references should be dynamically feasible before entering the torque controller. Desired positions, velocities, and accelerations must respect joint limits and actuator capabilities. Discontinuous acceleration references can produce sharp torque commands, while excessive desired acceleration can immediately saturate the actuators. Smooth trajectory generation with bounded velocity, acceleration, and jerk improves both tracking performance and mechanical behavior.

Torque saturation must be enforced at the actuator interface. Each joint has continuous and peak torque limits determined by motor capability, transmission design, thermal state, drive electronics, and safety constraints. When saturation occurs, the commanded dynamics can no longer be achieved exactly. Controllers should detect saturation and prevent secondary effects such as integral windup, unstable compensation, or excessive commands at other joints.

Torque-rate limiting can also be important because rapidly changing commands may excite structural vibration, gearbox compliance, or current-control limitations. Filtering or rate limiting can produce smoother actuator behavior, but excessive filtering introduces phase delay and reduces control bandwidth. The selected strategy should therefore balance mechanical protection, noise sensitivity, and dynamic tracking requirements.

Sensor quality strongly influences torque-control performance. Joint encoders provide position measurements, while velocity may be measured directly or estimated through differentiation and filtering. Differentiating encoder signals can amplify measurement noise, which then enters derivative feedback. Velocity observers, low-pass filtering, or state estimation can provide cleaner signals, but filtering delay must be considered when tuning the controller.

Some manipulators include joint torque sensors, motor current sensing, or strain-based measurements that provide additional feedback. Motor current can estimate actuator torque when motor constants and transmission efficiencies are known, while dedicated torque sensors can measure interaction more directly. These signals support disturbance detection, collision monitoring, compliant control, and validation of model-predicted torques.

External forces applied to the end effector appear as joint torques through the Jacobian transpose relationship τext = JᵀFext. This relationship connects joint-space torque control with Cartesian interaction control. Force sensors or disturbance observers can estimate external loads, enabling the controller to distinguish commanded dynamic torque from contact forces and support compliant manipulation or human-robot interaction.

Joint-space impedance control can be implemented by defining virtual stiffness and damping around a desired joint configuration. Instead of enforcing position with extremely high gain, the controller generates restoring torque proportional to displacement and velocity error. The resulting mechanical behavior can be tuned from rigid tracking to compliant response, making impedance control useful for contact-rich manipulation and collaborative robotics.

Disturbance observers can improve robustness against unmodeled dynamics, friction, payload uncertainty, and external disturbances. The observer compares expected dynamic behavior with measured actuator or motion responses and estimates an equivalent disturbance torque. This estimate can be compensated or used for monitoring. Observer bandwidth must be selected carefully to avoid interpreting measurement noise as physical disturbance.

Model uncertainty prevents perfect cancellation of manipulator dynamics. Link masses, centers of mass, inertia tensors, gearbox efficiencies, cable forces, and payload properties may differ from their nominal values. Robust torque control therefore combines feedforward model compensation with feedback rather than relying entirely on inverse dynamics. Feedback preserves stability and tracking performance when the model is imperfect.

Inverse dynamics computation should be implemented efficiently for real-time operation. Recursive Newton--Euler algorithms can calculate required joint torques with computational complexity suitable for manipulators containing many joints. Rigid-body dynamics libraries can also provide inertia, gravity, Coriolis, and inverse-dynamics calculations from a common robot model, reducing the need for manually maintained symbolic equations.

Software architecture should separate trajectory input, robot state acquisition, dynamics computation, feedback control, torque limiting, safety supervision, and actuator communication. The dynamics engine should expose well-defined interfaces so that different control laws can reuse the same robot model. This modularity also allows simulation and physical hardware to share most of the torque-control software.

A real-time torque-control cycle typically acquires synchronized joint position and velocity measurements, evaluates the robot dynamics, calculates tracking errors, generates feedforward and feedback torque components, applies constraints, and transmits the final torque command to the drives. All operations in the critical path should execute deterministically within the available control period.

Control frequency is especially important because torque control interacts closely with the mechanical dynamics of the robot. High-bandwidth control generally requires fast and low-jitter execution, reliable communication, and responsive motor current loops. Delayed or irregular torque commands can reduce damping margins and destabilize the system. Worst-case latency and jitter should therefore be measured rather than inferred from average execution time.

Communication architecture must support deterministic delivery of joint states and torque commands. EtherCAT or comparable real-time fieldbus technologies are commonly used where tightly synchronized multi-axis control is required. Timestamping and distributed clock synchronization help ensure that measurements from different joints represent the same physical instant and that torque commands are applied consistently across the manipulator.

Simulation is essential before enabling direct torque control on physical hardware. A dynamic simulator can verify controller gains, gravity compensation, trajectory tracking, saturation behavior, payload sensitivity, and contact response without risking equipment. However, simulation models should include realistic actuator limits, delays, friction, and sensor noise because idealized models can conceal problems that appear immediately on hardware.

Hardware-in-the-loop testing provides an intermediate validation stage between simulation and full robot deployment. Real controller software, communication interfaces, and timing behavior can be evaluated while the robot dynamics are partially or completely simulated. This helps identify unit errors, sign mistakes, delayed signals, incorrect joint mappings, and command saturation before unrestricted actuator torque is enabled.

Safety requirements are particularly strict for torque-controlled robots because software commands directly influence actuator effort. Independent limits should supervise joint position, velocity, torque, Cartesian workspace, collision state, communication health, and controller timing. Emergency-stop mechanisms and safety-rated functions must remain capable of overriding the normal torque-control software when hazardous conditions occur.

Fault handling should define safe behavior for invalid sensor values, stale state data, model failures, communication loss, control-cycle overruns, excessive tracking error, and actuator faults. Simply setting torque to zero is not always equivalent to a safe state because gravity can cause an unsupported manipulator to fall. Depending on hardware architecture, controlled braking, gravity support, drive disable, or a dedicated safety controller may be required.

Diagnostics should expose the individual torque components rather than only the final command. Logging desired acceleration torque, gravity compensation, Coriolis compensation, friction compensation, feedback torque, estimated disturbance, saturation status, and measured actuator torque makes controller behavior much easier to interpret. These signals are also valuable for parameter identification and maintenance analysis.

Testing should include static holding, slow trajectories, high-speed motion, payload variation, near-limit configurations, torque saturation, external disturbances, communication delay, and sensor faults. Tracking error alone is insufficient as a validation metric. Peak torque, RMS torque, thermal load, timing jitter, model residuals, oscillation, settling behavior, and safety-limit activation should also be evaluated.

A robust torque-based joint-space controller therefore combines accurate robot dynamics, feedback stabilization, actuator constraints, deterministic real-time execution, diagnostics, and independent safety supervision. When implemented correctly, it provides a powerful foundation for high-performance trajectory tracking, compliant manipulation, force interaction, impedance control, and more advanced operational-space and whole-body control systems.

06.05 Task Space Impedance Control Implementation [w/Code]

Task-space impedance control regulates the dynamic relationship between a manipulator's end-effector motion and external interaction forces. Rather than forcing the tool to follow a Cartesian trajectory with very high stiffness, the controller defines a desired mechanical behavior resembling a virtual mass, spring, and damper. This enables accurate motion while allowing controlled compliance during physical contact.

The desired Cartesian impedance is commonly expressed as Md ë + Dd ė + Kd e = Fext, where e represents the difference between desired and actual end-effector pose. Md, Dd, and Kd define the virtual inertia, damping, and stiffness. By selecting these parameters, software can determine how strongly the robot resists displacement and how quickly it responds to external disturbances.

Task-space control requires an accurate estimate of the current end-effector pose and velocity. Joint positions are processed through forward kinematics, while joint velocities are mapped through the manipulator Jacobian to obtain the Cartesian twist. The resulting state must use coordinate frames consistent with the desired trajectory, force measurements, and impedance parameters to avoid incorrect interaction behavior.

Position error is normally calculated from the difference between desired and measured Cartesian positions. Orientation error requires a representation that respects rotational geometry, such as rotation matrices, quaternions, or rotation vectors. Translational and rotational errors are then combined into a six-dimensional task-space error, allowing the controller to regulate both tool position and orientation within a unified formulation.

A practical impedance controller often computes a desired Cartesian wrench from pose and velocity errors. A simplified relationship can be written as Fcmd = Kd e + Dd ė, with optional feedforward terms for desired acceleration or external force. The commanded wrench represents the virtual restoring and damping forces that should act at the tool when it deviates from the desired task-space behavior.

The commanded Cartesian wrench must ultimately be converted into actuator commands. The Jacobian transpose provides the fundamental mapping τtask = JᵀFcmd from Cartesian wrench to joint torque. Additional torque components may compensate for gravity, Coriolis effects, friction, or other robot dynamics. The final actuator torque therefore combines task-space impedance behavior with model-based dynamic compensation.

Operational-space dynamics can provide a more complete formulation by explicitly considering the effective inertia of the manipulator at the end effector. The task-space inertia matrix depends on the joint configuration and Jacobian. Incorporating this information allows the controller to shape Cartesian acceleration and interaction behavior more consistently across configurations where the apparent inertia of the robot changes significantly.

The stiffness matrix determines how strongly the robot attempts to return toward the desired pose after displacement. High stiffness produces accurate tracking but increases contact forces and sensitivity to model errors. Low stiffness provides greater compliance but permits larger deviations. Translational and rotational stiffness can be tuned independently, and anisotropic stiffness can assign different behavior to individual task directions.

Anisotropic impedance is particularly useful for constrained manipulation. During surface following, for example, the robot may require high stiffness along tangential directions to preserve trajectory accuracy while maintaining lower stiffness normal to the surface. Assembly tasks may similarly require directional compliance so that mechanical features can guide the tool into alignment without generating excessive contact forces.

Damping determines how oscillatory motion is dissipated. Insufficient damping can cause vibration or repeated contact oscillation, while excessive damping can make the robot sluggish and increase resistance to intentional motion. Damping values are commonly selected relative to stiffness and effective inertia, but practical tuning must also account for communication delay, actuator dynamics, filtering, and contact characteristics.

Virtual inertia affects how rapidly the commanded Cartesian behavior responds to applied forces. A low apparent inertia can create highly responsive motion, while larger inertia produces slower and more resistant behavior. Implementing inertia shaping requires careful consideration because aggressive cancellation of the robot's natural dynamics can become sensitive to modeling uncertainty and numerical errors.

Gravity compensation is frequently combined with impedance control so that the manipulator does not need large virtual stiffness merely to support its own weight. The dynamics model estimates configuration-dependent gravitational torque and adds it independently to the task-space command. Accurate compensation allows the robot to remain compliant while maintaining its nominal pose against gravity.

External force and torque sensing improves interaction control by providing direct information about contact. A six-axis force/torque sensor mounted near the wrist can measure the wrench applied to the tool, while joint torque sensors or motor-current estimates can provide alternative interaction information. Sensor bias, tool weight, coordinate transformation, and filtering must be handled before these measurements are used by the controller.

Force measurements must be transformed into the same task frame used by the impedance law. A wrench measured at the wrist sensor may need to be compensated for the mass and center of gravity of the attached tool and then transformed to the tool center point. Incorrect moment-arm calculations or frame conventions can create artificial forces and torques that degrade stability during contact.

When dedicated force sensors are unavailable, disturbance observers or momentum observers can estimate external interaction from robot dynamics and measured joint behavior. These approaches compare expected actuator effort with measured or inferred torque. Their accuracy depends on the quality of the dynamics model, friction compensation, sensor noise, and actuator calibration, but they can provide useful contact information without additional hardware.

Null-space control can be combined with task-space impedance when the manipulator has redundant degrees of freedom. The primary task regulates the end-effector behavior, while remaining joint freedom can maintain a preferred posture, avoid joint limits, increase manipulability, or move away from obstacles. Null-space torques must be projected carefully so that secondary objectives do not significantly disturb the primary Cartesian impedance.

Singularities require special treatment because the Jacobian and operational-space dynamics become poorly conditioned. Near singular configurations, Cartesian wrench commands can result in undesirable joint torques or reduced controllability in certain directions. Damped inverses, singular-value monitoring, torque limiting, and trajectory adaptation can maintain stable behavior while preventing excessive actuator commands.

Joint torque limits remain active regardless of the desired Cartesian impedance. A high stiffness combined with a large pose error can generate wrench commands that exceed actuator capability. Wrench saturation, torque saturation, or optimization-based allocation can keep commands within physical limits. Saturation behavior should be designed carefully because uncontrolled clipping may distort the intended Cartesian impedance.

Cartesian velocity, acceleration, and workspace limits should also constrain the controller. Compliance does not imply unrestricted motion, especially when the robot interacts with people or uncertain environments. Supervisory limits can restrict tool speed, allowed displacement from the reference pose, workspace boundaries, and maximum interaction wrench while preserving compliant behavior within the permitted operating region.

Contact transitions are among the most challenging phases of impedance control. A robot moving freely may suddenly encounter a rigid surface, producing a rapid force transient. Smooth approach velocity, appropriate stiffness, sufficient damping, filtered force measurements, and contact-state detection can reduce impact. Controller parameters may also transition between free-space and contact-specific impedance profiles.

Variable impedance control changes stiffness, damping, or other parameters according to the task state. A robot can remain relatively stiff during free-space trajectory tracking and become compliant as it approaches an object or human. Parameter transitions should be smooth because abrupt stiffness changes can generate discontinuous torque commands or inject unwanted energy into the mechanical system.

Passivity and energy behavior are important considerations when the robot physically interacts with an uncertain environment. Communication delay, discretization, filtering, model compensation, and rapidly changing impedance parameters can introduce energy that destabilizes contact. Passivity observers, energy tanks, conservative gain selection, or related techniques can be used when strict interaction stability is required.

Control frequency strongly influences achievable impedance bandwidth. The controller must acquire synchronized joint states, compute kinematics and dynamics, process force measurements, evaluate the impedance law, enforce constraints, and transmit torque commands within a bounded period. High stiffness generally demands faster and more deterministic execution because delay reduces the stability margin of the closed-loop interaction.

Filtering requires a compromise between noise suppression and phase delay. Force sensors, estimated velocities, and torque measurements may contain substantial high-frequency noise, but excessive low-pass filtering delays feedback and can destabilize stiff contact. Filter cutoff frequencies should therefore be selected together with controller bandwidth, mechanical resonance, sampling rate, and expected contact dynamics.

Software architecture should separate robot-state estimation, Cartesian reference generation, kinematics, dynamics, force processing, impedance computation, constraint handling, and actuator communication. This modular structure allows the same task-space controller to operate with different robot models, force sensors, simulation environments, and hardware interfaces while keeping the fundamental impedance law independent of platform-specific communication.

A real-time implementation should avoid dynamic memory allocation, blocking communication, and unpredictable computation inside the control loop. Matrices and buffers can be initialized before execution, while runtime operations use fixed-size structures whenever practical. Worst-case execution time, scheduling jitter, sensor latency, and actuator-command delay should be measured under realistic processor and communication loads.

Simulation provides an essential environment for tuning impedance parameters before physical deployment. Contact stiffness, friction, payload changes, sensor noise, and communication delay can be varied systematically while monitoring interaction forces and motion. Simulations should include realistic contact dynamics because an idealized environment may produce stable behavior that does not transfer directly to physical hardware.

Hardware-in-the-loop testing can validate controller timing, force-sensor interfaces, actuator communication, and fault handling before unrestricted contact experiments. Initial physical tests should use conservative stiffness, limited velocity, restricted workspace, and low-energy interaction conditions. Parameters can then be increased progressively while monitoring stability, tracking accuracy, and peak contact forces.

Task-space impedance control supports applications such as insertion, polishing, surface following, grasping, collaborative manipulation, and human-guided robot motion. Each task requires a different relationship between accuracy and compliance. Software should therefore support configurable impedance profiles rather than assuming a single set of stiffness and damping values for every manipulation operation.

Diagnostics should record desired and actual Cartesian pose, Cartesian velocity, pose error, commanded wrench, measured external wrench, task torque, gravity compensation, null-space torque, saturation state, and impedance parameters. Logging these quantities together with timestamps makes it possible to distinguish poor tuning from model errors, force-sensor problems, communication delay, or unexpected environmental contact.

Safety supervision must remain independent of the nominal impedance controller. A compliant controller can still generate hazardous forces when parameters, sensors, models, or commands are incorrect. Independent limits should monitor joint torque, Cartesian wrench, tool velocity, workspace, collision state, communication health, and controller timing, with safety-rated mechanisms capable of overriding normal control when necessary.

A robust task-space impedance implementation therefore combines Cartesian motion control, force interaction, robot dynamics, Jacobian mappings, redundancy management, constraint enforcement, deterministic execution, and independent safety supervision. Its central objective is to make the manipulator behave like a programmable mechanical system whose apparent stiffness, damping, and inertia can be shaped to match the physical requirements of each task.

06.06 MoveIt2 Integration: Planning and Execution Pipeline [w/Code]

MoveIt 2 integration provides a structured software pipeline that connects high-level manipulation objectives with collision-aware motion planning and executable robot trajectories. Within a ROS 2 manipulator system, MoveIt 2 coordinates robot models, planning algorithms, scene information, kinematics, trajectory processing, controllers, and execution monitoring so application software can request motion without implementing every planning function independently.

A typical manipulation request begins with a target expressed as a joint configuration, Cartesian pose, named robot state, or constrained motion objective. Application software can submit the request through MoveGroupInterface, MoveItCpp, planning components, or ROS 2 actions and services. The request is combined with planning-group information, tolerances, constraints, scaling factors, and the current robot state before entering the planning pipeline.

The robot model provides the structural foundation of the planning system. URDF describes links, joints, geometry, inertial properties, and basic kinematic relationships, while SRDF supplements this information with semantic definitions such as planning groups, end effectors, virtual joints, predefined states, and collision settings. Consistency between these models and the physical robot is essential for meaningful planning and execution.

RobotState represents a particular configuration of the modeled robot and supports forward kinematics, joint-limit checking, transforms, and state-dependent calculations. The current state is normally obtained from joint-state feedback and updated continuously during operation. Planning should begin from a state that accurately represents the physical manipulator, because stale or inconsistent joint information can produce trajectories that begin from an invalid configuration.

The PlanningScene combines the robot state with information about the surrounding environment. Collision objects, attached objects, occupancy information, allowed collision relationships, and world geometry are maintained within this representation. When perception detects a table, fixture, workpiece, or obstacle, the corresponding geometry can be inserted or updated so that motion planning evaluates candidate trajectories against the current environment.

Collision checking is integrated throughout the planning process rather than treated only as a final verification step. Candidate robot states and trajectory segments are evaluated for self-collision and environmental collision. Collision geometry should approximate the physical robot accurately enough for safety while remaining computationally efficient. Padding and scaling can provide additional margins when calibration or perception uncertainty exists.

The planning pipeline receives a MotionPlanRequest containing the start state, goal constraints, path constraints, planning group, allowed planning time, and other parameters. Planning request adapters or preprocessing stages can modify or validate the request before the core planner executes. This architecture allows common operations such as start-state correction, constraint handling, or trajectory processing to be inserted without changing the planner itself.

Sampling-based planners from OMPL are frequently used for manipulator motion planning. Algorithms such as RRTConnect explore configuration space to identify collision-free paths between the initial and goal states. Different planners provide different tradeoffs among planning time, path quality, repeatability, and performance in constrained spaces. The planning configuration should therefore be selected according to the robot and task rather than treated as a universal setting.

MoveIt 2 can also integrate alternative planning approaches for specialized motion requirements. Industrial trajectory generation, optimization-based planning, Cartesian path generation, and custom planning plugins can coexist within the architecture. A system may use different planners for free-space transfer, constrained insertion, repetitive industrial motion, or optimization-intensive tasks while maintaining a common application-level interface.

Kinematics plugins provide inverse and forward kinematics capabilities required by planning and goal processing. Numerical solvers can support general robot structures, while specialized analytical solvers can provide faster or more deterministic solutions for suitable manipulators. Solver timeout, search resolution, joint limits, redundancy behavior, and consistency limits should be configured according to the manipulator geometry and planning requirements.

Pose goals require careful treatment of coordinate frames. A target may originate from a camera, world model, mobile base, fixture frame, or tool-relative coordinate system. ROS 2 TF transformations must convert these poses consistently into frames understood by the planning model. Timestamp mismatches or incorrect frame definitions can create planning failures that appear to be kinematic problems even when the target itself is valid.

Planning constraints allow the motion request to express requirements beyond reaching a final pose. Position constraints can restrict the end effector to a region, orientation constraints can maintain tool alignment, and joint constraints can limit selected joint motion. Path constraints are especially important for tasks such as carrying liquids, maintaining sensor orientation, or moving a tool through a constrained workspace.

Cartesian path generation can be used when the end effector should follow a sequence of Cartesian waypoints more directly than a general configuration-space planner provides. The system interpolates between waypoints and repeatedly solves kinematics while checking motion validity. The resulting path fraction should be evaluated because failure to achieve the entire Cartesian sequence can indicate singularities, collisions, unreachable poses, or excessive waypoint spacing.

Once a geometric path has been found, it must be converted into a time-parameterized trajectory suitable for execution. Joint velocity and acceleration limits are applied to assign timestamps to trajectory points. Depending on configuration, trajectory-processing algorithms can improve timing and smoothness. A geometrically valid path is not automatically executable unless its dynamic limits are compatible with the manipulator and controller.

Velocity and acceleration scaling factors provide application-level control over trajectory aggressiveness. Reduced scaling is useful during initial testing, operation near obstacles, manipulation of uncertain payloads, or human-shared environments. However, scaling should not be considered a substitute for correct joint limits and controller configuration. The underlying robot model must represent realistic actuator capabilities.

Trajectory validation should occur before execution. The planned trajectory can be checked for joint limits, collisions, discontinuities, timing consistency, and agreement with the expected starting state. If the physical robot has moved significantly since planning began, execution of the original trajectory may no longer be appropriate. Start-state tolerance and trajectory monitoring help prevent commands from being applied to an inconsistent configuration.

Execution is typically coordinated through MoveIt's trajectory execution infrastructure and ROS 2 controller interfaces. Joint trajectories are forwarded to controllers such as a JointTrajectoryController, which converts timestamped joint references into lower-level actuator commands. Controller names, joint ordering, interfaces, namespaces, and action endpoints must match the MoveIt configuration and ros2_control hardware description.

The ros2_control framework commonly provides the connection between trajectory-level commands and robot hardware. Hardware interfaces expose joint position, velocity, effort, or command resources, while controller managers activate the appropriate control modules. MoveIt 2 should remain responsible for planning and trajectory coordination, whereas deterministic low-level servo behavior remains within the robot controller and hardware interface layers.

Execution monitoring compares commanded trajectory progress with measured robot state. Deviations may occur because of actuator saturation, controller tuning, communication delays, collisions, or unexpected external forces. Monitoring allows the system to detect when actual motion no longer follows the planned trajectory within acceptable tolerances and to stop or report failure rather than assuming successful completion.

Planning-scene updates must remain synchronized with execution. An object that moves after a trajectory has been planned can invalidate the collision assumptions used during planning. Dynamic applications therefore require perception and scene-management software to update relevant obstacles while coordinating replanning or execution interruption. The required update rate depends on environmental dynamics and robot speed.

Replanning can be triggered when the original plan becomes invalid or execution cannot proceed as expected. The current measured robot state should become the new planning start state, and updated environmental information should be incorporated. Replanning policies require careful design because repeated planning attempts can introduce latency or unpredictable motion if goals, constraints, or environmental conditions remain infeasible.

MoveIt Servo provides a complementary approach for high-rate local Cartesian or joint-space commands. Instead of computing a complete trajectory to a distant target, Servo continuously converts incremental commands into safe robot motion while considering limits and collision proximity. This is useful for teleoperation, visual servoing, joystick control, and reactive manipulation where targets change faster than conventional global replanning can respond.

A practical system can combine global planning and local servo control. MoveIt planning may generate collision-free motion to an approach region, after which MoveIt Servo or a dedicated task-space controller performs precise alignment or contact interaction. Explicit mode transitions are important so multiple control components do not simultaneously command the same joints or create discontinuities when control authority changes.

Manipulation pipelines often include grasp generation, pre-grasp motion, approach, gripper actuation, object attachment, transfer, placement, and retreat. The PlanningScene should update the object state when a grasp succeeds so collision checking treats the object as attached to the robot. Allowed collision settings may also be modified for intentional contact between the gripper and the grasped object.

Planning quality depends heavily on configuration rather than only on planner selection. Joint limits, collision meshes, kinematic solver parameters, planning time, goal tolerances, sampling settings, and scene resolution all affect behavior. Configuration should therefore be version-controlled together with the robot software so planning results can be reproduced and changes can be traced across development releases.

Software architecture should separate application logic from MoveIt-specific integration where practical. A manipulation task layer can request operations such as move-to-pose, approach-object, or place-object through a stable interface, while a motion-planning adapter translates these operations into MoveIt requests. This reduces coupling and allows planners, robot models, or execution backends to evolve without rewriting task-level behavior.

ROS 2 lifecycle management and startup sequencing are important for reliable integration. Robot description parameters, TF publishers, joint-state sources, controller managers, planning-scene monitors, MoveIt nodes, and application components must become available in a predictable order. Readiness checks are preferable to fixed startup delays because initialization time can vary across computers, simulation environments, and physical hardware.

Simulation provides the first environment for validating the complete planning and execution pipeline. RViz can visualize robot states, planning scenes, collision geometry, goals, and planned trajectories, while physics simulators can test controller execution and interaction with the environment. Simulation should verify frame definitions, joint ordering, controller mappings, collision models, and trajectory behavior before physical deployment.

Hardware validation should begin with reduced velocity and acceleration scaling and generous clearance from obstacles. Planned and measured trajectories should be compared while monitoring controller errors, communication latency, joint limits, and emergency-stop behavior. Progressive testing can then introduce tighter spaces, higher speeds, payloads, perception updates, and more complex manipulation sequences after the basic pipeline is verified.

Diagnostics should capture planning request parameters, selected planner, planning duration, start and goal states, failure codes, collision information, trajectory duration, execution status, controller feedback, and scene-update timestamps. Structured logging makes it possible to distinguish failures caused by kinematics, planning, collision geometry, controller configuration, communication, or changing environmental conditions.

Failure handling should provide meaningful recovery behavior rather than repeatedly issuing the same request. Invalid goals, unavailable IK solutions, planning timeout, collision conflicts, controller rejection, execution deviation, stale robot state, and communication failures require different responses. The application layer should receive explicit failure information so it can retry with modified parameters, replan, select another strategy, or enter a safe state.

Safety supervision must remain independent of successful MoveIt planning. A collision-free planned trajectory is based on the robot model and available environment information and cannot guarantee that the physical world exactly matches those assumptions. Hardware limits, safety-rated monitoring, emergency stopping, protective zones, and application-specific checks should remain capable of overriding trajectory execution whenever unsafe conditions are detected.

A robust MoveIt 2 integration therefore connects task commands, robot models, kinematics, planning scenes, collision checking, motion planners, trajectory processing, ros2_control, execution monitoring, and recovery logic into one coherent pipeline. The objective is not merely to generate a collision-free path, but to transform manipulation intent into validated, executable, monitored, and recoverable robot motion suitable for real robotic systems.

06.07 ros2.control Plugin Development [w/Code]

ros2_control plugin development provides the software interface that connects ROS 2 controllers with physical robot actuators, sensors, and communication buses. Instead of embedding hardware-specific code inside motion applications, ros2_control separates controller algorithms from device access through standardized interfaces. This architecture allows the same manipulator software to operate with simulation, prototype hardware, and production robots while changing primarily the hardware plugin and configuration.

The core architecture consists of the Controller Manager, Resource Manager, hardware components, controllers, and state or command interfaces. The Controller Manager coordinates controller lifecycle and periodic execution, while the Resource Manager loads hardware plugins and manages access to their resources. Controllers consume state interfaces and claim command interfaces without needing to understand the communication protocol used by the underlying actuator hardware.

Hardware plugins are commonly implemented using the SystemInterface, ActuatorInterface, or SensorInterface abstractions. A SystemInterface is appropriate when multiple joints and sensors belong to one coordinated hardware system, while ActuatorInterface represents a more isolated actuator and SensorInterface exposes sensing resources. Manipulators frequently use SystemInterface because synchronized communication and shared buses naturally span multiple joints.

A plugin is typically implemented as a C++ class derived from the appropriate ros2_control hardware interface. The class processes hardware information from the robot description, initializes internal resources, exports state and command interfaces, manages lifecycle transitions, and performs cyclic read and write operations. Pluginlib registration allows the Resource Manager to instantiate the implementation dynamically from its configured class name.

The robot description defines the hardware component through the ros2_control section of the URDF or Xacro model. Each joint specifies available command interfaces such as position, velocity, or effort and state interfaces such as position, velocity, or effort feedback. Hardware parameters can describe communication ports, device addresses, reduction ratios, encoder resolutions, limits, or other implementation-specific settings required by the plugin.

Interface definitions must reflect actual hardware capability rather than desired application behavior. A motor drive accepting only velocity commands should not be exposed as a native torque interface unless a reliable lower-level torque controller exists. Similarly, state interfaces should clearly represent measured or estimated quantities. Incorrect abstraction can produce control behavior that appears valid at the ROS level but cannot be realized safely by the hardware.

Initialization parses configuration data and prepares internal structures without beginning uncontrolled device motion. The plugin should validate joint names, expected interfaces, required parameters, communication settings, and device configuration. Invalid hardware descriptions should generate explicit errors and prevent activation rather than allowing execution with incomplete mappings or default values that may command the wrong actuator.

Lifecycle management provides controlled transitions between initialization, configuration, activation, deactivation, cleanup, and error handling. Communication resources can be opened during configuration, while actuator commands should generally become active only after the hardware reaches a verified operational state. Deactivation should place the system into a defined safe condition instead of simply terminating software communication.

The read operation transfers physical hardware state into ros2_control state interfaces. Depending on the system, this may include encoder positions, velocities, measured torque, motor current, temperature, fault status, or sensor values. The read path should minimize unnecessary computation and blocking because it executes as part of the real-time control cycle and directly affects controller timing.

The write operation transfers controller outputs from command interfaces to the hardware. Position, velocity, or effort commands may require conversion into encoder counts, motor current, drive-specific units, or network messages. Unit conversion, gear ratios, sign conventions, offsets, and joint ordering must be defined consistently because a single mapping error can produce incorrect motion despite correct controller calculations.

A manipulator with multiple actuators requires deterministic correspondence between ROS joint names and physical drive identifiers. Software should not depend on accidental ordering in configuration files or network discovery. Explicit mappings between joint names, actuator IDs, bus addresses, and interface indices improve reproducibility and reduce the risk that a valid command for one joint is transmitted to another.

Communication may use EtherCAT, CAN, CAN FD, serial links, Ethernet, vendor APIs, or shared-memory interfaces. The hardware plugin should isolate protocol-specific operations from controller logic. A transport abstraction can further separate packet encoding and bus access from joint-level conversion, making it easier to test communication independently or migrate the robot to different drive electronics.

Real-time behavior is a central requirement for high-performance control. The cyclic read-update-write sequence should execute with bounded latency and minimal jitter. Dynamic memory allocation, blocking file operations, console output, unpredictable network calls, and long mutex waits should be avoided in the critical path. Buffers and message structures should be prepared before activation whenever practical.

The Controller Manager typically performs the repeated sequence of reading hardware state, updating active controllers, and writing new commands. The update rate should match actuator dynamics, communication bandwidth, and controller requirements. Position-controlled industrial manipulators may tolerate lower rates than direct torque-control systems, which can require substantially tighter timing and lower latency.

Asynchronous hardware execution can be useful when a device operates at a different rate from the main controller loop. A slow sensor or network interface should not necessarily block high-rate joint control. Separate execution rates require carefully designed data exchange so controllers always receive coherent state information while avoiding races between real-time and non-real-time threads.

Command-mode switching becomes important when a robot supports multiple control modes such as position, velocity, and effort. The hardware plugin may need to prepare and perform transitions when controllers claim different command interfaces. Mode switching should verify drive capability, reset incompatible internal states, and prevent discontinuous commands when control authority moves from one interface to another.

Controller plugins provide another extension layer within ros2_control. A custom controller can derive from the appropriate controller interface and implement lifecycle callbacks, interface configuration, and periodic update logic. This is useful when standard controllers do not provide the required behavior, such as specialized joint control, coordinated multi-axis algorithms, impedance control, or application-specific actuator coordination.

A custom controller should explicitly declare which command and state interfaces it requires. Resource claiming prevents incompatible controllers from commanding the same hardware resource simultaneously. Controller switching can then activate or deactivate control strategies while preserving ownership rules. Clear interface declarations are especially important when combining trajectory control, torque control, gripper control, and sensor broadcasting.

Real-time communication between ROS 2 callbacks and controller update loops requires special care. Commands arriving through topics, actions, or services originate in non-real-time execution contexts. Real-time buffers or lock-minimized data structures can transfer the latest command into the deterministic update loop without introducing unbounded synchronization delays.

Joint limits should be enforced close to the actuator command path. Position, velocity, acceleration, effort, and rate limits can originate from robot descriptions, controller configuration, or hardware specifications. The software should define which layer owns each limit and avoid inconsistent duplicate implementations. Hardware-level protection should remain available even when higher-level software already constrains trajectories.

Fault information from drives and sensors should propagate through the hardware layer in a structured manner. Overcurrent, overtemperature, encoder faults, communication loss, following error, emergency-stop state, or amplifier faults may require immediate action. The plugin should distinguish recoverable communication problems from conditions requiring controller deactivation or hardware shutdown.

Returning an error from read or write operations should correspond to a deliberate recovery strategy. Repeatedly continuing after corrupted state data can be more dangerous than stopping the controller. Error handling should define whether the system retries communication, transitions lifecycle state, disables actuators, engages brakes, or transfers authority to an independent safety system.

Diagnostics should be separated from the real-time critical path whenever possible. Communication statistics, bus errors, temperatures, voltages, controller timing, packet loss, and actuator status are valuable for maintenance and debugging, but formatting and publishing large diagnostic messages inside the control loop can introduce jitter. Non-real-time threads can consume snapshots of diagnostic data for publication.

Time synchronization is important when state information originates from distributed devices. Encoder measurements, force sensors, IMUs, and external sensing systems may operate on different clocks. Hardware timestamps or synchronized clocks can improve temporal alignment, while the plugin should clearly define whether state values represent acquisition time, reception time, or the most recent available sample.

Simulation plugins allow the same controller architecture to operate without physical hardware. Mock components, Gazebo integrations, or custom simulated systems can expose interfaces identical to those of the production robot. This allows controller logic, launch configuration, lifecycle behavior, and higher-level MoveIt 2 integration to be tested before connecting to actual actuators.

A useful development strategy is to separate hardware-independent conversion logic from device I/O. Encoder conversion, joint offsets, gear ratios, command saturation, and state validation can be unit-tested without a physical bus. Transport code can then be tested using recorded packets, loopback devices, or simulated drives, reducing the amount of debugging that must occur on an energized robot.

Integration testing should verify startup, controller activation, command execution, controller switching, communication interruption, sensor faults, limit violations, and shutdown behavior. Testing only nominal trajectory execution is insufficient because many serious failures occur during initialization or transitions. Automated tests can repeatedly verify interface mappings and lifecycle behavior after software changes.

Hardware commissioning should proceed incrementally. Communication can first be validated with actuator power disabled, followed by state reading, individual low-energy joint commands, limit verification, and coordinated motion. Position, velocity, and torque directions should be checked independently. Conservative command limits reduce risk while joint mappings and sign conventions are being verified.

MoveIt 2 integration usually occurs above ros2_control. MoveIt generates and supervises trajectories, while controllers such as JointTrajectoryController execute the requested joint motion through ros2_control hardware interfaces. Maintaining this separation allows motion planning to remain independent of motor protocol details and allows the hardware layer to evolve without changing the planning application.

Direct torque and impedance controllers require additional attention because they expose more of the physical dynamics to software. High-rate effort commands depend on reliable state feedback, deterministic bus timing, calibrated torque conversion, saturation handling, and independent safety limits. The hardware plugin should avoid hiding delays or filtering that materially change the dynamic behavior assumed by the controller.

Configuration management is essential when one software stack supports several robot variants. Joint names, drive IDs, reductions, limits, control modes, bus parameters, and calibration values should be version-controlled and associated with specific hardware revisions. Runtime validation can reject incompatible combinations rather than allowing a configuration intended for one manipulator to operate another.

Logging should capture controller lifecycle events, hardware state transitions, communication failures, interface claims, mode switches, timing statistics, and safety-related faults. High-rate numerical data should be recorded through efficient mechanisms rather than excessive console output. Consistent timestamps allow hardware behavior to be correlated with controller commands, planner events, and external sensor data.

Independent safety mechanisms remain necessary even when the plugin performs extensive validation. ros2_control is part of the functional control architecture and should not be assumed to replace safety-rated torque limits, emergency stops, protective stops, brake control, or certified monitoring devices. The hardware plugin should cooperate with these mechanisms and expose their state without bypassing their authority.

A robust ros2_control plugin therefore combines standardized interfaces, deterministic hardware access, lifecycle management, explicit resource ownership, correct unit conversion, fault propagation, controller integration, testing, and safety coordination. Properly designed plugins create a stable boundary between robot-specific electronics and reusable ROS 2 control software, enabling manipulators to progress from simulation to real hardware without redesigning the complete control architecture.

06.08 End Effector Force/Torque Control and Compliance [w/Code]

End-effector force and torque control regulates the physical interaction between a robot tool and its environment by controlling the wrench applied at the tool center point. A wrench consists of three-dimensional force and three-dimensional torque, allowing the controller to manage both translational and rotational interaction. This capability is essential for assembly, insertion, polishing, surface following, grasping, contact inspection, and collaborative manipulation.

A six-axis force/torque sensor is commonly mounted between the robot wrist and the end effector to measure interaction forces and moments. The sensor provides Fx, Fy, Fz, Tx, Ty, and Tz in its measurement frame. These measurements must be calibrated for sensor bias, tool mass, gravity, mounting orientation, and center-of-mass effects before being interpreted as external environmental forces.

Force and torque measurements are meaningful only when their coordinate frames are correctly defined. A sensor frame, tool frame, robot flange frame, and task frame may all have different orientations and origins. The measured wrench therefore requires appropriate rotation and moment transformations before being used by the controller. Incorrect frame transformations can create apparent forces or torques that do not physically exist.

Tool gravity compensation is particularly important because the force sensor measures both environmental interaction and the weight of the attached tool. If the tool mass and center of mass are known, its gravitational wrench can be calculated from the current orientation and removed from the sensor measurement. Accurate compensation allows the controller to distinguish actual contact forces from forces generated simply by holding the tool against gravity.

A basic force-control loop compares a desired wrench with the measured external wrench and generates a corrective command from the error. The control objective can be represented conceptually as Fcmd = Fd + Kf(Fd − Fm), where Fd is the desired wrench and Fm is the measured wrench. Practical implementations normally include filtering, saturation, dynamic compensation, and carefully selected control gains to maintain stable interaction.

Direct force control attempts to regulate the measured interaction force toward a specified target. This approach is useful when maintaining a known contact force is more important than exact position. For example, a polishing tool may need to maintain a nearly constant normal force while following a surface. The controller continuously modifies robot motion so that environmental resistance remains close to the desired value.

Compliance control provides a related but different behavior by allowing controlled displacement in response to external force. Instead of attempting to eliminate every positional deviation, the robot intentionally yields when an external force is applied. Compliance can be implemented through impedance or admittance control, depending on whether the robot generates a force response from motion errors or generates motion commands from measured forces.

Impedance control defines a desired mechanical relationship between displacement, velocity, acceleration, and interaction force. A virtual spring and damper can cause the end effector to resist displacement while remaining compliant. The apparent stiffness and damping can be independently configured for translational and rotational directions, allowing the robot to behave rigidly in some directions while remaining flexible in others.

Admittance control uses measured external force or torque as an input to a virtual dynamic model that produces a desired motion response. This approach is particularly useful when the robot has a position-controlled interface but requires force-responsive behavior. The measured wrench is transformed into a desired velocity, displacement, or acceleration, which is then executed through the existing motion-control hierarchy.

Hybrid position-force control separates task directions according to their physical requirements. Position control can maintain accurate motion along directions where free-space tracking is required, while force control regulates contact along constrained directions. For surface interaction, for example, tangential motion may be controlled primarily by position while the normal direction is controlled according to a desired contact force.

The Jacobian provides the mathematical connection between Cartesian wrench and joint torque. A Cartesian wrench can be mapped into joint space using τ = JᵀF. This relationship allows an end-effector force-control objective to influence individual joint actuators. Gravity, Coriolis effects, friction, and other dynamic compensation terms may then be added before the final torque command is transmitted to the robot hardware.

Contact detection can be implemented by monitoring measured force, torque, or estimated external disturbances. When the measured wrench exceeds expected free-space values, the system can identify a possible collision or contact event. Thresholds should account for sensor noise, robot acceleration, gravity compensation errors, and expected dynamic loads. More advanced methods can use residual models or observers to improve detection reliability.

Force and torque signals normally require filtering because sensors contain electrical noise, mechanical vibration, quantization effects, and structural resonances. However, excessive filtering introduces phase delay that can reduce control stability, especially during stiff contact. Filter parameters must therefore be selected together with the control frequency, mechanical dynamics, expected contact bandwidth, and desired interaction response.

Saturation and safety limits are essential because environmental contact can generate forces that exceed the mechanical capability of the robot or tool. Maximum allowable force, torque, velocity, joint effort, and Cartesian displacement should be enforced independently of the nominal control law. When a limit is reached, the system should enter a defined protective behavior rather than continuing to increase the commanded interaction effort.

End-effector compliance is strongly influenced by mechanical transmission characteristics. Gearbox elasticity, harmonic-drive compliance, structural flexibility, tool deformation, and mounting stiffness can affect the relationship between actuator torque and actual tool motion. A controller designed using an ideal rigid-body model may therefore produce unexpected interaction behavior unless these mechanical characteristics are considered during calibration and tuning.

Tool changes require corresponding updates to the force-control model. Different tools can have different masses, centers of gravity, contact geometries, stiffness, and allowable force limits. A practical system should load tool-specific parameters and verify them before activating force control. Incorrect tool parameters can cause gravity-compensation errors and may produce unintended contact forces.

Real-time execution is critical for stable force interaction. Sensor acquisition, coordinate transformation, filtering, force-control calculation, kinematic or dynamic compensation, constraint processing, and actuator command transmission must occur within predictable timing limits. Communication delays and jitter reduce the effective control bandwidth and can destabilize interaction, particularly when high stiffness or rapid force changes are required.

Force-control software should separate sensor processing, wrench transformation, contact estimation, control-law calculation, safety supervision, and hardware communication. This modular architecture allows different force sensors and control strategies to share common infrastructure. The same framework can support direct force control, impedance control, admittance control, hybrid position-force control, and contact-event detection without duplicating the complete hardware interface.

Calibration should include sensor zeroing, tool mass identification, center-of-mass estimation, coordinate-frame verification, and force-direction validation. Calibration results should be stored with the corresponding tool and robot configuration. Before operation, the system should verify that measured static forces and moments are consistent with expected values. Large residual errors can indicate incorrect mounting, sensor drift, or an invalid tool model.

Testing should begin with free-space motion before introducing physical contact. The controller can first verify sensor readings, gravity compensation, coordinate transformations, and command limits. Contact experiments should then use low velocity, low target force, compliant surfaces, and restricted workspace conditions. Gradually increasing contact force and motion speed provides a controlled method for evaluating stability and interaction performance.

Applications such as insertion, connector mating, grinding, polishing, deburring, surface inspection, and human-guided manipulation require different combinations of force accuracy and compliance. A robust system should therefore support task-specific force profiles rather than relying on a single fixed force-control configuration. Directional force limits, variable stiffness, contact-state transitions, and tool-specific parameters can be combined according to the physical requirements of each operation.

A reliable end-effector force and torque control system integrates accurate sensing, frame transformation, gravity compensation, force regulation, compliance behavior, dynamic compensation, real-time execution, constraint handling, and independent safety supervision. The objective is not simply to measure contact forces, but to make the robot respond to physical interaction in a predictable and programmable manner while maintaining stable motion, protecting hardware, and supporting precise manipulation in uncertain environments.

06.09 Manipulator Collision Detection and Safe Stop SW [w/Code]

Manipulator collision detection and safe-stop software provides a protective layer between robot motion commands and physical interaction with the environment. Its purpose is to detect unexpected contact, excessive external force, abnormal motion, or hazardous operating conditions and then transition the manipulator into a controlled protective state. The software should operate independently from normal motion planning so that a valid trajectory cannot prevent protective intervention when the physical situation differs from the planning assumptions.

Collision detection can use several complementary sources of information. Joint torque, motor current, force/torque sensor measurements, joint position error, velocity deviation, and external perception can each provide evidence of contact. A robust system combines these signals according to the robot architecture instead of depending on one measurement alone. Sensor-based detection can respond directly to contact, while model-based methods can identify unexpected disturbances even when a dedicated force/torque sensor is not installed.

Model-based collision detection estimates the torque or force that should occur during normal motion and compares it with the measured actuator response. A simplified residual can be expressed as r = τmeasured − τexpected, where the expected value may include gravity, friction, inertial, and Coriolis effects. A sufficiently large residual can indicate an external disturbance. The model must be calibrated carefully because inaccurate dynamics, friction, payload parameters, or sensor offsets can otherwise produce false collision detections.

Joint torque sensing or motor-current estimation provides a useful low-level collision signal. When an actuator encounters an unexpected obstacle, the required torque or motor current can increase rapidly. However, acceleration and dynamic motion can also produce legitimate torque changes, so a fixed threshold is usually insufficient. Thresholds should consider joint position, velocity, acceleration, direction of motion, payload, and expected dynamic load to distinguish normal operation from abnormal contact.

A six-axis force/torque sensor at the wrist provides direct information about external interaction at the end effector. The measured wrench can be transformed into the appropriate task frame after removing sensor bias and compensating for tool gravity. Force and torque thresholds can then be applied according to the task. This approach is particularly effective for contact-sensitive manipulation, although sensor saturation, vibration, calibration errors, and tool-specific loads must be considered.

Perception-based collision detection complements internal robot sensing by monitoring the external environment. Cameras, depth sensors, LiDAR, proximity sensors, or safety scanners can detect obstacles before physical contact occurs. A perception system can update the planning scene and trigger a protective response when an object enters a restricted region. Because perception has latency and uncertainty, it should not be treated as the only safety mechanism for stopping fast-moving machinery.

Collision detection should distinguish expected contact from unexpected collision. In assembly, insertion, grasping, surface following, and tool interaction, physical contact may be part of the normal task. The software therefore needs a task state that defines when contact is permitted and what force or torque range is acceptable. A force exceeding the expected range during an authorized insertion can trigger recovery, while the same force during free-space motion may require an immediate protective stop.

Threshold design is a critical part of collision detection. Excessively low thresholds can generate nuisance stops caused by sensor noise, friction variation, acceleration, or normal contact. Excessively high thresholds may allow damaging forces to develop before the system reacts. Thresholds should therefore be evaluated using measured robot dynamics and task requirements, with separate limits for warning, protective response, and emergency intervention where appropriate.

Signal filtering is necessary because force, torque, current, and position signals contain noise and mechanical vibration. Low-pass filters, moving averages, observer-based estimation, or adaptive filtering can improve detection reliability. However, excessive filtering introduces delay between physical contact and detection. The collision-detection path should therefore use a predictable and sufficiently high update rate, with filter characteristics selected according to the required reaction time.

A safe-stop mechanism should define what happens immediately after collision detection. Depending on the hazard and robot architecture, the system may reduce commanded velocity, stop trajectory execution, apply controlled deceleration, disable a controller, remove actuator torque, engage brakes, or transfer control to an independent safety system. These responses should be deterministic and clearly associated with defined fault classes rather than being improvised by application-level code.

Controlled stopping and emergency stopping are different functions. A controlled stop attempts to reduce robot motion in a managed manner while preserving system integrity, whereas an emergency stop is intended for situations requiring rapid removal of hazardous motion. The selected response depends on kinetic energy, payload, robot speed, workspace conditions, and safety requirements. Software should never assume that a conventional trajectory cancellation is equivalent to a safety-rated stop.

The safe-stop architecture should remain effective even when higher-level software fails. MoveIt 2, task planners, perception nodes, and application processes may become unavailable, but the protective mechanism should still be capable of stopping the robot when required. Hardware-level limits, safety controllers, emergency-stop circuits, protective scanners, and drive-level functions can provide independent layers. This layered architecture reduces dependence on a single software component.

After a protective stop, the system should enter a defined fault or protective state rather than automatically resuming motion. The robot state, detected cause, measured forces, joint positions, controller status, and environmental information should be recorded. Recovery may require clearing the obstacle, acknowledging the fault, validating the workspace, recalibrating sensors, and generating a new trajectory from the current physical state. Automatic recovery should only be permitted when the conditions are explicitly defined as safe.

Collision detection and safe stop must account for communication failures. Loss of Ethernet, CAN, EtherCAT, ROS 2 communication, controller heartbeat, or sensor data can leave the software without reliable knowledge of robot state. Timeouts and watchdog mechanisms should detect stale information and transition the system to an appropriate protective condition. A missing update should never be interpreted as evidence that the robot is stationary or operating normally.

Real-time implementation is essential because collision detection must react within a predictable time. Sensor acquisition, state estimation, residual calculation, threshold evaluation, and protective command generation should execute with bounded latency. Non-real-time logging, visualization, parameter services, and diagnostic processing should remain outside the critical path. Measured reaction time should include sensing delay, computation time, communication delay, controller response, and actuator stopping behavior.

The collision detector should support hysteresis and persistence logic to avoid repeated triggering around a threshold. A single noisy sample should not necessarily cause a protective stop, while a rapidly increasing residual should be detected immediately when the risk is significant. Different detection strategies can use persistence time, rate-of-change thresholds, direction-dependent limits, or multi-sensor agreement. These mechanisms should be tuned without compromising the required protective response time.

Safe-stop software should also monitor abnormal robot motion that may not correspond to physical collision. Excessive following error, unexpected joint velocity, encoder disagreement, actuator saturation, position drift, or motion outside the commanded workspace can indicate a control or hardware problem. These conditions should be treated as safety-relevant faults because uncontrolled motion can create a hazardous situation even when no obstacle has yet been detected.

The software architecture should separate detection, decision, stopping, diagnostics, and recovery functions. A collision detector produces structured events containing the affected joint or Cartesian direction, estimated force or residual, confidence, timestamp, and fault classification. A safety decision layer determines the appropriate response, while the execution layer applies the stop command. This separation makes the system easier to validate and allows different sensing methods to share a common protective framework.

Integration with ros2_control requires clear ownership of stopping authority. A normal trajectory controller may stop by cancelling its command, but a safety layer should be capable of overriding ordinary trajectory execution. Controller switching, command-interface release, hardware fault handling, and drive disable functions must be coordinated so that stopping one controller does not unintentionally activate another motion source. The final state of every actuator should be explicitly defined.

Testing should include both nominal and abnormal operating conditions. Free-space motion can be used to establish normal residual and force ranges, followed by controlled contact experiments using compliant targets and conservative velocities. Tests should progressively evaluate different payloads, joint configurations, directions of contact, communication delays, sensor faults, controller failures, and obstacle locations. Measured detection and stopping times should be compared with the system requirements.

A robust manipulator collision-detection and safe-stop system combines model-based monitoring, force and torque sensing, actuator feedback, environmental perception, real-time threshold evaluation, deterministic stopping, independent safety mechanisms, fault management, and controlled recovery. The objective is not simply to detect that a collision has occurred, but to identify abnormal interaction early, limit hazardous energy, stop the robot predictably, preserve diagnostic information, and provide a controlled path toward safe recovery and continued operation.

06.10 Industrial Manipulator Control SW Integration Case

An industrial manipulator control software integration case combines motion planning, robot modeling, real-time control, hardware interfaces, safety supervision, and production-level diagnostics into one coordinated system. The objective is to transform a manufacturing task such as assembly, handling, machine tending, or inspection into deterministic robot motion while maintaining synchronization between high-level planning and low-level actuator control. The integrated architecture must remain modular so that individual components can be tested, replaced, and maintained without redesigning the complete control stack.

The system typically begins with a task application that defines the required operation using objects, poses, trajectories, process parameters, and execution conditions. A task-management layer converts these requirements into motion requests and process states. MoveIt 2 can provide collision-aware planning and trajectory generation, while ROS 2 nodes coordinate perception, planning, execution, diagnostics, and production interfaces. The application should not directly manipulate motor commands because doing so would bypass the control and safety hierarchy.

The robot model provides the common representation shared by planning and control software. URDF or Xacro describes links, joints, geometry, inertial properties, and coordinate relationships, while SRDF defines semantic groups, end effectors, virtual joints, and collision relationships. The model must correspond closely to the physical manipulator, including joint limits, tool geometry, payload assumptions, and transmission characteristics. Errors in the model can propagate into inverse kinematics, collision checking, trajectory generation, and dynamic compensation.

A typical industrial execution sequence starts by acquiring the current robot state and validating the task conditions. The system checks joint positions, controller availability, safety status, tool configuration, payload information, and relevant production signals before motion begins. The planner then generates a valid trajectory from the measured starting state toward the required target. Before execution, the trajectory is checked against joint limits, collision constraints, velocity and acceleration limits, and the expected controller capabilities.

The motion trajectory is transferred to a trajectory controller through ros2_control. MoveIt 2 remains responsible for high-level planning and execution coordination, while the controller converts timestamped joint trajectories into real-time actuator commands. The hardware interface manages communication with motor drives, encoders, brakes, and other devices. This separation allows the same planning software to operate with different robot hardware while maintaining a stable interface between application-level motion and device-level control.

For applications requiring precise joint regulation, the controller can use position or velocity feedback to reduce trajectory error. More advanced manipulators may provide effort or torque interfaces that allow dynamic compensation, impedance control, or direct interaction control. The selected control mode must correspond to the actual actuator capabilities and real-time performance of the hardware. Control software should not expose an interface that suggests capabilities unavailable in the underlying drive system.

Industrial manipulation often requires accurate tool-center-point control. The software maintains the relationship between the robot flange, tool frame, workpiece frame, and manufacturing station. A camera or external sensor may provide an object pose in another coordinate frame, requiring TF-based transformation before planning. Calibration data must be managed as configuration rather than embedded in application code so that tool changes, fixture changes, and station recalibration can be handled systematically.

Production environments introduce objects and constraints that may change during operation. Workpieces, fixtures, conveyors, pallets, and temporary obstacles can be represented in the PlanningScene. Perception software can update relevant objects before planning or during selected execution states. If a critical environmental change invalidates the planned path, the system should stop or replan according to the task policy rather than blindly executing a trajectory generated from outdated assumptions.

Industrial cycle performance depends on more than planning speed. The system must coordinate perception, planning, controller activation, robot motion, gripper operation, process equipment, and completion confirmation. A state-machine or task-execution framework can represent transitions such as ready, approach, grasp, transfer, place, verify, retreat, and complete. Each transition should define entry conditions, expected results, timeout behavior, and recovery actions so that abnormal production states do not become uncontrolled motion.

End-effector control is closely integrated with the manipulation sequence. A gripper may require position, velocity, or effort commands, while a welding torch, polishing tool, suction device, or inspection sensor may require specialized interfaces. The task layer should treat these devices as coordinated resources rather than independent accessories. Tool activation, robot motion, process timing, and verification signals must be synchronized to prevent operations from occurring in the wrong sequence.

Force and compliance control can extend the industrial manipulator beyond rigid trajectory tracking. During insertion, fitting, surface contact, or polishing, the robot may need to maintain a controlled force while allowing small positional deviations. A force/torque sensor, joint torque estimate, or disturbance observer can provide interaction information. The controller can then use impedance, admittance, or hybrid position-force control while maintaining independent force, torque, velocity, and workspace limits.

Collision detection provides another protection layer during industrial operation. Expected actuator torque can be compared with measured torque or motor current to identify abnormal interaction, while force sensors and external perception can provide complementary evidence. Collision thresholds should depend on configuration, velocity, payload, and task state. A contact that is normal during insertion may represent a fault during free-space transfer, so collision logic must understand the current operating mode.

Safe-stop behavior must be independent of normal trajectory execution. When a hazardous condition is detected, the system may perform a controlled stop, protective stop, drive disable, or emergency stop depending on the severity and safety architecture. The final stopping authority should not depend solely on MoveIt 2 or an application node. ros2_control, hardware protection, safety controllers, emergency-stop circuits, and certified monitoring devices may provide independent layers that remain active when high-level software fails.

Real-time performance becomes increasingly important as the system moves from planning into physical control. The controller cycle must acquire state data, update the control law, enforce limits, and transmit actuator commands within a bounded period. Dynamic memory allocation, blocking operations, excessive logging, and unpredictable communication should be removed from the critical control path. Worst-case execution time, jitter, sensor latency, communication delay, and actuator response should be measured rather than estimated.

Industrial systems also require deterministic startup and shutdown behavior. Robot descriptions, hardware interfaces, controller managers, MoveIt nodes, TF publishers, sensors, and task-management nodes should become ready in a controlled sequence. The system should verify hardware availability and safety state before enabling motion. During shutdown, active trajectories should be terminated safely, actuators should enter their defined state, and communication resources should be released without leaving a partially active controller.

Controller switching is particularly important when a manipulator supports multiple operating modes. A production system may use trajectory control for normal transport, servo control for visual alignment, impedance control for contact operations, and specialized controllers for tools. Only the intended controller should own the corresponding command interfaces at a given time. Mode transitions must avoid discontinuous commands and should verify that the newly activated controller has valid state information before it receives authority.

Diagnostics should be integrated across all layers of the system. Planning duration, planner results, trajectory status, joint tracking error, controller timing, actuator temperature, drive faults, communication statistics, collision events, force measurements, and safety transitions should be recorded with consistent timestamps. This information allows engineers to distinguish production-process failures from planning failures, controller problems, hardware faults, calibration errors, and environmental changes.

A practical industrial integration should also provide structured recovery behavior. A failed grasp may require reopening the gripper and retrying, while an invalid path may require replanning. A controller fault may require hardware inspection rather than another motion attempt. A detected collision may require operator confirmation and workspace verification. Recovery logic should therefore classify failures instead of applying one generic retry mechanism to every abnormal condition.

Simulation and hardware-in-the-loop testing provide an important bridge to production deployment. The same robot model, controller configuration, task logic, and planning pipeline should be exercised in simulation before physical operation. Hardware-in-the-loop testing can additionally verify communication timing, interface mappings, controller lifecycle behavior, and fault handling. The purpose is to detect configuration and integration errors before they can cause mechanical damage or production downtime.

Commissioning should proceed from low-risk operations toward full production cycles. Initial tests can verify joint directions, encoder mappings, limits, tool frames, controller interfaces, and emergency-stop behavior. Subsequent tests can introduce low-speed trajectories, tool operation, payloads, collision detection, and process synchronization. Only after these functions are verified should the system operate at production velocity and cycle time.

Industrial performance should be evaluated using both control and production metrics. Position and orientation accuracy, trajectory tracking error, force accuracy, cycle time, repeatability, actuator temperature, communication latency, fault frequency, recovery time, and production throughput provide complementary information. Optimizing only cycle time can hide excessive actuator stress or poor process quality, while optimizing only motion accuracy may produce unnecessarily slow production.

A robust software architecture separates task planning, motion planning, robot control, hardware communication, tool control, safety supervision, diagnostics, and production integration while maintaining well-defined interfaces between them. ROS 2 provides communication and lifecycle mechanisms, MoveIt 2 provides manipulation planning, ros2_control provides standardized hardware and controller integration, and dedicated safety mechanisms protect the physical system. The resulting architecture can support both conventional industrial automation and more advanced force-aware, perception-driven manipulation.

The complete industrial manipulator integration case therefore forms a closed operational loop from production intent to task planning, perception, motion planning, trajectory execution, actuator control, physical interaction, monitoring, fault detection, safe stopping, and recovery. The essential design principle is to keep each layer responsible for its proper function while allowing measured robot state and safety conditions to propagate upward and protective commands to propagate downward. This creates an industrial control system that is reusable, diagnosable, deterministic, and capable of operating safely under changing production conditions.

---

Author: Richard Minsoo Go

LinkedIn: Richard Minsoo Go