def update_home_position(self):
h_dist_home = (Leg.OUTER_WS_LIMIT + Leg.INNER_WS_LIMIT) / 2
roll_dist_home = h_dist_home * cosd(self.cs.roty)
self.home_position_leg = Position(self.cs, roll_dist_home * sind(-self.home_angle), roll_dist_home * cosd(-self.home_angle), 0)
self.home_position_leg.z = -38
self.home_position_thorax = self.cs.parent.to_this(self.home_position_leg)
return self.home_position_leg
def update_swings_inwards(self):
# Indicates whether the swing movement is towards inner limit of workspace
coxa = self.get_node("coxa")
current_foot = self.get_node("foot")
moved_foot = current_foot.clone
moved_foot.x += .0001 * sind(-1 * self.task_cs.rotz)
moved_foot.y += .0001 * cosd(-1 * self.task_cs.rotz)
self.swings_inwards = coxa.distance_to(current_foot) > coxa.distance_to(moved_foot)
def x_y(l, coxa_angle):
x = l * sind(-coxa_angle)
y = l * cosd(-coxa_angle)
return x, y
def store_node(self, name):
# noinspection PyShadowingNames
def x_y(l, coxa_angle):
x = l * sind(-coxa_angle)
y = l * cosd(-coxa_angle)
return x, y
if name == "foot":
segment_coxa = self.segments["coxa"]
coxa_len = segment_coxa.length
coxa_angle = segment_coxa.current_angle
segment_femur = self.segments["femur"]
femur_angle = segment_femur.current_angle
femur_projection_h = segment_femur.length * cosd(femur_angle)
femur_projection_v = segment_femur.length * sind(femur_angle)
segment_tibia = self.segments["tibia"]
tibia_angle = segment_tibia.current_angle
tibia_projection_h = segment_tibia.length * cosd(femur_angle + tibia_angle)
tibia_projection_v = segment_tibia.length * sind(femur_angle + tibia_angle)
segment_tarsus = self.segments["tarsus"]
tarsus_angle = segment_tarsus.current_angle
tarsus_projection_h = segment_tarsus.length * sind(90 + (tibia_angle + femur_angle + tarsus_angle) - 180)
tarsus_projection_v = segment_tarsus.length * cosd(90 + (tibia_angle + femur_angle + tarsus_angle) - 180)
l = coxa_len + femur_projection_h + tibia_projection_h + tarsus_projection_h
z = femur_projection_v + tibia_projection_v + tarsus_projection_v
x, y = x_y(l, coxa_angle)
node_pos_converted = Position(self.cs)
node_pos_converted.x, node_pos_converted.y, node_pos_converted.z = x, y, z
elif name == "foot-odom":
foot_thorax = self.get_node("foot-thorax")
node_pos_converted = self.cs.parent.parent.to_this(foot_thorax)
elif name == "coxa-thorax":
segment_coxa = self.segments["coxa"]
node_pos = Position(segment_coxa.cs)
node_pos_converted = self.cs.parent.to_this(node_pos)
elif name == "foot-thorax":
foot = self.get_node("foot")
node_pos_converted = self.cs.parent.to_this(foot)
elif name == "tibia-thorax":
segment_coxa = self.segments["coxa"]
coxa_len = segment_coxa.length
coxa_angle = segment_coxa.current_angle
segment_femur = self.segments["femur"]
femur_angle = segment_femur.current_angle
femur_projection_h = segment_femur.length * cosd(femur_angle)
femur_projection_v = segment_femur.length * sind(femur_angle)
l = coxa_len + femur_projection_h
z = femur_projection_v
x, y = x_y(l, coxa_angle)
node_pos_converted = Position(self.cs)
node_pos_converted.x, node_pos_converted.y, node_pos_converted.z = x, y, z
else:
node_pos = Position(self.segments[name].cs)
node_pos_converted = self.cs.to_this(node_pos)
self.stored_node_positions[name] = node_pos_converted
return node_pos_converted