Skip to content

Commit e1cb294

Browse files
Samuel-DrewSamuel-Drew
committed
Added Irb140 and Panda and meshes
1 parent cd3fc5b commit e1cb294

File tree

21 files changed

+370
-66
lines changed

21 files changed

+370
-66
lines changed

‎roboticstoolbox/models/Panda.py

Lines changed: 122 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -1,85 +1,142 @@
1-
#!/usr/bin/env python
1+
"""
2+
@author: Peter Corke
3+
@author: Samuel Drew
4+
"""
25

6+
from roboticstoolbox.robot.serial_link import *
7+
from roboticstoolbox.robot.Link import RevoluteDH
8+
from math import pi
39
import numpy as np
4-
from spatialmath.base import trotz, transl
5-
from roboticstoolbox.robot.ETS import ETS
6-
from roboticstoolbox.robot.ET import ET
710

811

9-
class Panda(ETS):
10-
"""
11-
A class representing the Franka Emika Panda robot arm. ETS taken from [1]
12-
based on https://frankaemika.github.io/docs/control_parameters.html
12+
class Panda(SerialLink):
1313

14-
:param et_list: List of elementary transforms which represent the robot
15-
kinematics
16-
:type et_list: list of etb.robot.et
17-
:param q_idx: List of indexes within the ets_list which correspond to
18-
joints
19-
:type q_idx: list of int
20-
:param name: Name of the robot
21-
:type name: str, optional
22-
:param manufacturer: Manufacturer of the robot
23-
:type manufacturer: str, optional
24-
:param base: Location of the base is the world frame
25-
:type base: float np.ndarray(4,4), optional
26-
:param tool: Offset of the flange of the robot to the end-effector
27-
:type tool: float np.ndarray(4,4), optional
28-
:param qz: The zero joint angle configuration of the robot
29-
:type qz: float np.ndarray(7,)
30-
:param qr: The ready state joint angle configuration of the robot
31-
:type qr: float np.ndarray(7,)
32-
33-
References: [1] Kinematic Derivatives using the Elementary Transform
34-
Sequence, J. Haviland and P. Corke
35-
"""
14+
# %MDL_PANDA Create model of Franka-Emika PANDA robot
15+
# %
16+
# % MDL_PANDA is a script that creates the workspace variable panda which
17+
# % describes the kinematic characteristics of a Franka-Emika PANDA manipulator
18+
# % using standard DH conventions.
19+
# %
20+
# % Also define the workspace vectors:
21+
# % qz zero joint angle configuration
22+
# % qr arm along +ve x-axis configuration
23+
# %
24+
# % Reference::
25+
# % - http://www.diag.uniroma1.it/~deluca/rob1_en/WrittenExamsRob1/Robotics1_18.01.11.pdf
26+
# % - "Dynamic Identification of the Franka Emika Panda Robot With Retrieval of Feasible Parameters Using Penalty-Based Optimization"
27+
# % C. Gaz, M. Cognetti, A. Oliva, P. Robuffo Giordano and A. De Luca
28+
# % IEEE Robotics and Automation Letters 4(4), pp. 4147-4154, Oct. 2019, doi: 10.1109/LRA.2019.2931248
29+
# %
30+
# % Notes::
31+
# % - SI units of metres are used.
32+
# % - Unlike most other mdl_xxx scripts this one is actually a function that
33+
# % behaves like a script and writes to the global workspace.
34+
# %
35+
# % See also mdl_sawyer, SerialLink.
36+
#
37+
# % MODEL: Franka-Emika, PANDA, 7DOF, standard_DH
38+
#
39+
# % Copyright (C) 1993-2018, by Peter I. Corke
40+
# %
41+
# % This file is part of The Robotics Toolbox for MATLAB (RTB).
42+
# %
43+
# % RTB is free software: you can redistribute it and/or modify
44+
# % it under the terms of the GNU Lesser General Public License as published by
45+
# % the Free Software Foundation, either version 3 of the License, or
46+
# % (at your option) any later version.
47+
# %
48+
# % RTB is distributed in the hope that it will be useful,
49+
# % but WITHOUT ANY WARRANTY; without even the implied warranty of
50+
# % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
51+
# % GNU Lesser General Public License for more details.
52+
# %
53+
# % You should have received a copy of the GNU Leser General Public License
54+
# % along with RTB. If not, see <http://www.gnu.org/licenses/>.
55+
# %
56+
# % http://www.petercorke.com
3657
def __init__(self):
58+
deg = pi/180
3759

38-
deg = np.pi/180
39-
mm = 1e-3
40-
tool_offset = (103)*mm
60+
# Define links (thanks Alex Smith for this code)
61+
L1 = RevoluteDH(a=0.0, d=0.333,
62+
alpha=0.0,
63+
qlim=[-2.8973, 2.8973],
64+
m=4.970684,
65+
r=[3.875e-03, 2.081e-03, 0],
66+
I=[7.03370e-01, 7.06610e-01, 9.11700e-03, -1.39000e-04, 1.91690e-02, 6.77200e-03],
67+
G=1,
68+
mesh='FRANKA-EMIKA/Panda/link1.stl')
69+
L2 = RevoluteDH(a=0.0, d=0.0,
70+
alpha=-pi/2,
71+
qlim=[-1.7628, 1.7628],
72+
m=0.646926,
73+
r=[-3.141e-03, -2.872e-02, 3.495e-03],
74+
I=[7.96200e-03, 2.81100e-02, 2.59950e-02, -3.92500e-03, 7.04000e-04, 1.02540e-02],
75+
G=1,
76+
mesh='FRANKA-EMIKA/Panda/link2.stl')
77+
L3 = RevoluteDH(a=0.0,
78+
d=0.316,
79+
alpha=pi/2,
80+
qlim=[-2.8973, 2.8973],
81+
m=3.228604,
82+
r=[ 2.7518e-02, 3.9252e-02, -6.6502e-02],
83+
I=[3.72420e-02, 3.61550e-02, 1.08300e-02, -4.76100e-03, -1.28050e-02, -1.13960e-02],
84+
G=1,
85+
mesh='FRANKA-EMIKA/Panda/link3.stl')
86+
L4 = RevoluteDH(a=0.0825,
87+
d=0.0,
88+
alpha=pi/2,
89+
qlim=[-3.0718, -0.0698],
90+
m=3.587895,
91+
r=[-5.317e-02, 1.04419e-01, 2.7454e-02],
92+
I=[2.58530e-02, 1.95520e-02, 2.83230e-02, 7.79600e-03, 8.64100e-03, -1.33200e-03],
93+
G=1,
94+
mesh='FRANKA-EMIKA/Panda/link4.stl')
95+
L5 = RevoluteDH(a=-0.0825,
96+
d=0.384,
97+
alpha=-pi/2,
98+
qlim=[-2.8973, 2.8973],
99+
m=1.225946,
100+
r=[-1.1953e-02, 4.1065e-02, -3.8437e-02],
101+
I=[3.55490e-02, 2.94740e-02, 8.62700e-03, -2.11700e-03, 2.29000e-04, -4.03700e-03],
102+
G=1,
103+
mesh='FRANKA-EMIKA/Panda/link5.stl')
104+
L6 = RevoluteDH(a=0.0,
105+
d=0.0,
106+
alpha=pi/2,
107+
qlim=[-0.0175, 3.7525],
108+
m=1.666555,
109+
r=[6.0149e-02, -1.4117e-02, -1.0517e-02],
110+
I=[1.96400e-03, 4.35400e-03, 5.43300e-03, 1.09000e-04, 3.41000e-04, -1.15800e-03],
111+
G=1,
112+
mesh='FRANKA-EMIKA/Panda/link6.stl')
113+
L7 = RevoluteDH(a=0.088,
114+
d=0.0,
115+
alpha=pi/2,
116+
qlim=[-2.8973, 2.8973],
117+
m=7.35522e-01,
118+
r=[1.0517e-02, -4.252e-03, 6.1597e-02],
119+
I=[1.25160e-02, 1.00270e-02, 4.81500e-03, -4.28000e-04, -7.41000e-04, -1.19600e-03],
120+
G=1,
121+
mesh='FRANKA-EMIKA/Panda/link7.stl')
41122

42-
et_list = [
43-
ET.Ttz(0.333),
44-
ET.TRz(joint=1),
45-
ET.TRx(-90*deg),
46-
ET.TRz(joint=2),
47-
ET.TRx(90*deg),
48-
ET.Ttz(0.316),
49-
ET.TRz(joint=3),
50-
ET.Ttx(0.0825),
51-
ET.TRx(90*deg),
52-
ET.TRz(joint=4),
53-
ET.Ttx(-0.0825),
54-
ET.TRx(-90*deg),
55-
ET.Ttz(0.384),
56-
ET.TRz(joint=5),
57-
ET.TRx(90*deg),
58-
ET.TRz(joint=6),
59-
ET.Ttx(0.088),
60-
ET.TRx(90*deg),
61-
ET.Ttz(0.107),
62-
ET.TRz(joint=7),
63-
]
123+
L = [L1, L2, L3, L4, L5, L6, L7]
64124

65-
q_idx = [1, 3, 6, 9, 13, 15, 19]
125+
self._qz = np.array([0, 0, 0, 0, 0, 0, 0])
66126

67-
tool = transl(0, 0, tool_offset) @ trotz(-np.pi/4)
127+
self._qr = np.array([0, -90*deg, -90*deg, 90*deg, 0, -90*deg, 90*deg])
68128

129+
# Create SerialLink object
69130
super(Panda, self).__init__(
70-
et_list,
71-
q_idx,
131+
L,
132+
basemesh="FRANKA-EMIKA/Panda/link0.stl",
72133
name='Panda',
73-
manufacturer='Franka Emika',
74-
tool=tool)
75-
76-
self._qz = np.array([0, 0, 0, 0, 0, 0, 0])
77-
self._qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg
134+
manufacturer='FRANKA-EMIKA')
78135

79136
@property
80137
def qz(self):
81138
return self._qz
82139

83140
@property
84141
def qr(self):
85-
return self._qr
142+
return self._qr

‎roboticstoolbox/models/PandaETS.py

Lines changed: 85 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#!/usr/bin/env python
2+
3+
import numpy as np
4+
from spatialmath.base import trotz, transl
5+
from roboticstoolbox.robot.ETS import ETS
6+
from roboticstoolbox.robot.ET import ET
7+
8+
9+
class Panda(ETS):
10+
"""
11+
A class representing the Franka Emika Panda robot arm. ETS taken from [1]
12+
based on https://frankaemika.github.io/docs/control_parameters.html
13+
14+
:param et_list: List of elementary transforms which represent the robot
15+
kinematics
16+
:type et_list: list of etb.robot.et
17+
:param q_idx: List of indexes within the ets_list which correspond to
18+
joints
19+
:type q_idx: list of int
20+
:param name: Name of the robot
21+
:type name: str, optional
22+
:param manufacturer: Manufacturer of the robot
23+
:type manufacturer: str, optional
24+
:param base: Location of the base is the world frame
25+
:type base: float np.ndarray(4,4), optional
26+
:param tool: Offset of the flange of the robot to the end-effector
27+
:type tool: float np.ndarray(4,4), optional
28+
:param qz: The zero joint angle configuration of the robot
29+
:type qz: float np.ndarray(7,)
30+
:param qr: The ready state joint angle configuration of the robot
31+
:type qr: float np.ndarray(7,)
32+
33+
References: [1] Kinematic Derivatives using the Elementary Transform
34+
Sequence, J. Haviland and P. Corke
35+
"""
36+
def __init__(self):
37+
38+
deg = np.pi/180
39+
mm = 1e-3
40+
tool_offset = (103)*mm
41+
42+
et_list = [
43+
ET.Ttz(0.333),
44+
ET.TRz(joint=1),
45+
ET.TRx(-90*deg),
46+
ET.TRz(joint=2),
47+
ET.TRx(90*deg),
48+
ET.Ttz(0.316),
49+
ET.TRz(joint=3),
50+
ET.Ttx(0.0825),
51+
ET.TRx(90*deg),
52+
ET.TRz(joint=4),
53+
ET.Ttx(-0.0825),
54+
ET.TRx(-90*deg),
55+
ET.Ttz(0.384),
56+
ET.TRz(joint=5),
57+
ET.TRx(90*deg),
58+
ET.TRz(joint=6),
59+
ET.Ttx(0.088),
60+
ET.TRx(90*deg),
61+
ET.Ttz(0.107),
62+
ET.TRz(joint=7),
63+
]
64+
65+
q_idx = [1, 3, 6, 9, 13, 15, 19]
66+
67+
tool = transl(0, 0, tool_offset) @ trotz(-np.pi/4)
68+
69+
super(Panda, self).__init__(
70+
et_list,
71+
q_idx,
72+
name='Panda',
73+
manufacturer='Franka Emika',
74+
tool=tool)
75+
76+
self._qz = np.array([0, 0, 0, 0, 0, 0, 0])
77+
self._qr = np.array([0, -90, -90, 90, 0, -90, 90]) * deg
78+
79+
@property
80+
def qz(self):
81+
return self._qz
82+
83+
@property
84+
def qr(self):
85+
return self._qr

‎roboticstoolbox/models/__init__.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
# Robot Models
2-
from roboticstoolbox.models.Panda import Panda
2+
from roboticstoolbox.models.PandaETS import Panda
33
from roboticstoolbox.models.Puma560 import Puma560
44
# from roboticstoolbox.models.PandaMDH import PandaMDH
55

0 commit comments

Comments
 (0)