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 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 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