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