Mercurial > Kubik
changeset 144:f06c9a2fd33f
leg directions
| author | Radomir Dopieralski <sheep-devel@sheep.art.pl> |
|---|---|
| date | Wed, 06 Aug 2014 18:27:17 +0200 |
| parents | b86bba4dc6a2 |
| children | f72b9da2f73c |
| files | kubik.py leg.py |
| diffstat | 2 files changed, 18 insertions(+), 16 deletions(-) [+] |
line wrap: on
line diff
--- a/kubik.py Wed Aug 06 18:17:28 2014 +0200 +++ b/kubik.py Wed Aug 06 18:27:17 2014 +0200 @@ -47,19 +47,19 @@ Leg(HipServo(m, 1, reverse=True, trim=250), KneeServo(m, 2, reverse=True, trim=-200), AnkleServo1(m, 0, reverse=True, trim=0, center=1500 * 4), - False), + (-1, 1)), Leg(HipServo(m, 4, reverse=True, trim=-100), KneeServo(m, 5, reverse=False, trim=-200), AnkleServo2(m, 3, reverse=False, trim=-240, center=1500 * 4), - True), + (1, 1)), Leg(HipServo(m, 7, reverse=False, trim=-150), KneeServo(m, 8, reverse=True, trim=100), AnkleServo1(m, 6, reverse=True, trim=0, center=1500 * 4), - True), + (1, -1)), Leg(HipServo(m, 10, reverse=False, trim=-250), KneeServo(m, 11, reverse=False, trim=-100), AnkleServo2(m, 9, reverse=False, trim=100, center=1500 * 4), - False), + (-1, -1)), ] self.shift = [0.0, 0.0, 0.0]
--- a/leg.py Wed Aug 06 18:17:28 2014 +0200 +++ b/leg.py Wed Aug 06 18:27:17 2014 +0200 @@ -2,6 +2,10 @@ # -*- coding: utf-8 -*- import math + +SQRT2 = math.sqrt(2) + + def _solve_triangle(a, b, c): """Calculate an angle of a triangle using the law of cosines.""" a, b, c = abs(a), abs(b), abs(c) @@ -35,20 +39,18 @@ femur = 50.0 # between the knee and the ankle tibia = 45.0 # between the ankle and the foot base = 45.0, 45.0 # from the center of body - home = 45.0, 45.0, -35.0 - def __init__(self, hip, knee, ankle, front=True): + def __init__(self, hip, knee, ankle, (x_dir, y_dir)): self.hip = hip self.knee = knee self.ankle = ankle - self.front = front - if self.front: - self.y_direction = 1 - else: - self.y_direction = -1 - self.hip_offset = -45 * self.y_direction + self.direction = x_dir, y_dir + self.hip_offset = -45 * self.direction[1] self.home = ( - self.home[0], self.home[1] * self.y_direction, self.home[2]) + (self.coxa + self.femur) / SQRT2, + -(self.coxa + self.femur) / SQRT2 * self.direction[1], + -self.tibia, + ) self.position = list(self.home) def __repr__(self): @@ -93,15 +95,15 @@ def rotate(self, degrees): """Rotate the leg around the center of the body horizontally.""" - radians = math.radians(degrees * self.y_direction) + radians = math.radians(degrees) x, y, z = self.position bx = self.base[0] + x - by = self.base[1] + y * self.y_direction + by = self.base[1] - y * self.direction[1] r = _length(bx, by) a = math.atan2(bx, by) rx = r * math.sin(a + radians) - self.base[0] ry = r * math.cos(a + radians) - self.base[1] - self.move_to((rx, ry * self.y_direction, None)) + self.move_to((rx, -ry * self.direction[1], None)) def update(self, shift): dx, dy, dz = shift
