diff --git a/arm_control/arm_control/human_arm_control.py b/arm_control/arm_control/human_arm_control.py index 7349cfda..7aee6bfb 100644 --- a/arm_control/arm_control/human_arm_control.py +++ b/arm_control/arm_control/human_arm_control.py @@ -43,4 +43,51 @@ def move_joint(joystick_input, cur_angle, index): #sets the waists angle to be at the limit if it exeeds the limit cur_angle[index] = max(joint_lower_limits[index], min(cur_angle[index], joint_upper_limits[index])) - return cur_angle \ No newline at end of file + return cur_angle + +def claw_control(joystick_input, cur_angle, curr_opening): + """ + This function takes in the angle of the motor and the opening of claw, + and returns how much the claw opens up with joystick input + """ + #finding the change angle of opening + #the limits of the angle must be changed in joint limit + angle_change = speed * joystick_input * joint_max_speed[hand_index] + + + #when the angle is at 2pi, we assume that the opening change is 1 inch + #Also assuming the max opening to 5 inches for now + one_turn_change = 1 + open_max = 5 + open_min = 0 + + # In the case that the motor has reaches its limits in terms or rotation angle + if cur_angle[hand_index] + angle_change >= joint_upper_limits[hand_index]: + return open_max + + if cur_angle[hand_index] + angle_change <= joint_lower_limits[hand_index]: + return open_min + + #set the new change in opening proportionally + open_change = (angle_change*one_turn_change)/(2*np.pi) + new_opening = curr_opening + open_change + #ensures that new opening is not too big or small + new_opening = max(open_min, min(new_opening, open_max)) + return new_opening + + +print(claw_control(-0.1, np.full(5,np.pi), 1)) #good case + +print(claw_control(0.1, np.full(5,np.pi), 1)) #upper bound on angle case, fully opens + +print(claw_control(-0.1, np.full(5,-np.pi), 1)) #lower bound on angle case, fully closes + +print(claw_control(-0.1, np.full(5,0.3*np.pi), 0)) #lower bound on opening case, fully closes + +print(claw_control(0.1, np.full(5,0.3*np.pi), 5)) #upper bound on opening case, fully opens + + + + + +