HW2 << Previous Next >> Topics

Plotter

To determine the drawable workspace of the five-bar linkage mechanism described in the provided Webots controller code and to outline it with a green contour, we need to compute the reachable workspace of the end-effector (point C) based on the kinematic constraints of the mechanism. Then, we will generate the outer and inner boundary contours and instruct the plotter to trace these contours. Below is the step-by-step solution.

Step 1: Understanding the Five-Bar Linkage and Workspace

The five-bar linkage mechanism consists of two fixed points (A and E) and two actuated joints (θ1 and θ2) that control the positions of points B and D, respectively. Point C is the end-effector, determined by the intersection of two circles centered at B (radius L2) and D (radius L3). The mechanism parameters are:

L1 = L4 = 0.1682 m (lengths of the first and fourth links)
L2 = L3 = 0.275 m (lengths of the second and third links)
A = [0.0625, 0.15] m (fixed pivot for θ1)
E = [-0.0625, 0.15] m (fixed pivot for θ2)
offset = 41.9872° (affects the angle calculations)

The workspace is the set of all possible positions of point C, constrained by the joint angles θ1 and θ2 (ranging from -180° to 180°) and the geometric constraints of the linkage. The workspace is typically an annulus-like region (a ring-shaped area) due to the fixed distances from A to B (L1), B to C (L2), E to D (L4), and D to C (L3).

Step 2: Computing the Workspace Boundaries

To find the workspace, we need to determine the outer and inner boundaries of the region where point C can exist. This involves:

Outer boundary: The maximum extent of point C when the linkage is fully extended.
Inner boundary: The minimum extent when the linkage is folded inward, subject to valid kinematic solutions.

The workspace can be approximated by sampling the joint angles θ1 and θ2 over their range and computing the corresponding positions of point C using the forward kinematics function. The boundaries are traced by finding the extreme points (maximum and minimum radial distances from a reference point, e.g., the origin or the midpoint between A and E).

Forward Kinematics Recap

The forward_kinematics(theta1_rad, theta2_rad) function computes point C as follows:

Point B: B = A + L1 * [cos(θ1 + offset), -sin(θ1 + offset)]
Point D: D = E - L4 * [cos(θ2 + offset), sin(θ2 + offset)]
Point C: The intersection of a circle centered at B with radius L2 and a circle centered at D with radius L3. The function selects the intersection with the smaller y-coordinate (elbow-down configuration).

Workspace Boundary Calculation

To trace the workspace boundaries:

Outer boundary: Occurs when the linkage is fully extended, e.g., when B and C are aligned such that the distance from A to C is maximized (approximately L1 + L2 = 0.1682 + 0.275 = 0.4432 m).
Inner boundary: Occurs when the linkage is folded, and the distance from A to C is minimized, subject to valid intersections between the circles at B and D.

We can sample θ1 and θ2 over [-180°, 180°] with a fine grid (e.g., 360 points per angle) to compute all possible C points. The convex hull of these points approximates the outer boundary, while the inner boundary requires identifying configurations where the distance from a reference point (e.g., the origin) is minimized, often when the linkage is folded inward.

To simplify, we assume the workspace is symmetric around the line connecting A and E (y = 0.15 m) due to the symmetry in L1 = L4 and L2 = L3. We compute the workspace by:

Sampling θ1 and θ2 in increments (e.g., 5°).
Computing C for each (θ1, θ2) pair using forward_kinematics.
Collecting valid C points (where intersections exist).
Determining the outer and inner boundaries by finding the maximum and minimum radial distances from a reference point (e.g., the origin [0, 0]) for each angular direction.

Step 3: Generating the Workspace Contours

To generate the outer and inner contours:

Outer contour: For each angle φ around the origin (0, 0), find the C point with the maximum radial distance |C| = sqrt(x² + y²).
Inner contour: For each angle φ, find the C point with the minimum radial distance, ensuring valid kinematic solutions.

The algorithm to compute the contours is as follows:

Define a range of angles φ (0 to 360°, e.g., 360 points).
For each φ, iterate over a grid of θ1 and θ2 values (e.g., 72 points each, 5° increments).
Compute C for each (θ1, θ2) using forward_kinematics.
Calculate the radial distance |C| and the angle of C relative to the origin.
Group C points by their angle φ (within a small tolerance, e.g., ±2.5°).
Select the maximum (outer) and minimum (inner) |C| for each φ bin.
Sort the points by φ to create smooth contours.

Step 4: Plotting the Contours in Webots

The provided code already includes a mechanism to draw a green line in Webots using an IndexedLineSet. We will modify this to draw two separate contours (outer and inner) in green, ensuring they are closed loops if the workspace is continuous.

Step 5: Sweeping the Contours with the Plotter

To make the plotter trace the outer and inner contours:

Use the inverse_kinematics function to compute (θ1, θ2) for each point on the outer and inner contours.
Apply the same interpolation and control logic as in the original code to move the plotter along each contour sequentially.
Ensure smooth transitions between points using linear interpolation in joint space, as done in the original code.

Implementation

Below is the modified code to compute the workspace boundaries, draw the green contours, and control the plotter to trace both contours. This code extends the original by adding workspace computation and contour tracing.

HW2 << Previous Next >> Topics