changeset 11:dbd54564dbf0 default tip

Use common coordinate system for all legs
author Radomir Dopieralski <sheep-devel@sheep.art.pl>
date Sat, 24 Jan 2015 10:51:54 +0100
parents f43f0ee169ce
children
files leg.py
diffstat 1 files changed, 16 insertions(+), 4 deletions(-) [+]
line wrap: on
line diff
--- a/leg.py	Sat Jan 24 00:59:54 2015 +0100
+++ b/leg.py	Sat Jan 24 10:51:54 2015 +0100
@@ -32,12 +32,12 @@
         self.servos = servos
         self.trims = trims
         self.base = (
+            self.body_radius * math.sin(self.angle),
             self.body_radius * math.cos(self.angle),
-            self.body_radius * math.sin(self.angle)
         )
         self.home = (
-            (self.coxa + self.femur) * math.sin(self.angle),
-            (self.coxa + self.femur) * math.cos(self.angle),
+            self.base[0] + (self.coxa + self.femur) * math.sin(self.angle),
+            self.base[1] + (self.coxa + self.femur) * math.cos(self.angle),
             -self.tibia,
         )
         self.position = [0, 0, 0]
@@ -53,18 +53,30 @@
         return [hip, knee, ankle]
 
     def move_to(self, x=None, y=None, z=None):
+        """Move the leg to the specified position.
+
+        The coordinates are relative to the center of the body. If any of
+        the coordinates is None, current value will be used.
+        """
         x, y, z = [given if given is not None else current
                    for (given, current) in zip((x, y, z), self.position)]
-        angles = self._inverse_kinematics(x, y, z)
+
+        # Correct for the leg's attachment point and angle.
+        leg_x = x + self.base[0]
+        leg_y = y + self.base[1]
+        angles = self._inverse_kinematics(leg_x, leg_y, z)
         angles[0] += self.angle
+
         self.angles = angles
         self.position = x, y ,z
 
     def move_by(self, dx, dy, dz):
+        """Move the leg by specified offset."""
         x, y, z = self.position
         self.move_to(x + dx, y + dy, z + dz)
 
     def update(self, servos):
+        """Send the current joint angles to the servos."""
         for servo, angle, trim in zip(self.servos, self.angles, self.trims):
             while angle > math.pi:
                 angle -= math.pi * 2