8000 First commit, after major working over of the code. First cut · vinjk/robotics-toolbox-python@413022f · GitHub
[go: up one dir, main page]
Skip to content 8000

Commit 413022f

Browse files
petercorkepetercorke
committed
First commit, after major working over of the code. First cut
of epydoc documentation.
1 parent d93d794 commit 413022f
  • transform.py
  • trplot.py
  • twolink.py
  • utility.py
  • svn-commit.tmp
  • test

    • Some content is hidden

    Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

    52 files changed

    +5824
    -0
    lines changed

    ‎robotics-toolbox-python/demo/rtandemo.py

    Lines changed: 54 additions & 0 deletions
    Original file line numberDiff line numberDiff line change
    @@ -0,0 +1,54 @@
    1+
    2+
    #**************************animation********************************************
    3+
    figure(2)
    4+
    echo on
    5+
    clf
    6+
    #
    7+
    # The trajectory demonstration has shown how a joint coordinate trajectory
    8+
    # may be generated
    9+
    t = [0:.056:2]'; % generate a time vector
    10+
    q = jtraj(qz, qr, t); % generate joint coordinate trajectory
    11+
    #
    12+
    # the overloaded function plot() animates a stick figure robot moving
    13+
    # along a trajectory.
    14+
    15+
    plot(p560, q);
    16+
    # The drawn line segments do not necessarily correspond to robot links, but
    17+
    # join the origins of sequential link coordinate frames.
    18+
    #
    19+
    # A small right-angle coordinate frame is drawn on the end of the robot to show
    20+
    # the wrist orientation.
    21+
    #
    22+
    # A shadow appears on the ground which helps to give some better idea of the
    23+
    # 3D object.
    24+
    25+
    pause % any key to continue
    26+
    #
    27+
    # We can also place additional robots into a figure.
    28+
    #
    29+
    # Let's make a clone of the Puma robot, but change its name and base location
    30+
    31+
    p560_2 = p560;
    32+
    p560_2.name = 'another Puma';
    33+
    p560_2.base = transl(-0.5, 0.5, 0);
    34+
    hold on
    35+
    plot(p560_2, q);
    36+
    pause % any key to continue
    37+
    38+
    # We can also have multiple views of the same robot
    39+
    clf
    40+
    plot(p560, qr);
    41+
    figure
    42+
    plot(p560, qr);
    43+
    view(40,50)
    44+
    plot(p560, q)
    45+
    pause % any key to continue
    46+
    #
    47+
    # Sometimes it's useful to be able to manually drive the robot around to
    48+
    # get an understanding of how it works.
    49+
    50+
    drivebot(p560)
    51+
    #
    52+
    # use the sliders to control the robot (in fact both views). Hit the red quit
    53+
    # button when you are done.
    54+
    echo off

    ‎robotics-toolbox-python/demo/rtdemo.py

    Lines changed: 66 additions & 0 deletions
    Original file line numberDiff line numberDiff line change
    @@ -0,0 +1,66 @@
    1+
    #RTDEMO Robot toolbox demonstrations
    2+
    #
    3+
    # Displays popup menu of toolbox demonstration scripts that illustrate:
    4+
    # * homogeneous transformations
    5+
    # * trajectories
    6+
    # * forward kinematics
    7+
    # * inverse kinematics
    8+
    # * robot animation
    9+
    # * inverse dynamics
    10+
    # * forward dynamics
    11+
    #
    12+
    # The scripts require the user to periodically hit <Enter> in order to move
    13+
    # through the explanation. Set PAUSE OFF if you want the scripts to run
    14+
    # completely automatically.
    15+
    16+
    # $Log: rtdemo.m,v $
    17+
    # Revision 1.3 2002/04/02 12:26:48 pic
    18+
    # Handle figures better, control echo at end of each script.
    19+
    # Fix bug in calling ctraj.
    20+
    #
    21+
    # Revision 1.2 2002/04/01 11:47:17 pic
    22+
    # General cleanup of code: help comments, see also, copyright, remnant dh/dyn
    23+
    # references, clarification of functions.
    24+
    #
    25+
    # $Revision: 1.3 $
    26+
    # Copyright (C) 1993-2002, by Peter I. Corke
    27+
    28+
    echo off
    29+
    clear all
    30+
    delete( get(0, 'Children') );
    31+
    32+
    puma560
    33+
    while 1,
    34+
    selection = menu('Robot Toolbox demonstrations', ...
    35+
    'Transformations', ...
    36+
    'Trajectory', ...
    37+
    'Forward kinematics', ...
    38+
    'Animation', ...
    39+
    'Inverse kinematics', ...
    40+
    'Jacobians', ...
    41+
    'Inverse dynamics', ...
    42+
    'Forward dynamics', ...
    43+
    'Exit');
    44+
    45+
    switch selection,
    46+
    case 1,
    47+
    rttrdemo
    48+
    case 2,
    49+
    rttgdemo
    50+
    case 3,
    51+
    rtfkdemo
    52+
    case 4,
    53+
    rtandemo
    54+
    case 5,
    55+
    rtikdemo
    56+
    case 6,
    57+
    rtjademo
    58+
    case 7,
    59+
    rtidemo
    60+
    case 8,
    61+
    rtfddemo
    62+
    case 9,
    63+
    delete( get(0, 'Children') );
    64+
    break;
    65+
    end
    66+
    end

    ‎robotics-toolbox-python/demo/rtfddemo.py

    Lines changed: 60 additions & 0 deletions
    Original file line numberDiff line numberDiff line change
    @@ -0,0 +1,60 @@
    1+
    # Copyright (C) 1993-2002, by Peter I. Corke
    2+
    3+
    # $Log: rtfddemo.m,v $
    4+
    # Revision 1.4 2002/04/02 12:26:48 pic
    5+
    # Handle figures better, control echo at end of each script.
    6+
    # Fix bug in calling ctraj.
    7+
    #
    8+
    # Revision 1.3 2002/04/01 11:47:17 pic
    9+
    # General cleanup of code: help comments, see also, copyright, remnant dh/dyn
    10+
    # references, clarification of functions.
    11+
    #
    12+
    # $Revision: 1.4 $
    13+
    figure(2)
    14+
    echo on
    15+
    #
    16+
    # Forward dynamics is the computation of joint accelerations given position and
    17+
    # velocity state, and actuator torques. It is useful in simulation of a robot
    18+
    # control system.
    19+
    #
    20+
    # Consider a Puma 560 at rest in the zero angle pose, with zero applied joint
    21+
    # torques. The joint acceleration would be given by
    22+
    accel(p560, qz, zeros(1,6), zeros(1,6))
    23+
    pause % any key to continue
    24+
    #
    25+
    # To be useful for simulation this function must be integrated. fdyn() uses the
    26+
    # MATLAB function ode45() to integrate the joint acceleration. It also allows
    27+
    # for a user written function to compute the joint torque as a function of
    28+
    # manipulator state.
    29+
    #
    30+
    # To simulate the motion of the Puma 560 from rest in the zero angle pose
    31+
    # with zero applied joint torques
    32+
    tic
    33+
    [t q qd] = fdyn(nofriction(p560), 0, 10);
    34+
    toc
    35+
    #
    36+
    # and the resulting motion can be plotted versus time
    37+
    subplot(3,1,1)
    38+
    plot(t,q(:,1))
    39+
    xlabel('Time (s)');
    40+
    ylabel('Joint 1 (rad)')
    41+
    subplot(3,1,2)
    42+
    plot(t,q(:,2))
    43+
    xlabel('Time (s)');
    44+
    ylabel('Joint 2 (rad)')
    45+
    subplot(3,1,3)
    46+
    plot(t,q(:,3))
    47+
    xlabel('Time (s)');
    48+
    ylabel('Joint 3 (rad)')
    49+
    #
    50+
    # Clearly the robot is collapsing under gravity, but it is interesting to
    51+
    # note that rotational velocity of the upper and lower arm are exerting
    52+
    # centripetal and Coriolis torques on the waist joint, causing it to rotate.
    53+
    54+
    pause % hit any key to continue
    55+
    #
    56+
    # This can be shown in animation also
    57+
    clf
    58+
    plot(p560, q)
    59+
    pause % hit any key to continue
    60+
    echo off

    ‎robotics-toolbox-python/demo/rtfkdemo.py

    Lines changed: 77 additions & 0 deletions
    Original file line numberDiff line numberDiff line change
    @@ -0,0 +1,77 @@
    1+
    # Copyright (C) 1993-2002, by Peter I. Corke
    2+
    3+
    # $Log: rtfkdemo.m,v $
    4+
    # Revision 1.3 2002/04/02 12:26:48 pic
    5+
    # Handle figures better, control echo at end of each script.
    6+
    # Fix bug in calling ctraj.
    7+
    #
    8+
    # Revision 1.2 2002/04/01 11:47:17 pic
    9+
    # General cleanup of code: help comments, see also, copyright, remnant dh/dyn
    10+
    # references, clarification of functions.
    11+
    #
    12+
    # $Revision: 1.3 $
    13+
    figure(2)
    14+
    echo on
    15+
    # Forward kinematics is the problem of solving the Cartesian position and
    16+
    # orientation of a mechanism given knowledge of the kinematic structure and
    17+
    # the joint coordinates.
    18+
    #
    19+
    # Consider the Puma 560 example again, and the joint coordinates of zero,
    20+
    # which are defined by qz
    21+
    qz
    22+
    #
    23+
    # The forward kinematics may be computed using fkine() with an appropropriate
    24+
    # kinematic description, in this case, the matrix p560 which defines
    25+
    # kinematics for the 6-axis Puma 560.
    26+
    fkine(p560, qz)
    27+
    #
    28+
    # returns the homogeneous transform corresponding to the last link of the
    29+
    # manipulator
    30+
    pause % any key to continue
    31+
    #
    32+
    # fkine() can also be used with a time sequence of joint coordinates, or
    33+
    # trajectory, which is generated by jtraj()
    34+
    #
    35+
    t = [0:.056:2]; % generate a time vector
    36+
    q = jtraj(qz, qr, t); % compute the joint coordinate trajectory
    37+
    #
    38+
    # then the homogeneous transform for each set of joint coordinates is given by
    39+
    T = fkine(p560, q);
    40+
    41+
    #
    42+
    # where T is a 3-dimensional matrix, the first two dimensions are a 4x4
    43+
    # homogeneous transformation and the third dimension is time.
    44+
    #
    45+
    # For example, the first point is
    46+
    T(:,:,1)
    47+
    #
    48+
    # and the tenth point is
    49+
    T(:,:,10)
    50+
    pause % any key to continue
    51+
    #
    52+
    # Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and
    53+
    # may be plotted against time
    54+
    subplot(3,1,1)
    55+
    plot(t, squeeze(T(1,4,:)))
    56+
    xlabel('Time (s)');
    57+
    ylabel('X (m)')
    58+
    subplot(3,1,2)
    59+
    plot(t, squeeze(T(2,4,:)))
    60+
    xlabel('Time (s)');
    61+
    ylabel('Y (m)')
    62+
    subplot(3,1,3)
    63+
    plot(t, squeeze(T(3,4,:)))
    64+
    xlabel('Time (s)');
    65+
    ylabel('Z (m)')
    66+
    pause % any key to continue
    67+
    #
    68+
    # or we could plot X against Z to get some idea of the Cartesian path followed
    69+
    # by the manipulator.
    70+
    #
    71+
    subplot(1,1,1)
    72+
    plot(squeeze(T(1,4,:)), squeeze(T(3,4,:)));
    73+
    xlabel('X (m)')
    74+
    ylabel('Z (m)')
    75+
    grid
    76+
    pause % any key to continue
    77+
    echo off

    ‎robotics-toolbox-python/demo/rtidemo.py

    Lines changed: 85 additions & 0 deletions
    Original file line numberDiff line numberDiff line change
    @@ -0,0 +1,85 @@
    1+
    # Copyright (C) 1993-2002, by Peter I. Corke
    2+
    echo off
    3+
    # 6/99 fix syntax errors
    4+
    # $Log: rtidemo.m,v $
    5+
    # Revision 1.3 2002/04/02 12:26:49 pic
    6+
    # Handle figures better, control echo at end of each script.
    7+
    # Fix bug in calling ctraj.
    8+
    #
    9+
    # Revision 1.2 2002/04/01 11:47:17 pic
    10+
    # General cleanup of code: help comments, see also, copyright, remnant dh/dyn
    11+
    # references, clarification of functions.
    12+
    #
    13+
    # $Revision: 1.3 $
    14+
    figure(2)
    15+
    echo on
    16+
    #
    17+
    # Inverse dynamics computes the joint torques required to achieve the specified
    18+
    # state of joint position, velocity and acceleration.
    19+
    # The recursive Newton-Euler formulation is an efficient matrix oriented
    20+
    # algorithm for computing the inverse dynamics, and is implemented in the
    21+
    # function rne().
    22+
    #
    23+
    # Inverse dynamics requires inertial and mass parameters of each link, as well
    24+
    # as the kinematic parameters. This is achieved by augmenting the kinematic
    25+
    # description matrix with additional columns for the inertial and mass
    26+
    # parameters for each link.
    27+
    #
    28+
    # For example, for a Puma 560 in the zero angle pose, with all joint velocities
    29+
    # of 5rad/s and accelerations of 1rad/s/s, the joint torques required are
    30+
    #
    31+
    tau = rne(p560, qz, 5*ones(1,6), ones(1,6))
    32+
    pause % any key to continue
    33+
    34+
    # As with other functions the inverse dynamics can be computed for each point
    35+
    # on a trajectory. Create a joint coordinate trajectory and compute velocity
    36+
    # and acceleration as well
    37+
    t = [0:.056:2]; % create time vector
    38+
    [q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory
    39+
    tau = rne(p560, q, qd, qdd); % compute inverse dynamics
    40+
    #
    41+
    # Now the joint torques can be plotted as a function of time
    42+
    plot(t, tau(:,1:3))
    43+
    xlabel('Time (s)');
    44+
    ylabel('Joint torque (Nm)')
    45+
    pause % any key to continue
    46+
    47+
    #
    48+
    # Much of the torque on joints 2 and 3 of a Puma 560 (mounted conventionally) is
    49+
    # due to gravity. That component can be computed using gravload()
    50+
    taug = gravload(p560, q);
    51+
    plot(t, taug(:,1:3))
    52+
    xlabel('Time (s)');
    53+
    ylabel('Gravity torque (Nm)')
    54+
    pause % any key to continue
    55+
    56+
    # Now lets plot that as a fraction of the total torque required over the
    57+
    # trajectory
    58+
    subplot(2,1,1)
    59+
    plot(t,[tau(:,2) taug(:,2)])
    60+
    xlabel('Time (s)');
    61+
    ylabel('Torque on joint 2 (Nm)')
    62+
    subplot(2,1,2)
    63+
    plot(t,[tau(:,3) taug(:,3)])
    64+
    xlabel('Time (s)');
    65+
    ylabel('Torque on joint 3 (Nm)')
    66+
    pause % any key to continue
    67+
    #
    68+
    # The inertia seen by the waist (joint 1) motor changes markedly with robot
    69+
    # configuration. The function inertia() computes the manipulator inertia matrix
    70+
    # for any given configuration.
    71+
    #
    72+
    # Let's compute the variation in joint 1 inertia, that is M(1,1), as the
    73+
    # manipulator moves along the trajectory (this may take a few minutes)
    74+
    M = inertia(p560, q);
    75+
    M11 = squeeze(M(1,1,:));
    76+
    plot(t, M11);
    77+
    xlabel('Time (s)');
    78+
    ylabel('Inertia on joint 1 (kgms2)')
    79+
    # Clearly the inertia seen by joint 1 varies considerably over this path.
    80+
    # This is one of many challenges to control design in robotics, achieving
    81+
    # stability and high-performance in the face of plant variation. In fact
    82+
    # for this example the inertia varies by a factor of
    83+
    max(M11)/min(M11)
    84+
    pause % any key to continue
    85+
    echo off

    0 commit comments

    Comments
     (0)
    0