Skip to content

Commit 8847d89

Browse files
jhavljhavl
committed
velocity damper updated
1 parent dd7cd64 commit 8847d89

File tree

6 files changed

+138
-34
lines changed

6 files changed

+138
-34
lines changed

‎examples/swift2.py‎

Lines changed: 87 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -9,32 +9,102 @@
99
import numpy as np
1010

1111
env = swift.Swift()
12-
env.launch()
12+
env.launch(realtime=True)
1313

1414

15+
r = rtb.models.YuMi()
16+
17+
# r.q = [
18+
# -0.6, -0.3, -0.3, 0.2, 0, 0.3,
19+
# 0, 0,
20+
# 0.6, -0.3, 0.3, 0.2, 0, 0.3,
21+
# 0, 0
22+
# ]
23+
24+
env.add(r)
25+
26+
27+
ev_r = [0, 0.1, 0, 0, 0, 0]
28+
ev_l = [0, -0.001, 0, 0, 0, 0]
29+
30+
for i, link in enumerate(r.links):
31+
print(link)
32+
print(i)
33+
34+
35+
# print()
36+
37+
# path, n, _ = r.get_path(end=r.grippers[0])
38+
39+
40+
# def path_to_q(q_partial, robot, path):
41+
# j = 0
42+
# for link in path:
43+
# if link.isjoint:
44+
# q_whole[link.jindex] = q_partial[j]
45+
# j += 1
46+
47+
48+
# for link in path:
49+
# print(link.jindex)
50+
51+
52+
# for i in range(10000):
53+
# qd_r = np.linalg.pinv(r.jacob0(r.q, end=r.grippers[0])) @ ev_r
54+
# # # qd_l = np.linalg.pinv(r.jacob0(r.q, end=r.grippers[1])) @ ev_l
55+
56+
# r.qd[:6] = qd_r[:6]
57+
58+
# # r.qd = resolve_qd(path, qd_whole, qd_subset)
59+
60+
# print(qd_r)
61+
# # # print(qd_l)
62+
# # # r.qd[3] = 1
63+
# env.step(0.001)
64+
65+
env.hold()
66+
1567
# ur = rtb.models.UR5()
1668
# ur.base = sm.SE3(0.3, 1, 0) * sm.SE3.Rz(np.pi / 2)
1769
# ur.q = [-0.4, -np.pi / 2 - 0.3, np.pi / 2 + 0.3, -np.pi / 2, -np.pi / 2, 0]
1870
# env.add(ur)
1971

20-
# lbr = rtb.models.LBR()
21-
# lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi / 2)
22-
# lbr.q = lbr.qr
23-
# env.add(lbr)
72+
# # lbr = rtb.models.LBR()
73+
# # lbr.base = sm.SE3(1.8, 1, 0) * sm.SE3.Rz(np.pi / 2)
74+
# # lbr.q = lbr.qr
75+
# # env.add(lbr)
2476

25-
# k = rtb.models.KinovaGen3()
26-
# k.q = k.qr
27-
# k.q[0] = np.pi + 0.15
28-
# k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi / 2)
29-
# env.add(k)
77+
# # k = rtb.models.KinovaGen3()
78+
# # k.q = k.qr
79+
# # k.q[0] = np.pi + 0.15
80+
# # k.base = sm.SE3(0.7, 1, 0) * sm.SE3.Rz(np.pi / 2)
81+
# # env.add(k)
3082

31-
# panda = rtb.models.Panda()
32-
# panda.q = panda.qr
33-
# panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi / 2)
34-
# env.add(panda, show_robot=True)
83+
# # panda = rtb.models.Panda()
84+
# # panda.q = panda.qr
85+
# # panda.base = sm.SE3(1.2, 1, 0) * sm.SE3.Rz(np.pi / 2)
86+
# # env.add(panda, show_robot=True)
3587

36-
r = rtb.models.YuMi()
37-
env.add(r)
88+
# r = rtb.models.YuMi()
89+
# env.add(r)
3890

3991

40-
env.hold()
92+
# env.hold()
93+
94+
# import roboticstoolbox as rtb
95+
# from spatialmath import SE3
96+
# from swift import Swift
97+
98+
# total_robots = 1000
99+
# robots = [] # list of robots
100+
# d = 1 # robot spacing
101+
# env = Swift(_dev=True)
102+
# env.launch()
103+
# for i in range(total_robots):
104+
# base = SE3(d * (i % 2), d * (i // 2), 0.0) # place them on grid
105+
# robot = rtb.models.URDF.Puma560()
106+
# robot.base = base
107+
# robots.append(robot)
108+
# env.add(robots[i])
109+
110+
# env.hold()

‎roboticstoolbox/core/fknm.c‎

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -723,7 +723,7 @@ void _fkine(PyObject *links, int n, npy_float64 *q, npy_float64 *etool, npy_floa
723723
}
724724

725725
mult(current, etool, ret);
726-
copy(temp, current);
726+
copy(ret, current);
727727
mult(current, tool, ret);
728728
}
729729

@@ -1091,8 +1091,8 @@ void _r2q(npy_float64 *r, npy_float64 *q)
10911091
t13m = pow((r[0 * 4 + 2] - r[2 * 4 + 0]), 2);
10921092
t23m = pow((r[1 * 4 + 2] - r[2 * 4 + 1]), 2);
10931093

1094-
d1 = pow(( r[0 * 4 + 0] + r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
1095-
d2 = pow(( r[0 * 4 + 0] - r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
1094+
d1 = pow((r[0 * 4 + 0] + r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
1095+
d2 = pow((r[0 * 4 + 0] - r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
10961096
d3 = pow((-r[0 * 4 + 0] + r[1 * 4 + 1] - r[2 * 4 + 2] + 1), 2);
10971097
d4 = pow((-r[0 * 4 + 0] - r[1 * 4 + 1] + r[2 * 4 + 2] + 1), 2);
10981098

‎roboticstoolbox/examples/swift_benchmark.py‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -5,10 +5,8 @@
55

66
from roboticstoolbox.backends import swift
77
import roboticstoolbox as rtb
8-
import spatialmath as sm
98
import numpy as np
109
import cProfile
11-
# import numpy.testing as nt
1210

1311
env = swift.Swift(_dev=False)
1412
env.launch()

‎roboticstoolbox/models/URDF/PR2.py‎

Lines changed: 36 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,7 @@
11
#!/usr/bin/env python
22

33
from roboticstoolbox.robot.ERobot import ERobot
4+
import numpy as np
45

56

67
class PR2(ERobot):
@@ -13,11 +14,44 @@ def __init__(self):
1314

1415
super().__init__(
1516
links,
17+
gripper_links=[links[53], links[73]],
1618
name=name)
1719

20+
self.grippers[0].tool = self.link_dict["r_gripper_tool_frame"].A()
21+
self.grippers[1].tool = self.link_dict["l_gripper_tool_frame"].A()
22+
1823
self.manufacturer = 'Willow Garage'
1924

25+
self.qz = np.zeros(31)
26+
27+
2028
if __name__ == '__main__': # pragma nocover
2129

22-
robot = PR2()
23-
print(robot)
30+
r = PR2()
31+
32+
# i = 0
33+
34+
# for link in r.links:
35+
# if link.isjoint:
36+
# print(i, link.name)
37+
38+
# i += 1
39+
40+
# path, n, t = r.get_path(end=r.grippers[0])
41+
42+
# print(n)
43+
# print(t)
44+
45+
# for l in path[1:]:
46+
# if len(l.collision) > 0:
47+
# print(l.isjoint)
48+
# print(l.name)
49+
# print(l.parent.name)
50+
# print()
51+
52+
# for l in r.grippers[0].links:
53+
# if len(l.collision) > 0:
54+
# print(l.isjoint)
55+
# print(l.name)
56+
# print(l.parent.name)
57+
# print()

‎roboticstoolbox/models/URDF/YuMi.py‎

Lines changed: 6 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -29,17 +29,18 @@ class YuMi(ERobot):
2929
.. codeauthor:: Jesse Haviland
3030
.. sectionauthor:: Peter Corke
3131
"""
32+
3233
def __init__(self):
3334

3435
links, name = self.URDF_read(
3536
"yumi_description/urdf/yumi.urdf")
3637

3738
super().__init__(
38-
links,
39-
name=name,
40-
manufacturer='ABB',
41-
# gripper_links=links[20]
42-
)
39+
links,
40+
name=name,
41+
manufacturer='ABB',
42+
gripper_links=[links[20]]
43+
)
4344

4445
# self.addconfiguration(
4546
# "qz", np.zeros((14,)))
@@ -51,4 +52,3 @@ def __init__(self):
5152

5253
robot = YuMi()
5354
print(robot)
54-

‎roboticstoolbox/robot/ERobot.py‎

Lines changed: 6 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -182,6 +182,8 @@ def __init__(
182182
# Make a gripper object for each gripper
183183
for link in gripper_links:
184184
g_links = self.dfs_links(link)
185+
# for g in g_links:
186+
# print(g)
185187

186188
# Remove gripper links from the robot
187189
for g_link in g_links:
@@ -2006,10 +2008,10 @@ def link_collision_damper(
20062008

20072009
links, n, _ = self.get_path(start=start, end=end)
20082010

2009-
if q is None:
2010-
q = np.copy(self.q)
2011-
else:
2012-
q = getvector(q, n)
2011+
# if q is None:
2012+
# q = np.copy(self.q)
2013+
# else:
2014+
# q = getvector(q, n)
20132015

20142016
j = 0
20152017
Ain = None

0 commit comments

Comments
 (0)