class Segment(object):
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 __str__(self):
return self.name
@property
def current_angle_sideaware(self):
if self.mirrored:
mirror_coef = -1
else:
mirror_coef = 1
return self._current_angle * mirror_coef
@property
def current_angle(self):
return self._current_angle
@property
def servo_angle(self):
if "tibia" in self.name:
reverse = -1
else:
reverse = 1
if self.name[:1] == "l":
side = -1 * reverse
else:
side = 1 * reverse
return side * (self.current_angle_sideaware + self.neutral_angle + self.mount_correction)
@current_angle.setter
def current_angle(self, value):
# assign angle to segment
# make sure that segment never goes beyond angle limits
if self.min_angle <= value <= self.max_angle:
self._current_angle = value
elif value < self.min_angle:
self._current_angle = self.min_angle
elif value < self.min_angle:
self._current_angle = self.min_angle
if self.z_rot:
self.cs.redefine(new_rotz=self._current_angle)
else:
self.cs.redefine(new_roty=self._current_angle)
def set_angle_from_servo(self, angle):
self._current_angle = self.angle_from_servo(angle)
def angle_from_servo(self, angle):
if "tibia" in self.name:
reverse = -1
else:
reverse = 1
if self.name[:1] == "l":
side = -1 * reverse
else:
side = 1 * reverse
if self.mirrored:
mirror_coef = -1
else:
mirror_coef = 1
angle = ((angle / side) - self.neutral_angle - self.mount_correction) / mirror_coef
return angle
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))
self.legs["lf"] = Leg("lf", self, MountDescriptor(m(+182.343), m(+104.843), m(0), r(+045.0)), (116, 117, 118, 13))
self.legs["lm"] = Leg("lm", self, MountDescriptor(m(0000.000), m(+114.198), m(0), r(+90.00)), (113, 114, 115, 20))
self.legs["lr"] = Leg("lr", self, MountDescriptor(m(+182.343), m(+084.843), m(0), r(+135.0)), (110, 111, 112, 15))
self.legs["rf"].rostral_neighbor = self.legs["lf"]
self.legs["rf"].caudal_neighbor = self.legs["rm"]
self.legs["rm"].rostral_neighbor = self.legs["rf"]
self.legs["rm"].caudal_neighbor = self.legs["rr"]
self.legs["rr"].rostral_neighbor = self.legs["rm"]
self.legs["rr"].caudal_neighbor = self.legs["lr"]
self.legs["lf"].rostral_neighbor = self.legs["rf"]
self.legs["lf"].caudal_neighbor = self.legs["lm"]
self.legs["lm"].rostral_neighbor = self.legs["lf"]
self.legs["lm"].caudal_neighbor = self.legs["lr"]
self.legs["lr"].rostral_neighbor = self.legs["lm"]
self.legs["lr"].caudal_neighbor = self.legs["rr"]
# noinspection PyPep8Naming
@property
def ground_clearance(self):
# returns ground clearance based on current leg positions
# legs are attached to a top surface of thorax
# as leg Z in thorax cs is negative when foot is below thorax, ground clearance is assumed to be (max leg Z) - (thorax height)
THORAX_HEIGHT = m(48)
z_list = []
for leg in self.legs.values():
if not leg.in_swing: # only legs in stance are considered
z_list.append(leg.get_node("foot-thorax").z) # get coordinates of all feet in thorax cs
if len(z_list) > 0:
return -1 * max(z_list) - THORAX_HEIGHT
else:
return 0 # this generally should not happen, unless hex is flying
@property
def r_stance(self):
# returns maximum restrictedness of all legs in stance
r_max = 0
for leg in self.legs.values():
if not leg.in_swing:
r_max = max(r_max, leg.r_stance)
return r_max
def move(self, keep_feet_odom=False, new_x=None, new_y=None, new_z=None, new_rotx=None, new_roty=None, new_rotz=None):
# moves thorax
# keep_feet_odom = true means that feet must stay where they are in odom cs and not move together with thorax
self.cs.redefine(new_x, new_y, new_z, new_rotx, new_roty, new_rotz)
for leg in self.legs.values():
leg._r_ws_inner = None
leg._r_ws_outer = None
if not keep_feet_odom:
try:
del leg.stored_node_positions["foot-odom"] # feet shift together with thorax, so their odom coordinates must be updated
except:
pass # feet remain where they are, so no need to update their odom coordinates
def rollback(self, keep_feet_odom):
# moves thorax back to a stored position
# keep_feet_odom = true means that feet must stay where they are in odom cs and not move together with thorax
self.cs.rollback()
for leg in self.legs.values():
leg._r_ws_inner = None
leg._r_ws_outer = None
if not keep_feet_odom:
try:
del leg.stored_node_positions["foot-odom"] # feet shift together with thorax, so their odom coordinates must be updated
except:
pass # feet remain where they are, so no need to update their odom coordinates
