Implement week04 FK and IK kinematics

Kinematics/kinematics.py

@@ -64,27 +64,40 @@ def bodyIK(self,omega,phi,psi,xm,ym,zm):
                 Tm.dot(np.array([[cHp,0,sHp,-L/2],[0,1,0,0],[-sHp,0,cHp,W/2],[0,0,0,1]])),
                 Tm.dot(np.array([[cHp,0,sHp,-L/2],[0,1,0,0],[-sHp,0,cHp,-W/2],[0,0,0,1]]))])
 
-    def legIK(self,point):
-        (x,y,z)=(point[0],point[1],point[2])
-        (l1,l2,l3,l4)=(self.l1,self.l2,self.l3,self.l4)
-        try:        
-            F=sqrt(x**2+y**2-l1**2)
-        except ValueError:
-            print("Error in legIK with x {} y {} and l1 {}".format(x,y,l1))
-            F=l1
-        G=F-l2  
-        H=sqrt(G**2+z**2)
-        theta1=-atan2(y,x)-atan2(F,-l1)
-
-        D=(H**2-l3**2-l4**2)/(2*l3*l4)
-        try:        
-            theta3=acos(D) 
-        except ValueError:
-            print("Error in legIK with x {} y {} and D {}".format(x,y,D))
-            theta3=0
-        theta2=atan2(z,G)-atan2(l4*sin(theta3),l3+l4*cos(theta3))
-
-        return(theta1,theta2,theta3)
+    def legIK(self, point):
+        """
+        Calculate inverse kinematics for a single leg.
+
+        Args:
+            point: target foot position [x, y, z, 1]
+
+        Returns:
+            tuple: joint angles (theta1, theta2, theta3)
+        """
+
+        x, y, z = point[0], point[1], point[2]
+        l1, l2, l3, l4 = self.l1, self.l2, self.l3, self.l4
+
+        F_square = x ** 2 + y ** 2 - l1 ** 2
+        if F_square < 0:
+            print("Error in legIK with x {} y {} and l1 {}".format(x, y, l1))
+            F_square = 0
+
+        F = sqrt(F_square)
+        G = F - l2
+        H = sqrt(G ** 2 + z ** 2)
+
+        theta1 = -atan2(y, x) - atan2(F, -l1)
+
+        D = (H ** 2 - l3 ** 2 - l4 ** 2) / (2 * l3 * l4)
+
+        # Numerical safety for acos input.
+        D = np.clip(D, -1.0, 1.0)
+
+        theta3 = acos(D)
+        theta2 = atan2(z, G) - atan2(l4 * sin(theta3), l3 + l4 * cos(theta3))
+
+        return theta1, theta2, theta3
 
     def calcLegPoints(self,angles):
         (l1,l2,l3,l4)=(self.l1,self.l2,self.l3,self.l4)