def get_position_from_stable(stable_position, delta_params):
dp = delta_params
sphere_coords = [
(t[0], t[1], es - sp * sd)
for sd, t, es, sp in zip(
dp.stepdists, dp.towers, dp.abs_endstops, stable_position) ]
return mathutil.trilateration(sphere_coords, [a**2 for a in dp.arms])
def get_position_from_stable(self, stable_position):
# Return cartesian coordinates for the given stable_position
sphere_coords = [
(t[0], t[1], es - sp * sd)
for sd, t, es, sp in zip(self.stepdists, self.towers,
self.abs_endstops, stable_position)]
return mathutil.trilateration(sphere_coords, [a ** 2 for a in self.arms])
def get_position_from_stable(spos, params):
angles = [params['angle_a'], params['angle_b'], params['angle_c']]
radius = params['radius']
radius2 = radius**2
towers = [(math.cos(angle) * radius, math.sin(angle) * radius)
for angle in map(math.radians, angles)]
arm2 = [a**2 for a in [params['arm_a'], params['arm_b'], params['arm_c']]]
endstops = [params['endstop_a'], params['endstop_b'], params['endstop_c']]
sphere_coords = [(t[0], t[1], es + math.sqrt(a2 - radius2) - p)
for t, es, a2, p in zip(towers, endstops, arm2, spos)]
return mathutil.trilateration(sphere_coords, arm2)
def get_positions_from_stable(self, stable_positions, params):
angle_names = ['angle_a', 'angle_b', 'angle_c']
angles = [math.radians(params[an]) for an in angle_names]
radius = params['radius']
radius2 = radius**2
towers = [(math.cos(a) * radius, math.sin(a) * radius) for a in angles]
arm2 = [params[an]**2 for an in ['arm_a', 'arm_b', 'arm_c']]
endstop_names = ['endstop_a', 'endstop_b', 'endstop_c']
endstops = [
params[en] + math.sqrt(a2 - radius2)
for en, a2 in zip(endstop_names, arm2)
]
out = []
for spos in stable_positions:
sphere_coords = [(t[0], t[1], es - sp)
for t, es, sp in zip(towers, endstops, spos)]
out.append(mathutil.trilateration(sphere_coords, arm2))
return out
def get_positions_from_stable(self, stable_positions, params):
angle_names = ['angle_a', 'angle_b', 'angle_c']
angles = [math.radians(params[an]) for an in angle_names]
radius = params['radius']
radius2 = radius**2
towers = [(math.cos(a) * radius, math.sin(a) * radius) for a in angles]
arm2 = [params[an]**2 for an in ['arm_a', 'arm_b', 'arm_c']]
stepdist_names = ['stepdist_a', 'stepdist_b', 'stepdist_c']
stepdists = [params[sn] for sn in stepdist_names]
endstop_names = ['endstop_a', 'endstop_b', 'endstop_c']
endstops = [params[en] + math.sqrt(a2 - radius2)
for en, a2 in zip(endstop_names, arm2)]
out = []
for spos in stable_positions:
sphere_coords = [
(t[0], t[1], es - sp * sd)
for t, es, sd, sp in zip(towers, endstops, stepdists, spos) ]
out.append(mathutil.trilateration(sphere_coords, arm2))
return out
def get_position_from_stable(stable_position, delta_params):
dp = delta_params
sphere_coords = [(t[0], t[1], es - sp * sd) for sd, t, es, sp in zip(
dp.stepdists, dp.towers, dp.abs_endstops, stable_position)]
return mathutil.trilateration(sphere_coords, [a**2 for a in dp.arms])
def _actuator_to_cartesian(self, spos):
sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
return mathutil.trilateration(sphere_coords, self.arm2)
def calc_tag_position(self):
# Use only first three steppers to calculate cartesian position
spos = [s.get_tag_position() for s in self.steppers[:3]]
return mathutil.trilateration(self.anchors[:3],
[sp * sp for sp in spos])
def actuator_to_cartesian(self, spos):
sphere_coords = [self.elbow_coord(i, sp) for i, sp in enumerate(spos)]
lower_arm2 = [la**2 for la in self.lower_arms]
return mathutil.trilateration(sphere_coords, lower_arm2)
def calc_position(self):
# Use only first three steppers to calculate cartesian position
spos = [s.get_commanded_position() for s in self.steppers[:3]]
return mathutil.trilateration(self.anchors[:3], [sp*sp for sp in spos])
def _actuator_to_cartesian(self, spos):
sphere_coords = [(t[0], t[1], sp) for t, sp in zip(self.towers, spos)]
return mathutil.trilateration(sphere_coords, self.arm2)
