I'm trying to write a Newton Solver with gsl. It's a system of two equations that take cartesian coordinate (x, y) and translate them into two angles (it's for a robot arm project I'm building; these angles are sent to the stepper motors to move the effector to the cartesian input coordinates).
Here is my function:
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;
}
The function calc_angles takes two vectors, one with the input coordinates that are used to calculate the angles I'm looking for, and a second vector where I want to store said angles. Now, this works as expected and the function does calculate the correct angles for my input cartesian coordinates (I haven't implemented the input reading function yet; so I'm using hard coded values for testing).
Here's my problem: when I call calc_angles in my main function, when I return to main and try to print the resulting angles, they no longer match the calculated values:
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;
}
Here's the output of the program:
./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
You can see that alpha and beta don't match the calculated values any longer. I used gdb to check the memory locations, the values didn't change (so gsl_vector_memcpy() does work correctly). So I assume it's something with printf that isn't working. What am I missing?
I'm new to gsl. Here's the full source code (copied into one file for simplicity):
#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;
}
Short answer: s -> x stores the state of the solver, you shouldn't use the input array angles to get the output. This is the result of your code after I fixed a couple of minor details
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
calc_angles worked
Calculated angles: alpha: 2.342582, beta: 0.799011
And here is the actual code
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <gsl/gsl_errno.h>
#include <gsl/gsl_math.h>
#include <gsl/gsl_vector.h>
#include <gsl/gsl_matrix.h>
#include <gsl/gsl_multiroots.h>
#include <gsl/gsl_sf.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\n"); }
// 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 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, s -> x);
// 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 = %3ld 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;
}