hMatrix Inverse_Kinematics(hMatrix Initial_T,hMatrix Goal_T,double *Initial_t, double *DH_alpha, double *DH_a, double *DH_d, int joint){
for(int i=0; i<joint; i++){
Initial_theta[i] = *Initial_t;
Initial_t++;
}
hMatrix Initial_Theta(7,1);
hMatrix J(6,7), Pinv_J(7,6);
hMatrix n_a(3,1),s_a(3,1),a_a(3,1),n_t(3,1),s_t(3,1),a_t(3,1),p_del(3,1);
double x,y,z,rx,ry,rz;
double error_position[3]= {Goal_T.element(0,3)-Initial_T.element(0,3),Goal_T.element(1,3)-Initial_T.element(1,3),Goal_T.element(2,3)-Initial_T.element(2,3)};
hMatrix P(3,1),R(3,1),Rotation(3,3),dx_temp1(3,1),dx_temp2(3,1),dX(6,1),del_Theta(7,1),Temp(7,1);
Initial_Theta.SET(7,1,Initial_theta);
Initial_T = T_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0], joint);
J = Jacobian_hMatrix(&Initial_theta[0], &DH_alpha[0], &DH_a[0], &DH_d[0]);
Pinv_J = Pseudo_Inverse(J);
for(int i = 0; i<3; i++){
n_a.SetElement(i,0,Initial_T.element(i,0));
s_a.SetElement(i,0,Initial_T.element(i,1));
a_a.SetElement(i,0,Initial_T.element(i,2));
n_t.SetElement(i,0,Goal_T.element(i,0));
s_t.SetElement(i,0,Goal_T.element(i,1));
a_t.SetElement(i,0,Goal_T.element(i,2));
p_del.SetElement(i,0,Goal_T.element(i,3)-Initial_T.element(i,3));
}
x = dot(n_a, p_del);
y = dot(s_a, p_del);
z = dot(a_a, p_del); ;
rx = (dot(a_a,s_t)-dot(a_t,s_a))/2;
ry = (dot(n_a,a_t)-dot(n_t,a_a))/2;
rz = (dot(s_a,n_t)-dot(s_t,n_a))/2;
double dx_P[3] = {x,y,z},dx_R[3] = {rx,ry,rz};
P.SET(3,1,&dx_P[0]);
R.SET(3,1,&dx_R[0]);
Rotation = T_Rotation(Initial_T);
dx_temp1 = Rotation*P;
dx_temp2 = Rotation*R;
for(int i =0; i<3; i++){
dX.SetElement(i,0,dx_temp1.element(i,0));
dX.SetElement(i+3,0,dx_temp2.element(i,0));
}
del_Theta = Pinv_J*dX;
for(int i=0; i<joint; i++)
Temp.SetElement(i,0,Initial_Theta.element(i,0) + del_Theta.element(i,0));
Initial_Theta = Temp;
return Initial_Theta;
}
void ss(t_content *axx)
{
s_a(axx);
s_b(axx);
if (VALUE_I(2, 0) == 1)
ft_putstr("ss ");
if (VALUE_I(2, 0) == 2)
{
ft_putstr("ss :");
verboz(axx);
}
}
int do_memory_uplo(int n, W& workspace ) {
typedef typename bindings::remove_imaginary<T>::type real_type ;
typedef ublas::matrix<T, ublas::column_major> matrix_type ;
typedef ublas::symmetric_adaptor<matrix_type, UPLO> symmetric_type ;
typedef ublas::vector<real_type> vector_type ;
// Set matrix
matrix_type a( n, n ); a.clear();
vector_type e1( n );
vector_type e2( n );
fill( a );
matrix_type a2( a );
// Compute eigen decomposition.
symmetric_type s_a( a );
lapack::syev( 'V', bindings::noop(s_a), e1, workspace ) ;
if (check_residual( a2, e1, a )) return 255 ;
symmetric_type s_a2( a2 );
lapack::syev( 'N', s_a2, e2, workspace ) ;
if (norm_2( e1 - e2 ) > n * norm_2( e1 ) * std::numeric_limits< real_type >::epsilon()) return 255 ;
// Test for a matrix range
fill( a ); a2.assign( a );
typedef ublas::matrix_range< matrix_type > matrix_range ;
typedef ublas::symmetric_adaptor<matrix_range, UPLO> symmetric_range_type;
ublas::range r(1,n-1) ;
matrix_range a_r( a, r, r );
ublas::vector_range< vector_type> e_r( e1, r );
symmetric_range_type s_a_r( a_r );
lapack::syev('V', s_a_r, e_r, workspace );
matrix_range a2_r( a2, r, r );
if (check_residual( a2_r, e_r, a_r )) return 255 ;
// Test for symmetric_adaptor
fill( a ); a2.assign( a );
ublas::symmetric_adaptor< matrix_type, UPLO> a_uplo( a ) ;
lapack::syev( 'V', a_uplo, e1, workspace ) ;
if (check_residual( a2, e1, a )) return 255 ;
return 0 ;
} // do_memory_uplo()
