clc
clear
close all
addpath(genpath('../'))

%% LOAD HAND MODEL

load('hand_model.mat');

[hand_model] = reindex_fullhand(hand_model);

hand_model.global_pose = eye(4);

%% ROTATION

delta_theta = {zeros(4,1), zeros(4,1), zeros(4,1), 0*[0.5;0.0;0.4;0.5], zeros(4,1)};

pose.global_rotation = [0.0;0.0;pi];
pose.global_translation = [0;0;0];
[ hand_model ] = pose_model(hand_model, delta_theta,pose );

backup_hand_model = hand_model;

%% DISPLAY HAND MODEL

display_model(hand_model, 0.9, 'big')
mypoints({[10;5;-30]}, [0.3, 0.8, 0.3], 20);



 pose.global_rotation = [0.0;0.0;0.0];
 pose.global_translation = [12.6262; -3.7320; 17.3163];
 [ hand_model ] = pose_model(hand_model, delta_theta,pose );


% pose.global_rotation = [0.0;0.0;0.0];
% pose.global_translation = [2.1313; -2.5005; 6.7003];
% [ hand_model ] = pose_model(hand_model, delta_theta,pose );
% 
% pose.global_rotation = [0.0;0.0;0.0];
% pose.global_translation = [20.0; 0.0; 0.0];
% [ hand_model ] = pose_model(hand_model, delta_theta,pose );

pose.global_rotation = [0.0;pi;0.0];
pose.global_translation = zeros(3,1);
[ hand_model ] = pose_model(hand_model, delta_theta,pose );

pose.global_rotation = zeros(3,1);
pose.global_translation = [12.0;40.5;10.0];
[ hand_model ] = pose_model(hand_model, delta_theta,pose );


% pose.global_rotation = [0.0;-0.9;0.0];
% pose.global_translation = zeros(3,1);
% [ hand_model ] = pose_model(hand_model, delta_theta,pose );

% pose.global_rotation = zeros(3,1);
% pose.global_translation = -[12.0;40.5;10.0];
% [ hand_model ] = pose_model(hand_model, delta_theta,pose );






%% DISPLAY POSED HAND MODEL

display_model(hand_model, 0.9, 'big')
mypoints({[10;5;-30]}, [0.3, 0.8, 0.3], 20);