8000 Bring rest of demo files up to spec. Graphics using pylab included. · tixidi/robotics-toolbox-python@c32848b · GitHub
[go: up one dir, main page]
Skip to content

Commit c32848b

Browse files
petercorkepetercorke
committed
Bring rest of demo files up to spec. Graphics using pylab included.
1 parent 708f43b commit c32848b

File tree

5 files changed

+165
-146
lines changed

5 files changed

+165
-146
lines changed

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

Lines changed: 39 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -9,9 +9,18 @@
99
# General cleanup of code: help comments, see also, copyright, remnant dh/dyn
1010
# references, clarification of functions.
1111
#
12-
# $Revision: 1.3 $
13-
figure(2)
14-
echo on
12+
13+
import robot.parsedemo as p;
14+
import sys
15+
16+
print sys.modules
17+
18+
if __name__ == '__main__':
19+
20+
s = '''
21+
# First we will define a robot
22+
from robot.puma560 import *
23+
1524
# Forward kinematics is the problem of solving the Cartesian position and
1625
# orientation of a mechanism given knowledge of the kinematic structure and
1726
# the joint coordinates.
@@ -32,46 +41,52 @@
3241
# fkine() can also be used with a time sequence of joint coordinates, or
3342
# trajectory, which is generated by jtraj()
3443
#
35-
t = [0:.056:2]; % generate a time vector
36-
q = jtraj(qz, qr, t); % compute the joint coordinate trajectory
44+
t = arange(0, 2, 0.056) % generate a time vector
45+
(q, qd, qdd) = jtraj(qz, qr, t); % compute the joint coordinate trajectory
3746
#
3847
# then the homogeneous transform for each set of joint coordinates is given by
3948
T = fkine(p560, q);
4049
4150
#
42-
# where T is a 3-dimensional matrix, the first two dimensions are a 4x4
43-
# homogeneous transformation and the third dimension is time.
51+
# where T is a list of matrices.
4452
#
4553
# For example, the first point is
46-
T(:,:,1)
54+
T[0]
4755
#
4856
# and the tenth point is
49-
T(:,:,10)
57+
T[9]
5058
pause % any key to continue
5159
#
52-
# Elements (1:3,4) correspond to the X, Y and Z coordinates respectively, and
60+
# Elements (0:2,3) correspond to the X, Y and Z coordinates respectively, and
5361
# may be plotted against time
54-
subplot(3,1,1)
55-
plot(t, squeeze(T(1,4,:)))
62+
x = array([e[0,3] for e in T])
63+
y = array([e[1,3] for e in T]);
64+
z = array([e[2,3] for e in T]);
65+
subplot(3,1,1);
66+
plot(t, x);
5667
xlabel('Time (s)');
57-
ylabel('X (m)')
58-
subplot(3,1,2)
59-
plot(t, squeeze(T(2,4,:)))
68+
ylabel('X (m)');
69+
subplot(3,1,2);
70+
plot(t, y);
6071
xlabel('Time (s)');
61-
ylabel('Y (m)')
62-
subplot(3,1,3)
63-
plot(t, squeeze(T(3,4,:)))
72+
ylabel('Y (m)');
73+
subplot(3,1,3);
74+
plot(t, z);
6475
xlabel('Time (s)');
65-
ylabel('Z (m)')
76+
ylabel('Z (m)');
77+
show()
6678
pause % any key to continue
6779
#
6880
# or we could plot X against Z to get some idea of the Cartesian path followed
6981
# by the manipulator.
7082
#
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
83+
clf();
84+
plot(x, z);
85+
xlabel('X (m)');
86+
ylabel('Z (m)');
87+
grid();
7688
pause % any key to continue
7789
echo off
90+
'''
91+
92+
p.parsedemo(s);

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

Lines changed: 35 additions & 35 deletions
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,14 @@
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-
#
1+
import robot.parsedemo as p;
2+
import sys
3+
4+
print sys.modules
5+
6+
if __name__ == '__main__':
7+
8+
s = '''
9+
# First we will define a robot
10+
from robot.puma560 import *
11+
1712
# Inverse dynamics computes the joint torques required to achieve the specified
1813
# state of joint position, velocity and acceleration.
1914
# The recursive Newton-Euler formulation is an efficient matrix oriented
@@ -28,51 +23,54 @@
2823
# For example, for a Puma 560 in the zero angle pose, with all joint velocities
2924
# of 5rad/s and accelerations of 1rad/s/s, the joint torques required are
3025
#
31-
tau = rne(p560, qz, 5*ones(1,6), ones(1,6))
26+
tau = rne(p560, qz, 5*ones((1,6)), ones((1,6)))
3227
pause % any key to continue
3328
3429
# As with other functions the inverse dynamics can be computed for each point
3530
# on a trajectory. Create a joint coordinate trajectory and compute velocity
3631
# and acceleration as well
37-
t = [0:.056:2]; % create time vector
38-
[q,qd,qdd] = jtraj(qz, qr, t); % compute joint coordinate trajectory
32+
t = arange(0, 2, 0.056); % create time vector
33+
(q,qd,qdd) = jtraj(qz, qr, t); % compute joint coordinate trajectory
3934
tau = rne(p560, q, qd, qdd); % compute inverse dynamics
4035
#
4136
# Now the joint torques can be plotted as a function of time
42-
plot(t, tau(:,1:3))
37+
plot(t, tau[:,0:2]);
4338
xlabel('Time (s)');
44-
ylabel('Joint torque (Nm)')
45-
pause % any key to continue
39+
ylabel('Joint torque (Nm)');
40+
show()
4641
4742
#
4843
# Much of the torque on joints 2 and 3 of a Puma 560 (mounted conventionally) is
4944
# due to gravity. That component can be computed using gravload()
45+
clf();
5046
taug = gravload(p560, q);
51-
plot(t, taug(:,1:3))
47+
plot(t, taug[:,0:2]);
5248
xlabel('Time (s)');
53-
ylabel('Gravity torque (Nm)')
54-
pause % any key to continue
49+
ylabel('Gravity torque (Nm)');
50+
show()
5551
5652
# Now lets plot that as a fraction of the total torque required over the
5753
# trajectory
58-
subplot(2,1,1)
59-
plot(t,[tau(:,2) taug(:,2)])
54+
clf();
55+
subplot(2,1,1);
56+
plot(t, hstack((tau[:,1], taug[:,1])));
6057
xlabel('Time (s)');
61-
ylabel('Torque on joint 2 (Nm)')
62-
subplot(2,1,2)
63-
plot(t,[tau(:,3) taug(:,3)])
58+
ylabel('Torque on joint 2 (Nm)');
59+
subplot(2,1,2);
60+
plot(t, hstack((tau[:,2], taug[:,2])));
6461
xlabel('Time (s)');
65-
ylabel('Torque on joint 3 (Nm)')
62+
ylabel('Torque on joint 3 (Nm)');
6663
pause % any key to continue
6764
#
6865
# The inertia seen by the waist (joint 1) motor changes markedly with robot
6966
# configuration. The function inertia() computes the manipulator inertia matrix
7067
# for any given configuration.
7168
#
7269
# 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)
70+
# manipulator moves along the trajectory (this may take a few seconds)
7471
M = inertia(p560, q);
75-
M11 = squeeze(M(1,1,:));
72+
M11 = array([m[0,0] for m in M]);
73+
clf();
7674
plot(t, M11);
7775
xlabel('Time (s)');
7876
ylabel('Inertia on joint 1 (kgms2)')
@@ -81,5 +79,7 @@
8179
# stability and high-performance in the face of plant variation. In fact
8280
# for this example the inertia varies by a factor of
8381
max(M11)/min(M11)
84-
pause % any key to continue
85-
echo off
82+
pause
83+
'''
84+
85+
p.parsedemo(s);

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

Lines changed: 32 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -1,25 +1,23 @@
11
# Copyright (C) 1993-2002, by Peter I. Corke
22

3-
# $Log: rtikdemo.m,v $
4-
# Revision 1.3 2002/04/02 12:26:49 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-
#
3+
4+
import robot.parsedemo as p;
5+
import sys
6+
7+
8+
if __name__ == '__main__':
9+
10+
s = '''
11+
# First we will define a robot
12+
from robot.puma560 import *
13+
1614
# Inverse kinematics is the problem of finding the robot joint coordinates,
1715
# given a homogeneous transform representing the last link of the manipulator.
1816
# It is very useful when the path is planned in Cartesian space, for instance
1917
# a straight line path as shown in the trajectory demonstration.
2018
#
2119
# First generate the transform corresponding to a particular joint coordinate,
22-
q = [0 -pi/4 -pi/4 0 pi/8 0]
20+
q = mat([0, -pi/4, -pi/4, 0, pi/8, 0])
2321
T = fkine(p560, q);
2422
#
2523
# Now the inverse kinematic procedure for any specific robot can be derived
@@ -32,7 +30,7 @@
3230
# link. The starting point for the first point may be specified, or else it
3331
# defaults to zero (which is not a particularly good choice in this case)
3432
qi = ikine(p560, T);
35-
qi'
33+
qi.T
3634
#
3735
# Compared with the original value
3836
q
@@ -49,7 +47,7 @@
4947
# aligned resulting in the loss of one degree of freedom.
5048
T = fkine(p560, qr);
5149
qi = ikine(p560, T);
52-
qi'
50+
qi.T
5351
#
5452
# which is not the same as the original joint angle
5553
qr
@@ -61,41 +59,42 @@
6159
6260
# Inverse kinematics may also be computed for a trajectory.
6361
# If we take a Cartesian straight line path
64-
t = [0:.056:2]; % create a time vector
62+
t = arange(0, 2, 0.056); % create a time vector
6563
T1 = transl(0.6, -0.5, 0.0) % define the start point
6664
T2 = transl(0.4, 0.5, 0.2) % and destination
67-
T = ctraj(T1, T2, length(t)); % compute a Cartesian path
65+
T = ctraj(T1, T2, len(t)); % compute a Cartesian path
6866
6967
#
7068
# now solve the inverse kinematics. When solving for a trajectory, the
7169
# starting joint coordinates for each point is taken as the result of the
7270
# previous inverse solution.
7371
#
74-
tic
7572
q = ikine(p560, T);
76-
toc
7773
#
7874
# Clearly this approach is slow, and not suitable for a real robot controller
7975
# where an inverse kinematic solution would be required in a few milliseconds.
8076
#
8177
# Let's examine the joint space trajectory that results in straightline
8278
# Cartesian motion
83-
subplot(3,1,1)
84-
plot(t,q(:,1))
79+
subplot(3,1,1);
80+
plot(t,q[:,1]);
8581
xlabel('Time (s)');
86-
ylabel('Joint 1 (rad)')
87-
subplot(3,1,2)
88-
plot(t,q(:,2))
82+
ylabel('Joint 1 (rad)');
83+
subplot(3,1,2);
84+
plot(t,q[:,2]);
8985
xlabel('Time (s)');
90-
ylabel('Joint 2 (rad)')
91-
subplot(3,1,3)
92-
plot(t,q(:,3))
86+
ylabel('Joint 2 (rad)');
87+
subplot(3,1,3);
88+
plot(t,q[:,3]);
9389
xlabel('Time (s)');
94-
ylabel('Joint 3 (rad)')
95-
90+
ylabel('Joint 3 (rad)');
91+
show();
9692
pause % hit any key to continue
9793
9894
# This joint space trajectory can now be animated
99-
plot(p560, q)
100-
pause % any key to continue
101-
echo off
95+
# plot(p560, q)
96+
# show()
97+
#pause % any key to continue
98+
'''
99+
100+
p.parsedemo(s);

0 commit comments

Comments
 (0)
0