add main

code/main.py

@@ -0,0 +1,37 @@
+from module.trajectory_generatator import TrajectoryGenerator
+from module.motion_model import next_state
+from module.feedback_control import controller
+from robot import Robot
+import numpy as np
+import matplotlib.pyplot as plt
+import csv
+def main():
+    K_p = 0.1*np.eye(6)
+    K_i = 10*np.zeros(6)
+    robot = Robot(K_p, K_i)
+
+    traj = robot.TrajectoryGenerator()
+    current_state = [0]*13
+    state_hitsory = []
+    state_hitsory.append(current_state)
+    X_error_history = []
+    for i in range(len(traj)-1):
+        X_d = traj[i]
+        V_b, cmd, X_err = robot.controller(X_d=traj[i], X_d_next=traj[i+1], current_state= current_state)
+
+        cmd = cmd[0].tolist()+[traj[i][-1]]
+        current_state = robot.next_state(current_state, cmd, 0.01, 1)
+        X_error_history.append(X_err)
+        state_hitsory.append(current_state)
+    plt.plot(X_error_history)
+    plt.show()
+
+    # Write the reference configurations to a .csv file
+    with open("test1.csv", "w", newline="") as csvfile:
+        writer = csv.writer(csvfile)
+        for state in state_hitsory:
+            writer.writerow(state)
+
+
+if __name__ == '__main__':
+    main()

code/module/feedback_control.py

@@ -3,25 +3,78 @@
 import numpy as np
 import modern_robotics as mr
 import matplotlib.pyplot as plt
-def controller(X_d, X_d_next, X, delta_t, K_p, K_i):
+def rearrange_back(list):
+    """
+    rearrange the 1x12 vector to a matrix T
+    r11, r12, r13, r21, r22, r23, r31, r32, r33, px, py, pz, gripper state
+    """
+    return np.array(
+        [
+            [list[0], list[1], list[2], list[9]],
+            [list[3], list[4], list[5], list[10]],
+            [list[6], list[7], list[8], list[11]],
+            [0, 0, 0, 1],
+        ]
+    )
+
+def state_vector2matrix(state):
+    """
+    state:(odemetry_new, arm_joint_angles_new, wheel_angles_new, gripper_state_new)
+    return matrix T
+    """
+    M0_e = np.array([[1, 0, 0, 0.033],
+                     [0, 1, 0, 0],
+                     [0, 0, 1, 0.6546],
+                     [0, 0, 0, 1]])
+
+    Blist = np.array([[0, 0, 1, 0, 0.033, 0],
+                      [0, -1, 0, -0.5076, 0, 0],
+                      [0, -1, 0, -0.3526, 0, 0],
+                      [0, -1, 0, -0.2176, 0, 0],
+                      [0, 0, 1, 0, 0, 0]]).T
+
+    theta_list = np.array(state[3:8])
+    T_b0 = np.array([[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.0026], [0, 0, 0, 1]])
+    phi, x, y = state[0], state[1],state[2]
+    T_0e = mr.FKinBody(M0_e, Blist, theta_list)
+    T_sb = np.array(
+        [[np.cos(phi), -np.sin(phi), 0, x], [np.sin(phi), np.cos(phi), 0, y], [0, 0, 1, 0.0963], [0, 0, 0, 1]])
+    return  T_sb@T_b0@T_0e
+def controller(X_d, X_d_next, current_state, delta_t, K_p, K_i):
+    """
+    return
+    V_b
+    cmd
+    X_err
+    """
     #robot config
     r = 0.0475
     l = 0.235
     w = 0.15
 
-    T_b0 = np.array([[1, 0, 0, 0.1662],
-                    [0, 1, 0, 0],
-                    [0, 0, 1, 0.0026],
-                    [0, 0, 0, 1]])
-
-    M_0e = np.array([[1, 0, 0, 0.033],
+    M0_e = np.array([[1, 0, 0, 0.033],
                     [0, 1, 0, 0],
                     [0, 0, 1, 0.6546],
                     [0, 0, 0, 1]])
 
+    Blist = np.array([[0, 0, 1, 0, 0.033, 0],
+                      [0, -1, 0, -0.5076, 0, 0],
+                      [0, -1, 0, -0.3526, 0, 0],
+                      [0, -1, 0, -0.2176, 0, 0],
+                      [0, 0, 1, 0, 0, 0]]).T
+
+    theta_list = np.array(current_state[3:8])
     #
+    X_d = state_vector2matrix(X_d)
+    X_d_next = state_vector2matrix(X_d_next)
+
     V_d = 1./delta_t * mr.MatrixLog6(np.linalg.inv(X_d)@X_d_next)
     V_d = mr.se3ToVec(V_d)
+
+    # compute X
+    X = state_vector2matrix(current_state)
+
+    #
     Ad_V_d = mr.Adjoint(np.linalg.inv(X)@X_d)@V_d
     X_err = mr.MatrixLog6(np.linalg.inv(X)@X_d)
     X_err = mr.se3ToVec(X_err)
@@ -30,40 +83,34 @@ def controller(X_d, X_d_next, X, delta_t, K_p, K_i):
     V_b = Ad_V_d + K_p@X_err + delta_t * K_i@X_err
 
     # jacobian
-    Blist = np.array([[0, 0, 1, 0, 0.033, 0],
-                      [0, -1, 0, -0.5076, 0, 0],
-                      [0, -1, 0, -0.3526, 0, 0],
-                      [0, -1, 0, -0.2176, 0, 0],
-                      [0, 0, 1, 0, 0, 0]]).T
-    theta_list = np.array([0,0,0.2,-1.6, 0])
+
     J_arm = mr.JacobianBody(Blist, theta_list)
 
     F = r/4 * np.array([[0,0,0,0],[0,0,0,0],[-1/(l+w),1/(l+w),1/(l+w),-1/(l+w)],[1,1,1,1],[-1,1,-1,1],[0,0,0,0]])
-    T_0e = mr.FKinBody(M_0e, Blist, theta_list)
+    T_b0 = np.array([[1, 0, 0, 0.1662], [0, 1, 0, 0], [0, 0, 1, 0.0026], [0, 0, 0, 1]])
+    T_0e = mr.FKinBody(M0_e, Blist, theta_list)
     J_base = mr.Adjoint(np.linalg.inv(T_0e)@np.linalg.inv(T_b0))@F
     Je = np.hstack((J_base, J_arm))
-    Je_inv = np.linalg.pinv(Je)
 
-    cmd = Je_inv@V_b
-    return V_b, cmd
-def main():
-    X_d = np.array([[0,0,1,0.5],
-                    [0,1,0,0],
-                    [-1,0,0,0.5],
-                    [0,0,0,1]])
-    X_d_next = np.array([[0,0,1,0.6],
-                         [0,1,0,0],
-                         [-1,0,0,0.3],
-                         [0,0,0,1]])
-    X = np.array([[0.170,0,0.985,0.387],
-                  [0,1,0,0],
-                  [-0.985,0,0.170,0.570],
-                  [0,0,0,1]]) # calculated from config
-    delta_t = 0.01
-    K_p = np.zeros((6,6))
-    K_i = np.zeros((6,6))
-    V_b, cmd = controller(X_d, X_d_next, X, delta_t, K_p, K_i)
-    print(V_b, cmd)
+    Je_inv = np.linalg.pinv(Je)
 
-if __name__ == '__main__':
-    main()
+    cmd = Je_inv@V_b,
+    return V_b, cmd, X_err
+# def main():
+#     X_d = np.array([[0,0,1,0.5],
+#                     [0,1,0,0],
+#                     [-1,0,0,0.5],
+#                     [0,0,0,1]])
+#     X_d_next = np.array([[0,0,1,0.6],
+#                          [0,1,0,0],
+#                          [-1,0,0,0.3],
+#                          [0,0,0,1]])
+#     current_state = [0,0,0,0,0,0.2,-1.6,0, ] + [0,0,0,0,0]
+#     delta_t = 0.01
+#     K_p = np.zeros((6,6))
+#     K_i = np.zeros((6,6))
+#     V_b, cmd , X_err= controller(X_d, X_d_next, current_state, delta_t, K_p, K_i)
+#     print(V_b, cmd)
+#
+# if __name__ == '__main__':
+#     main()

code/module/motion_model.py

@@ -125,6 +125,7 @@ def main():
         state_trajectory.append(new_state)
         # print(new_state)
         current_state = new_state
+
 
     print(state_trajectory[0:10])
     #writing csv files in Python