% Creator := Javier Dario Sanjuan De Caro
% email := jsanjuan@uwm.edu/jdecaro@uninorte.edu.co
% Function := RobotShow
% Description := This algorithm calls the URDF file and show the robot. Use
% testTheta to change the values of each joint. 
%
clear
clc
close all
robot=importrobot('.\SolidWorks\Assem1\urdf\Assem1.urdf');
Robot = interactiveRigidBodyTree(robot);
ax = gca;
ax.ZLim = [-1.5 1.5];
ax.YLim = [-3 3];
ax.XLim = [-3 3];
set(gcf, 'Position', get(0, 'Screensize'));
Robot.ShowMarker = false;  % Hide the marker
showFigure(Robot)
zoom on
zoom(1)
Fixrotation = [1 1];
testTheta = [0 0];
Robot.Configuration = deg2rad(testTheta)';
load('ysolJeffChristoffel01');
for i=1:length(ysolJeffVar)
    theta = ysolJeffVar(2:3, i)';
    Robot.Configuration = theta; % deg2rad(theta)';
    pause(0.5)
end