fork download
copy 
  1. function [link_set, R_joints, R_links, link_set_local, link_vectors_in_world, links_in_world, link_end_set, link_end_set_with_base] = planar_robot_arm_links(link_vectors, joint_angles)
  2.     % Initialize the number of links
  3.     n = length(link_vectors);
  4.  
  5.     % Initialize cell arrays for outputs
  6.     R_joints = cell(n, 1);
  7.     R_links = cell(n, 1);
  8.     link_set_local = cell(n, 1);
  9.     link_vectors_in_world = cell(n, 1);
  10.     links_in_world = cell(n, 1);
  11.     link_end_set = cell(n, 1);
  12.  
  13.     % First step: Generate rotation matrices for the joints
  14.     for i = 1:n
  15.         R_joints{i} = planar_rotation_set(joint_angles(i));
  16.     end
  17.  
  18.     % Second step: Generate cumulative product of joint rotation matrices
  19.     R_links{1} = R_joints{1}; % The first link's orientation is just its rotation
  20.     for i = 2:n
  21.         R_links{i} = R_links{i-1} * R_joints{i}; % Cumulative product
  22.     end
  23.  
  24.     % Third step: Generate local link endpoints
  25.     for i = 1:n
  26.         link_set_local{i} = [zeros(2, 1), link_vectors{i}]; % [0; 0] and link vector
  27.     end
  28.  
  29.     % Fourth step: Rotate link vectors to world coordinates
  30.     for i = 1:n
  31.         link_vectors_in_world{i} = R_links{i} * link_vectors{i}; % Rotate link vectors
  32.     end
  33.  
  34.     % Fifth step: Calculate the world positions of the links
  35.     for i = 1:n
  36.         if i == 1
  37.             links_in_world{i} = link_set_local{i}; % First link
  38.         else
  39.             % Previous link's end position
  40.             prev_end_pos = link_end_set{i-1}(:, end);
  41.             links_in_world{i} = prev_end_pos + link_vectors_in_world{i}; % Translate by previous end position
  42.         end
  43.     end
  44.  
  45.     % Sixth step: Cumulative sum of link vectors to get endpoints
  46.     link_end_set{1} = link_vectors_in_world{1}; % First link endpoint
  47.     for i = 2:n
  48.         link_end_set{i} = link_end_set{i-1} + link_vectors_in_world{i}; % Cumulative sum
  49.     end
  50.  
  51.     % Seventh step: Add the base point (origin) to link_end_set
  52.     link_end_set_with_base = cell(n, 1);
  53.     link_end_set_with_base{1} = [zeros(2, 1), link_end_set{1}]; % Origin to first endpoint
  54.     for i = 2:n
  55.         link_end_set_with_base{i} = [link_end_set_with_base{i-1}(:, end), link_end_set{i}]; % Append each endpoint
  56.     end
  57.  
  58.     % Eighth step: Generate the final link set with the start and endpoints
  59.     link_set = cell(n, 1);
  60.     for i = 1:n
  61.         link_set{i} = links_in_world{i}; % Combine the basepoint and endpoints
  62.     end
  63. end
  64. link_vectors = {[1; 0], [1; 0]};
  65. joint_angles = [pi/4; -pi/2];
  66. link_set = planar_robot_arm_links(link_vectors, joint_angles);
  67. disp(link_set{:});
  68.  
Success #stdin #stdout #stderr 0.1s 49428KB
comments (?)
stdin
copy 
Standard input is empty
stdout
copy 
Standard output is empty
stderr
copy 
error: 'planar_rotation_set' undefined near line 15 column 22
error: called from
    planar_robot_arm_links at line 15 column 20
    /home/fRkdGa/prog.octave at line 66 column 10
https://ideone.com/Uaefo2
language:
Octave (octave 4.4.1)
created:
8 days ago
visibility:
public

Share or Embed source code

Discover > Sphere Engine API

The brand new service which powers Ideone!

Discover > IDE Widget

Widget for compiling and running the source code in a web browser!