A simple walking robot - last link with wrong orientation

[Thomas Barbier]

Hi there,

I'm trying to get familiar with the robot kinematics before going further and I'm trying to apply the example of "A simple Walking Robot" (p.221). However, when I try to construct the leg as in the book, I end up with the last link with a wrong orientation (see the file 'leg.jpg'), while all the outputs are correct according to the output of the book.

>> s = 'Rz(q1) Rx(q2) Ty(L1) Rx(q3) Tz(L2)'

s =

    'Rz(q1) Rx(q2) Ty(L1) Rx(q3) Tz(L2)'

>> dh = DHFactor(s)

INIT: Rz(q1) Rx(q2) Ty(L1) Rx(q3) Tz(L2)

PARSED: Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)

Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)

initial merge + swap

Rz(q1).Rx(q2).Ty(L1).Rx(q3).Tz(L2)

ReplaceToZ: Rx(q2) := Ry(+90)Rz(q2)Ry(-90)

ReplaceToZ: Rx(q3) := Ry(+90)Rz(q3)Ry(-90)

joint vars to Z

Rz(q1).Ry(+90).Rz(q2).Ry(-90).Ty(L1).Ry(+90).Rz(q3).Ry(-90).Tz(L2)

0---------------------------------------

Swap: Ry(-90) <-> Ty(L1)

Eliminate: Ry(-90) Ry(+90)

ReplaceY: Ry(-90)Tz(L2) := Tx(-L2)Ry(-90)

Rz(q1).Ry(+90).Rz(q2).Ty(L1).Rz(q3).Tx(-L2).Ry(-90)

1---------------------------------------

Rz(q1).Ry(+90).Rz(q2).Ty(L1).Rz(q3).Tx(-L2).Ry(-90)

1---------------------------------------

** deal with Ry/Ty

ReplaceToZ2: Ry(+90) := Rz(+90)Rx(+90)Rz(-90)

ReplaceToZ2: Ty(L1) := Rz(+90)Tx(L1)Rz(-90)

ReplaceToZ2: Ry(-90) := Rz(+90)Rx(-90)Rz(-90)

Merge: Rz(q1) Rz(+90) := Rz(q1+90)

Merge: Rz(-90) Rz(q2) := Rz(q2-90)

Merge: Rz(-90) Rz(q3) := Rz(q3-90)

Merge: Rz(q2-90) Rz(+90) := Rz(q2)

Rz(q1+90).Rx(+90).Rz(q2).Tx(L1).Rz(q3-90).Tx(-L2).Rz(+90).Rx(-90).Rz(-90)

1---------------------------------------

Rz(q1+90).Rx(+90).Rz(q2).Tx(L1).Rz(q3-90).Tx(-L2).Rz(+90).Rx(-90).Rz(-90)

1---------------------------------------

** deal with Ry/Ty

Rz(q1+90).Rx(+90).Rz(q2).Tx(L1).Rz(q3-90).Tx(-L2).Rz(+90).Rx(-90).Rz(-90)

  adding: DH(null, 0, 0, 0) += Rz(q1+90)

  adding: DH(q1+90, 0, 0, 0) += Rx(+90)

  adding: DH(null, 0, 0, 0) += Rz(q2)

  adding: DH(q2, 0, 0, 0) += Tx(L1)

  adding: DH(null, 0, 0, 0) += Rz(q3-90)

  adding: DH(q3-90, 0, 0, 0) += Tx(-L2)

DH(q1+90, 0, 0, 90).DH(q2, 0, L1, 0).DH(q3-90, 0, -L2, 0).Rz(+90).Rx(-90).Rz(-90)

In DHFactor, parseString is done

dh =

DH(q1+90, 0, 0, 90).DH(q2, 0, L1, 0).DH(q3-90, 0, -L2, 0).Rz(+90).Rx(-90).Rz(-90)

>> dh.command('leg')

ans =

SerialLink([0, 0, 0, pi/2, 0; 0, 0, L1, 0, 0; 0, 0, -L2, 0, 0; ], 'name', 'leg', 'base', eye(4,4), 'tool', trotz(pi/2)*trotx(-pi/2)*trotz(-pi/2), 'offset', [pi/2 0 -pi/2 ])

>> leg = eval(dh.command('leg'))

 

leg = 

 

leg:: 3 axis, RRR, stdDH, slowRNE                                

+---+-----------+-----------+-----------+-----------+-----------+

| j |     theta |         d |         a |     alpha |    offset |

+---+-----------+-----------+-----------+-----------+-----------+

|  1|         q1|          0|          0|     1.5708|     1.5708|

|  2|         q2|          0|        0.1|          0|          0|

|  3|         q3|          0|       -0.1|          0|    -1.5708|

+---+-----------+-----------+-----------+-----------+-----------+

tool:    t = (0, 0, 0), RPY/xyz = (0, -90, 0) deg                

 

>> leg.plot([0,0,0], 'nobase', 'noshadow','notiles')

>> set(gca, 'Zdir', 'reverse'); view(137,48);

I suspect the convention change from zyx to xyz, but I couldn't find a string of joints and link that would solve the problem. My second guess is a plot 'bug'. Does anyone encountered the problem and/or knows how to fix it?

I am using Matlab 2018b and the latest version of the robotic toolbox (10.3.1).

Thank you for your help.

Regards,

Thomas

[Thomas Barbier]

Hi everyone.

I finally found where this came from. This is a plotting bug that comes from the file rvctools\robot\@SerialLink\animate.m. Line 114 is commented when it should not, leading to an improper rotation of the final element.

@Peter: I tried to find where does that comes from but I couldn't find any commit on Github where this wild comment appears. However, it is there when the toolbox is downloaded from here: http://petercorke.com/wordpress/toolboxes/robotics-toolbox.

Hope that helps.

Regards,

Thomas

[Erik van Oene]

Good job of finding the bug... !

Erik

[Peter Corke]

Hi Thomas and thanks for digging into the code:

is that the line

T = T*robot.tool;

I can only assume I was fiddling and that got into the last distro.  My working copy on GH does not have this line commented.  Sorry for the time wasting.

peter

[Thomas Barbier]

Hi,

Yes, it's exactly that line. I should have mentioned it.

Regards,

Thomas

[dongchen]

Hi thomas,

thank you for your correction. It helps me.

Dongchen

在 2019年2月20日星期三 UTC+8上午6:33:49，Thomas Barbier写道：

[dongchen]

Hi  Thomsas,

i would like to know if you have created the motion of 4 legs successfully. I try the code from page 222 to page 224, everything goes well, motion of one leg works. However, when i try the code to create the motion of four legs on page 224 and 225, matlab create a wrong simulation, as bellow. Did you have the same problem? 

I use Robotics Toolbox for MATLAB (release 10.3.1), matlab R2018B.

here is the code:

%kinematics

s = 'Rz(q1) Rx(q2) Ty(L1) Rx(q3) Tz(L2)';

dh = DHFactor(s);

dh.tool;

dh.command('leg');

L1 = 0.1; L2 = 0.1;

leg = eval( dh.command('leg') );

transl( leg.fkine([0,0,0]) );

leg.plot([0,0,0], 'nobase', 'noshadow', 'notiles');    %animate, line 114, no comment

set(gca, 'Zdir', 'reverse'); view(137,48);

transl( leg.fkine([0.2,0,0]) );

transl( leg.fkine([0,0.2,0]) );

transl( leg.fkine([0,0,0.2]) );

%motion of one leg

xf = 50; xb = -xf; y = 50; zu = 20; zd = 50 ;

path = [xf y zd; xb y zd; xb y zu; xf y zu; xf y zd] * 1e-3 ;

p = mstraj(path, [], [0, 3, 0.25, 0.5, 0.25]', path(1,:), 0.01, 0);

qcycle = leg.ikine( SE3(p), 'mask', [1 1 1 0 0 0] );

leg.plot(qcycle, 'loop') ;

%motion of four legs

W = 0.1; L = 0.2;

legs(1) = SerialLink(leg, 'name', 'leg1');

legs(2) = SerialLink(leg, 'name', 'leg2', 'base', SE3(-L, 0, 0));

legs(3) = SerialLink(leg, 'name', 'leg3', 'base', SE3(-L, -W, 0)*SE3.Rz(pi));

legs(4) = SerialLink(leg, 'name', 'leg4', 'base', SE3(0, -W, 0)* SE3.Rz(pi));

clf; k = 1;

while 1

legs(1).plot( gait(qcycle, k, 0,   false) );

if k == 1, hold on; end

legs(2).plot( gait(qcycle, k, 100, false) );

legs(3).plot( gait(qcycle, k, 200, true) );

legs(4).plot( gait(qcycle, k, 300, true) );

drawnow

k = k+1;

end

Thank you in advance.

Dongchen

[Peter Corke]

The version in the book is bare bones, a more complete example is shipped with the toolbox examples/walking.m, this is mention right at the end of the section.  Just noticed a bug in it, change line 79 to

tseg = [tseg; tseg];

peter