diff --git a/klippy/chelper/__init__.py b/klippy/chelper/__init__.py index 46e6bb46d..7de16d212 100644 --- a/klippy/chelper/__init__.py +++ b/klippy/chelper/__init__.py @@ -166,6 +166,10 @@ defs_kin_winch = """ void winch_flex_set_enabled(struct winch_flex *wf, int enabled); double winch_flex_motor_to_line_pos(struct winch_flex *wf, int index, double motor_pos); + int winch_forward_solve(struct winch_flex *wf, const double *motor_pos, + const double *initial_guess, double eta, double tol, + int halley_iters, int max_iters, + double *out_pos, double *out_cost, int *out_iters); struct stepper_kinematics *winch_stepper_alloc(struct winch_flex *wf, int index); """ diff --git a/klippy/chelper/kin_winch.c b/klippy/chelper/kin_winch.c index 601473235..059076a61 100644 --- a/klippy/chelper/kin_winch.c +++ b/klippy/chelper/kin_winch.c @@ -650,6 +650,272 @@ motorpos_to_linepos(struct winch_flex *wf, int idx, double motor_pos) return delta_len; } +// ---- Forward transform (motor positions -> Cartesian) ---- +static inline double +vec_norm3(const double v[3]) +{ + return sqrt(v[0] * v[0] + v[1] * v[1] + v[2] * v[2]); +} + +static double +residuals_and_derivatives(struct winch_flex *wf, const double *line_pos, int N, + const struct coord *pos, double *residuals, + double *jac, double *hess) +{ + const double impact_step = 1e-3; + double base_flex[WINCH_MAX_ANCHORS]; + double impact_plus[3][WINCH_MAX_ANCHORS]; + double impact_minus[3][WINCH_MAX_ANCHORS]; + memset(base_flex, 0, sizeof(base_flex)); + memset(impact_plus, 0, sizeof(impact_plus)); + memset(impact_minus, 0, sizeof(impact_minus)); + + if (wf && wf->enabled) { + double distances[WINCH_MAX_ANCHORS]; + compute_flex(wf, pos->x, pos->y, pos->z, distances, base_flex); + for (int axis = 0; axis < 3; ++axis) { + struct coord shifted = *pos; + if (axis == 0) + shifted.x += impact_step; + else if (axis == 1) + shifted.y += impact_step; + else + shifted.z += impact_step; + compute_flex(wf, shifted.x, shifted.y, shifted.z, + distances, impact_plus[axis]); + if (axis == 0) + shifted.x = pos->x - impact_step; + else if (axis == 1) + shifted.y = pos->y - impact_step; + else + shifted.z = pos->z - impact_step; + compute_flex(wf, shifted.x, shifted.y, shifted.z, + distances, impact_minus[axis]); + } + } + + double cost = 0.; + for (int i = 0; i < N; ++i) { + double dx = pos->x - wf->anchors[i].x; + double dy = pos->y - wf->anchors[i].y; + double dz = pos->z - wf->anchors[i].z; + double dist = hypot3(dx, dy, dz); + if (dist < EPSILON) + dist = EPSILON; + double inv_len = 1. / dist; + double inv_len3 = inv_len * inv_len * inv_len; + + double flex = base_flex[i]; + double found_line_pos = dist - wf->distances_origin[i] + flex; + double res = found_line_pos - line_pos[i]; + if (residuals) + residuals[i] = res; + + double impact_deriv_x = (impact_plus[0][i] - impact_minus[0][i]) + / (2. * impact_step); + double impact_deriv_y = (impact_plus[1][i] - impact_minus[1][i]) + / (2. * impact_step); + double impact_deriv_z = (impact_plus[2][i] - impact_minus[2][i]) + / (2. * impact_step); + + if (jac) { + jac[i * 3 + 0] = dx * inv_len + impact_deriv_x; + jac[i * 3 + 1] = dy * inv_len + impact_deriv_y; + jac[i * 3 + 2] = dz * inv_len + impact_deriv_z; + } + + if (hess) { + double *hrow = hess + i * 9; + hrow[0] = inv_len - dx * dx * inv_len3; + hrow[1] = -dx * dy * inv_len3; + hrow[2] = -dx * dz * inv_len3; + hrow[3] = -dy * dx * inv_len3; + hrow[4] = inv_len - dy * dy * inv_len3; + hrow[5] = -dy * dz * inv_len3; + hrow[6] = -dz * dx * inv_len3; + hrow[7] = -dz * dy * inv_len3; + hrow[8] = inv_len - dz * dz * inv_len3; + } + + cost += 0.5 * res * res; + } + return cost; +} + +static void +accumulate_normals(const double *jac, const double *residuals, int N, + double JTJ[3][3], double grad[3]) +{ + grad[0] = grad[1] = grad[2] = 0.; + JTJ[0][0] = JTJ[0][1] = JTJ[0][2] = 0.; + JTJ[1][0] = JTJ[1][1] = JTJ[1][2] = 0.; + JTJ[2][0] = JTJ[2][1] = JTJ[2][2] = 0.; + for (int i = 0; i < N; ++i) { + double jx = jac[i * 3 + 0]; + double jy = jac[i * 3 + 1]; + double jz = jac[i * 3 + 2]; + double r = residuals[i]; + grad[0] += jx * r; + grad[1] += jy * r; + grad[2] += jz * r; + JTJ[0][0] += jx * jx; + JTJ[0][1] += jx * jy; + JTJ[0][2] += jx * jz; + JTJ[1][0] += jy * jx; + JTJ[1][1] += jy * jy; + JTJ[1][2] += jy * jz; + JTJ[2][0] += jz * jx; + JTJ[2][1] += jz * jy; + JTJ[2][2] += jz * jz; + } +} + +static int +solve_normal_system(double JTJ[3][3], const double rhs[3], double delta[3]) +{ + double JTJ_inv[3][3]; + if (!invert3x3((const double (*)[3])JTJ, JTJ_inv)) + return 0; + delta[0] = JTJ_inv[0][0] * rhs[0] + JTJ_inv[0][1] * rhs[1] + JTJ_inv[0][2] * rhs[2]; + delta[1] = JTJ_inv[1][0] * rhs[0] + JTJ_inv[1][1] * rhs[1] + JTJ_inv[1][2] * rhs[2]; + delta[2] = JTJ_inv[2][0] * rhs[0] + JTJ_inv[2][1] * rhs[1] + JTJ_inv[2][2] * rhs[2]; + return 1; +} + +static int +solve_hybrid_halley(struct winch_flex *wf, const double *line_pos, int N, + struct coord *pos, double eta, double tol, + int halley_iters, int max_iters, + double *cost_out, int *iters_out) +{ + int iter_count = 0; + int converged = 0; + double cost = 0.; + double residuals[WINCH_MAX_ANCHORS]; + double jac[WINCH_MAX_ANCHORS * 3]; + double hess[WINCH_MAX_ANCHORS * 9]; + int hi_limit = halley_iters < max_iters ? halley_iters : max_iters; + + for (int iter = 0; iter < hi_limit; ++iter) { + cost = residuals_and_derivatives(wf, line_pos, N, pos, + residuals, jac, hess); + double JTJ[3][3], grad[3]; + accumulate_normals(jac, residuals, N, JTJ, grad); + JTJ[0][0] += eta; + JTJ[1][1] += eta; + JTJ[2][2] += eta; + double rhs1[3] = { -grad[0], -grad[1], -grad[2] }; + double delta_lm[3]; + if (!solve_normal_system(JTJ, rhs1, delta_lm)) + break; + + double Hbar[WINCH_MAX_ANCHORS][3]; + for (int i = 0; i < N; ++i) { + double *hrow = hess + i * 9; + Hbar[i][0] = delta_lm[0] * hrow[0] + delta_lm[1] * hrow[3] + delta_lm[2] * hrow[6]; + Hbar[i][1] = delta_lm[0] * hrow[1] + delta_lm[1] * hrow[4] + delta_lm[2] * hrow[7]; + Hbar[i][2] = delta_lm[0] * hrow[2] + delta_lm[1] * hrow[5] + delta_lm[2] * hrow[8]; + } + + double jbar[WINCH_MAX_ANCHORS * 3]; + for (int i = 0; i < N; ++i) { + jbar[i * 3 + 0] = jac[i * 3 + 0] + 0.5 * Hbar[i][0]; + jbar[i * 3 + 1] = jac[i * 3 + 1] + 0.5 * Hbar[i][1]; + jbar[i * 3 + 2] = jac[i * 3 + 2] + 0.5 * Hbar[i][2]; + } + + double JTJ2[3][3], grad2[3]; + accumulate_normals(jbar, residuals, N, JTJ2, grad2); + JTJ2[0][0] += eta; + JTJ2[1][1] += eta; + JTJ2[2][2] += eta; + double rhs2[3] = { -grad2[0], -grad2[1], -grad2[2] }; + double delta[3]; + if (!solve_normal_system(JTJ2, rhs2, delta)) + break; + pos->x += delta[0]; + pos->y += delta[1]; + pos->z += delta[2]; + iter_count = iter + 1; + if (vec_norm3(delta) < tol) { + converged = 1; + break; + } + } + + for (int iter = halley_iters; iter < max_iters && !converged; ++iter) { + cost = residuals_and_derivatives(wf, line_pos, N, pos, + residuals, jac, NULL); + double JTJ[3][3], grad[3]; + accumulate_normals(jac, residuals, N, JTJ, grad); + JTJ[0][0] += eta; + JTJ[1][1] += eta; + JTJ[2][2] += eta; + double rhs[3] = { -grad[0], -grad[1], -grad[2] }; + double delta[3]; + if (!solve_normal_system(JTJ, rhs, delta)) + break; + pos->x += delta[0]; + pos->y += delta[1]; + pos->z += delta[2]; + iter_count = iter + 1; + if (vec_norm3(delta) < tol) { + converged = 1; + break; + } + } + + double residuals_final[WINCH_MAX_ANCHORS]; + double jac_tmp[WINCH_MAX_ANCHORS * 3]; + cost = residuals_and_derivatives(wf, line_pos, N, pos, + residuals_final, jac_tmp, NULL); + if (iters_out) + *iters_out = iter_count; + if (cost_out) + *cost_out = cost; + return converged; +} + +int __visible +winch_forward_solve(struct winch_flex *wf, const double *motor_pos, + const double *initial_guess, double eta, double tol, + int halley_iters, int max_iters, + double *out_pos, double *out_cost, int *out_iters) +{ + if (!wf || !motor_pos || !out_pos) + return 0; + int N = wf->num_anchors; + if (N < 3 || N > WINCH_MAX_ANCHORS) + return 0; + if (halley_iters < 0) + halley_iters = 0; + if (max_iters < 0) + max_iters = 0; + double line_pos[WINCH_MAX_ANCHORS]; + for (int i = 0; i < N; ++i) + line_pos[i] = motorpos_to_linepos(wf, i, motor_pos[i]); + + struct coord pos; + pos.x = initial_guess ? initial_guess[0] : 0.; + pos.y = initial_guess ? initial_guess[1] : 0.; + pos.z = initial_guess ? initial_guess[2] : 0.; + + double cost = 0.; + int iters = 0; + int ok = solve_hybrid_halley(wf, line_pos, N, &pos, eta, tol, + halley_iters, max_iters, &cost, &iters); + out_pos[0] = pos.x; + out_pos[1] = pos.y; + out_pos[2] = pos.z; + if (out_cost) + *out_cost = cost; + if (out_iters) + *out_iters = iters; + if (!ok || cost > 10.) + return 0; + return 1; +} + static double calc_position_common(struct winch_stepper *ws, struct move *m, double move_time) { diff --git a/klippy/kinematics/winch.py b/klippy/kinematics/winch.py index 91188a458..bd4b0f21e 100644 --- a/klippy/kinematics/winch.py +++ b/klippy/kinematics/winch.py @@ -165,11 +165,35 @@ class WinchKinematics: def get_steppers(self): return list(self.steppers) def calc_position(self, stepper_positions): - guess = [0., 0., 0.] - return guess + flex_ptr = self.flex_helper.get_ptr() + ffi_main, ffi_lib = self.flex_helper.ffi_main, self.flex_helper.ffi_lib + if not flex_ptr or ffi_main is None or ffi_lib is None: + return [None, None, None] + try: + motor_pos = [stepper_positions[s.get_name()] for s in self.steppers] + except KeyError: + return [None, None, None] + motor_c = ffi_main.new("double[]", motor_pos) + guess = self._last_forward if self._last_forward is not None else [0., 0., 0.] + guess_c = ffi_main.new("double[3]", guess) + out_pos = ffi_main.new("double[3]") + out_cost = ffi_main.new("double[1]") + out_iters = ffi_main.new("int[1]") + ok = ffi_lib.winch_forward_solve( + flex_ptr, motor_c, guess_c, + self._halley_eta, self._halley_tol, + self._halley_hybrid_iters, self._halley_max_iters, + out_pos, out_cost, out_iters) + if not ok: + return [None, None, None] + result = [out_pos[0], out_pos[1], out_pos[2]] + self._last_forward = result + return result def set_position(self, newpos, homing_axes): for s in self.steppers: s.set_position(newpos) + if len(newpos) >= 3: + self._last_forward = list(newpos[:3]) def clear_homing_state(self, clear_axes): # XXX - homing not implemented pass