For loop and calculating at each interval of t

I just want to know how can I rewrite the code so that the for loop will include a correct dhparams. Since I have 26 values of theta1,theta2,theta3 how can I have my dhparams have the first value and undergo each calculation with the intervals as follows. I want to avoid cell arrays errors ;

->When t = 0

theta1 = -2, theta2= 2, theta 3= 3

theta4= -2, theta5= 2, theta6= 3

Therefore for t=0;

dhparams = {-2,0,145,0;
            2+pi/2,pi/2,145,0;
            3+pi/2,0,0,260;
            -2,pi/2,40,30;
            2,-pi/2,230,0;
            3,pi/2,90,0};

Then this point would be plotted in the axis.

->Then I want this to happen with the next interval at t=0.2;

theta1 = -2.98, theta2= 1.984, theta 3= 2.88

theta4= -2.98, theta5= 1.984, theta6= 2.88

Therefore for t=0.2;

dhparams = {-2.98,0,145,0;
            1.984+pi/2,pi/2,145,0;
            2.88+pi/2,0,0,260;
            -2.98,pi/2,40,30;
            1.984,-pi/2,230,0;
            2.88,pi/2,90,0};

Then this point would be plotted in the axis.

I want this to contiune until t=5 and the figure to have the complete plot.

This is the full code.

theta1 = zeros(1,26)         ;
theta2 = zeros(1,26)         ;
theta3= zeros(1,26)          ;
t = 0:0.2:5                  ;
for k= 1:26            
    theta1(k) = -2*(1-0.1*(k));  
    theta2(k) = 2*(1-0.04*(k));
    theta3(k) = 3*(1-0.2.*(k));
    theta4=theta1(k);
    theta5=theta2(k);
    theta6=theta3(k);
end  
dhparams = {theta1,0,145,0;
            theta2+pi/2,pi/2,145,0;
            theta3+pi/2,0,0,260;
            theta4,pi/2,40,30;
            theta5,-pi/2,230,0;
            theta6,pi/2,90,0};
robot = rigidBodyTree;
body1 = rigidBody('body1');
jnt1 = rigidBodyJoint('jnt1','revolute');

setFixedTransform(jnt1,dhparams(1,:),'dh');
body1.Joint = jnt1;

addBody(robot,body1,'base')
body2 = rigidBody('body2');
jnt2 = rigidBodyJoint('jnt2','revolute');
body3 = rigidBody('body3');
jnt3 = rigidBodyJoint('jnt3','revolute');
body4 = rigidBody('body4');
jnt4 = rigidBodyJoint('jnt4','revolute');
body5 = rigidBody('body5');
jnt5 = rigidBodyJoint('jnt5','revolute');
body6 = rigidBody('body6');
jnt6 = rigidBodyJoint('jnt6','revolute');

setFixedTransform(jnt2,dhparams(2,:),'dh');
setFixedTransform(jnt3,dhparams(3,:),'dh');
setFixedTransform(jnt4,dhparams(4,:),'dh');
setFixedTransform(jnt5,dhparams(5,:),'dh');
setFixedTransform(jnt6,dhparams(6,:),'dh');

body2.Joint = jnt2;
body3.Joint = jnt3;
body4.Joint = jnt4;
body5.Joint = jnt5;
body6.Joint = jnt6;

addBody(robot,body2,'body1')
addBody(robot,body3,'body2')
addBody(robot,body4,'body3')
addBody(robot,body5,'body4')
addBody(robot,body6,'body5')
showdetails(robot)
show(robot);
axis([-10,10,-10,10,-10,10])

Answer 1

[Image Analyst]

Put the theta into a vector and then use them in the loop to build dhparams:

theta = -3 + 6 * rand(1, 26); % Whatever they are....
for k = 1 : numel(theta) - 6
    dhparams{k} = [-theta(k),0,145,0;
        theta(k+1)+pi/2,pi/2,145,0;
        theta(k+2)+pi/2,0,0,260;
        theta(k+3),pi/2,40,30;
        theta(k+4),-pi/2,230,0;
        theta(k+5),pi/2,90,0];
end
celldisp(dhparams)

[Andre Khosrow]

I did as you suggested and it only included 20 dhparams not 26. The plot also didn't open.

[Image Analyst]

I don't understand the rule for which set of 6 thetas you're using, out of the 26 total thetas, for each iteration to get the first column of dhparams. Please edit what I've done and if the theta's aren't right, let me know what they should be.

[Andre Khosrow]

The theta's have a formula, (i had k in there for the loop) . t = 0:0.2:5. So for the dhparams the matrix would have 26 values in each row of the first column.

theta1 = -2*(1-0.1*(t));  
theta2 = 2*(1-0.04*(t));
theta3 = 3*(1-0.2.*(t));
theta4 = theta1;
theta5= theta2;
theta6= theta3;

[Image Analyst]

But you're overwriting theta 4, 5, and 6 in your for loop. Wouldn't you want an index for them too?

t = 0:0.2:5                  ;
for k= 1:26            
    theta1(k) = -2*(1-0.1*(k));  
    theta2(k) = 2*(1-0.04*(k));
    theta3(k) = 3*(1-0.2.*(k));
    theta4(k)=theta1(k);
    theta5(k)=theta2(k);
    theta6(k)=theta3(k);
end