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;
}
int main()
{
typedef double real_type;
typedef unsigned long uint_type;
typedef ::std::size_t size_type;
typedef ::dcs::math::la::dense_matrix<real_type> matrix_type;
typedef ::dcs::math::la::dense_vector<real_type> vector_type;
uint_type seed(5489);
uint_type num_obs(50);
real_type ff(0.98); // forgetting factor
uint_type n_a(2);
uint_type n_b(2);
uint_type delay(0);
::dcs::math::random::mt19937 urng(seed);
::dcs::math::stats::normal_distribution<real_type> dist;
// Create a SISO model with ARX structure
vector_type a(n_a);
a(0) = -1.5; a(1) = 1.0;
vector_type b(n_b+1);
b(0) = 0; b(1) = 1.0; b(2) = 0.5;
real_type c = 1.0; // noise variance
::dcs::sysid::darx_siso_model<vector_type,real_type,uint_type> siso_model(a, b, c);
::std::cout << siso_model << ::std::endl;
// Generate random input data
::dcs::math::la::dense_vector<real_type> u(num_obs);
for (size_type i = 0; i < num_obs; ++i)
{
u(i) = ::dcs::math::sign(::dcs::math::stats::rand(dist, urng));
}
// Generate random noise
::dcs::math::la::dense_vector<real_type> e(num_obs);
for (size_type i = 0; i < num_obs; ++i)
{
e(i) = 0.2*::dcs::math::stats::rand(dist, urng);
}
::std::cout << "RLS with forgetting factor for SISO models:" << ::std::endl;
vector_type theta_hat;
matrix_type P;
vector_type phi;
::std::cout << "N_A: " << n_a << ::std::endl;
::std::cout << "N_B: " << n_b << ::std::endl;
::std::cout << "D: " << delay << ::std::endl;
::dcs::sysid::rls_arx_siso_init(n_a, n_b, delay, theta_hat, P, phi);
::std::cout << "\tInput Data: " << u << ::std::endl;
::std::cout << "\tNoise Data: " << e << ::std::endl;
::std::cout << "\tInitial Estimated Parameters: " << theta_hat << ::std::endl;
::std::cout << "\tInitial Covariance Matrix: " << P << ::std::endl;
::std::cout << "\tInitial Regressor: " << phi << ::std::endl;
vector_type y;
y = ::dcs::sysid::simulate(siso_model, u, e);
for (size_type i = 0; i < num_obs; ++i)
{
::std::cout << "\n\tObservation #" << i << ::std::endl;
::dcs::sysid::rls_ff_arx_siso(y(i), u(i), ff, n_a, n_b, delay, theta_hat, P, phi);
//real_type y_hat = ::dcs::math::la::inner_prod(theta_hat, phi);
::std::cout << "\t\tInput Data: " << u(i) << ::std::endl;
::std::cout << "\t\tOutput Data: " << y(i) << ::std::endl;
::std::cout << "\t\tEstimated Parameters: " << theta_hat << ::std::endl;
::std::cout << "\t\tCovariance Matrix: " << P << ::std::endl;
::std::cout << "\t\tRegressor: " << phi << ::std::endl;
//::std::cout << "\t\tEstimated Output Data: " << y_hat << ::std::endl;
//::std::cout << "\t\tRelative Error: " << ::std::abs((y(i)-y_hat)/y(i)) << ::std::endl;
}
// ::std::cout << "Simulation without noise: " << ::std::endl;
// ::std::cout << "\tInput Data: " << u << ::std::endl;
// ::std::cout << "\tOutput Data: " << y << ::std::endl;
//
// y = ::dcs::sysid::simulate(siso_model, u, e);
// ::std::cout << "Simulation with noise: " << ::std::endl;
// ::std::cout << "\tInput Data: " << u << ::std::endl;
// ::std::cout << "\tNoise Data: " << e << ::std::endl;
// ::std::cout << "\tOutput Data: " << y << ::std::endl;
}
