forked from A2R-Lab/GRiD
-
Notifications
You must be signed in to change notification settings - Fork 0
/
printGRiD.cu
88 lines (65 loc) · 3.27 KB
/
printGRiD.cu
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/***
nvcc -std=c++11 -o printGRiD.exe printGRiD.cu -gencode arch=compute_86,code=sm_86
***/
#include <random>
#include <algorithm>
#include "grid.cuh"
#define RANDOM_MEAN 0
#define RANDOM_STDEV 1
std::default_random_engine randEng(1337); // fixed seed
std::normal_distribution<double> randDist(RANDOM_MEAN, RANDOM_STDEV); //mean followed by stdiv
template <typename T>
T getRand(){return static_cast<T>(randDist(randEng));}
template <typename T>
__host__
void test(){
T gravity = static_cast<T>(9.81);
dim3 dimms(grid::SUGGESTED_THREADS,1,1);
cudaStream_t *streams = grid::init_grid<T>();
grid::robotModel<T> *d_robotModel = grid::init_robotModel<T>();
grid::gridData<T> *hd_data = grid::init_gridData<T,1>();
for (int j = 0; j < grid::NUM_JOINTS; j++) hd_data->h_q_qd_u[j] = getRand<double>();
for (int j = 0; j < grid::NUM_VEL; j++) hd_data->h_q_qd_u[j+grid::NUM_JOINTS] = getRand<double>();
for (int j = 0; j < grid::NUM_VEL; j++) hd_data->h_q_qd_u[j+grid::NUM_JOINTS+grid::NUM_VEL] = static_cast<T>(0);
gpuErrchk(cudaMemcpy(hd_data->d_q_qd_u,hd_data->h_q_qd_u,3*grid::NUM_JOINTS*sizeof(T),cudaMemcpyHostToDevice));
gpuErrchk(cudaDeviceSynchronize());
printf("q,qd,u\n");
printMat<T,1,grid::NUM_JOINTS>(hd_data->h_q_qd_u,1);
printMat<T,1,grid::NUM_VEL>(&hd_data->h_q_qd_u[grid::NUM_JOINTS],1);
printMat<T,1,grid::NUM_VEL>(&hd_data->h_q_qd_u[grid::NUM_JOINTS+grid::NUM_VEL],1);
grid::crba<T>(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams);
printf("crba\n");
printMat<T,grid::NUM_JOINTS,grid::NUM_JOINTS>(hd_data->h_M,grid::NUM_JOINTS);
printf("c\n");
grid::inverse_dynamics<T,false,false>(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams);
printMat<T,1,grid::NUM_VEL>(hd_data->h_c,1);
printf("Minv\n");
grid::direct_minv<T,false>(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams);
printMat<T,grid::NUM_VEL,grid::NUM_VEL>(hd_data->h_Minv,grid::NUM_VEL);
printf("qdd\n");
grid::forward_dynamics<T>(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams);
printMat<T,1,grid::NUM_VEL>(hd_data->h_qdd,1);
grid::inverse_dynamics_gradient<T,true,false>(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams);
printf("dc_dq\n");
printMat<T,grid::NUM_VEL,grid::NUM_VEL>(hd_data->h_dc_du,grid::NUM_VEL);
printf("dc_dqd\n");
printMat<T,grid::NUM_VEL,grid::NUM_VEL>(&hd_data->h_dc_du[grid::NUM_VEL*grid::NUM_VEL],grid::NUM_VEL);
grid::forward_dynamics_gradient<T,false>(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams);
printf("df_dq\n");
printMat<T,grid::NUM_VEL,grid::NUM_VEL>(hd_data->h_df_du,grid::NUM_VEL);
printf("df_dqd\n");
printMat<T,grid::NUM_VEL,grid::NUM_VEL>(&hd_data->h_df_du[grid::NUM_VEL*grid::NUM_VEL],grid::NUM_VEL);
// printf("eePos\n");
// grid::end_effector_positions<T,false>(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams);
// printMat<T,1,6*grid::NUM_EES>(hd_data->h_eePos,1);
// printf("deePos\n");
// grid::end_effector_positions_gradient<T,false>(hd_data,d_robotModel,1,dim3(1,1,1),dimms,streams);
// printMat<T,6,grid::NUM_EES*grid::NUM_JOINTS>(hd_data->h_deePos,6);
// grid::aba<T>(hd_data,d_robotModel,gravity,1,dim3(1,1,1),dimms,streams);
// printf("aba\n");
// printMat<T,1,grid::NUM_JOINTS>(hd_data->h_qdd,1);
grid::close_grid<T>(streams,d_robotModel,hd_data);
}
int main(void){
test<float>(); return 0;
}