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)
pFad - Phonifier reborn

Pfad - The Proxy pFad of © 2024 Garber Painting. All rights reserved.

Note: This service is not intended for secure transactions such as banking, social media, email, or purchasing. Use at your own risk. We assume no liability whatsoever for broken pages.


Alternative Proxies:

Alternative Proxy

pFad Proxy

pFad v3 Proxy

pFad v4 Proxy