Tried implementing LFC

Author: mmcza

.idea/.gitignore

@@ -12,4 +12,12 @@ def calculate_control(self, x, q_r, q_r_dot, q_r_ddot):
         Please implement the feedback linearization using self.model (which you have to implement also),
         robot state x and desired control v.
         """
-        return NotImplementedError()
+        inverted_M = np.linalg.inv(self.model.M(x))
+        v = -self.model.C(x) @ inverted_M @ x[2:] + np.eye(2) @ inverted_M @ np.ones((2, 1))
+
+        # zeros = np.zeros((2, 2), dtype=np.float32)
+        # A = np.concatenate([np.concatenate([zeros, np.eye(2)], 1), np.concatenate([zeros, -invM @ self.C(x)], 1)], 0)
+        # b = np.concatenate([zeros, invM], 0)
+        # return A @ x[:, np.newaxis] + b @ u
+
+        return v

.idea/TSwR_student.iml

@@ -15,14 +15,14 @@
 """
 Switch to FeedbackLinearizationController as soon as you implement it
 """
-#controller = FeedbackLinearizationController(Tp)
-controller = DummyController(Tp)
+controller = FeedbackLinearizationController(Tp)
+#controller = DummyController(Tp)
 
 """
 Here you have some trajectory generators. You can use them to check your implementations.
 At the end implement Point2point trajectory generator to move your manipulator to some desired state.
 """
-# traj_gen = ConstantTorque(np.array([0., 1.0])[:, np.newaxis])
+#traj_gen = ConstantTorque(np.array([0., 1.0])[:, np.newaxis])
 traj_gen = Sinusoidal(np.array([0., 1.]), np.array([2., 2.]), np.array([0., 0.]))
 #traj_gen = Poly3(np.array([0., 0.]), np.array([pi/4, pi/6]), end)