Master Forward Kinematics: Spider Robot Series

Welcome, tech enthusiasts and future roboticists! You're tuning in to our comprehensive tutorial series on building a spider robot. Whether you're just starting out or leveling up your skills, this guide is here to make your journey an enlightening one.Today, we're focusing on a crucial aspect of robotic movement in our spider robot - the forward kinematics.

Supplies

Python programming environment (that allows you to run Jupyter Notebooks)
Understanding of:
High school trigonometry (projectiona, sine, cosine, tangent, and their inverse functions)
Coordinate systems (Cartesian and polar)
Vectors and vector operations
Basic programming concepts (variables, functions, loops, and conditional statements)

Step 1: Brief Introduction to Trigonometry

Before we dive into the code, let's briefly touch upon some basic trigonometry concepts that are used in this method:

- **Sine (sin)**: In a right-angled triangle, the sine of an angle is the length of the side opposite to that angle divided by the length of the hypotenuse (the longest side, opposite the right angle).

- **Cosine (cos)**: Similarly, the cosine of an angle in a right-angled triangle is the length of the side adjacent to the angle divided by the length of the hypotenuse.

- **Tangent (tan)**: The tangent of an angle is defined as the ratio of the sine to the cosine of that angle, or equivalently, the ratio of the side opposite the angle to the side adjacent to the angle in a right-angled triangle.

- **Pythagorean theorem**: In any right-angled triangle, the square of the length of the hypotenuse (the side opposite the right angle) is equal to the sum of the squares of the lengths of the other two sides. This can be written as where (c) is the length of the hypotenuse, and (a) and (b) are the lengths of the other two sides.

These concepts allow us to calculate the lengths of sides in a right triangle when an angle is known, or to find an angle when the lengths of the sides are known. They form the basis for many geometric calculations and transformations, including those used in our forward kinematics.

Let's get started!

Step 2: The `forward_kinematics` Method: a Deep Dive

In this tutorial, we'll dissect the `forward_kinematics_naive` method from the `SpiderLeg` class in our spider robot project. This method is pivotal for calculating the positions of the leg's joints based on the joint angles. Here's a detailed breakdown of each step, complete with trigonometry explanations and inline code snippets.

1. **Angle Conversion**:

The method begins by converting the joint angles from degrees to radians. This conversion is essential as trigonometric functions in Python's math library require angles in radians.

theta1, theta2, theta3 = map(math.radians, self.get_angles())

Step 3: Coxa-Femur Joint Calculation

- `Xa` and `Ya` determine the x and y coordinates of the coxa-femur joint (the hip joint). They are calculated using the cosine and sine of `theta1`, respectively, multiplied by the length of the coxa (`self.COXA`). Cosine gives the horizontal component, and sine provides the vertical component.

Xa = self.COXA * math.cos(theta1)
Ya = self.COXA * math.sin(theta1)

Step 4: Femur Segment Calculations

- The vertical component `G2` of the femur length is calculated by the femur length (`self.FEMUR`) multiplied by the sine of `theta2`. This gives the vertical displacement.

- The horizontal component `P1` is found by multiplying the femur length by the cosine of `theta2`. This gives the horizontal displacement.

G2 = math.sin(theta2) * self.FEMUR
P1 = math.cos(theta2) * self.FEMUR

Step 5: Femur-Tibia Joint Calculation

- `Xc` and `Yc` are the x and y coordinates of the femur-tibia joint (knee joint), computed by multiplying `P1` with the cosine and sine of `theta1`.

Xc = math.cos(theta1) * P1
Yc = math.sin(theta1) * P1

Step 6: Tibia-Tip Joint Calculation

- This step involves calculating the position of the tibia-tip joint (foot of the leg) using a series of trigonometric calculations.

- `H` is the straight-line distance from the femur-tibia joint to the tibia-tip joint, calculated using the Law of Cosines.


H = math.sqrt(self.TIBIA**2 + self.FEMUR**2 - 2*self.TIBIA*self.FEMUR*math.cos(math.radians(180) - theta3))

- `phi1` is calculated using the Law of Cosines again, to find the angle at the femur-tibia joint.


phi1 = math.acos((self.FEMUR**2 + H**2 - self.TIBIA**2) / (2*self.FEMUR*H))

- `phi2` is the angle at the tibia-tip joint. It's calculated by subtracting `phi1` and the external angle from 180 degrees.

- `phi3` is an auxiliary angle, used to find the horizontal and vertical projections. It's calculated by subtracting `theta2` from `phi1`.


phi2 = math.radians(180) - (math.radians(180) - theta3) - phi1
phi3 = phi1 - theta2

- `Pp` is the horizontal projection of the tibia-tip joint on the plane of the femur. It is calculated using the cosine of `phi3` multiplied by `H`.


Pp = math.cos(phi3) * H

- `P2` is the horizontal distance between the femur-tibia joint and the tibia-tip joint. It is calculated as the difference between `Pp` and `P1`.


P2 = Pp - P1

- `Yb` and `Xb` are the x and y coordinates of the tibia-tip joint, calculated by multiplying the sine and cosine of `theta1` by `Pp`, respectively.

Yb = math.sin(theta1) * Pp
Xb = math.cos(theta1) * Pp

- `G1` is the vertical component (z-coordinate) of the tibia-tip joint. It is calculated using the sine of `phi3` multiplied by `H`. The result is negated as the z-axis might be defined downwards in the robot's coordinate system.

G1 = math.sin(phi3) * H
G1 = -G1

Step 7: Joint Locations List

- The method concludes by creating a list of joint locations. Each joint location is represented by a set of x, y, and z coordinates. This list includes the positions of the base joint, coxa-femur joint, femur-tibia joint, and the tip of the leg.


joint_locations = [
[0, 0, 0], # Base joint
[Xa, Ya, 0], # Coxa-femur joint
[Xa + Xc, Ya + Yc, G2], # Femur-tibia joint
[Xa + Xb, Ya + Yb, G1] # Tip of the leg
]

Remember, the beauty of robotics lies not just in the final product, but in the intricate processes that bring the robot to life. Happy coding!

Step 8: Final Code

Python Jupyter Notebook Code:

# Import necessary libraries
import math
import matplotlib.pyplot as plt
%matplotlib ipympl
# Define the SpiderLeg class
class SpiderLeg:
    # Initialize the class with lengths of leg segments
    def __init__(self, COXA, FEMUR, TIBIA):
        self.COXA = COXA
        self.FEMUR = FEMUR
        self.TIBIA = TIBIA
        self.theta1 = 0.
        self.theta2 = 0.
        self.theta3 = 0.


    # Method to set joint angles
    def set_angles(self, angles):
        self.theta1, self.theta2, self.theta3 = angles


    # Method to get current joint angles
    def get_angles(self):
        return [self.theta1, self.theta2, self.theta3]


    # Method to perform forward kinematics
    def forward_kinematics_naive(self):
        # Convert joint angles from degrees to radians
        theta1, theta2, theta3 = map(math.radians, self.get_angles())

        # Calculate x and y coordinates of coxa-femur joint
        Xa = self.COXA * math.cos(theta1)
        Ya = self.COXA * math.sin(theta1)

        # Calculate vertical component of femur length
        G2 = math.sin(theta2) * self.FEMUR

        # Calculate horizontal component of femur length
        P1 = math.cos(theta2) * self.FEMUR

        # Calculate x and y coordinates of femur-tibia joint
        Xc = math.cos(theta1) * P1
        Yc = math.sin(theta1) * P1

        # Calculate H, phi1, phi2, phi3, Pp, P2, Yb, Xb, G1
        # to get the coordinates of the tibia-tip joint
        H = math.sqrt(self.TIBIA**2 + self.FEMUR**2 - 2*self.TIBIA*self.FEMUR*math.cos(math.radians(180) - theta3))
        phi1 = math.acos((self.FEMUR**2 + H**2 - self.TIBIA**2) / (2*self.FEMUR*H))
        #phi2 = math.radians(180) - (math.radians(180) - theta3) - phi1
        phi3 = phi1 - theta2
        Pp = math.cos(phi3) * H
        P2 = Pp - P1
        Yb = math.sin(theta1) * Pp
        Xb = math.cos(theta1) * Pp
        G1 = math.sin(phi3) * H
        G1 = -G1


        # Create a list of joint locations
        joint_locations = [
            [0, 0, 0],  # start joint
            [Xa, Ya, 0],  # coxa-femur joint
            [Xa + Xc, Ya + Yc, G2],  # femur-tibia joint
            [Xa + Xb, Ya + Yb, G1]  # tip of the leg
        ]
        return joint_locations


    # Method to plot the leg and joint positions
    def plot(self, ax):
        joint_locations = self.forward_kinematics_naive()
        for idx, point in enumerate(joint_locations):
            ax.scatter(point[0], point[1], point[2])
            if idx > 0:
                ax.plot([joint_locations[idx - 1][0], point[0]],
                        [joint_locations[idx - 1][1], point[1]],
                        [joint_locations[idx - 1][2], point[2]], color='blue')
                ax.text(joint_locations[idx - 1][0], joint_locations[idx - 1][1], joint_locations[idx - 1][2], f"θ{idx}={self.get_angles()[idx-1]}°")
                if(idx==3):
                  ax.text(point[0], point[1], point[2], f"({round(point[0],1)}, {round(point[1],1)}, {round(point[2],1)})")


# Function to create and plot a SpiderLeg
def create_and_plot_leg(ax, angles, title):
    leg = SpiderLeg(1, 1, 1)
    leg.set_angles(angles)
    leg.plot(ax)
    ax.set_title(title)
    ax.set_xlabel('X')
    ax.set_ylabel('Y')
    ax.set_zlabel('Z')


# Create a figure with three subplots and plot the joint positions of the legs
fig = plt.figure(figsize=(10, 4))


ax1 = fig.add_subplot(131, projection='3d')
create_and_plot_leg(ax1, [120, 60, 30], "Leg 1 Configuration")


ax2 = fig.add_subplot(132, projection='3d')
create_and_plot_leg(ax2, [60, 30, 60], "Leg 2 Configuration")


ax3 = fig.add_subplot(133, projection='3d')
create_and_plot_leg(ax3, [0, 30, 90], "Leg 3 Configuration")


# Add a description to the plot
plt.suptitle("Forward kinematics of a spider robot leg in different configurations")


plt.show()