% This class creates a bridge between matlab and ros for controlling the
% AMBF simulation for the galen robot. The simulation will be controlled
% via class methods in found in the galen_interface objects.
% These functions will move the robot according to velocity, position, or
% effort commands defined in the set_control_type functions. This interface
% is only intended to control the robot in simulation.
% To use this class to control the robot,ensure ROS is running by running
% roscore in a terminal and then open the ambf model in a separate terminal
% Instructions on how to install the AMBF application and how to use the
% robot with ambf are found in:

% TOGAC25_AMBF_IN_AMBFforGalen.pdf

% Instructions for using matlab with ambf are found in:

% TOGAC29_AMBF_IN_ConnectMatlab2AMBF.pdf

% Modified 03-24-2023
% Created by Brevin Banks



classdef galen_interface < handle


    % These settings will not change after class construction
    properties (SetAccess = immutable)
        % Joint limits, speed limits, acceleration limits, home pose
        % information should go here

        joint_names = {...
            'carriage1_joint',...
            'carriage2_joint',...
            'elbow_joint',...
            'roll_joint',...
            'TiltDistalLinkageandForceSensor-Endoscope35degreeinREMS',...
            'tilt_joint'};

        children_names = {...
            'Carriage1',...
            'Carriage2',...
            'Carriage3',...
            'RollArmBase',...
            'Endoscope35degreeinREMS',...
            'TiltProximalLinkage'};
        % From AMBF ADF model
        Body_masses = [0 10 10 10 10 10 10 10 10 10 0 0.5 0.1 5 5 5 5 5 5 10 5 0.5 0.5 0.5 1];
        Body_names = {'Base','Carriage1','Carriage1_lh','Carriage1_rh', ...
            'Carriage2','Carriage2_lh','Carriage2_rh', ...
            'Carriage3','Carriage3_lh','Carriage3_rh', ...
            'Chassis', 'Endoscope 35 degree in REMS','Endoscope Tip', ...
            'Leg1_l','Leg1_r','Leg2_l','Leg2_r','Leg3_l','Leg3_r', ...
            'Mobile Platform','Roll Arm Base', 'Roll Arm Housing', ...
            'Tilt Distal Linkage and Force Sensor', 'Tilt Middle Linkage', ...
            'Tilt proximal Linkage'};


    end

    % values set by this class, can be read by others
    properties (SetAccess = protected)
        galen_joint_states   % Last received galen joint_states
        E35state
        % Admittance Properties
        m_ad
        b_ad

        % The sum of the lumped mass for 3 carriage identical joint control
        Mext_Lumped_mass = 95; % kg
        % Mass and friction of one of the three carriage joints
        Single_carriage_mass = 30; % kg
        carriage_kinetic_friction = 0.1;
        % The angle of the carriage leg to the platform of the wrist when
        % all 3 carriage joints are held at the same reference pose
        carriage_triad_steady_angle = 1.842; % in rad

        g = 9.81; % gravity constant


        %PID gains from the ADF model
        P = 11000;
        I = 110;
        D = 110;

        T = 1/200; % desired sample time

    end

    % only this class methods can view/modify
    properties (SetAccess = private)

        % subscribers
        galen_joint_states_sub
        E35_wrench_sub
        % Individual frame subs

        % publishers
        galen_joint_cmd_pub

        % messages
        galen_command_message

        % Properties from messages
        JointCntrlType
        JointMoveCmd

        galen_joint_positions
        
        prev_cmd
        Ts
    end

    methods
        % constructor
        function self = galen_interface()
            % stop and start the ross communication

            % setenv('ROS_MASTER_URI','http://10.0.0.192:11311')
            % setenv('ROS_IP','10.0.0.141') %Home
            % setenv('ROS_MASTER_URI','http://10.0.0.192:11311')
            % setenv('ROS_MASTER_URI','http://ubuntu:11311')
            setenv('ROS_IP','10.203.104.9') %Campus
            
            setenv('ROS_MASTER_URI','http://192.168.127.148:11311')
            rosshutdown;
            fprintf('Shutdown... \n')
            % rosinit('http://ubuntu:11311/'); % may need to change depending on how you choose to speak with ros

            rosinit('http://192.168.127.148:11311/');

            % rosinit('http://10.0.0.192:11311')
            fprintf('Initialized...')
            % for rosinit you need to include the name of the ros master
            % node or IP address. See rosinit documentation for more
            % details.
            fprintf('Creating Subscribers... ')
            % subscribers
            self.galen_joint_states_sub = rossubscriber('/ambf/env/Base/State','ambf_msgs/RigidBodyState',"DataFormat","struct");
            self.E35_wrench_sub = rossubscriber('/ambf/env/Endoscope35degreeinREMS/State','ambf_msgs/RigidBodyState',"DataFormat","struct");
            fprintf('Creating Publishers... ')
            % publishers
            self.galen_joint_cmd_pub = rospublisher('/ambf/env/Base/Command','ambf_msgs/RigidBodyCmd',"DataFormat","struct");

            fprintf('Receiving Initial States... ')
            % Grab the current state of the robot
            self.galen_joint_states = receive(self.galen_joint_states_sub);
            fprintf('\n   Joint State received \n ')
            self.E35state = receive(self.E35_wrench_sub);
            fprintf('\n   Initial Force State received \n ')
            fprintf('Creating message.. ')
            % Initiate messages
            self.galen_command_message = rosmessage('ambf_msgs/RigidBodyCmd',"DataFormat","struct");
            fprintf('Complete\n')
            % Initiate property values and types assume velocity commands
            self.JointMoveCmd = int8([0,0,0,0,0,0]);
            self.JointCntrlType = single([2,2,2,2,2,2]);

            % For implimentation of admittance controller in the future
            % within this class we can assign the admittance properites
            self.m_ad = 5;
            self.b_ad = 100;

            self.galen_joint_positions = [0,0,0,0,0,0];
            self.Ts = 0;
            self.prev_cmd = single([0,0,0,0,0,0]);
        end

        % Set the control type for all joints
        % Inputs:
        %   control_type - a 1x6 vector of with the desired int
        %   representing control type or a char string
        function set_control_type(self,control_type)

            if isa(control_type,'char')
                switch control_type
                    case 'Effort'
                        type_message = [0,0,0,0,0,0];
                    case 'Position'
                        type_message = [1,1,1,1,1,1];
                    case 'Velocity'
                        type_message = [2,2,2,2,2,2];
                    otherwise
                        type_message = [2,2,2,2,2,2];
                end
            elseif isa(control_type,'double')
                type_message = control_type;
            end

            type_message = int8(type_message);
            self.galen_command_message.JointCmdsTypes = type_message;
            self.galen_command_message.JointCmds=single(self.JointMoveCmd);
            send(self.galen_joint_cmd_pub,self.galen_command_message);
            self.JointCntrlType  = type_message;

        end

        % grab the galen joint states from the state subscriber
        function joint_states = get_current_joints(self)

            self.galen_joint_states = receive(self.galen_joint_states_sub);
            switch self.JointCntrlType(1)
                case 0
                    joint_states = self.galen_joint_states.JointEfforts;
                case 1
                    joint_states = self.galen_joint_states.JointPositions;
                case 2
                    joint_states = self.galen_joint_states.JointVelocities;
            end

        end

        function joint_positions = extract_galen_pose(self)
            self.galen_joint_positions = self.galen_joint_states.JointPositions;
            joint_positions = self.galen_joint_positions;
        end

        % Move the galen robot given the command control type
        % Input:
        %   jointcmd - a 6x1 vector of desired states
        function move_galen(self,jointcmd)
            jointcmd = self.limiter(jointcmd);
            self.galen_command_message.JointCmds = jointcmd;
            send(self.galen_joint_cmd_pub,self.galen_command_message);
            self.prev_cmd = jointcmd;
        end


        function Fk = galen_fk(self,q_state)
            if nargin==2
                theta1 = q_state(1);
                theta2 = q_state(2);
                theta3 = q_state(3);
                theta4 = q_state(4);
                theta5 = q_state(5);
            else
                self.extract_galen_pose();
                theta1 = double(self.galen_joint_positions(1));
                theta2 = double(self.galen_joint_positions(2));
                theta3 = double(self.galen_joint_positions(3));
                theta4 = double(self.galen_joint_positions(5));
                theta5 = double(self.galen_joint_positions(6));
            end
   

            Fk = galen_fk2([theta1,theta2,theta3,theta4,theta5]);

        end

        function J = galen_jacob(self,q_state)
            if nargin==2
                theta1 = q_state(1);
                theta2 = q_state(2);
                theta3 = q_state(3);
                theta4 = q_state(4);
                theta5 = q_state(5);
            else
                self.extract_galen_pose();
                theta1 = double(self.galen_joint_positions(1));
                theta2 = double(self.galen_joint_positions(2));
                theta3 = double(self.galen_joint_positions(3));
                theta4 = double(self.galen_joint_positions(5));
                theta5 = double(self.galen_joint_positions(6));
            end

            J = galen_jacob2([theta1,theta2,theta3,theta4,theta5]);
            J(1:3,4:5) = zeros(3,2);
            J = round(J,4);
        end


        function Wrench = read_E35_wrench(self)
            Wrench = zeros(6,1);
            self.E35state = receive(self.E35_wrench_sub);
            Wrench(1) = self.E35state.Wrench.Force.X;
            Wrench(2) = self.E35state.Wrench.Force.Y;
            Wrench(3) = self.E35state.Wrench.Force.Z;
            Wrench(4) = self.E35state.Wrench.Torque.X;
            Wrench(5) = self.E35state.Wrench.Torque.Y;
            Wrench(6) = self.E35state.Wrench.Torque.Z;
        end


        function jointcmd = limiter(self,jointcmd)
            if self.Ts>5
                tic
                self.Ts = toc;
            else
                self.Ts = toc;
            end
            %limit acceleration
          
            for k=1:6
                if k<4
                    if (jointcmd(k)-self.prev_cmd(k))/self.Ts>200
                        jointcmd(k) = 200*self.Ts+self.prev_cmd(k);
                    elseif (jointcmd(k)-self.prev_cmd(k))/self.Ts<-200
                        jointcmd(k) = -200*self.Ts+self.prev_cmd(k);
                    end
                else
                    if (jointcmd(k)-self.prev_cmd(k))/self.Ts>45*pi/180
                        jointcmd(k) = (45*pi/180)*self.Ts+self.prev_cmd(k);
                    elseif (jointcmd(k)-self.prev_cmd(k))/self.Ts<-45*pi/180
                        jointcmd(k) = (-45*pi/180)*self.Ts+self.prev_cmd(k);
                    end
                end
            end
            
            %limit velocity
            if jointcmd(1)>0.04
                jointcmd(1) = 0.04;
            elseif jointcmd(1)<-.04
                jointcmd(1) = -0.04;
            end
            if jointcmd(2)>0.04
                jointcmd(2) = 0.04;
            elseif jointcmd(2)<-.04
                jointcmd(2) = -0.04;
            end
            if jointcmd(3)>0.04
                jointcmd(3) = 0.04;
            elseif jointcmd(3)<-.04
                jointcmd(3) = -0.04;
            end
            if jointcmd(4)>45*pi/180
                jointcmd(4) = 45*pi/180;
            elseif jointcmd(4)<-45*pi/180
                jointcmd(4) = -45*pi/180;
            end
            if jointcmd(5)>45*pi/180
                jointcmd(5) = 45*pi/180;
            elseif jointcmd(5)<-45*pi/180
                jointcmd(5) = -45*pi/180;
            end
            if jointcmd(6)>45*pi/180
                jointcmd(6) = 45*pi/180;
            elseif jointcmd(6)<-45*pi/180
                jointcmd(6) = -45*pi/180;
            end

        end

    end % methods

end % class