Forward kinematics initial commit

fk_poe.md

@@ -0,0 +1,47 @@
+In this tutorial, we will study the forward kinematics of Stretch. Technically, kinematics is the study of motion of objects without references to the forces which cause the motion. For Stretch or any other robot, the objects are the joints and links of the robot. Our intention is to study how these objects move with respect to each other and how changing one or more variables in the kinematic chain affects the other variables. To understand this, we derive the mathematical equations that define the motion of these frames.
+
+The two most important frames in a robot are its base_link and ee_frame (ee short for end-effector). For Stretch, the base_link is defined squarely at the center of the wheels with axes pointing as shown in figure 1. The link_grasp_center (ee_frame) is defined as the center of the distal extremities of Stretch’s fingers as shown in figure 2.
+
+Insert figure 1 with base_link
+
+Insert figure 2 with link_grasp_center
+
+Often, we need to obtain the end effector pose (task space) with respect to the base_link given some joint values (joint space) or vice-versa. The kinematic equations allow us to make these calculations.
+
+Remember:
+The forward kinematics allows us to convert from joint space to task space
+The inverse kinematics allows us to convert from task space to joint space
+
+We are going to use the Product of Exponentials method to derive the forward kinematics of Stretch. The steps are as outlined below:
+
+(All matrix operations are denoted in MATLAB notation for convenience)
+
+Step 1. Assign coordinate frames for all links of interest. Fortunately, the TF tree allows us to visualize the frames that have already been assigned to various links on Stretch. The links of interest are: base_link, link_lift, link_arm_l0, link_wrist_yaw, link_grasp_center (ee_frame). Go ahead and visualize them in RViz.
+
+Step 2. Find the homogeneous transformation matrix for the link_grasp_center frame with respect to the base_link frame in the robot home configuration, when all joints are at their zero value. Let’s call this matrix M.
+
+M = [R, p; 0, 1]
+Where R is the 3x3 rotational matrix defined as,
+R = [0, 1, 0; -1, 0, 0; 0, 0, 1] 
+and p is the position vector of the ee_frame w.r.t. the base_link defined as,
+p = [x; y; z]
+
+Step 3. Calculate the twists for all joints: 
+(Refer to book for an explanation on how the twists were derived)
+S0 = [0, 0, 1, 0, 0, 0]’
+S1 = [0, 0, 0, 0, 0, 1]’
+S2 = [0, 0, 0, 0, -1, 0]’
+S3 = [0, 0, 1, x1, y1, z1]’ 
+where x1, y1, z1 are obtained by computing the cross-product of w3 and -(position of link_wrist_yaw)
+
+Step 4. Use Rodriguez’s formula to obtain the transformation matrices for intermediate links:
+Figure 3
+
+Insert figure 3 to illustrate Rodriguez's formula
+
+Step 5. Use Product of Exponentials formula to compute the forward kinematics
+Figure 4
+
+Insert figure 4 to illustrate PoE formula
+
+

stretch_ros_tutorials/fk_poe.py

@@ -0,0 +1,91 @@
+import rclpy
+import math
+import numpy as np
+from rclpy.node import Node
+from math import sin as s, cos as c
+
+# Users should input joint values from the command line (joint_lift, joint_arm, joint_wrist_yaw)
+# Robot should determine the position of link_grasp_center at home position when all joints are zero?
+
+
+class ForwardKinematics():
+    def __init__(self, joint_lift, joint_arm, joint_wrist_yaw):
+        # Get joint values as node parameters that can be set from CLI
+        self.joint_lift = joint_lift
+        self.joint_arm = joint_arm # joint_arm is the addition of joint_arm_l0 to l3
+        self.joint_wrist_yaw = joint_wrist_yaw
+  
+    # Home configuration of the robot is when all the joint values are zero
+    def get_home_configuration(self):
+        # Rotation of link_grasp_center w.r.t. base_link
+        # Assumed to be ideal for each robot but might have minor calibration errors
+        R = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])
+
+        # Position of link_grasp_center w.r.t. base_link
+        # should be fetched for each robot to account for minor calibration errors
+        p = np.array([[-0.0107], [-0.3445], [0.0205]])
+
+        M = np.block([[R, p], [0, 0, 0, 1]])
+        return M
+
+    def get_screw_axes(self):
+        S1 = np.array([0, 0, 1, 0, 0, 0])
+        S2 = np.array([0, 0, 0, 0, 0, 1])
+        S3 = np.array([0, 0, 0, 0, -1, 0])
+        S4 = np.array([0, 0, -1, 0.1406, -0.0173, 0]) # Must be fetched for each robot
+        
+        S = np.block([[S1], [S2], [S3], [S4]])
+        return np.transpose(S)
+
+    def get_transformation_matrix(self):
+        l1 = self.joint_lift
+        l2 = self.joint_arm
+        t3 = -self.joint_wrist_yaw
+        E1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, l1], [0, 0, 0, 1]])
+        E2 = np.array([[1, 0, 0, 0], [0, 1, 0, -l2], [0, 0, 1, 0], [0, 0, 0, 1]])
+        Y = np.array([[s(t3), 1-c(t3), 0], [c(t3)-1, s(t3), 0], [0, 0, t3]])
+        V = np.array([[0.1406], [-0.0173], [0]])
+        RE3 = np.array([[c(t3), s(t3), 0], [-s(t3), c(t3), 0], [0, 0, 1]])
+        E3 = np.block([[RE3, np.matmul(Y, V)], [0, 0, 0, 1]])
+        E12 = np.matmul(E1, E2)
+
+        return np.matmul(E12, E3)
+
+    def compute_fk(self):
+        M = self.get_home_configuration()
+        E = self.get_transformation_matrix()
+
+        return np.matmul(E, M)
+
+
+if __name__ == '__main__':
+    fk = ForwardKinematics(0.5, 0.2, -1.34)
+    result = fk.compute_fk()
+    print(result)
+
+
+
+# import numpy as np
+# from math import sin as s, cos as c
+
+# R = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]])
+
+# # Position of link_grasp_center w.r.t. base_link
+# # should be fetched for each robot to account for minor calibration errors
+# p = np.array([[-0.0107], [-0.3445], [0.0205]])
+
+# M = np.block([[R, p], [0, 0, 0, 1]])
+
+# t3 = -1.33
+# l1 = 0.5003
+# l2 = 0.2025
+# E1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, l1], [0, 0, 0, 1]])
+# E2 = np.array([[1, 0, 0, 0], [0, 1, 0, -l2], [0, 0, 1, 0], [0, 0, 0, 1]])
+# Y = np.array([[s(t3), 1-c(t3), 0], [c(t3)-1, s(t3), 0], [0, 0, t3]])
+# V = np.array([[-0.1406], [0.0173], [0]])
+# RE3 = np.array([[c(t3), s(t3), 0], [-s(t3), c(t3), 0], [0, 0, 1]])
+# E3 = np.block([[RE3, np.matmul(Y, V)], [0, 0, 0, 1]])
+
+# E12 = np.matmul(E1, E2)
+# E3M = np.matmul(E3, M)
+# print(np.matmul(E12, E3M))