def __init__(self, name, servo_id, min_angle, neutral_angle, max_angle, length, parent, leg, offset_in_parent, mirrored=False, z_rot=False):
self.name = name
self.parent = parent # segment to which this one is attached
self.leg = leg # leg to which this segment belongs
self.length = length
self.offset_in_parent = offset_in_parent # X coordinate of this segment's origin in parent cs
self.min_angle = min_angle
self.max_angle = max_angle
self.neutral_angle = neutral_angle
if self.name == "lm_coxa":
self.mount_correction = 45
else:
self.mount_correction = 0
if "femur" in self.name:
self.mount_correction = -90
if "tibia" in self.name:
self.load_direction = -1
else:
self.load_direction = 1
self.mirrored = mirrored
self.z_rot = z_rot # indicates when segment rotates around Z-axis and not Y-axis, as usual
self._current_angle = 0
self.servo_id = servo_id # dynamixel id of segment's servo
if self.z_rot:
self.cs = CoordinateSystem(parent.cs, self.name, self.offset_in_parent, 0, 0, 0, 0, self._current_angle)
else:
self.cs = CoordinateSystem(parent.cs, self.name, self.offset_in_parent, 0, 0, 0, self._current_angle, 0)
def __init__(self, name, parent, mount_descriptor, servo_ids):
global COXA_LENGTH, FEMUR_LENGTH, TIBIA_LENGTH, TARSUS_LENGTH
self.name = name
self.parent = parent
self.mount_descriptor = mount_descriptor
self.swing_order = 0 # used to keep track of the order in which legs swing. If two legs are ready to swing, priority is given to the one that has been in stance longer
self.r_was_increasing = False # indicates whether restrictedness of this leg was increasing before reaching flat plateau
self.cs = CoordinateSystem(parent.cs, self.name, mount_descriptor.x, mount_descriptor.y, mount_descriptor.z, 0, 0, mount_descriptor.rotz)
self.segments = dict()
self.segments["coxa"] = Segment(name=name + "_coxa", servo_id=servo_ids[0],
min_angle=r(-35), neutral_angle=r(0), max_angle=r(35),
length=COXA_LENGTH, parent=self,
leg=self,
offset_in_parent=m(0), mirrored=(mount_descriptor.x < 0), z_rot=True)
if self.name == "rf":
self.segments["coxa"].max_angle = r(10)
elif self.name == "lf":
self.segments["coxa"].min_angle = r(-10)
elif self.name == "rr":
self.segments["coxa"].min_angle = r(-10)
elif self.name == "lr":
self.segments["coxa"].max_angle = r(10)
self.segments["femur"] = Segment(name=name + "_femur", servo_id=servo_ids[1],
min_angle=r(-45), neutral_angle=r(20), max_angle=r(90), length=FEMUR_LENGTH,
parent=self.segments["coxa"], leg=self,
offset_in_parent=COXA_LENGTH)
self.segments["tibia"] = Segment(name=name + "_tibia", servo_id=servo_ids[2],
min_angle=r(-115), neutral_angle=r(20), max_angle=r(20), length=TIBIA_LENGTH,
parent=self.segments["femur"], leg=self,
offset_in_parent=FEMUR_LENGTH)
self.segments["tarsus"] = Segment(name=name + "_tarsus", servo_id=servo_ids[3],
min_angle=r(-90), neutral_angle=r(0), max_angle=r(0), length=TARSUS_LENGTH,
parent=self.segments["tibia"], leg=self,
offset_in_parent=TIBIA_LENGTH)
self.rostral_neighbor = None # leg in front
self.caudal_neighbor = None # leg behind
self.in_swing = False # is this leg in swing
self.swing_z = 0 # target highest Z of current swing (calculated as Z of lift-off point + DEFAULT_SWING_HEIGHT), all in thorax cs
self.touched_down = False # whether the leg actually touches ground
self.wants_to_swing = False # previously the leg wanted to swing, but was not allowed to do it
self.landing = False # leg is forced to land
self.raising = False # leg is in initial phase of swing
coxa_seg = self.segments["coxa"]
self.home_angle = (coxa_seg.min_angle + coxa_seg.max_angle) / 2
self.home_position_leg = Position(self.cs)
self.home_position_thorax = Position(self.cs.parent)
self.task_cs = CoordinateSystem(parent.cs, self.name + "_tcs", 0, 0, 0, 0, 0, 0)
self.swings_inwards = False # foot swings towards thorax
self._r_ws_inner = None # restrictedness by inner edge of foot workspace
self._r_ws_outer = None # restrictedness by outer edge of foot workspace
# stored previous values of restrictedness components
self._r_coxa_prev = 0
self._r_femur_prev = 0
self._r_tibia_prev = 0
self._r_tarsus_prev = 0
self._r_stretch_prev = 0
self._r_contract_prev = 0
self._r_ws_outer_prev = 0
self._r_ws_inner_prev = 0
self._r_prev = 0 # previous total restrictedness
# back-ups for angles and leg node positions
self.angles_backup = StoredAngles(0, 0, 0, 0)
self.nodes_backup = dict()
self.stored_node_positions = dict() # already calculated coordinates of leg nodes
# coxa coordinates in leg cs and thorax cs never change
self.stored_node_positions["coxa"] = Position(self.cs, 0, 0, 0)
self.stored_node_positions["coxa-thorax"] = self.cs.parent.to_this(self.stored_node_positions["coxa"])
def __init__(self):
self.cs = CoordinateSystem(g_odom_cs, "thorax", 0, 0, 0, 0, 0, 0)
self.legs = dict()
self.fill_legs()
#! /usr/bin/env python
# BigBug hexapod body description
from collections import namedtuple
from math import sqrt, degrees as d, radians as r, atan2
from bb_cs import CoordinateSystem, Position
from bb_utility import angle_ab, angle_ac, exp_r, sind, cosd, third_side, mm_to_m as m
MountDescriptor = namedtuple('MountDescriptor', 'x y z rotz') # describes attachment point for a leg in Thorax cs
StoredAngles = namedtuple('StoredAngles', 'coxa femur tibia tarsus')
# noinspection PyTypeChecker
g_odom_cs = CoordinateSystem(None, "odom", 0, 0, 0, 0, 0, 0)
# noinspection PyBroadException
class Thorax(object):
def __init__(self):
self.cs = CoordinateSystem(g_odom_cs, "thorax", 0, 0, 0, 0, 0, 0)
self.legs = dict()
self.fill_legs()
def fill_legs(self):
# initializes legs and puts them in a dictionary
# leg mount point is on the top surface of thorax
self.legs["rf"] = Leg("rf", self, MountDescriptor(m(+182.343), m(-104.843), m(0), r(-045.0)), (101, 102, 103, 19))
self.legs["rm"] = Leg("rm", self, MountDescriptor(m(0000.000), m(-114.198), m(0), r(-90.00)), (104, 105, 106, 11))
self.legs["rr"] = Leg("rr", self, MountDescriptor(m(-182.343), m(-084.843), m(0), r(-135.0)), (107, 108, 109, 14))
