Mercurial > henk
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
