def r_tarsus(self):
lower_limit = self.segments["tarsus"].min_angle
upper_limit = self.segments["tarsus"].max_angle
transition_zone = 10
parameter = self.segments["tarsus"].current_angle
return exp_r(parameter, lower_limit, upper_limit, transition_zone)
def r_coxa(self):
lower_limit = self.segments["coxa"].min_angle
upper_limit = self.segments["coxa"].max_angle
transition_zone = 20
parameter = self.segments["coxa"].current_angle
return exp_r(parameter, lower_limit, upper_limit, transition_zone)
def update_r_ws(self):
# changes either when thorax CS is redefined (body moved) or when this leg is moved
foot_position_odom = self.cs.parent.parent.to_this(self.get_node("foot-thorax")) # leg->thorax->odom
coxa_position_odom = self.cs.parent.parent.to_this(self.get_node("coxa-thorax")) # leg->thorax->odom
foot_position_odom.z = 0
coxa_position_odom.z = 0
transition_zone = 10
parameter = coxa_position_odom.distance_to(foot_position_odom)
lower_limit = Leg.INNER_WS_LIMIT
upper_limit = 100000000
self._r_ws_inner = exp_r(parameter, lower_limit, upper_limit, transition_zone)
lower_limit = -100000000
upper_limit = Leg.OUTER_WS_LIMIT
self._r_ws_outer = exp_r(parameter, lower_limit, upper_limit, transition_zone)
def r_contract(self):
lower_limit = third_side(self.segments["femur"].length, self.segments["tibia"].length, 180 + self.segments["tibia"].min_angle)
upper_limit = 100000000
transition_zone = 20
try:
tarsus = self.stored_node_positions["tarsus"]
except:
tarsus = self.store_node("tarsus")
try:
femur = self.stored_node_positions["femur"]
except:
femur = self.store_node("femur")
parameter = femur.distance_to(tarsus)
return exp_r(parameter, lower_limit, upper_limit, transition_zone)
def r_stretch(self):
lower_limit = -100000000
upper_limit = (self.segments["femur"].length + self.segments["tibia"].length) * 0.9
transition_zone = 5
try:
tarsus = self.stored_node_positions["tarsus"]
except:
tarsus = self.store_node("tarsus")
try:
femur = self.stored_node_positions["femur"]
except:
femur = self.store_node("femur")
parameter = femur.distance_to(tarsus)
return exp_r(parameter, lower_limit, upper_limit, transition_zone)
