Я пытаюсь написать Ньютон Солвер с gsl .Это система из двух уравнений, которые берут декартову координату (x, y) и переводят их в два угла (это для проекта манипулятора, который я строю; эти углы посылаются шаговым двигателям для перемещения эффектора к входным декартовым координатам)).
Вот моя функция:
int
calc_angles(gsl_vector* coords, gsl_vector* result_angles)
{
const gsl_multiroot_fdfsolver_type* T;
gsl_multiroot_fdfsolver* s;
int status;
size_t i, iter = 0;
const size_t n = 2;
// coordinates whose's angles is to be found
struct rparams p = { gsl_vector_get(coords, 0), gsl_vector_get(coords, 1) };
gsl_multiroot_function_fdf f = { &coords_f,
&coords_df,
&coords_fdf,
n, &p};
// TODO: calculate inital guess
double angles_init[2] = { 140.0*M_PI/180.0, 30*M_PI/180.0 };
gsl_vector* angles = gsl_vector_alloc(n);
gsl_vector_set(angles, 0, angles_init[0]);
gsl_vector_set(angles, 1, angles_init[1]);
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc(T, n);
gsl_multiroot_fdfsolver_set(s, &f, angles);
print_state(iter, s);
do
{
iter++;
status = gsl_multiroot_fdfsolver_iterate(s);
print_state(iter, s);
if(status) { break; }
status = gsl_multiroot_test_residual(s->f, 1e-7);
} while (status == GSL_CONTINUE && iter < 1000);
printf("status = %s\n", gsl_strerror(status));
print_state(iter, s);
// store results in result_angles
gsl_vector_memcpy(result_angles, angles);
// sanity check
if(gsl_vector_equal(result_angles, angles))
{
printf("Vectors are equal\n");
}
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(angles);
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------
int
print_state(size_t iter, gsl_multiroot_fdfsolver* s)
{
printf("iter = %3u x = % .6f % .6f "
"f(x) = % .3e % .3e\n",
iter,
gsl_vector_get(s->x, 0),
gsl_vector_get(s->x, 1),
gsl_vector_get(s->f, 0),
gsl_vector_get(s->f, 1) );
// all good, return success
return GSL_SUCCESS;
}
Функция calc_angles принимает два вектора, один с входными координатами, которые используются для вычисления углов, которые я ищу, ивторой вектор, где я хочу сохранить указанные углы.Теперь это работает, как и ожидалось, и функция вычисляет правильные углы для моих входных декартовых координат (я еще не реализовал функцию чтения входных данных; поэтому я использую жестко закодированные значения для тестирования).
ВотМоя проблема: когда я вызываю calc_angles в моей основной функции, когда я возвращаюсь к main и пытаюсь напечатать полученные углы, они больше не соответствуют вычисленным значениям:
int
main(int argc, char* argv[])
{
const size_t n = 2;
// initialize vectors: input coords, initial guess, result angles
gsl_vector* coords = gsl_vector_alloc(n);
gsl_vector* result_angles = gsl_vector_alloc(n);
gsl_vector_set(result_angles, 0, 0.0); // set result angles to zero
gsl_vector_set(result_angles, 1, 0.0);
// TODO: read coordinates from input
// get_coords(coords);
gsl_vector_set(coords, 0, 0.0);
gsl_vector_set(coords, 1, 8.6);
// calc new angles
if(calc_angles(coords, result_angles)) { printf("calc_angles worked"); }
// output new angles
printf("Calculated angles: alpha: % .6f, beta: % .6f\n",
gsl_vector_get(result_angles, 0),
gsl_vector_get(result_angles, 1) );
// deallocate memory
gsl_vector_free(coords);
gsl_vector_free(result_angles);
return 0;
}
Вот выводпрограмма:
./bin/example_app
iter = 0 x = 2.443461 0.523599 f(x) = 9.998e-02 -2.905e-01
iter = 1 x = 2.308197 0.897453 f(x) = -4.876e-02 8.863e-02
iter = 2 x = 2.336417 0.808354 f(x) = -2.295e-03 1.077e-02
iter = 3 x = 2.342411 0.799205 f(x) = -1.653e-05 2.539e-04
iter = 4 x = 2.342579 0.799014 f(x) = -2.884e-09 3.705e-06
iter = 5 x = 2.342582 0.799011 f(x) = -7.438e-15 5.048e-08
status = success
iter = 5 x = 2.342582 0.799011 f(x) = -7.438e-15 5.048e-08
Vectors are equal
Calculated angles: alpha: 2.443461, beta: 0.523599
Вы можете видеть, что альфа и бета больше не соответствуют рассчитанным значениям.Я использовал gdb для проверки мест в памяти, значения не изменились (поэтому gsl_vector_memcpy() работает правильно).Поэтому я предполагаю, что это что-то с printf, которое не работает.Что мне не хватает?
Я новичок в gsl.Вот полный исходный код (для простоты скопированный в один файл):
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_sf.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multiroots.h>
// constants
const double LENGTH = 6.0;
const double TOL = 1.0e-6;
// params struct
struct rparams
{
double x, y;
};
// calculate the primary and secondary angles for the given coordinates in coords
int calc_angles(gsl_vector* coords, gsl_vector* result_angles);
int coords_f(const gsl_vector* angles, void* params, gsl_vector* f);
int coords_df(const gsl_vector* angles, void* params, gsl_matrix* J);
int coords_fdf(const gsl_vector* angles, void* params, gsl_vector* f, gsl_matrix* J);
// IO functions
int get_coords(gsl_vector* coords);
// helper/debug functions
int print_state(size_t iter, gsl_multiroot_fdfsolver* s);
int
main(int argc, char* argv[])
{
const size_t n = 2;
// initialize vectors: input coords, initial guess, result angles
gsl_vector* coords = gsl_vector_alloc(n);
gsl_vector* result_angles = gsl_vector_alloc(n);
gsl_vector_set(result_angles, 0, 0.0); // set result angles to zero
gsl_vector_set(result_angles, 1, 0.0);
// TODO: read coordinates from input
// get_coords(coords);
gsl_vector_set(coords, 0, 0.0);
gsl_vector_set(coords, 1, 8.6);
// calc new angles
if(calc_angles(coords, result_angles)) { printf("calc_angles worked"); }
// output new angles
printf("Calculated angles: alpha: % .6f, beta: % .6f\n",
gsl_vector_get(result_angles, 0),
gsl_vector_get(result_angles, 1) );
// deallocate memory
gsl_vector_free(coords);
gsl_vector_free(result_angles);
return 0;
}
//------------------------------------------------------------------------------
int
calc_angles(gsl_vector* coords, gsl_vector* result_angles)
{
const gsl_multiroot_fdfsolver_type* T;
gsl_multiroot_fdfsolver* s;
int status;
size_t i, iter = 0;
const size_t n = 2;
// coordinates whose's angles is to be found
struct rparams p = { gsl_vector_get(coords, 0), gsl_vector_get(coords, 1) };
gsl_multiroot_function_fdf f = { &coords_f,
&coords_df,
&coords_fdf,
n, &p};
// TODO: calculate inital guess
double angles_init[2] = { 140.0*M_PI/180.0, 30*M_PI/180.0 };
gsl_vector* angles = gsl_vector_alloc(n);
gsl_vector_set(angles, 0, angles_init[0]);
gsl_vector_set(angles, 1, angles_init[1]);
T = gsl_multiroot_fdfsolver_gnewton;
s = gsl_multiroot_fdfsolver_alloc(T, n);
gsl_multiroot_fdfsolver_set(s, &f, angles);
print_state(iter, s);
do
{
iter++;
status = gsl_multiroot_fdfsolver_iterate(s);
print_state(iter, s);
if(status) { break; }
status = gsl_multiroot_test_residual(s->f, 1e-7);
} while (status == GSL_CONTINUE && iter < 1000);
printf("status = %s\n", gsl_strerror(status));
print_state(iter, s);
// store results in result_angles
gsl_vector_memcpy(result_angles, angles);
// sanity check
if(gsl_vector_equal(result_angles, angles))
{
printf("Vectors are equal\n");
}
gsl_multiroot_fdfsolver_free(s);
gsl_vector_free(angles);
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------
int
coords_f(const gsl_vector* angles, void* params, gsl_vector* f)
{
// extract c and y coordinates
double x = ((struct rparams*) params)->x;
double y = ((struct rparams*) params)->y;
// extract input angles
const double alpha = gsl_vector_get(angles, 0);
const double beta = gsl_vector_get(angles, 1);
// calculate coordinates from angles
const double x0 = gsl_sf_cos(alpha) + gsl_sf_cos(beta) - x / LENGTH;
const double y0 = gsl_sf_sin(alpha) + gsl_sf_sin(beta) - y / LENGTH;
// save results
gsl_vector_set(f, 0, x0);
gsl_vector_set(f, 1, y0);
// all good, return success
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------
int
coords_df(const gsl_vector* angles, void* params, gsl_matrix* J)
{
// extract input angle
const double alpha = gsl_vector_get(angles, 0);
const double beta = gsl_vector_get(angles, 1);
// calculate partial derivatives for Jacobian matrix
const double df00 = -gsl_sf_sin(alpha);
const double df01 = -gsl_sf_sin(beta);
const double df10 = gsl_sf_cos(alpha);
const double df11 = gsl_sf_sin(beta);
// set Jacobian matrix
gsl_matrix_set(J, 0, 0, df00);
gsl_matrix_set(J, 0, 1, df01);
gsl_matrix_set(J, 1, 0, df10);
gsl_matrix_set(J, 1, 1, df11);
// all good, return success
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------
int
coords_fdf(const gsl_vector* angles, void* params, gsl_vector* f, gsl_matrix* J)
{
coords_f(angles, params, f);
coords_df(angles, params, J);
// all good, return success
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------
int
get_coords(gsl_vector* coords)
{
// TODO: replace with platform specific code
// read new coordinates from input
float x, y;
printf("Enter new X coordinate: ");
scanf("%f", &x);
printf("Enter new Y coordinate: ");
scanf("%f", &y);
// TODO: check for legal input bounds
// store input in memory
gsl_vector_set(coords, 0, x);
gsl_vector_set(coords, 1, y);
printf("x: %3.3f, y: %3.3f\n", x, y);
// all good, return success
return GSL_SUCCESS;
}
//------------------------------------------------------------------------------
int
print_state(size_t iter, gsl_multiroot_fdfsolver* s)
{
printf("iter = %3u x = % .6f % .6f "
"f(x) = % .3e % .3e\n",
iter,
gsl_vector_get(s->x, 0),
gsl_vector_get(s->x, 1),
gsl_vector_get(s->f, 0),
gsl_vector_get(s->f, 1) );
// all good, return success
return GSL_SUCCESS;
}