Robotics Course: Kinematics & Dynamics (Theory to Simulation in ROS 2 Python with UR Robot)
Summary
This comprehensive robotics course covers essential concepts from position and orientation to manipulator dynamics, utilizing ROS 2 and Python with a UR robot. It progresses through rotation matrices, Euler angles, axis-angle representation, quaternions, transformation matrices, DH parameters, forward and inverse kinematics, Jacobian matrices, singularity analysis, workspace definition, and finally, manipulator dynamics using both Newton-Euler and Lagrangian methods. The course emphasizes both theoretical understanding and practical simulation.
Key Insights
Rotation matrices describe frame orientation and rotate vectors.
A rotation matrix describes the orientation of one frame relative to another and can also be used to rotate a vector by multiplying the matrix with the vector.
Euler angles define orientation through three consecutive rotations about specific axes.
Euler angles represent any orientation by performing three consecutive rotations, such as ZYX or XYZ. The order of these rotations is crucial.
Axis-angle representation describes rotation by an angle theta about a unit vector K.
This method generalizes rotation by allowing it to occur about any arbitrary vector K, not just the fixed X, Y, or Z axes, defined by an angle theta.
Quaternions are four-dimensional numbers used for representing orientation.
A quaternion is represented as q = w + xi + yj + zk, where w is the scalar part and (x, y, z) is the vector part. They avoid gimbal lock and are efficient for interpolation.
Transformation matrices combine rotation and translation to describe frame changes.
A 4x4 transformation matrix encapsulates both the orientation (rotation matrix R) and position (translation vector P) of one frame relative to another. It includes padding with zeros and a one for mathematical convenience.
DH parameters define the kinematics of serial-link robots.
Denavit-Hartenberg (DH) parameters provide a systematic way to describe the geometry of each link and the relationship between successive joints in a serial manipulator.
Forward kinematics calculates the tool end's position and orientation from joint angles.
Given the values of all joint angles (Q), forward kinematics determines the transformation matrix (T0N) from the base frame to the end-effector frame.
The Jacobian matrix maps joint velocities to tool end linear and angular velocities.
It's a generalized velocity transformation, typically a 6xN matrix (where N is the number of DOFs), relating joint velocities (Q-dot) to the Cartesian velocities (X-dot) of the end-effector.
Joint space trajectory plans motion by interpolating joint angles.
This involves defining a smooth path between an initial and final joint configuration, typically using cubic polynomials to ensure zero initial and final velocities.
Cartesian space trajectory interpolates the tool end's pose (position and orientation).
This requires interpolating both the position (linearly) and orientation (using SLERP for quaternions) between start and end poses, ensuring zero velocity and acceleration at endpoints.
Singularity occurs when a robot loses one or more degrees of freedom.
Mathematically, this happens when the Jacobian matrix becomes rank-deficient (columns are not linearly independent), meaning certain Cartesian motions are impossible or equivalent.
Newton-Euler dynamics calculates the joint torques required for robot motion.
This method determines the torques needed to achieve specific accelerations by analyzing forces and moments acting on each link, considering gravity, inertia, Coriolis, and centrifugal effects.
Lagrangian dynamics uses energy principles (kinetic and potential) to derive robot equations of motion.
It defines the Lagrangian (L = Kinetic Energy - Potential Energy) and uses Euler-Lagrange equations to find the dynamics, focusing on energy states rather than direct force/moment calculations.
Sections
Introduction to Robot Kinematics Course
Course focuses on both math theory and physical understanding for robotics.
The course emphasizes not only the mathematical theory behind robotics but also the physical understanding and simulation aspects, using the UR robot with ROS 2 and Python.
Course structure covers five key parts of robotics.
The course is divided into five parts: 1. Position and Orientation (rotation matrix, Euler angles, axis-angle, quaternions, transformation matrix). 2. Kinematics (DH parameters, forward and inverse kinematics). 3. Numerical Methods (Jacobian, numerical Jacobian, numerical inverse kinematics). 4. Trajectory Planning (joint space, Cartesian space, singularities, workspace analysis). 5. Manipulator Dynamics (Newton-Euler, Lagrangian).
Part 1: Position and Orientation - Rotation Matrices
Rotation matrices describe frame orientation and rotate vectors.
A rotation matrix describes the orientation of one frame relative to another and can also be used to rotate a vector by multiplying the matrix with the vector.
Three interpretations of rotation matrices exist.
The three interpretations of a rotation matrix are: 1. Describing a rotated frame relative to an original frame. 2. Rotating a vector by a certain angle. 3. Expressing a vector in another, rotated frame.
3D rotation matrices extend 2D by including a third dimension.
The 3D rotation matrix incorporates a Z-component, typically set to 1 when rotating around the Z-axis if the Z-axes of both frames are aligned. Specific matrices exist for rotations around X, Y, and Z axes.
Rotation matrix operations include multiplication, inverse, and transpose.
Operations like matrix multiplication are used for sequential rotations, where order matters (RX * RY != RY * RX). The inverse of a rotation matrix is its transpose, simplifying calculations.
Visualizing transforms with UR robot in Arvis confirms rotation matrix calculations.
Using Arvis to visualize the UR robot's transforms and comparing the axes' directions with calculated rotation matrix components validates the mathematical concepts. For example, observing the tool end's X-axis relative to the base confirms the matrix entries.
Part 1: Position and Orientation - Euler Angles
Euler angles define orientation through three consecutive rotations about specific axes.
Euler angles represent any orientation by performing three consecutive rotations, such as ZYX or XYZ. The order of these rotations is crucial.
Proper Euler angles and Tait-Bryan angles are common types.
Proper Euler angles involve the first and third rotations using the same axis, while Tait-Bryan angles use three distinct axes (e.g., roll, pitch, yaw).
Calculating Euler angles from a rotation matrix involves trigonometric functions and comparing matrix elements.
Specific trigonometric formulas using `atan2` are used to extract Euler angles (e.g., theta, phi, psi) from the components of a known rotation matrix.
Gimbal lock occurs when one degree of freedom is lost due to joint alignment.
Gimbal lock happens when the second rotation axis aligns with either the first or third, causing two rotational axes to behave identically and reducing the system's controllable degrees of freedom from three to two.
Example demonstrates calculating Euler angles from a UR robot's base-to-tool transform.
The process of calculating Euler angles from the UR robot's transformation matrix is shown, confirming the derived angles correspond to the physical orientation of the end-effector relative to the base.
Part 1: Position and Orientation - Axis Angle Rotation
Axis-angle representation describes rotation by an angle theta about a unit vector K.
This method generalizes rotation by allowing it to occur about any arbitrary vector K, not just the fixed X, Y, or Z axes, defined by an angle theta.
Axis-angle can be converted to a rotation matrix using Rodrigues' rotation formula.
The conversion involves the identity matrix, the angle theta, the unit vector K, and the cross-product matrix of K, combined as R = I + sin(theta) * [K]x + (1 - cos(theta)) * [K]x^2.
A rotation matrix can be converted back to axis-angle using its trace and off-diagonal elements.
The angle theta is found using arccosine of (trace(R) - 1) / 2, and the axis K is derived from the skew-symmetric parts of the rotation matrix (R - R^T) / (2 * sin(theta)).
Example shows converting a 90-degree Z-axis rotation to axis-angle and back.
A concrete example demonstrates taking a 90-degree rotation about the Z-axis, converting it to its rotation matrix, then deriving the axis-angle (theta=90, K=[0,0,1]), and finally reconstructing the rotation matrix from the axis-angle.
Axis-angle representation of UR robot's tool frame matches visual interpretation.
The calculated axis-angle representation for the UR robot's tool frame relative to the base aligns with the visual feedback from Arvis, confirming the rotation vector and angle.
Part 1: Position and Orientation - Quaternions
Quaternions are four-dimensional numbers used for representing orientation.
A quaternion is represented as q = w + xi + yj + zk, where w is the scalar part and (x, y, z) is the vector part. They avoid gimbal lock and are efficient for interpolation.
Quaternions can be derived from axis-angle rotation.
A quaternion representing a rotation by angle theta about unit axis K = (kx, ky, kz) is given by w = cos(theta/2), x = kx*sin(theta/2), y = ky*sin(theta/2), z = kz*sin(theta/2).
Quaternions have a 'double cover' property: q and -q represent the same rotation.
This property means that two different quaternions can describe the identical physical orientation, which is important to consider in calculations.
Vectors can be rotated using quaternions via the formula p' = q * p * q^-1.
To rotate a vector v, it's represented as a pure quaternion p = (0, v). The rotation is performed by multiplying q, p, and the conjugate of q (which is the inverse for unit quaternions).
Quaternions can be converted to and from rotation matrices.
Formulas exist to convert between rotation matrices and quaternions, allowing flexibility in representation. Converting a quaternion to a rotation matrix involves plugging its components into a specific 3x3 matrix formula.
Spherical Linear Interpolation (SLERP) provides smooth orientation changes using quaternions.
SLERP interpolates between two quaternions over a parameter t (0 to 1), ensuring a smooth, non-discontinuous path in orientation, unlike Euler angles which can suffer from discontinuities.
UR robot's quaternion output is verified against its rotation matrix.
The quaternion output from the UR robot's kinematics node is compared with its corresponding rotation matrix, demonstrating how to verify the numerical results by plugging them back into the conversion formulas.
Part 1: Position and Orientation - Transformation Matrices
Transformation matrices combine rotation and translation to describe frame changes.
A 4x4 transformation matrix encapsulates both the orientation (rotation matrix R) and position (translation vector P) of one frame relative to another. It includes padding with zeros and a one for mathematical convenience.
Multiplying transformation matrices allows chaining frame transformations.
To find the transformation from frame 0 to frame 2 (T02), you multiply the transformation from 0 to 1 (T01) by the transformation from 1 to 2 (T12), i.e., T02 = T01 * T12.
The inverse of a transformation matrix represents the transformation in the reverse direction.
The inverse transformation T_ba is found by taking the transpose of the rotation part and applying the negative transformed translation vector to the original vector: T_ba = [R_ab^T | -R_ab^T * P_ab].
Translation-only and rotation-only matrices are special cases of transformation matrices.
A translation-only matrix has an identity matrix for rotation and the translation vector. A rotation-only matrix has the rotation matrix and a zero translation vector.
UR robot's tool end transformation matrix is derived and visualized.
The code calculates the 4x4 transformation matrix for the UR robot's tool end relative to its base. This matrix combines the rotation and translation components, which are then visualized and interpreted.
Part 2: Kinematics - DH Parameters
DH parameters define the kinematics of serial-link robots.
Denavit-Hartenberg (DH) parameters provide a systematic way to describe the geometry of each link and the relationship between successive joints in a serial manipulator.
Standard and Modified DH conventions exist, differing in frame assignment and indexing.
The main difference lies in how coordinate frames are attached to links (e.g., frame i+1 for standard vs. frame i for modified) and the indexing convention for parameters.
Four DH parameters define the transformation between adjacent joint frames: link length, link twist, link offset, and joint angle.
These parameters (a, alpha, d, theta) describe the relative position and orientation between frame i and frame i+1, specifically: distance along x, angle about x, distance along z, and angle about z.
The DH parameter table simplifies defining the robot's kinematic structure.
A table lists the DH parameters for each link, allowing for straightforward calculation of the transformation matrix between any two frames on the robot.
DH parameters for the UR robot are extracted and verified using ROS.
A ROS node calculates the modified DH parameters for the UR robot by analyzing its URDF. The parameters are then verified against the robot's physical configuration by examining the frame transformations at zero joint values.
Part 2: Kinematics - Forward Kinematics
Forward kinematics calculates the tool end's position and orientation from joint angles.
Given the values of all joint angles (Q), forward kinematics determines the transformation matrix (T0N) from the base frame to the end-effector frame.
The final transformation is the product of individual link transformation matrices.
The overall transformation matrix T0N is obtained by multiplying the sequence of transformation matrices derived from DH parameters: T01 * T12 * ... * T(N-1)N.
Forward kinematics provides both rotational and translational information.
The resulting transformation matrix contains a 3x3 rotation matrix (orientation) and a 3x1 position vector (location of the tool end relative to the base).
A 2-DOF robot example illustrates calculating forward kinematics.
For a simple 2-DOF robot, the DH parameters are used to find T01, T12, and T23. Multiplying these gives the final T03, which describes the tool end's pose. Examples with zero and 90-degree angles are shown.
UR robot's forward kinematics is demonstrated using its transformation matrix node.
The previously developed transformation matrix node effectively represents the forward kinematics for the UR robot, providing the tool end's pose based on the current joint configurations.
Part 3: Numerical Methods - Jacobian Matrix
The Jacobian matrix maps joint velocities to tool end linear and angular velocities.
It's a generalized velocity transformation, typically a 6xN matrix (where N is the number of DOFs), relating joint velocities (Q-dot) to the Cartesian velocities (X-dot) of the end-effector.
The Jacobian can be derived geometrically using joint axes and position vectors.
For revolute joints, the geometric Jacobian column is Zi x (P - Pi), where Zi is the joint axis, P is the tool tip position, and Pi is the joint i position. For prismatic joints, it's just Zi.
The analytical Jacobian is derived by differentiating the forward kinematics equations.
This requires having the forward kinematics expressed in a differentiable form (e.g., XYZ position with Euler angles, quaternions, or rotation matrices) and then taking partial derivatives.
The geometric Jacobian for a planar 2-DOF robot shows angular velocity only in the Z-axis.
For planar robots, the angular velocity components of the Jacobian are entirely along the Z-axis, reflecting the planar nature of the motion.
UR robot's Jacobian analysis reveals joint contributions to linear and angular velocity.
Visualizing the UR robot's Jacobian shows how each joint contributes to the tool end's linear and angular velocity. Notably, the last joint's rotation doesn't affect linear velocity, and base joints have larger magnitudes for linear velocity contributions.
Part 3: Numerical Methods - Numerical Jacobian
Numerical Jacobian approximates derivatives using finite differences.
Instead of analytical differentiation, it calculates the Jacobian by perturbing each joint angle slightly and observing the resulting change in the forward kinematics output (position, orientation).
Methods include forward, backward, and central finite differences.
Central difference is generally preferred for accuracy as it uses perturbations on both sides of the current state (fk(q+eps) - fk(q-eps)) / (2*eps).
The choice of rotation representation (Euler, Quaternion) affects numerical Jacobian calculation.
While linear position components are straightforward, the orientation part requires careful handling depending on whether Euler angles, quaternions, or rotation matrices are used.
Numerical Jacobian for UR robot matches analytical results.
Running a ROS node to compute the numerical Jacobian for the UR robot and comparing its output to the analytical Jacobian confirms the accuracy of the approximation method.
Part 4: Trajectory Planning
Joint space trajectory plans motion by interpolating joint angles.
This involves defining a smooth path between an initial and final joint configuration, typically using cubic polynomials to ensure zero initial and final velocities.
Cubic polynomial trajectories ensure smooth transitions with zero start/end velocity.
A cubic polynomial has four coefficients (a0, a1, a2, a3) that can be solved using initial/final position and velocity constraints, guaranteeing zero velocity at the start and end points.
Quintic polynomials allow for control over acceleration, in addition to position and velocity.
A fifth-order polynomial has six coefficients, enabling the specification of zero initial and final acceleration, leading to even smoother motion.
Cartesian space trajectory interpolates the tool end's pose (position and orientation).
This requires interpolating both the position (linearly) and orientation (using SLERP for quaternions) between start and end poses, ensuring zero velocity and acceleration at endpoints.
Smoothness in Cartesian space requires interpolating position and orientation separately.
Linear interpolation (LERP) handles position, while Spherical Linear Interpolation (SLERP) handles orientation, ensuring smooth transitions without sudden jumps in velocity or acceleration.
ROS code demonstrates generating smooth joint space trajectories for the UR robot.
A Python script in ROS allows setting a target joint configuration, reads the initial state, and generates a smooth cubic polynomial trajectory for each joint, visualized in plot juggler.
Cartesian space trajectory generation for UR robot ensures smooth tool end motion.
By interpolating between two poses using LERP for position and SLERP for orientation, and then solving inverse kinematics for each intermediate step, a smooth Cartesian trajectory is achieved.
Part 4: Trajectory Planning - Singularity
Singularity occurs when a robot loses one or more degrees of freedom.
Mathematically, this happens when the Jacobian matrix becomes rank-deficient (columns are not linearly independent), meaning certain Cartesian motions are impossible or equivalent.
For a 2-DOF planar robot, full extension and full fold are singular configurations.
At these configurations, the determinant of the Jacobian is zero, indicating that the robot cannot move in certain directions or that multiple joint movements result in the same end-effector motion.
Singularity analysis involves checking the Jacobian's determinant or linear independence.
A zero determinant or linearly dependent columns in the Jacobian signify a singular state. Visually, this often corresponds to all joints of a chain aligning in the same plane.
UR robot's singularity analysis confirms linear dependence in its Jacobian.
Checking the UR robot's Jacobian reveals linearly dependent columns, particularly involving joints 2, 3, and 4, indicating singular configurations exist within its workspace.
Part 4: Trajectory Planning - Workspace
Workspace defines the set of reachable points for the robot's end-effector.
It's determined by the robot's physical constraints: number/type of joints, link lengths, and joint limits. The workspace can be visualized as a region in space.
Reachable workspace requires at least one orientation at a point, dextrous workspace requires all orientations.
The reachable workspace is the set of points the end-effector can reach, regardless of orientation. The dextrous workspace is the subset where the end-effector can achieve *any* orientation.
Monte Carlo sampling is an efficient method to estimate the workspace.
Randomly sampling joint angles within their limits and calculating the resulting end-effector positions provides a good approximation of the reachable workspace.
UR robot's workspace is visualized as a sphere (or half-sphere for clarity).
Using ROS markers, the workspace of the UR robot is plotted, showing a spherical shape that aligns with expectations for a 6-DOF robot capable of reaching points in 3D space.
Part 5: Manipulator Dynamics - Newton-Euler Method
Newton-Euler dynamics calculates the joint torques required for robot motion.
This method determines the torques needed to achieve specific accelerations by analyzing forces and moments acting on each link, considering gravity, inertia, Coriolis, and centrifugal effects.
The method involves an 'outward' pass (0 to N-1) followed by an 'inward' pass (N to 1).
The outward pass calculates velocities and accelerations from base to tip, while the inward pass calculates forces and torques from tip to base, ultimately yielding the joint torques.
Torque equation includes inertia (M), Coriolis/centrifugal (C), and gravity (G) matrices.
The general form is Tau = M(Q)*Q'' + C(Q, Q')*Q' + G(Q). Each component matrix/vector corresponds to different dynamic effects.
Initial conditions (velocities, accelerations, gravity) are required starting points.
Assumptions are made for base frame gravity, angular velocities/accelerations (often zero), and linear accelerations, as well as forces/torques at the tool tip.
UR robot simulation visualizes torques during motion.
By running the Newton-Euler dynamics script in ROS, the required torques for joints are plotted in real-time as the UR robot performs movements like drawing a circle, showing J2 experiencing the most variation.
Part 5: Manipulator Dynamics - Lagrangian Dynamics
Lagrangian dynamics uses energy principles (kinetic and potential) to derive robot equations of motion.
It defines the Lagrangian (L = Kinetic Energy - Potential Energy) and uses Euler-Lagrange equations to find the dynamics, focusing on energy states rather than direct force/moment calculations.
Kinetic energy includes linear and rotational components, often expressed using the mass matrix.
Kinetic energy (KE) is calculated as 1/2 * sum(m_i * v_i^2) + 1/2 * sum(omega_i^T * I_i * omega_i) for all links, which simplifies to 1/2 * Q_dot^T * M * Q_dot.
Potential energy is calculated as mgh, considering the height of each link's center of mass.
Potential energy (PE) for link i is m_i * g * h_i, where h_i is the vertical component of the center of mass position vector.
Torques are derived by taking partial derivatives of the Lagrangian.
The torque equation is found using Tau_j = d/dt (dL / d(Q_dot_j)) - dL / d(Q_j), which ultimately results in the same M, C, and G matrices as the Newton-Euler method.
Lagrangian dynamics for UR robot simulation confirms torque calculations.
Running the Lagrangian dynamics script in ROS generates plots of joint torques during motion, which should align with results from the Newton-Euler method, providing a cross-validation of the dynamic models.
Ask a Question
*Uses 1 Wisdom coin from your coin balance
