Implementation of RRT or PRM with 3D maps and link arm robot

[Jordan Sorg]

Prof. Corke,

Thank you for adding me to the group. I am new to MATLAB and your Robotics toolbox. I was introduced to them at the same time and made good use of the SerialLink and ctraj, oa2r, ikine6s and fkine commands to make my virtual robot perform some interesting tasks.

The problem at hand it is to path plan an armed robot (Exact Dynamics iARM) around a 3D generated model of a skull. I have gone through chapter 5 of your book and ran the dstar, PRM, and RRT examples to get familiar as well as makemap() to generate a 2D obstacle course that resembled something like my problem. I am now trying to understand the Vehicle class to customize a robot that is a 3-link 6 DOF armed robot. Is this possible in the RTB 9.9? (Also, using MVTB 3.3) If it is not, is it possible to create multiple  vehicles that are linked together? Ideally I would like to use my robot below and plan obstacle free paths that it can follow.

This is attempt to create a 3D matrix of ones and zeros , i.e. a cubed obstacle in the center and then trying to plan around it with the RRT but when this is visualized the key points seem to be right where the cube is centered and around. The vehicle was just clipped from one of your examples.

%% 3d attempt

map3d = zeros(500,500,500);

map3d(150:350, 150:350, 150:350) =1;

close all

rrt=RRT(map3d, Vehicle([], 'stlim', 1.2));

rrt.plan();

rrt.visualize()

goal=[0 1.3 0]

start=[.4 .5 0]

p=rrt.path(start,goal)

about(p)

figure(1);

hold on

plot3(p(:,1), p(:,2), p(:,3), 'r', 'linewidth', 3)

WIll imorph work on a 3D obstacle? I have not tried this command yet.

As a side note, when plotting my linked robot now, the look of the figure is different than previous and a warning (below) is thrown which only started to happen since I downloaded the machine vision toolbox. Any thoughts?

%% ARMED Robot

L(1)=Link([0 0 0 pi/2]);

L(2)=Link([0 0 1 0]);

L(3)=Link([0 0 1 -pi/2]);

L(4)=Link([0 0 0 pi/2]);

L(5)=Link([0 0 0 -pi/2]);

L(6)=Link([0 0 0 0]);

robot=SerialLink(L)

robot.tool=rt2tr(eye(3),[0 0 .5]');

Q=[0 0 0 0 0 0];

T1= rt2tr(eye(3),[0 2 1]')

T2= rt2tr(eye(3),[1.5 -1.5 1]')

Tmove=ctraj(T1,T2,20)

Qmove=robot.ikine6s(Tmove,'d')

for i=1:20

    clf

    drawrobot(robot,Qmove(i,:))

    hold on

end

creating new robot

Warning: Stretch-to-fill scaling not supported;

use DASPECT or PBASPECT before calling ARROW3. 

> In arrow3 at 377

  In trplot at 232

  In SerialLink.plot>create_robot at 443

  In SerialLink.plot at 247

  In drawrobot at 9 

DRAWROBOT function:

function drawrobot( robot, Q )

        Ttip0=robot.fkine(Q);

        T60 = Ttip0*inv(robot.tool);

        P60 = transl(T60);

        Ptip0 = transl(Ttip0);        

        axis([-2 2 -2 2 -2 2])

        line([P60(1) Ptip0(1)],[P60(2) Ptip0(2)], [P60(3) Ptip0(3)],'LineWidth',2,'Color', 'g')

        hold on

        robot.plot(Q)

        hold off

        robot.ikine6s(T60)

end

Thank you so much for your time and creating these products,

Jordan Sorg

[Peter Corke]

Jordon,

comments inline below:

The problem at hand it is to path plan an armed robot (Exact Dynamics iARM) around a 3D generated model of a skull. I have gone through chapter 5 of your book and ran the dstar, PRM, and RRT examples to get familiar as well as makemap() to generate a 2D obstacle course that resembled something like my problem.

As implemented in the book these are all 2D planners, not 3D.  D* would be a problem, but PRM and RRT would be easily modified to support 3D planning.

 

I am now trying to understand the Vehicle class to customize a robot that is a 3-link 6 DOF armed robot. Is this possible in the RTB 9.9? (Also, using MVTB 3.3)

A 3-link robot only has 3 DOF, that is it can move to an arbitrary (x,y,z) coordinate.  This might be enough for your problem, but if you wanted the tool to have a certain orientation with respect to the skull surface you would need 5 DOF>

 

If it is not, is it possible to create multiple  vehicles that are linked together? Ideally I would like to use my robot below and plan obstacle free paths that it can follow.

The key subproblems are:

- compute the 3D path required

- use inverse kinematics to figure the joint angles

I think looking at Vehicle models is a blind alley.

 

This is attempt to create a 3D matrix of ones and zeros , i.e. a cubed obstacle in the center and then trying to plan around it with the RRT but when this is visualized the key points seem to be right where the cube is centered and around. The vehicle was just clipped from one of your examples.

RRT is only designed to work with robots with 3DOF, but those DOF are (x,y,theta).

 

As a side note, when plotting my linked robot now, the look of the figure is different than previous and a warning (below) is thrown which only started to happen since I downloaded the machine vision toolbox. Any thoughts?

This is due to the use of arrow3 to draw the end-effector coordinate frame, sorry.

[Jordan Sorg]

Thank you again. I will start by trying to adapt the RRT for a 3D occupancy grid.