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 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 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 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,
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 set_state(self, q):
q = np.array(q).reshape((-1, 1))
self.set_transform_world(SE3.exp(q[self.mask]))