Building a 2DOF Robotic Shoulder Joint with Brushless Actuators

Field-Oriented Control (FOC) is a sophisticated motor control technique that transforms three-phase sinusoidal current control into manageable two-axis (D and Q) control using Clark and Park transformations, enabling precise and efficient BLDC motor operation by aligning the control axes with the rotor's magnetic field; the implementation involves configuring PWM with dead time and center-aligned mode, synchronizing ADC current sampling with PWM switching, calibrating the magnetic encoder offset, and tuning PI controllers for stable current regulation.

Field Oriented Control (FOC) is a sophisticated motor control technique that regulates brushless motor torque by precisely controlling the current in each phase, using encoder feedback to keep the electromagnetic force vector aligned with the rotor's position for maximum efficiency; this involves converting measured phase currents into a stationary reference frame, applying PI controllers to regulate torque-producing and non-torque-producing currents, then transforming back to phase voltages and generating PWM signals through a full bridge inverter.

Field-Oriented Control (FOC) enables extremely smooth and precise motor control at low speeds by using two adjacent coils to trap and guide the motor's magnet, unlike traditional methods that cause jittery motion; this technique, implemented with a microcontroller, motor driver, and magnetic encoder, allows applications like smooth volume knobs with simulated detents.

Field Oriented Control (FOC) is an advanced motor control technique that optimizes brushless DC motor performance by maintaining the magnetic field at a 90-degree angle relative to the rotor position, which maximizes torque output and efficiency compared to traditional ESC methods; this is achieved through complex mathematical transformations that convert three-phase sinusoidal signals into space vectors, enabling precise control of motor speed and torque with minimal energy loss.

Dynamic modeling of robots is a mathematical tool for simulating robots and computing actuator torques for desired motion. The model contains parameters (mass, inertia, center of mass locations) that must be identified for specific robots. Kinematic control commands joint velocities directly, masking dynamic interactions between links. In a horizontal plane, kinematic control shows no interaction between links, but torque control reveals dynamic coupling where links move even when not commanded. Dynamic parameters (mass, center of mass location, inertia) determine torque requirements and cannot be determined from kinematics alone. When gravity is present, robot behavior changes dramatically - a robot released from rest will fall, with motion depending on mass distribution and initial conditions. When a robot holds a payload in a fixed configuration, actuators must produce torque to compensate for gravity even with zero velocity. The inverse dynamics problem computes torques needed to produce a desired motion, involving Jacobian inversion and dynamic model application.

The final step extracts the torque required at each joint from the computed force/torque vectors. Since actuators only apply force along the joint axis (typically the z-axis), the joint torque command is the z-component of the computed torque vector. This scalar value represents the motor torque needed to achieve the desired motion while overcoming inertial, gravitational, and external forces. Gravity compensation emerges naturally when gravitational acceleration is incorporated into the dynamics computation, allowing controllers to precompute necessary torques to hold the robot in any posture. This systematic approach enables precise robot control by translating complex multi-body dynamics into actionable joint-level commands.

Gravitational potential energy for a manipulator is PE_grav = g^T Σ[m_i r_ci(q)], where g is the gravity vector (typically [0, -g, 0]^T), m_i is the mass of link i, and r_ci(q) is the position vector from the origin to the center of mass of link i. This term appears in the Lagrangian and generates gravity compensation torques in the dynamic model.

The InTree library provides functions for computing dynamics: forward dynamics to compute joint torques from desired motion, backward dynamics to compute joint torques from desired motion, inertia matrix computation, Coriolis and centrifugal force computation, and gravitational force computation. To simplify dynamic analysis, a virtual acceleration can be applied to the base frame to compensate for gravity, equal and opposite to gravitational acceleration. This automatically accounts for gravitational forces on all links in the inertial terms, simplifying the dynamic equations by eliminating the need to explicitly compute gravitational forces on each link.

Arbitrary feed forward compensates for gravity in mechanisms like elevators and arms. For elevators, the horizontal hold power (joystick value needed to maintain position) is measured and added as a feed forward term. For arms, the arbitrary feed forward is calculated by multiplying the hold power at zero degrees by the cosine of the current angle, providing position-dependent gravity compensation. This prevents mechanisms from drifting or crashing at the end of strokes. The D term also helps smooth out bottoming out behavior by holding back the system slightly.

Impedance control is a control strategy that makes robots compliant (yield to external forces) rather than rigid. This allows robots to safely interact with humans by absorbing impacts and yielding when contact occurs, preventing injury. This is essential for collaborative robots working in shared workspaces.

This lecture introduces control strategies for robot-environment interaction, explaining that traditional position control fails when robots contact environments because it treats external forces as disturbances to reject, leading to steady-state errors and configuration-dependent behavior. The course covers two main approaches: compliance control (allowing the environment to constrain motion) and impedance control (shaping the robot's interaction function). Key concepts include active compliance (controller-imposed) versus passive compliance (mechanical structure), and how the Jacobian matrix affects interaction behavior, with singular configurations causing force absorption without motion.

Impedance control is a robot control strategy that assigns a desired dynamic behavior (mass, damping, and stiffness) to the robot's end-effector, allowing it to interact safely with the environment by behaving like a virtual mass-spring-damper system. Unlike position control (which acts like a configuration-dependent spring) or pure force control (which rejects disturbances), impedance control enables the robot to be compliant with rigid environments and rigid with compliant environments, making it suitable for unstructured environments and human-robot interaction. The control law compensates for the robot's dynamics and environment interaction, with parameters chosen based on the desired interaction behavior: high stiffness and low mass for free motion, and low stiffness and high mass during contact.

Compliance control modifies Cartesian stiffness without force feedback, making the robot behave like a spring at the end-effector. Impedance control extends this by modifying apparent inertia, stiffness, and damping in Cartesian space, requiring force/torque sensing. Anisotropic impedance control allows different stiffness values in different directions, enabling robots to be stiff in some directions while compliant in others. These approaches enable safe human-robot interaction and complex tasks requiring simultaneous force and motion regulation.

Stiffness/compliance control relates displacement from reference positions to force commands, where low stiffness (high compliance) means yielding to forces while high stiffness resists displacement. Impedance control designs the robot to behave like a virtual mass-spring-damper system, matching desired dynamic parameters rather than explicitly controlling forces or positions. This approach mimics human behavior—being compliant enough for uncertain environments while providing sufficient stiffness for deliberate manipulation. Hybrid force-motion control defines task-specific coordinate frames where each Cartesian direction can be independently designated for force or motion control, enabling simultaneous execution of accurate motion and precise force application in dynamic interaction scenarios.

This section covers trajectory optimization and different velocity profile methods. The lecture explains that cubic polynomials have discontinuous jerk at start and end times, which can cause motor vibrations. Alternative methods include trapezoidal velocity profiles (maximum acceleration, constant velocity, maximum deceleration) and S-curve profiles (seven phases with constant jerk for smoothest transitions). The lecture shows how to compute trajectory parameters for each profile and explains that S-curve profiles provide the smoothest possible trajectory by using higher-order derivatives. For complex trajectories, trajectory generation must consider robot dynamics including joint moments and inertias, requiring optimization problems that account for all dynamic constraints.

This section covers planning smooth robot movements. Motion profiling ensures actuators operate within physical limits by gradually accelerating, cruising, and decelerating rather than instantaneously changing setpoints. Trapezoid profiles are widely used in FRC because motor voltage is proportional to acceleration, making them straightforward to implement with feed-forward control. S-curve profiles provide smoother acceleration profiles but are rarely used in FRC due to computational complexity. Splines generate smooth paths between waypoints, with clamped cubic splines determining intermediate headings and quintic splines allowing specified headings at each point. WPILib provides libraries for generating and following these trajectories.

S-curve motion profiles limit acceleration and velocity during position changes to reduce mechanical stress and improve smoothness. The profile consists of acceleration phase, constant velocity phase, and deceleration phase. The set_position_with_limits command allows specifying maximum speed and acceleration parameters for controlled motion.

The S-curve trajectory has five phases with linearly changing acceleration: (1) acceleration increases from 0 to a_max, (2) acceleration decreases from a_max to 0, (3) constant velocity, (4) acceleration decreases from 0 to -a_max, (5) acceleration increases from -a_max to 0. Key formulas: jerk j_max = a_max²/v_max, t1 = a_max/j_max, t2 = 2×t1, s = (a_max×t1²)/2, t3 = t2 + (q_final-q_initial-2s)/v_max. Implementation example: from 0 to 20 degrees with v_max=2 deg/s and a_max=1 deg/s² yields t1=1s, t2=2s, t3=3s, t4=10s, total time=14s. The acceleration profile is piecewise with linear changes in each phase.

S-curve trajectory generation uses a third-order polynomial equation to create smooth robot motion profiles. The general form is θ(t) = c₀ + c₁t + c₂t² + c₃t³. By taking derivatives, we obtain velocity (first derivative) and acceleration (second derivative). The coefficients c₀-c₃ are determined using boundary conditions: initial position, final position, initial velocity (zero), and final velocity (zero). This method ensures continuous acceleration without sudden changes that would cause jerky motion.