def control_attitude(self, quad_att_des, quad_dcm, quad_omega, J):
quad_att = np.vstack(
rot.dcm2angle(quad_dcm.T)[::-1]
)
phi, theta, _ = quad_att.squeeze()
omega = quad_omega
omega_hat = hat(omega)
wx, wy, wz = quad_omega.squeeze()
L = np.array([
[1, np.sin(phi)*np.tan(theta), np.cos(phi)*np.tan(theta)],
[0, np.cos(phi), -np.sin(phi)],
[0, np.sin(phi)/np.cos(theta), np.cos(phi)/np.cos(theta)]
])
L2 = np.array([
[wy*np.cos(phi)*np.tan(theta) - wz*np.sin(phi)*np.tan(theta),
wy*np.sin(phi)/(np.cos(theta))**2 \
+ wz*np.cos(phi)/(np.cos(theta))**2,
0],
[-wy*np.sin(phi) - wz*np.cos(phi), 0, 0],
[wy*np.cos(phi)/np.cos(theta) - wz*np.sin(phi)*np.cos(theta),
wy*np.sin(phi)*np.tan(theta)/np.cos(theta) \
- wz*np.cos(phi)*np.tan(theta)/np.cos(theta),
0]
])
b = np.vstack((wx, 0., 0.))
e2 = L.dot(omega)
e1 = quad_att - quad_att_des
s = self.cfg.controller.Ke*e1 + e2
s_clip = np.clip(s/self.cfg.controller.chattering_bound, -1, 1)
M = (J.dot(nla.inv(L))).dot(
-self.cfg.controller.Ke*e2 - b - L2.dot(e2) \
- s_clip*(self.cfg.controller.unc_max + self.cfg.controller.Ks)
) + omega_hat.dot(J.dot(omega))
return M
def logger_callback(self, t, quad_att_des, f_des):
load_att = np.vstack(rot.dcm2angle(self.load.dcm.state.T)[::-1])
quad_pos = [None]*self.cfg.quad.num
quad_vel = [None]*self.cfg.quad.num
quad_att = [None]*self.cfg.quad.num
anchor_pos = [None]*self.cfg.quad.num
distance_btw_quad2anchor = [None]*self.cfg.quad.num
# check_dynamics = [None]*self.cfg.quad.num
for i, (link, quad) in enumerate(
zip(self.links.systems, self.quads.systems)
):
quad_pos[i] = self.load.pos.state \
+ self.load.dcm.state.dot(link.anchor) \
- link.len*link.uvec.state
quad_vel[i] = self.load.vel.state \
+ self.load.dcm.state.dot(
hat(self.load.omega.state).dot(link.anchor)
) \
- link.len*hat(link.omega.state).dot(link.uvec.state)
quad_att[i] = np.array(rot.dcm2angle(quad.dcm.state.T))[::-1]
anchor_pos[i] = self.load.pos.state \
+ self.load.dcm.state.dot(link.anchor)
distance_btw_quad2anchor[i] = \
np.sqrt(
(quad_pos[i][0][0] - anchor_pos[i][0][0])**2 \
+ (quad_pos[i][1][0] - anchor_pos[i][1][0])**2 \
+ (quad_pos[i][2][0] - anchor_pos[i][2][0])**2
)
if distance_btw_quad2anchor[i] < 0.1 or distance_btw_quad2anchor[i] > 1.:
print('problem!')
# check_dynamics[i] = np.dot(
# links[f'link{i:02d}']['uvec'].reshape(-1, ),
# links[f'link{i:02d}']['omega'].reshape(-1,)
# )
distance_btw_quads = self.check_collision(quad_pos)
return dict(time=t, **self.observe_dict(), load_att=load_att,
anchor_pos=anchor_pos, quad_vel=quad_vel,
quad_att=quad_att, quad_pos=quad_pos,
distance_btw_quads=distance_btw_quads,
distance_btw_quad2anchor=distance_btw_quad2anchor)
def get_grasp_map(contacts, normals, num_facets, mu, gamma):
""" Compute the grasp map given the contact points and their surface normals
Parameters
----------
contacts : :obj:`numpy.ndarray`
obj mesh vertices on which the fingers will be placed
normals : :obj:`numpy.ndarray`
obj mesh normals at the contact points
num_facets : int
number of vectors to use to approximate the friction cone. these vectors will be along the friction cone boundary
mu : float
coefficient of friction
gamma : float
torsional friction coefficient
Returns
-------
:obj:`numpy.ndarray` grasp map
"""
# YOUR CODE HERE
step = 2 * np.pi / num_facets
up = np.array([0, 0, 1])
grasp_maps = []
for num in range(len(contacts)):
R = look_at(normals[num], up)
p = contacts[num]
G = np.zeros((num_facets + 1, 6))
for i in range(num_facets):
f_finger = np.array(
[np.sin(step * i) * mu,
np.cos(step * i) * mu, 1])
f_object = np.dot(R, f_finger)
torque = np.dot(np.dot(hat(p), R), f_finger)
G[i, :3] = f_object
G[i, 3:] = torque
# print("f_object is: ", f_object)
# print("torque is: ", torque)
# print G[i,:]
G[num_facets, :] = np.append(np.zeros((3, 1)),
np.dot(R, np.array([0, 0, 1])))
G = G.T
# print(G)
grasp_maps.append(G)
# print np.linalg.matrix_rank(G)
# print(np.concatenate(grasp_maps ,axis = 1))
return np.concatenate(grasp_maps, axis=1)
def set_dot(self, moment):
self.dcm.dot = self.dcm.state.dot(hat(self.omega.state))
self.omega.dot = nla.inv(self.J).dot(
moment - hat(self.omega.state).dot(self.J.dot(self.omega.state))
)
def set_dot(self, ang_acc):
self.uvec.dot = hat(self.omega.state).dot(self.uvec.state)
self.omega.dot = ang_acc
def set_dot(self, acc, ang_acc):
self.pos.dot = self.vel.state
self.vel.dot = acc
self.omega.dot = ang_acc
self.dcm.dot = self.dcm.state.dot(hat(self.omega.state))
def set_dot(self, t, quad_att_des, f_des):
m_T = self.load.mass
R0 = self.load.dcm.state
omega = self.load.omega.state
omega_hat = hat(omega)
omega_hat_square = omega_hat.dot(omega_hat)
S1_set = [None] * self.cfg.quad.num
S2_set = [None] * self.cfg.quad.num
S3_set = [None] * self.cfg.quad.num
S4_set = [None] * self.cfg.quad.num
S5_set = [None] * self.cfg.quad.num
S6_set = [None] * self.cfg.quad.num
S7_set = [None] * self.cfg.quad.num
for i, (link, quad) in enumerate(
zip(self.links.systems, self.quads.systems)
):
l = link.len
rho = link.anchor
q = link.uvec.state
w = link.omega.state
m = quad.mass
R = quad.dcm.state
u = f_des[i] * R.dot(self.e3)
m_T += m
q_hat_square = (hat(q)).dot(hat(q))
q_qT = self.eye + q_hat_square
rho_hat = hat(rho)
rhohat_R0T = rho_hat.dot(R0.T)
w_norm = np.sqrt(w[0]**2 + w[1]**2 + w[2]**2)
l_w_square_q = l * w_norm * w_norm * q
R0_omega_square_rho = R0.dot(omega_hat_square.dot(rho))
S1_temp = q_qT.dot(u - m*R0_omega_square_rho) - m*l_w_square_q
S2_temp = m * q_qT.dot(rhohat_R0T.T)
S3_temp = m * rhohat_R0T.dot(q_qT.dot(rhohat_R0T.T))
S4_temp = m * q_hat_square
S5_temp = m * rhohat_R0T.dot(q_qT)
S6_temp = rhohat_R0T.dot(
q_qT.dot(u + m*self.g) \
- m*q_hat_square.dot(R0_omega_square_rho) \
- m*l_w_square_q
)
S7_temp = m * rho_hat.dot(rho_hat)
S1_set[i] = S1_temp
S2_set[i] = S2_temp
S3_set[i] = S3_temp
S4_set[i] = S4_temp
S5_set[i] = S5_temp
S6_set[i] = S6_temp
S7_set[i] = S7_temp
S1 = sum(S1_set)
S2 = sum(S2_set)
S3 = sum(S3_set)
S4 = sum(S4_set)
S5 = sum(S5_set)
S6 = sum(S6_set)
S7 = sum(S7_set)
J_bar = self.load.J - S7
J_hat = self.load.J + S3
J_hat_inv = np.linalg.inv(J_hat)
Mq = m_T*self.eye + S4
A = -J_hat_inv.dot(S5)
B = J_hat_inv.dot(S6 - omega_hat.dot(J_bar.dot(omega)))
C = Mq + S2.dot(A)
load_acc = nla.inv(C).dot(Mq.dot(self.g) + S1 - S2.dot(B))
load_ang_acc = A.dot(load_acc) + B
self.load.set_dot(load_acc, load_ang_acc)
M = [None]*self.cfg.quad.num
for i, (link, quad) in enumerate(
zip(self.links.systems, self.quads.systems)
):
l = link.len
rho = link.anchor
q = link.uvec.state
q_hat = hat(q)
m = quad.mass
R = quad.dcm.state
u = f_des[i] * R.dot(self.e3)
R0_omega_square_rho = R0.dot(omega_hat_square.dot(rho))
D = R0.dot(hat(rho).dot(load_ang_acc)) + self.g + u/m
link_ang_acc = q_hat.dot(load_acc + R0_omega_square_rho - D) / l
link.set_dot(link_ang_acc)
M[i] = self.control_attitude(
quad_att_des[i],
R,
quad.omega.state,
quad.J
)
quad.set_dot(M[i])
return dict(quad_att_des=quad_att_des, quad_moment=M, f_des=f_des)
def get_grasp_map(vertices, normals, num_facets, mu, gamma):
"""
defined in the book on page 219. Compute the grasp map given the contact
points and their surface normals
Parameters
----------
vertices : 2x3 :obj:`numpy.ndarray`
obj mesh vertices on which the fingers will be placed
normals : 2x3 :obj:`numpy.ndarray`
obj mesh normals at the contact points
num_facets : int
number of vectors to use to approximate the friction cone. these vectors
will be along the friction cone boundary
mu : float
coefficient of friction
gamma : float
torsional friction coefficient
Returns
-------
:obj:`numpy.ndarray` grasp map
"""
B1 = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 1],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1]])
B2 = np.array([[1, 0, 0, 0],
[0, 1, 0, 0],
[0, 0, 1, 1],
[0, 0, 0, 0],
[0, 0, 0, 0],
[0, 0, 0, 1]])
g1 = np.zeros((4,4))
g1[:3, :3] = hat(normals[0])
g1[:3, 3] = vertices[0]
g1[3, 3] = 1
#g1 = np.linalg.inv(g1)
g1 = g_inv(g1)
G1 = np.matmul(adj(g1).T, B1)
g2 = np.zeros((4,4))
g2[:3, :3] = hat(normals[0])
g2[:3, 3] = vertices[0]
g2[3, 3] = 1
g2 = np.linalg.inv(g2)
G2 = np.matmul(adj(g2).T, B2)
G = np.zeros((6,8))
G[:,:4] = G1
G[:,4:] = G2
return G