calc_joint_angle.cpp File Reference

Functions to calculate collar angles. More...

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string>
#include <vector>
#include "postprocess.h"
#include "lowerleg_func.h"

Include dependency graph for calc_joint_angle.cpp:

This graph shows which files directly or indirectly include this file:

Functions
int	calc_joint_angle_err (double angle_rad, double DH_mat[], double Mat_loc[], double Ptloc[3], double Ptglob[3], double err[])
	Calculate joint angle error (for least square calculations)
int	calc_j_angle_wrap (const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_vector *err)
	Calculate joint angle.
int	jnt_angle_jacobian (double theta, double DH_mat[], double Mat_loc[], double Ptloc[3], double J[])
int	jnt_angle_jacob_wrap (const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_matrix *J)
int	calc_jnt_angle_jacob_wrap (const gsl_vector *angle_rad, struct angle_data *angle_d, gsl_vector *err, gsl_matrix *J)
int	calc_joint_ang (struct angle_data *angle_d, double theta_init, double precision, int nb_iter, double theta_sol)
	Calculate joint angle for given two plates.

---

Detailed Description

Functions to calculate collar angles.

---

Function Documentation

int calc_j_angle_wrap	(	const gsl_vector *	angle_rad,
		struct angle_data *	angle_d,
		gsl_vector *	err
	)

Calculate joint angle.

Wrapper function that calls the GSL Least Squares Solver and the joint angle error function

References calc_joint_angle_err(), angle_data::DH_mat, angle_data::Mat_loc, angle_data::Ptglob, and angle_data::Ptloc.

Referenced by calc_joint_ang().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_jnt_angle_jacob_wrap	(	const gsl_vector *	angle_rad,
		struct angle_data *	angle_d,
		gsl_vector *	err,
		gsl_matrix *	J
	)

References calc_joint_angle_err(), angle_data::DH_mat, jnt_angle_jacobian(), angle_data::Mat_loc, angle_data::Ptglob, and angle_data::Ptloc.

Referenced by calc_joint_ang().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_joint_ang	(	struct angle_data *	angle_d,
		double	theta_init,
		double	precision,
		int	nb_iter,
		double	theta_sol
	)

Calculate joint angle for given two plates.

Function uses gsl implementation of Least Squares to get the optimal value of joint angle.

Done using Julien's implementation

vecteur initial

appel du solver

affichage du resultat

References calc_j_angle_wrap(), calc_jnt_angle_jacob_wrap(), jnt_angle_jacob_wrap(), print_results_lm(), and solve_gsl_lm().

Referenced by calc_collar_angles().

Here is the call graph for this function:

Here is the caller graph for this function:

int calc_joint_angle_err	(	double	angle_rad,
		double	DH_mat[],
		double	Mat_loc[],
		double	Ptloc[3],
		double	Ptglob[3],
		double	err[]
	)

Calculate joint angle error (for least square calculations)

The functions takes in the estimated value of joint angle and calculates the error between measured point co-ordinates in local frame and estimated point co-ordinates, calculated using the theta.

DHmat is the DH matrix is the DH transformation matrix giving the transform matrix between the current plate and the plate to the "right" ie the plates between which the joint angle is defined. It is expressed as a 1d array, in the form of the data array in gsl_matrix.

Parameters:

[in]	angle_rad	Estimated joint angle in radians
[in]	DH_mat	DH transformation matrix between current (base) plate and next plate (already calculated in when performing collar calibration) (expressed as 1d array)
[in]	Mat_loc	Transformation matrix of "next" plate to convert local coordinates to DH frame coordinates.( Obtained from the Pvals and qvals files)
[in]	Ptloc	local co-ordinates of 1 point on the next plate.
[in]	Ptglob	measured global coordinates of the point (global refers to frame fixed to current base plate)
[out]	err	Error between estimated global coordinates and measured coordinates Ptglob

References gsl_dbl2matrix(), gsl_hom_matrix(), gsl_matrixprod(), gsl_matrixvecprod(), and rotZ().

Referenced by calc_j_angle_wrap(), and calc_jnt_angle_jacob_wrap().

Here is the call graph for this function:

Here is the caller graph for this function:

int jnt_angle_jacob_wrap	(	const gsl_vector *	angle_rad,
		struct angle_data *	angle_d,
		gsl_matrix *	J
	)

References angle_data::DH_mat, jnt_angle_jacobian(), angle_data::Mat_loc, angle_data::Ptglob, and angle_data::Ptloc.

Referenced by calc_joint_ang().

Here is the call graph for this function:

Here is the caller graph for this function:

int jnt_angle_jacobian	(	double	theta,
		double	DH_mat[],
		double	Mat_loc[],
		double	Ptloc[3],
		double	J[]
	)

Referenced by calc_jnt_angle_jacob_wrap(), and jnt_angle_jacob_wrap().

Here is the caller graph for this function: