-
Notifications
You must be signed in to change notification settings - Fork 2
/
ds.py
149 lines (119 loc) · 4.65 KB
/
ds.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
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#|
#| Copyright (C) 2021-2023 Learning Algorithms and Systems Laboratory, EPFL, Switzerland
#| Authors: Harshit Khurana (maintainer)
#|
#| email: harshit.khurana@epfl.ch
#|
#| website: lasa.epfl.ch
#|
#| This file is part of iam_dual_arm_control.
#| This work was supported by the European Community's Horizon 2020 Research and Innovation
#| programme (call: H2020-ICT-09-2019-2020, RIA), grant agreement 871899 Impact-Aware Manipulation.
#|
#| iam_dual_arm_control is free software: you can redistribute it and/or modify it under the terms
#| of the GNU General Public License as published by the Free Software Foundation,
#| either version 3 of the License, or (at your option) any later version.
#|
#| iam_dual_arm_control is distributed in the hope that it will be useful,
#| but WITHOUT ANY WARRANTY; without even the implied warranty of
#| MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
#| GNU General Public License for more details.
#|
import numpy as np
def linear_ds(A, X, X_d):
return A @ (X - X_d)
def second_order_ds(A1, A2, X, X_d, X_dot):
return A1 @ (X - X_d) + A2 @ X_dot
def linear_hitting_ds(A1, X, X_obj, v_hit):
'''
'''
obj_virtual = X_obj + np.dot((X - X_obj), v_hit) * v_hit / np.square(np.linalg.norm(v_hit))
sigma = 0.1
alpha = np.exp(-np.linalg.norm(X - obj_virtual)/np.square(sigma))
# print(alpha)
dX = alpha * v_hit + (1 - alpha) * A1 @ (X - obj_virtual)
return dX
def linear_hitting_ds_momentum(A1, X, X_obj, v_hit, p_des, lambda_current):
'''
'''
obj_virtual = X_obj + np.dot((X - X_obj), v_hit) * v_hit / np.square(np.linalg.norm(v_hit))
sigma = 0.1
alpha = np.exp(-np.linalg.norm(X - obj_virtual)/np.square(sigma))
dX = alpha * v_hit + (1 - alpha) * A1 @ (X - obj_virtual)
dX = (p_des/lambda_current) * dX / np.linalg.norm(dX)
return dX
def linear_hitting_ds_pre_impact(A1, X, X_obj, v_hit, p_des, lambda_current, m_obj):
'''
'''
obj_virtual = X_obj + np.dot((X - X_obj), v_hit) * v_hit / np.square(np.linalg.norm(v_hit))
sigma = 0.1
alpha = np.exp(-np.linalg.norm(X - obj_virtual)/np.square(sigma))
dX = alpha * v_hit + (1 - alpha) * A1 @ (X - obj_virtual)
dX = (p_des/lambda_current)*(lambda_current+m_obj) * dX / np.linalg.norm(dX)
return dX
def second_order_hitting_ds(A1, A2, X, X_obj, X_dot, v_des):
'''
The object here needs a virtualised motion
virtual object is the projection of the end effector on the hitting direction
hitting direction will be the same as the direction of the velocity!
'''
obj_virtual = X_obj + np.dot((X - X_obj), v_des) * v_des / np.square(np.linalg.norm(v_des))
# print(obj_virtual)
acc = A1 @ (X - obj_virtual) + A2 @ (X_dot - v_des)
return acc
def linear_ds_momentum(A, X, X_d, dir_inertia, mom_des):
fx = A @ (X - X_d)
fx = fx / np.linalg.norm(fx)
return (mom_des / dir_inertia) * fx
def modulation_matrix(E, V):
return E @ V @ E.T
def sphere_modulation_centre(position, radius):
'''
Places a sphere at the base of the robot
The robot end effector coming too close to the robot, increases the inertia
extremely in the x, y directions and z is high
'''
centre = np.array([0, 0, 0])
dist = np.square(np.linalg.norm(position - centre)) - np.square(radius) + 1
# print(dist)
e1 = position - centre
e1 = np.reshape(e1, (3, 1))
e1 = -e1 / np.linalg.norm(e1)
e2 = np.zeros((3, 1))
e2[1] = -e1[2]
e2[2] = e1[1]
e2 = e2 / np.linalg.norm(e2)
e3 = np.cross(e1.reshape((3,)), e2.reshape((3,)))
e3 = e3 / np.linalg.norm(e3)
e3 = np.reshape(e3, (3, 1))
E = np.hstack((e1, e2, e3))
v1 = 1 - 1/np.abs(dist)
v2 = 1 + 1/np.abs(dist)
V = np.diag(np.array([v1, v2, v2]))
return E @ V @ E.T
def cylinder_modulation_centre(position, radius):
'''
Places a sphere at the base of the robot
The robot end effector coming too close to the robot, increases the inertia
extremely in the x, y directions and z is high
'''
centre = np.array([0, 0, position[2]])
dist = np.square(np.linalg.norm(position - centre)) - np.square(radius) + 1
# print(dist)
e1 = position - centre
e1 = np.reshape(e1, (3, 1))
e1 = -e1 / np.linalg.norm(e1)
e2 = np.zeros((3, 1))
e2[1] = -e1[2]
e2[2] = e1[1]
e2 = e2 / np.linalg.norm(e2)
e3 = np.cross(e1.reshape((3,)), e2.reshape((3,)))
e3 = e3 / np.linalg.norm(e3)
e3 = np.reshape(e3, (3, 1))
E = np.hstack((e1, e2, e3))
v1 = 1 - 1/np.abs(dist)
v2 = 1 + 1/np.abs(dist)
V = np.diag(np.array([v1, v2, v2]))
return E @ V @ E.T
def inertia_modulation():
return 0