This repository has been archived by the owner on Nov 29, 2023. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 0
/
fr3.py
43 lines (34 loc) · 1.28 KB
/
fr3.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
# modified 07-14
import numpy as np
from roboticstoolbox.robot.Robot import Robot
from spatialmath import SE3
import os
import re
class fr3(Robot):
def get_tcp(self, link):
idx=[0,0,0,0]
idx[0] = self.urdf.find(link)
idx[1] = self.urdf.find("xyz", idx[0])
idx[2] = self.urdf.find('"/', idx[1])
xyz=[float(self.urdf[idx[1] + 5: idx[2]].split(' ')[i]) for i in range(3)]
return SE3.Trans(xyz)
def __init__(self):
links, name, self.urdf, _ = self.URDF_read(
"franka_description/robots/fr3/fr3.urdf.xacro",
tld=os.path.join(os.path.dirname(__file__), "franka_ros-develop"),
xacro_tld="franka_description"
)
qdlim = []
idx = [i.start() for i in re.finditer(" velocity", self.urdf)]
for i in range(7):
qdlim.append(self.urdf[idx[i] + 11: self.urdf.find('"', idx[i] + 11)])
self.qdlim = np.array(qdlim)
super().__init__(
links, name=name, gripper_links=links[17], tool=self.get_tcp("hand_tcp2_joint")
)
self.grippers[0].tool = self.get_tcp("hand_tcp_joint")
self.qr = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4])
if __name__ == "__main__":
robot = fr3()
print(robot.tool)
print(robot.grippers[0].tool)