def human_to_baxter(self, l_sh, l_el, l_ha, r_sh, r_el, r_ha, a):
"""
Computes angles sent to Baxter's arm, checks if gripper should adjust
"""
rospy.logdebug("Calling Crane().human_to_baxter(...)")
# Crane Arm
# Forms vectors between points in 3-space and uses that and
# vector operations to calculate the angles.
l_upper_arm = make_vector_from_POINTS(l_sh, l_el)
l_forearm = make_vector_from_POINTS(l_el, l_ha)
# S0
v_xz = vector_projection_onto_plane(l_upper_arm, [0, 0, -1],
[-1, 0, 0])
theta = angle_between_vectors(v_xz, [-1, 0, 0])
s0 = theta - math.pi / 4
# S1
theta = angle_between_vectors(l_upper_arm, v_xz)
if l_el.y > l_sh.y:
s1 = theta
else:
s1 = -theta
# E0
n_upper_arm = shortest_vector_from_point_to_vector(
l_ha, l_upper_arm, l_forearm, l_sh)
theta = angle_between_vectors(n_upper_arm, [0, 1, 0])
e0 = theta
# E1
theta = angle_between_vectors(l_upper_arm, l_forearm)
e1 = theta
# W0, W1, and W2
w0 = -1.57
w1 = 0.00
w2 = -0.30
# Check if angles are valid
self.check_angles(s0, s1, e0, e1, w0, w1, w2, a)
# Gripper Control Arm
r_upper_arm = make_vector_from_POINTS(r_sh, r_el)
r_forearm = make_vector_from_POINTS(r_el, r_ha)
# Event
theta = angle_between_vectors(r_upper_arm, r_forearm)
if theta > 0.8:
return True
return False
def human_to_baxter(self, l_sh, l_el, l_ha, r_sh, r_el, r_ha, a):
"""
Computes angles sent to Baxter's arm, checks if gripper should adjust
"""
rospy.logdebug("Calling Crane().human_to_baxter(...)")
# Crane Arm
# Forms vectors between points in 3-space and uses that and
# vector operations to calculate the angles.
l_upper_arm = make_vector_from_POINTS(l_sh, l_el)
l_forearm = make_vector_from_POINTS(l_el, l_ha)
# S0
v_xz = vector_projection_onto_plane(l_upper_arm, [0,0,-1], [-1,0,0])
theta = angle_between_vectors(v_xz, [-1,0,0])
s0 = theta - math.pi/4
# S1
theta = angle_between_vectors(l_upper_arm, v_xz)
if l_el.y > l_sh.y:
s1 = theta
else:
s1 = -theta
# E0
n_upper_arm = shortest_vector_from_point_to_vector(l_ha, l_upper_arm,
l_forearm, l_sh)
theta = angle_between_vectors(n_upper_arm, [0,1,0])
e0 = theta
# E1
theta = angle_between_vectors(l_upper_arm, l_forearm)
e1 = theta
# W0, W1, and W2
w0 = -1.57
w1 = 0.00
w2 = -0.30
# Check if angles are valid
self.check_angles(s0,s1,e0,e1,w0,w1,w2,a)
# Gripper Control Arm
r_upper_arm = make_vector_from_POINTS(r_sh, r_el)
r_forearm = make_vector_from_POINTS(r_el, r_ha)
# Event
theta = angle_between_vectors(r_upper_arm, r_forearm)
if theta > 0.8:
return True
return False
def human_to_baxter(self, l_sh, l_el, l_ha, r_sh, r_el, r_ha, a):
"""
Determines how to move Baxter's limbs in order to mimic user
"""
for arm in ['left', 'right']:
if arm=='left':
sh = l_sh
el = l_el
ha = l_ha
elif arm=='right':
sh = r_sh
el = r_el
ha = r_ha
upper_arm = make_vector_from_POINTS(sh, el)
forearm = make_vector_from_POINTS(el, ha)
# E0 computations
n_upper_arm = shortest_vector_from_point_to_vector(ha, upper_arm,
forearm, sh)
theta = angle_between_vectors(n_upper_arm, [0,1,0])
e0 = theta
# E1 computations
theta = angle_between_vectors(upper_arm, forearm)
e1 = theta
# W0, W1, and W2 computations
w0 = 0.00
w1 = 0.00
w2 = 0.00
if arm=='left':
# S0 computations
v_xz = vector_projection_onto_plane(upper_arm, [0,0,-1], [-1,0,0])
theta = angle_between_vectors(v_xz, [-1,0,0])
s0 = theta - math.pi/4
# S1 computations
theta = angle_between_vectors(upper_arm, v_xz)
if el.y > sh.y:
s1 = theta
else:
s1 = -theta
angles = a['right']
# Assume moveit will avoid wall
# angles.append(s0)
# s0 assignment in safe range
if -0.25 < s0 and s0 < 1.60:
angles.append(s0)
elif -0.25 < s0:
angles.append(1.60)
else:
angles.append(-0.25)
elif arm=='right':
# S0 computations
v_xz = vector_projection_onto_plane(upper_arm, [0,0,-1], [1,0,0])
theta = angle_between_vectors(v_xz, [-1,0,0])
s0 = theta - 3*math.pi/4
# S1 computations
theta = angle_between_vectors(upper_arm, v_xz)
if el.y > sh.y:
s1 = theta
else:
s1 = -theta
e0 = -e0
w0 = -w0
w2 = -w2
angles = a['left']
# Assume moveit will avoid walls
# angles.append(s0)
# s0 assignment in safe range
if -1.60 < s0 and s0 < 0.25:
angles.append(s0)
elif -1.60 < s0:
angles.append(0.25)
else:
angles.append(-1.60)
# s1 assignment in safe range
if -2.00 < s1 and s1 < 0.90:
angles.append(s1)
elif -2.00 < s1:
angles.append(0.90)
else:
angles.append(-2.00)
# e0 assignment
angles.append(e0)
# e1 assignment in safe range
if 0.10 < e1 and e1 < 2.50:
angles.append(e1)
elif 0.10 < e1:
angles.append(2.50)
else:
angles.append(0.10)
# w0, w1, and w2 assignment
angles.extend([w0,w1,w2])
return
