From 0276992d73d00675df6df2e234f6bce819da9884 Mon Sep 17 00:00:00 2001
From: cosmic_power <cosmic_power@163.com>
Date: Fri, 20 Sep 2024 00:29:22 +0800
Subject: [PATCH] add robot R1000

---
 get_robot_R1000.m | 79 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 79 insertions(+)
 create mode 100644 get_robot_R1000.m

diff --git a/get_robot_R1000.m b/get_robot_R1000.m
new file mode 100644
index 0000000..8b4b22c
--- /dev/null
+++ b/get_robot_R1000.m
@@ -0,0 +1,79 @@
+function robot = get_robot(file,opt)
+switch opt.robot_def
+    case 'direct'
+        ndof = 2;
+        robot.ndof = ndof;
+        % Kinematics parameters
+        if(opt.Isreal)
+            switch opt.KM_method
+                case 'SDH'
+                case 'MDH'
+                    robot.theta = zeros([1,ndof]);
+                    robot.a = [0,1,1];
+                    robot.d = [0,0,0];
+                    robot.alpha = [0,0,0];
+                otherwise
+                    disp('Bad opt.KM_method!')
+                    return;
+            end
+        else
+            % Create symbolic generilized coordiates, their first and second deriatives
+            q_sym = sym('q%d',[ndof+1,1],'real');
+            qd_sym = sym('qd%d',[ndof+1,1],'real');
+            q2d_sym = sym('qdd%d',[ndof+1,1],'real');
+            q_sym(ndof+1) = 0;
+            qd_sym(ndof+1) = 0;
+            q2d_sym(ndof+1) = 0;
+            robot.theta = q_sym;
+            robot.dtheta = qd_sym;
+            robot.ddtheta = q2d_sym;
+            switch opt.KM_method
+                case 'SDH'
+                case 'MDH'
+                    robot.a = [0,0.2,0.5,0.45,0.12,0,0,0];
+                    robot.d = [0,0,0,0,0,0,0,0];
+                    robot.alpha = [0,pi/2,0,0,-pi/2,-pi/2,-pi/2,-pi/2];
+                otherwise
+                    disp('Bad opt.KM_method!')
+                    return;
+            end
+        end
+        % Dynamics parameters
+        link_mass = [1,1,1,1,1,1,1,1];
+        %TODO in process, seems axis_of_rot useless 
+        axis_of_rot(:,:,1) = [0;0;1];
+        axis_of_rot(:,:,2) = [0;0;1];
+        % 画图
+        com_pos(:,:,1) = [1/2;0;0];
+        com_pos(:,:,2) = [1/2;0;0];
+        % the inertia tensor wrt the frame oriented as the body frame and with the
+        % origin in the COM
+        link_inertia(:,:,1) = diag([1,1,1]);
+        link_inertia(:,:,2) = diag([1,1,1]);
+        % manipulator regressor
+        for i = 1:ndof
+            robot.m(i) = link_mass(i);
+            robot.axis(:,i) = axis_of_rot(i);
+            robot.com(:,i) = com_pos(i);
+            robot.I(:,:,i) = link_inertia(i);
+            robot.mc(:,i) = link_mass*com_pos(i);
+            % the inertia tensor wrt the frame oriented as the body frame and with the
+            % origin in the Joint i
+            com_vec2mat = vec2skewSymMat(com_pos);
+            robot.I_vec(:,i) = inertiaMatrix2Vector(link_inertia-...
+                link_mass(i)*com_vec2mat*com_vec2mat);
+            robot.pi(:,i) = [robot.I_vec(:,i); robot.mc(:,i); robot.m(i)];
+        end
+    case 'urdf'
+        robot = parse_urdf(file);
+    case 'mat'
+        robot = [];
+        disp('TODO mat robot define options!')
+    otherwise
+        robot = [];
+        disp('Bad robot define options!')
+        return
+end
+%Gravity
+gravity = [0;0;9.8];
+robot.gravity = gravity;
\ No newline at end of file