Project - UR5e Robot Trajectory Planning for Triangle Painting

Introduction to Robotics (035001) | Spring Semester 2025

Ido Fang Bentov	Nir Karl	Ofir Rubin
CLASSIFIED	CLASSIFIED	CLASSIFIED
CLASSIFIED	CLASSIFIED	CLASSIFIED

---

---

Problem Description

An industrial 6-DOF Universal Robots UR5e manipulator is deployed in a door manufacturing facility to paint decorative triangular shapes on doors moving on a conveyor belt. The robot holds a paint sprayer, which must maintain a perpendicular orientation to the door surface with the nozzle positioned to away for uniform paint deposition.

The doors arrive vertically on a conveyor belt moving at a constant velocity of without stopping. Each triangle has equal sides of and must be painted within seconds while the door is in motion.

Key Assumptions

1. Each robot link weighs with center of mass at the midpoint between neighboring joints
2. Paint sprayer nozzle remains perpendicular to the door at all times
3. UR5e specifications are used for all kinematic and dynamic constraints

---

Assignment Solutions

Assignment 1: Robot Placement

We positioned the robot base at the origin of the world coordinate system: m. This choice establishes a standard reference frame and simplifies all kinematic calculations.

The door surface is positioned at from the robot base. Given the nozzle distance requirement of to , we selected for optimal paint deposition. Therefore, the nozzle operates at .

The triangle is positioned with its bottom center at (below the robot base level). For an equilateral triangle with sides, the height is:

Figure P1.1: Robot placement schematic showing the coordinate system, robot base at origin, door surface at , and triangle positioned with bottom center at .

Reach Verification:

The maximum reach occurs at the triangle’s top vertex when the door is at maximum X displacement. The top vertex is at:

• (triangle center at , plus maximum door offset)
• (nozzle distance)
• (triangle top vertex)

Maximum reach distance:

This is well within the robot’s maximum reach ().

Justification:

1. The door at allows comfortable nozzle positioning at
2. The triangle’s vertical position ( to ) is within the robot’s optimal workspace
3. All via-points remain within reach despite conveyor motion with safety margin (34.1%)
4. The configuration avoids singularities in the primary working region

Figure P1.2: Triangle trajectory visualization showing the 3D path (left) and XZ projection (right). The path accounts for conveyor motion during painting.

---

---

Assignment 2: Via-Points Trajectory

Triangle Geometry:

For an equilateral triangle with sides positioned with bottom center at in the door frame:

• Vertex 1 (top): m
• Vertex 2 (bottom right): m
• Vertex 3 (bottom left): m

Via-Point Generation Strategy:

We generated via-points distributed evenly along the triangle’s perimeter:

• Approximately 100 points per side
• Path order: V1 → V2 → V3 → V1 (closed loop)
• Sampling interval: as specified

Conveyor Synchronization:

The critical challenge is that the door moves during the 3-second painting time (at ). To keep the end-effector within reach, we offset the door’s initial position such that the painting trajectory is centered around :

At each via-point at time :

This approach keeps the end-effector X-coordinate range within , maintaining excellent reachability.

Position Accuracy:

The total expected position error has three main sources:

1. Via-point discretization: With points along perimeter, spacing is . When the robot uses cubic spline interpolation between discrete via-points on the straight triangle edges, the actual path deviates slightly from the ideal geometry. For a chord on a curve, the maximum deviation is approximately:
   $$
   \delta \approx \frac{s^2}{8R} \approx \frac{(0.004)^2}{8 \times 0.4} \approx 5 \times 10^{-6},\pu{m} \approx \pu{0.005mm}

2. IK convergence tolerance: The inverse kinematics solver converges when position error (balancing accuracy with computational efficiency)

3. Robot repeatability: The UR5e specification is , which represents the mechanical precision limit

Total expected error: Combining these sources, we expect maximum position errors:

This is acceptable for the painting application where the spray pattern width () dominates positioning requirements.

---

---

Assignment 3: Forward Kinematics

We implemented forward kinematics using the Denavit-Hartenberg (DH) convention. For the UR5e, the DH parameters are:

Joint	()	()	()

UR5e robot coordinate frames according to the Denavit-Hartenberg convention, showing the placement of each joint frame and the corresponding link parameters.

The transformation matrix from frame to frame is:

The forward kinematics provides the end-effector position and orientation:

Implementation:

The function forward_kinematics(theta, robot) computes this transformation for any given joint configuration :

function [T, T_all] = forward_kinematics(theta, robot)
	% Initialize transformation matrices
	T_all = cell(1, 6);
	T = eye(4);
	
	% Compute each joint transformation
	for i = 1:6
		Ti = dh_matrix(robot.a(i), robot.alpha(i), robot.d(i), theta(i));
		T = T * Ti;
		T_all{i} = T;
	end
end
 
function T = dh_matrix(a, alpha, d, theta)
	% DH transformation matrix
	T = [cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), a*cos(theta);
		 sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), a*sin(theta);
		 0,		   sin(alpha),			  cos(alpha),			d;
		 0,		   0,					   0,					 1];
end

Symbolic Representation:

The complete transformation from base to end-effector can be expressed symbolically as:

where each follows the DH convention shown above. The resulting end-effector position vector contains trigonometric expressions combining all six joint angles. For the UR5e, the full symbolic form still contains hundreds of terms and is not practical to display in full.

---

---

Assignment 4: Inverse Kinematics

The inverse kinematics problem requires finding joint angles given desired end-effector pose .

Analytical Approach:

For 6-DOF manipulators like the UR5e with a spherical wrist, we can decouple the problem:

1. Position problem (joints 1-3): Determine wrist center position
2. Orientation problem (joints 4-6): Determine wrist angles

The wrist center position is:

For a 6-DOF manipulator, up to 8 analytical solutions may exist due to:

• Joint 1: 2 solutions (elbow left/right)
• Joint 2-3: 2 solutions (elbow up/down)
• Joint 5: 2 solutions (wrist flip)

Solution Selection with Collision Avoidance:

We implement a constraint-based filtering approach with collision avoidance:

1. Joint Angle Limits: Enforce practical joint limits to avoid extreme configurations

   • Joint 2: (flexible shoulder range)
   • Joint 3: (reasonable elbow angle)

2. Collision Constraints: All joint positions must satisfy:

   • Door constraint: ( safety margin)
   • Floor constraint: ( safety margin)
   • Nozzle distance: ( to from door at )

3. Selection: From all collision-free solutions, select the one minimizing:
   $$
   \text{cost} = 1000 \cdot |\mathbf{p}{\text{achieved}} - \mathbf{p}{\text{desired}}|^{2} + 0.01 \cdot |\boldsymbol{\theta} - \boldsymbol{\theta}_{\text{current}}|^{2}

Verification:

For each via-point, we verify the solution by computing forward kinematics and checking position error:

---

---

Implementation:

function [theta_all, theta_best] = inverse_kinematics(T_desired, theta_current, robot, door_y, floor_z)
	% Extract desired position and orientation
	p_desired = T_desired(1:3, 4);
	R_desired = T_desired(1:3, 1:3);
	
	% Joint limits with practical constraints
	lb = -2*pi * ones(6,1);
	ub = 2*pi * ones(6,1);
	lb(2) = -3*pi/4; ub(2) = pi/4;   % Shoulder range
	lb(3) = -pi/6;   ub(3) = 5*pi/6; % Elbow range
	
	% Fix joint 6 for consistent orientation
	theta6_fixed = 0;
	Aeq = [0 0 0 0 0 1];
	beq = theta6_fixed;
	
	% Cost function: prioritize position accuracy, penalize large joint motion
	cost_function = @(theta) 1000*norm(forward_kinematics(theta, robot)(1:3,4) - p_desired)^2 ...
							+ 0.01*norm(theta - theta_current)^2;
	
	% Nonlinear collision constraints
	nonlcon = @(theta) collision_constraints(theta, robot, door_y, floor_z);
	
	% Optimization options
	options = optimoptions('fmincon', 'Display', 'off', 'Algorithm', 'sqp', ...
						  'MaxFunctionEvaluations', 5000);
	
	% Try multiple initial guesses
	initial_guesses = [theta_current, zeros(6,1), [0;-pi/4;pi/2;0;pi/2;0]];
	theta_all = [];
	for k = 1:size(initial_guesses, 2)
		theta0 = initial_guesses(:, k);
		[theta_sol, ~] = fmincon(cost_function, theta0, [], [], Aeq, beq, lb, ub, nonlcon, options);
		
		% Verify solution
		[c, ~] = collision_constraints(theta_sol, robot, door_y, floor_z);
		pos_error = norm(forward_kinematics(theta_sol, robot)(1:3,4) - p_desired);

		if pos_error < 0.0002 && all(c <= 0)  % 0.2mm tolerance
			theta_all = [theta_all, theta_sol];
		end
	end
	
	% Select best solution (smallest joint displacement)
	if ~isempty(theta_all)
		[~, idx] = min(vecnorm(theta_all - theta_current));
		theta_best = theta_all(:, idx);
	else
		theta_best = theta_current; % Fallback
	end
end
 
function [c, ceq] = collision_constraints(theta, robot, door_y, floor_z)
	% Compute all joint positions
	[~, T_all] = forward_kinematics(theta, robot);
	joint_positions = zeros(3, 7);
	joint_positions(:, 1) = [0; 0; 0];
	for i = 1:6
		joint_positions(:, i+1) = T_all{i}(1:3, 4);
	end
	
	safety_margin = 0.03; % 3 cm clearance
	c = [];
	
	% Door constraint: Y_joint < door_y - safety_margin
	for i = 1:7
		c = [c; joint_positions(2, i) - (door_y - safety_margin)];
	end
	
	% Floor constraint: Z_joint > floor_z + safety_margin
	for i = 1:7
		c = [c; (floor_z + safety_margin) - joint_positions(3, i)];
	end
	
	% Lateral limits: -1m < X < 1m
	for i = 2:7
		c = [c; joint_positions(1, i) - 1.0];
		c = [c; -1.0 - joint_positions(1, i)];
	end
	
	% End-effector Y constraint: 0.2m <= Y_ee <= 0.35m
	ee_pos = joint_positions(:, 7);
	c = [c; (door_y - 0.2) - ee_pos(2)];
	c = [c; ee_pos(2) - (door_y - 0.05)];
	
	ceq = [];
end

---

---

Assignment 5: Jacobian Matrix

The Jacobian matrix relates joint velocities to end-effector velocities:

where is linear velocity and is angular velocity.

Geometric Jacobian Formulation:

For each joint , we compute:

For revolute joints:

where:

• is the joint axis direction (third column of )
• is the position of frame
• is the end-effector position

The full Jacobian is:

Implementation:

The function compute_jacobian(theta, robot) returns both the full 6×6 Jacobian and the upper 3×6 linear velocity Jacobian for singularity analysis.

The Jacobian can be computed symbolically using the geometric approach or by taking partial derivatives of the forward kinematics. The linear velocity Jacobian is:

where is the end-effector position. Due to the UR5e’s specific DH parameters, many terms simplify, but the complete symbolic form still contains several hundred terms per matrix element and is not practical for manual display.

---

---

Numerical Implementation:

function [J, J_linear] = compute_jacobian(theta, robot)
	% Compute forward kinematics for all joints
	[~, T_all] = forward_kinematics(theta, robot);
	
	% Initialize Jacobian matrices
	J = zeros(6, 6);
	
	% End-effector position
	p_ee = T_all{6}(1:3, 4);
	
	% For each joint
	for i = 1:6
		if i == 1
			z_prev = [0; 0; 1];  % Base z-axis
			p_prev = [0; 0; 0];   % Base position
		else
			z_prev = T_all{i-1}(1:3, 3);  % Joint axis direction
			p_prev = T_all{i-1}(1:3, 4);   % Joint position
		end
		
		% Linear velocity component: J_v = z × (p_ee - p_joint)
		J(1:3, i) = cross(z_prev, p_ee - p_prev);
		
		% Angular velocity component: J_ω = z
		J(4:6, i) = z_prev;
	end
	
	% Extract linear velocity Jacobian (upper 3 rows)
	J_linear = J(1:3, :);
end

---

---

Assignment 6: Singularities

Singularities occur when , or equivalently when . At singularities, the robot loses one or more degrees of freedom for end-effector motion.

Singularity Analysis Method:

To identify singularities systematically, we analyze the linear velocity Jacobian structure. For the UR5e, each column represents:

A singularity occurs when these six vectors become linearly dependent (only three independent directions remain). We identify three primary cases:

1. Wrist singularity: When , the rotation axes of joints 4 and 6 become coplanar, causing the angular velocity Jacobian columns for these joints to lose independence. The robot cannot generate arbitrary rotations - one rotational degree of freedom is lost.

2. Shoulder singularity: When is parallel to (wrist on base axis), the term , eliminating one degree of freedom for end-effector translation.

3. Elbow singularity: When joints 2 and 3 are collinear ( or ), the arm loses radial mobility as the combined reach of links 2 and 3 cannot be changed - only the position along the extended arm can vary.

Workspace Visualization:

Figure P1.3: 3D workspace visualization showing the reachable positions of the end-effector. The point cloud represents sampled configurations within the robot’s operational range.

Singularity Avoidance in Our Task:

Our trajectory planning successfully avoids all singularities by:

1. Maintaining throughout the motion
2. Keeping the wrist center away from the base Z-axis
3. Operating well within the extended/folded limits

---

---

Assignment 7: Trajectory Planning

7a) Polynomial Selection:

We use cubic spline interpolation with optimized boundary conditions for smooth trajectories. MATLAB’s spline function creates piecewise cubic polynomials with continuity (continuous second derivatives), ensuring smooth acceleration profiles.

For each joint, the spline is defined with zero endpoint velocities:

This creates a smooth trajectory that:

• Passes through all via-points exactly
• Has continuous position, velocity, and acceleration
• Starts and ends with zero velocity (as required)
• Minimizes acceleration discontinuities

Why Cubic Splines:

Cubic splines provide an excellent balance between:

1. Smoothness: continuity ensures no sudden acceleration changes
2. Computational efficiency: Closed-form solution, no optimization required
3. Stability: Well-conditioned numerical properties
4. Flexibility: Natural interpolation through densely-spaced via-points ( intervals)

7b) Time Interval:

As specified, we use between via-points for a total time of , resulting in via-points.

7c) Velocity Constraint Verification:

UR5e maximum joint velocities: for each joint.

Our trajectory maximum velocities achieved (3-second trajectory):

Joint	Max Velocity (deg/s)	Limit (deg/s)	Status
			✓
			✓
			✓
			✓
			✓
			✓

7d) Timing Requirement:

We selected seconds, which satisfies the requirement of seconds. This timing was chosen to maximize efficiency while maintaining velocity constraints

Results and Verification

Constraint Verification Summary

Joint Torques (with payload):

Joint	Max Torque ()

Note on Joint Torques:

• Joint 1 shows : Joint 1 rotates around the vertical Z-axis , while gravity acts downward . The gravitational torque is . Since produces a vector in the XY plane (perpendicular to gravity), and points in Z direction, their dot product is zero. Joint 1 doesn’t fight gravity - it only provides lateral positioning.
• Joint 6 shows : Maintains constant orientation () throughout the trajectory with no payload torque about the wrist axis.
• Joint 2 (shoulder) has the highest torque requirement as it supports the entire arm weight plus payload against gravity with a large moment arm.

---

---

Figure P1.4: Joint position trajectories showing smooth motion profiles for all six joints throughout the 3-second painting cycle.

Figure P1.5: Joint velocity profiles with maximum velocity indicators (red stars) and velocity limits (red dashed lines). All velocities remain well within specifications.

---

---

Figure P1.6: Static joint torques computed for payload, showing maximum torque requirements at each joint.

Figure P1.7: 3D end-effector trajectory showing the triangular paint path in the robot’s workspace, with coordinate frame and door surface representation.

---

---

Figure P1.8: Robot configurations at key time points during the painting trajectory: start (left), midpoint (center), and end (right). The visualization shows the robot’s pose and end-effector position throughout the task.