def step(self, q_dot):
q_dot = np.array(q_dot).reshape((-1, 1))
self.set_velocity(q_dot)
v_b = q_dot[self.mask]
g_prev = self.get_transform_world()
v_s = SE3.Ad(g_prev) @ v_b
g_next = SE3.exp(v_s) * g_prev
self.set_transform_world(g_next)
def __init__(self, o2w=None, w2o=None):
if o2w is None:
o2w = SE3.identity()
if w2o is None:
w2o = SE3.identity()
self.o2w = o2w
self.w2o = w2o
self.parent = None
def set_state(self, q):
self.q = q[self.mask]
exp = SE3.identity()
for i in range(len(self.xi)):
q = self.q[i, 0]
xi = self.xi[i]
exp = exp * SE3.exp(xi * q)
g_wl = exp * self.g_wl0
self.set_transform_world(g_wl)
def get_spatial_jacobian(self):
J = np.zeros((6, len(self.xi)))
exp = SE3.identity()
for i in range(len(self.xi)):
if i - 1 >= 0:
exp = exp * SE3.exp(self.xi[i - 1] * self.q[i - 1, 0])
J[:, i, None] = SE3.Ad(exp) @ self.xi[i]
J_s = np.zeros((6, len(self.mask)))
J_s[:, self.mask] = J
return J_s
def supmap(self, v):
tf = self.get_tf_world()
x0 = self.x - 2 * self.margin()
y0 = self.y - 2 * self.margin()
z0 = self.z - 2 * self.margin()
# x_max_dot = -np.Inf
# x_max = np.zeros((3,1))
# for x in [-x0/2, x0/2]:
# for y in [-y0/2, y0/2]:
# for z in [-z0/2, z0/2]:
# pt = np.array([x, y, z])
# pt = SE3.transform_point(tf, pt.reshape((3,1)))
# if x_max_dot < np.dot(v.T, pt).item():
# x_max_dot = np.dot(v.T, pt).item()
# x_max = pt
v = SO3.transform_point_by_inverse(tf.R, v)
x = np.multiply(np.sign(v),
np.array([x0 / 2, y0 / 2, z0 / 2]).reshape((3, 1)))
x = SE3.transform_point(tf, x)
# v = SO3.transform_point(tf.R, v)
# x_dot = (v.T @ x).item()
# assert(np.linalg.norm(x_dot - x_max_dot) < 1e-9)
return x
def closest_points(self, points, normals, tangents, tf):
if DEBUG:
print(tf)
# Transform object points.
points = SE3.transform_point(tf, points)
if DEBUG:
print('points')
print(points)
# Compute closest points.
n_pts = points.shape[1]
n_pairs = len(self.pairs)
dists = np.zeros((n_pts, ))
frame_centers = np.zeros((3, n_pts))
for i in range(n_pairs):
p = self.pairs[i]
sphere = p[0]
sphere.get_tf_world().set_translation(points[:, i, None])
obstacle = p[1]
manifold = gjk(obstacle, sphere)
if DEBUG:
print(manifold.pts_A)
print(manifold.pts_B)
print(manifold.normal)
print(manifold.dist)
points[:, i, None] = manifold.pts_A
normals[:, i, None] = manifold.normal
dists[i] = manifold.dist + sphere.margin()
return points, normals, tangents, dists
def vertex_positions(self):
n = len(self.vertices)
V = np.zeros((3, n))
tf = self.get_tf_world()
for i, v in zip(range(n), self.vertices):
V[:, i, None] = SE3.transform_point(tf, v.position)
return V
def supmap(self, v, use_margin=False):
"""Returns the support mapping, i.e. argmax v⋅x, x ∈ g⋅V.
Arguments:
v {np.ndarray} -- Input vector (3x1).
Keyword Arguments:
use_margin {bool} -- Use the objects margin for V. (default: {False})
Returns:
tuple -- (x, vertex)
"""
g = self.o2w
x_max_dot = -np.Inf
x_max = np.zeros((3, 1))
vert = self.vertices[0]
while True:
adj_closer = False
for v_adj in vert.adjacent_vertices():
x = SE3.transform_point(g, v_adj.position)
if x_max_dot < np.dot(v.T, x).item():
x_max_dot = np.dot(v.T, x).item()
x_max = x
adj_closer = True
vert = v_adj
if not adj_closer:
return x_max
def __init__(self, name=None):
super(Link, self).__init__(name=name)
self.q = None # dofs
self.xi = [] # joint twists
self.g_wl0 = SE3.identity() # transform at q=0
self.parent = None
self.childs = []
def draw(self, shader):
# Compute scaled transforms for the arrow.
s = np.diag([self.radius, self.radius, self.length, 1.0])
# Set transforms
g = self.get_tf_world().matrix()
# y axis
z_to_y = SE3.exp([0, 0, 0, -np.pi / 2, 0, 0]).matrix()
self.y_axis.get_tf_world().set_matrix(g @ z_to_y @ s)
# x axis
z_to_x = SE3.exp([0, 0, 0, 0, np.pi / 2, 0]).matrix()
self.x_axis.get_tf_world().set_matrix(g @ z_to_x @ s)
# z axis
# s[0:3,3] = np.array([0, 0, self.length/2])
self.z_axis.get_tf_world().set_matrix(g @ s)
self.x_axis.draw(shader)
self.y_axis.draw(shader)
self.z_axis.draw(shader)
def close_step(self, i):
# Collide and write contacts to bodies.
self.collider.collide()
# Get variables.
link = self.hand.get_links()[i]
manifold = link.get_contacts()[0]
i_mask = np.array([False] * self.system.num_dofs())
i_mask[i] = True
if DEBUG:
print(manifold)
# Create contact frame g_oc.
g_wo = link.get_transform_world()
g_wc = manifold.frame_B()
g_oc = SE3.inverse(g_wo) * g_wc
# Create hand jacobian.
Ad_g_co = SE3.Ad(SE3.inverse(g_oc))
J_b = link.get_body_jacobian()
J_b[:,~i_mask] = 0
J_h = Ad_g_co @ J_b
B = np.array([0, 0, 1., 0, 0, 0]).reshape((6,1))
J_h = B.T @ J_h
if DEBUG:
print('g_oc')
print(g_oc)
print('J_b')
print(J_b)
print('Ad_g_co')
print(Ad_g_co)
print('J_h')
print(J_h)
# Take step.
alpha = 0.15
d = np.array([[manifold.dist]])
dq = np.linalg.lstsq(J_h, alpha*d)[0]
q = self.hand.get_state() + dq
return q
def draw_contact_frames(self, shader):
# Get collision manifolds.
manifolds = self.system.collider.manifolds
# Get contacting separating mode.
n_pts = len(manifolds)
csmode = self.lattice0.L[self.index0[0]][self.index0[1]].m
c = np.where(csmode == 'c')[0]
mask = np.zeros((n_pts,), dtype=bool)
mask[c] = 1
# Render contact spheres and normals.
for i in range(n_pts):
m = manifolds[i]
if DEBUG:
print(m)
body_A = m.shape_A
body_B = m.shape_B
for body, frame in zip([body_A, body_B], [m.frame_A, m.frame_B]):
if body.num_dofs() > 0:
g_wc = frame()
sphere = self.contact_spheres[int(not mask[i])]
arrow = self.contact_arrows[int(not mask[i])]
# Draw velocity of contact point A.
if self.show_velocities:
qdot = body.get_velocity()
J_s = body.get_spatial_jacobian()
v_c = SE3.velocity_at_point(J_s @ qdot, g_wc.t)
if norm(v_c) > 1e-8:
mag = norm(v_c)
vv = v_c / mag
arrow.set_origin(g_wc.t)
arrow.set_z_axis(vv)
l = arrow.get_shaft_length()
arrow.set_shaft_length(mag * l)
arrow.draw(shader)
arrow.set_shaft_length(l)
# Draw contact sphere.
if self.show_contacts:
sphere.get_tf_world().set_translation(g_wc.t)
sphere.draw(shader)
# Draw contact frame A.
if not mask[i]:
continue
if self.show_contact_frames:
self.frame.set_tf_world(g_wc)
self.frame.draw(shader)
def closest_points(self, points, normals, tangents, tf):
# Transform object points.
points = SE3.transform_point(tf, points)
# Transform object normals.
normals = SO3.transform_point(tf.R, normals)
# Transform object tangents.
n_pts = points.shape[1]
for i in range(n_pts):
tangents[:, i, 0] = SO3.transform_point(tf.R,
tangents[:, i,
0]).flatten()
tangents[:, i, 1] = SO3.transform_point(tf.R,
tangents[:, i,
1]).flatten()
# Get distances.
dists = np.zeros((n_pts, ))
return points, normals, tangents, dists
def init_hand_gui(self):
# Create hand + baton.
self.hand = AnthroHand()
self.baton = Body('baton')
self.baton.set_shape(Ellipse(10, 5, 5))
self.baton.set_transform_world(SE3.exp([0,2.5,5+0.6/2,0,0,0]))
self.collider = DynamicCollisionManager()
for link in self.hand.links:
self.collider.add_pair(self.baton, link)
manifolds = self.collider.collide()
for m in manifolds:
print(m)
self.system = System()
self.system.add_body(self.hand)
self.system.add_obstacle(self.baton)
self.system.set_collider(self.collider)
self.system.reindex_dof_masks()
# GUI parameters.
self.hand_color = get_color('clay')
self.hand_color[3] = 0.5
self.baton_color = get_color('teal')
self.baton_color[3] = 0.5
self.load_hand = True
def __init__(self, name=None):
self.g_wb = SE3.identity()
self.mask = np.array([True] * 6, bool)
self.name = name
self.contacts = []
self.qdot = np.zeros((6, 1))
def frame_A(self):
g_wa = SE3.identity()
g_wa.R.set_matrix(make_frame(self.normal))
g_wa.t = self.pts_A.copy()
return g_wa
def get_body_jacobian(self):
J_s = self.get_spatial_jacobian()
J_b = SE3.Ad(SE3.inverse(self.get_transform_world())) @ J_s
return J_b
def frame_B(self):
g_wb = SE3.identity()
g_wb.R.set_matrix(make_frame(self.normal))
g_wb.t = self.pts_B.copy()
return g_wb
def get_state(self):
q = np.zeros((len(self.mask), 1))
q[self.mask] = SE3.log(self.g_wb)
return q
def set_state(self, q):
q = np.array(q).reshape((-1, 1))
self.set_transform_world(SE3.exp(q[self.mask]))
def position(self, v):
return SE3.transform_point(self.get_tf_world(), v.position)
def init(self,
num_fingers=3,
num_digits=3,
finger_length=5,
finger_width=2,
finger_thickness=0.6,
palm_length=10.0,
palm_width=10.0,
palm_thickness=0.6):
# Create anthropomorphic hand.
num_dofs = num_fingers * num_digits + (num_digits - 1)
self.links = []
self.mask = np.array([True] * num_dofs, bool)
# Create gₛₗ(0), ξ's, and mask for each finger.
dof_id = 0
for i in range(num_fingers + 1):
finger = [Link('f%d_%d' % (i, 0))]
# Finger digit 0 position relative to palm.
g_sl0 = SE3.exp(
np.array([(palm_width - finger_width) / (num_fingers - 1) * i -
(palm_width - finger_width) / 2,
palm_length / 2 + finger_length / 2, 0, 0, 0, 0]))
if i == num_fingers: # thumb
g_sl0 = SE3.exp(
np.array([(palm_width + finger_width) / 2,
finger_length / 2, 0, 0, 0, 0]))
finger[0].set_transform_0(g_sl0)
# Joint twist for digit 0.
w_0 = np.array([1, 0, 0])
p_0 = finger[0].get_transform_0().t.copy()
p_0[1, 0] -= finger_length / 2
xi_0 = np.zeros((6, 1))
xi_0[0:3, 0, None] = -SO3.ad(w_0) @ p_0
xi_0[3:6, 0] = w_0
joint_twists = [xi_0]
finger[0].set_joint_twists(joint_twists.copy())
# Shape for digit 0.
finger[0].set_shape(
Box(finger_width, finger_length, finger_thickness))
# Mask for digit 0.
mask = np.array([False] * num_dofs)
mask[dof_id] = True
dof_id += 1
finger[0].set_dof_mask(mask.copy())
# Subsequent digit positions relative to previous digit.
n_d = num_digits
if i == num_fingers:
n_d -= 1
for j in range(1, n_d):
# Link.
digit = Link('f%d_%d' % (i, j))
finger.append(digit)
# Create gₛₗ(0).
xi_0 = np.zeros((6, 1))
xi_0[0:3, 0, None] = finger[j - 1].get_transform_0().t
xi_0[1, 0] += finger_length
g_slj = SE3.exp(xi_0)
# Set gₛₗ(0).
digit.set_transform_0(g_slj)
# Add joint twist ξⱼ.
w_j = np.array([1, 0, 0])
p_j = g_slj.t.copy()
p_j[1, 0] -= finger_length / 2
xi_j = np.zeros((6, 1))
xi_j[0:3, 0, None] = -SO3.ad(w_j) @ p_j
xi_j[3:6, 0] = w_j
joint_twists.append(xi_j)
finger[j].set_joint_twists(joint_twists.copy())
# Shape for digit j.
finger[j].set_shape(
Box(finger_width, finger_length, finger_thickness))
# Mask for digit j.
mask[dof_id] = True
dof_id += 1
finger[j].set_dof_mask(mask.copy())
# Add finger links.
self.links.extend(finger)
# Add palm.
self.links.append(Static('palm'))
self.links[-1].set_shape(Box(palm_width, palm_length, palm_thickness))
self.links[-1].set_dof_mask(np.array([False] * num_dofs))
# Set to 0 position and velocity.
self.set_state(np.zeros((num_dofs, 1)))
self.set_velocity(np.zeros((num_dofs, 1)))
# Create tree.
self.init_tree()
def get_spatial_jacobian(self):
return SE3.Ad(self.get_transform_world()) @ self.get_body_jacobian()
def init(self):
self.reindex()
self.set_tf_world(SE3.identity())
self.init_geometry()
self.init_opengl()
def __init__(self, name=None):
super(Proxy, self).__init__(name)
self.body = None
self.g_bp = SE3.identity()
self.m = 0
self.n_dofs = 1
