8000 neo · liang324/robotics-toolbox-python@2c66c69 · GitHub
[go: up one dir, main page]
Skip to content

Commit 2c66c69

Browse files
jhavljhavl
committed
neo
1 parent 78a4917 commit 2c66c69

File tree

3 files changed

+115
-81
lines changed

3 files changed

+115
-81
lines changed

‎examples/neo.py

Lines changed: 52 additions & 48 deletions
Original file line numberDiff line numberDiff line change
@@ -17,23 +17,30 @@
1717
panda = rtb.models.Panda()
1818

1919
# Set joint angles to ready configuration
20-
panda.q = [-0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173]
20+
# panda.q = [
21+
# -0.5653, -0.1941, -1.2602, -0.7896, -2.3227, -0.3919, -2.5173, 0.0, 0.0]
22+
panda.q = panda.qr
23+
24+
# Number of joint in the panda which we are controlling
25+
n = 7
2126

2227
# Make two obstacles with velocities
23-
s0 = rtb.Shape.Sphere(
28+
s0 = rtb.Sphere(
2429
radius=0.05,
2530
base=sm.SE3(0.45, 0.4, 0.3)
2631
)
2732
s0.v = [0.01, -0.2, 0, 0, 0, 0]
2833

29-
s1 = rtb.Shape.Sphere(
34+
s1 = rtb.Sphere(
3035
radius=0.05,
3136
base=sm.SE3(0.1, 0.35, 0.65)
3237
)
3338
s1.v = [0, -0.2, 0, 0, 0, 0]
3439

40+
collisions = [s0, s1]
41+
3542
# Make a target
36-
s2 = rtb.Shape.Sphere(
43+
target = rtb.Sphere(
3744
radius=0.02,
3845
base=sm.SE3(0.6, -0.2, 0.0)
3946
)
@@ -42,15 +49,12 @@
4249
env.add(panda)
4350
env.add(s0)
4451
env.add(s1)
45-
env.add(s2)
46-
time.sleep(1)
52+
env.add(target)
4753

48-
# Number of joint in the panda which we are controlling
49-
n = 7
50-
51-
# Set the desired end-effector pose to the location of s2
54+
# Set the desired end-effector pose to the location of target
5255
Tep = panda.fkine()
53-
Tep.A[:3, 3] = s2.base.t
56+
Tep.A[:3, 3] = target.base.t
57+
Tep.A[2, 3] += 0.1
5458

5559
arrived = False
5660

@@ -67,7 +71,7 @@
6771

6872
# Calulate the required end-effector spatial velocity for the robot
6973
# to approach the goal. Gain is set to 1.0
70-
v, arrived = rtb.p_servo(Te, Tep, 1.0)
74+
v, arrived = rtb.p_servo(Te, Tep, 0.5, 0.05)
7175

7276
# Gain term (lambda) for control minimisation
7377
Y = 0.01
@@ -85,42 +89,42 @@
8589
Aeq = np.c_[panda.jacobe(), np.eye(6)]
8690
beq = v.reshape((6,))
8791

88-
# The inequality constraints for joint limit avoidance
89-
Ain = np.zeros((n + 6, n + 6))
90-
bin = np.zeros(n + 6)
91-
92-
# The minimum angle (in radians) in which the joint is allowed to approach
93-
# to its limit
94-
ps = 0.05
95-
96-
# The influence angle (in radians) in which the velocity damper
97-
# becomes active
98-
pi = 0.9
99-
100-
# Form the joint limit velocity damper
101-
Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
102-
103-
for link in links:
104-
if link.jtype == link.VARIABLE:
105-
j += 1
106-
for col in link.collision:
107-
obj = s0
108-
l_Ain, l_bin, ret, wTcp = link_calc(link, col, obj, q[:j])
109-
if ret < closest:
110-
closest = ret
111-
closest_obj = obj
112-
closest_p = wTcp
113-
114-
if l_Ain is not None and l_bin is not None:
115-
if Ain is None:
116-
Ain = l_Ain
117-
else:
118-
Ain = np.r_[Ain, l_Ain]
119-
120-
if bin is None:
121-
bin = np.array(l_bin)
122-
else:
123-
bin = np.r_[bin, l_bin]
92+
# # The inequality constraints for joint limit avoidance
93+
# Ain = np.zeros((n + 6, n + 6))
94+
# bin = np.zeros(n + 6)
95+
96+
# # The minimum angle (in radians) in which the joint is allowed to approach
97+
# # to its limit
98+
# ps = 0.05
99+
100+
# # The influence angle (in radians) in which the velocity damper
101+
# # becomes active
102+
# pi = 0.9
103+
104+
# # Form the joint limit velocity damper
105+
# Ain[:n, :n], bin[:n] = panda.joint_velocity_damper(ps, pi, n)
106+
107+
Ain = None
108+
bin = None
109+
110+
for collision in collisions:
111+
112+
c_Ain, c_bin = panda.link_collision_damper(
113+
collision, panda.q[:n], 0.3, 0.05,
114+
panda.elinks['panda_joint1'], panda.elinks['panda_hand_joint'])
115+
116+
if c_Ain is not None and c_bin is not None:
117+
c_Ain = np.c_[c_Ain, np.zeros((c_Ain.shape[0], 6))]
118+
119+
if Ain is None:
120+
Ain = c_Ain
121+
else:
122+
Ain = np.r_[Ain, c_Ain]
123+
124+
if bin is None:
125+
bin = np.array(c_bin)
126+
else:
127+
bin = np.r_[bin, c_bin]
124128

125129
# Linear component of objective function: the manipulability Jacobian
126130
c = np.r_[-panda.jacobm().reshape((n,)), np.zeros(6)]

‎roboticstoolbox/backend/urdf/utils.py

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ def parse_origin(node):
5757
if 'rpy' in origin_node.attrib:
5858
rpy = np.fromstring(origin_node.attrib['rpy'], sep=' ')
5959
matrix.A[:3, :3] = sm.SE3.RPY(rpy).R
60-
60+
6161
return matrix.A, rpy
6262

6363

‎roboticstoolbox/robot/ERobot.py

Lines changed: 62 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -1142,39 +1142,69 @@ def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0):
11421142

11431143
return Ain, Bin
11441144

1145-
# def link_collision_damper(self, links=None, col, ob, q):
1146-
# dii = 5
1147-
# di = 0.3
1148-
# ds = 0.05
1149-
1150-
# if links is None:
1151-
# links = self.links[1:]
1152-
1153-
# ret = p.getClosestPoints(col.co, ob.co, dii)
1154-
1155-
# if len(ret) > 0:
1156-
# ret = ret[0]
1157-
# wTlp = sm.SE3(ret[5])
1158-
# wTcp = sm.SE3(ret[6])
1159-
# lpTcp = wTlp.inv() * wTcp
1160-
1161-
# d = ret[8]
1162-
1163-
# if d < di:
1164-
# n = lpTcp.t / d
1165-
# nh = np.expand_dims(np.r_[n, 0, 0, 0], axis=0)
1166-
1167-
# Je = r.jacobe(q, from_link=r.base_link, to_link=link, offset=col.base)
1168-
# n = Je.shape[1]
1169-
# dp = nh @ ob.v
1170-
# l_Ain = np.zeros((1, 13))
1171-
# l_Ain[0, :n] = nh @ Je
1172-
# l_bin = (0.8 * (d - ds) / (di - ds)) + dp
1173-
# else:
1174-
# l_Ain = None
1175-
# l_bin = None
1145+
def link_collision_damper(
1146+
self, shape, q=None, di=0.3, ds=0.05,
1147+
from_link=None, to_link=None):
1148+
1149+
if from_link is None:
1150+
from_link = self.base_link
1151+
1152+
if to_link is None:
1153+
to_link = self.ee_link
1154+
1155+
links, n = self.get_path(from_link, to_link)
1156+
1157+
if q is None:
1158+
q = np.copy(self.q)
1159+
else:
1160+
q = getvector(q, n)
1161+
1162+
j = 0
1163+
Ain = None
1164+
bin = None
1165+
1166+
def indiv_calculation(link, link_col, q):
1167+
d, wTlp, wTcp = link_col.closest_point(shape, di)
1168+
1169+
if d is not None:
1170+
lpTcp = wTlp.inv() * wTcp
1171+
norm = lpTcp.t / d
1172+
norm_h = np.expand_dims(np.r_[norm, 0, 0, 0], axis=0)
1173+
1174+
Je = self.jacobe(
1175+
q, from_link=self.base_link, to_link=link,
1176+
offset=link_col.base)
1177+
n_dim = Je.shape[1]
1178+
dp = norm_h @ shape.v
1179+
l_Ain = np.zeros((1, n))
1180+
l_Ain[0, :n_dim] = norm_h @ Je
1181+
l_bin = (0.8 * (d - ds) / (di - ds)) + dp
1182+
else:
1183+
l_Ain = None
1184+
l_bin = None
1185+
1186+
return l_Ain, l_bin, d, wTcp
1187+
1188+
for link in links:
1189+
if link.jtype == link.VARIABLE:
1190+
j += 1
1191+
1192+
for link_col in link.collision:
1193+
l_Ain, l_bin, d, wTcp = indiv_calculation(
1194+
link, link_col, q[:j])
1195+
1196+
if l_Ain is not None and l_bin is not None:
1197+
if Ain is None:
1198+
Ain = l_Ain
1199+
else:
1200+
Ain = np.r_[Ain, l_Ain]
1201+
1202+
if bin is None:
1203+
bin = np.array(l_bin)
1204+
else:
1205+
bin = np.r_[bin, l_bin]
11761206

1177-
# return l_Ain, l_bin, d, wTcp
1207+
return Ain, bin
11781208

11791209
def closest_point(self, shape, inf_dist=1.0):
11801210
'''

0 commit comments

Comments
 (0)
0