def desired_pose_vals(self, left_shoulder, left_elbow, left_hand,
right_shoulder, right_elbow, right_hand, torso):
"""
Returns the pose values based on the human skeleton. Similar to
desired_joint_vals. Currently not working as ideal.
"""
rospy.logdebug("Calling Crane().desired_pose_vals(...)")
# Use length of arms to potentially calculate a useful scaling
arm_length = (math.sqrt(math.pow((left_shoulder.x - left_elbow.x),2) +
math.pow((left_shoulder.y - left_elbow.y),2) +
math.pow((left_shoulder.z - left_elbow.z),2)) +
math.sqrt(math.pow((left_elbow.x - left_hand.x),2) +
math.pow((left_elbow.y - left_hand.y),2) +
math.pow((left_elbow.z - left_hand.z),2)))
RJ_ARM_LENGTH = 41*2.54/100.0
#From URDF, offsets from base/torso to right_uper_shoulder.
# x_offset = 0.062
# y_offset = -0.259
# z_offset = 0.120
# Baxter: x out, y left, z up from his perspective
# Kinect: x right, y down, z out from its perspective
# Best ones so far!
# x = (-left_hand.z + torso.z)
# y = (-torso.x + left_hand.x)
# z = (-torso.y + left_hand.y)
# x = (-left_hand.z + left_shoulder.z)*RJ_ARM_LENGTH/arm_length + x_offset
# y = (left_hand.x - left_shoulder.x)*RJ_ARM_LENGTH/arm_length + y_offset
# z = (-left_hand.y + left_shoulder.y)*RJ_ARM_LENGTH/arm_length + z_offset
# scaling = RJ_ARM_LENGTH/arm_length
scaling = 1.0
# Introduce a small offset
x = (torso.z - left_hand.z)*scaling + 0.2
y = (left_hand.x - torso.x)*scaling
z = (torso.y - left_hand.y)*scaling - 0.3
# print "x: ", x
# print "y: ", y
# print "z: ", z
# print self.arm.endpoint_pose()
# Set orientation
roll = 0 # Could be defined to be in line with the arm or something
pitch = math.pi # Could be defined to be in line with the arm or something
yaw = 0
pose = {'x': x, 'y': y, 'z': z, 'roll': roll, 'pitch': pitch, 'yaw': yaw}
# Gripper Control Arm
r_upper_arm = make_vector_from_POINTS(right_shoulder, right_elbow)
r_forearm = make_vector_from_POINTS(right_elbow, right_hand)
# Event
theta = angle_between_vectors(r_upper_arm, r_forearm)
if theta > 0.8:
self.gripper.close()
else:
self.gripper.open()
return pose
def desired_pose_vals(self, left_shoulder, left_elbow, left_hand,
right_shoulder, right_elbow, right_hand, torso):
# Find distance from shoulder to hand for human
# Total arm length:
arm_length = (math.sqrt(
math.pow((left_shoulder.x - left_elbow.x), 2) +
math.pow((left_shoulder.y - left_elbow.y), 2) +
math.pow((left_shoulder.z - left_elbow.z), 2)) + math.sqrt(
math.pow((left_elbow.x - left_hand.x), 2) +
math.pow((left_elbow.y - left_hand.y), 2) +
math.pow((left_elbow.z - left_hand.z), 2)))
RJ_ARM_LENGTH = 41 * 2.54 / 100.0
#From URDF,
x_offset = 0.055695
y_offset = 0
z_offset = 0.011038
# Use left values
x = (left_hand.x - left_shoulder.x -
torso.x) * RJ_ARM_LENGTH / arm_length + x_offset
y = (left_hand.y - left_shoulder.y -
torso.y) * RJ_ARM_LENGTH / arm_length + y_offset
z = (left_hand.z - left_shoulder.z -
torso.z) * RJ_ARM_LENGTH / arm_length + z_offset
# Set orientation
roll = 0 # Could be defined to be in line with the arm or something
pitch = math.pi / 2.0 # Could be defined to be in line with the arm or something
yaw = 0
pose = {
'x': x,
'y': y,
'z': z,
'roll': roll,
'pitch': pitch,
'yaw': yaw
}
# Gripper Control Arm
r_upper_arm = make_vector_from_POINTS(right_shoulder, right_elbow)
r_forearm = make_vector_from_POINTS(right_elbow, right_hand)
# Event
theta = angle_between_vectors(r_upper_arm, r_forearm)
if theta > 0.8:
self.gripper.close()
else:
self.gripper.open()
return pose
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 desired_pose_vals(self, left_shoulder, left_elbow, left_hand,
right_shoulder, right_elbow, right_hand, torso):
# Find distance from shoulder to hand for human
# Total arm length:
arm_length = (math.sqrt(math.pow((left_shoulder.x - left_elbow.x),2) +
math.pow((left_shoulder.y - left_elbow.y),2) +
math.pow((left_shoulder.z - left_elbow.z),2)) +
math.sqrt(math.pow((left_elbow.x - left_hand.x),2) +
math.pow((left_elbow.y - left_hand.y),2) +
math.pow((left_elbow.z - left_hand.z),2)))
RJ_ARM_LENGTH = 41*2.54/100.0
#From URDF,
x_offset = 0.055695
y_offset = 0
z_offset = 0.011038
# Use left values
x = (left_hand.x - left_shoulder.x - torso.x)*RJ_ARM_LENGTH/arm_length + x_offset
y = (left_hand.y - left_shoulder.y - torso.y)*RJ_ARM_LENGTH/arm_length + y_offset
z = (left_hand.z - left_shoulder.z - torso.z)*RJ_ARM_LENGTH/arm_length + z_offset
# Set orientation
roll = 0 # Could be defined to be in line with the arm or something
pitch = math.pi/2.0 # Could be defined to be in line with the arm or something
yaw = 0
pose = {'x': x, 'y': y, 'z': z, 'roll': roll, 'pitch': pitch, 'yaw': yaw}
# Gripper Control Arm
r_upper_arm = make_vector_from_POINTS(right_shoulder, right_elbow)
r_forearm = make_vector_from_POINTS(right_elbow, right_hand)
# Event
theta = angle_between_vectors(r_upper_arm, r_forearm)
if theta > 0.8:
self.gripper.close()
else:
self.gripper.open()
return pose
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
def desired_pose_vals(self, left_shoulder, left_elbow, left_hand,
right_shoulder, right_elbow, right_hand, torso):
"""
Returns the pose values based on the human skeleton. Similar to
desired_joint_vals. Currently not working as ideal.
"""
rospy.logdebug("Calling Crane().desired_pose_vals(...)")
# Use length of arms to potentially calculate a useful scaling
arm_length = (math.sqrt(
math.pow((left_shoulder.x - left_elbow.x), 2) +
math.pow((left_shoulder.y - left_elbow.y), 2) +
math.pow((left_shoulder.z - left_elbow.z), 2)) + math.sqrt(
math.pow((left_elbow.x - left_hand.x), 2) +
math.pow((left_elbow.y - left_hand.y), 2) +
math.pow((left_elbow.z - left_hand.z), 2)))
RJ_ARM_LENGTH = 41 * 2.54 / 100.0
#From URDF, offsets from base/torso to right_uper_shoulder.
# x_offset = 0.062
# y_offset = -0.259
# z_offset = 0.120
# Baxter: x out, y left, z up from his perspective
# Kinect: x right, y down, z out from its perspective
# Best ones so far!
# x = (-left_hand.z + torso.z)
# y = (-torso.x + left_hand.x)
# z = (-torso.y + left_hand.y)
# x = (-left_hand.z + left_shoulder.z)*RJ_ARM_LENGTH/arm_length + x_offset
# y = (left_hand.x - left_shoulder.x)*RJ_ARM_LENGTH/arm_length + y_offset
# z = (-left_hand.y + left_shoulder.y)*RJ_ARM_LENGTH/arm_length + z_offset
# scaling = RJ_ARM_LENGTH/arm_length
scaling = 1.0
# Introduce a small offset
x = (torso.z - left_hand.z) * scaling + 0.2
y = (left_hand.x - torso.x) * scaling
z = (torso.y - left_hand.y) * scaling - 0.3
# print "x: ", x
# print "y: ", y
# print "z: ", z
# print self.arm.endpoint_pose()
# Set orientation
roll = 0 # Could be defined to be in line with the arm or something
pitch = math.pi # Could be defined to be in line with the arm or something
yaw = 0
pose = {
'x': x,
'y': y,
'z': z,
'roll': roll,
'pitch': pitch,
'yaw': yaw
}
# Gripper Control Arm
r_upper_arm = make_vector_from_POINTS(right_shoulder, right_elbow)
r_forearm = make_vector_from_POINTS(right_elbow, right_hand)
# Event
theta = angle_between_vectors(r_upper_arm, r_forearm)
if theta > 0.8:
self.gripper.close()
else:
self.gripper.open()
return pose
