From 88936f805c9d1a81197a0d5799f69e741755f50f Mon Sep 17 00:00:00 2001 From: Ayush Patel Date: Sat, 23 Nov 2024 19:11:28 +0000 Subject: [PATCH 1/2] added claw control - Ayush --- arm_control/arm_control/human_arm_control.py | 44 +++++++++++++++++++- 1 file changed, 43 insertions(+), 1 deletion(-) diff --git a/arm_control/arm_control/human_arm_control.py b/arm_control/arm_control/human_arm_control.py index 7349cfda..cd36d994 100644 --- a/arm_control/arm_control/human_arm_control.py +++ b/arm_control/arm_control/human_arm_control.py @@ -43,4 +43,46 @@ 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] + + if (cur_angle[hand_index] + angle_change >= joint_upper_limits[hand_index] or + cur_angle[hand_index] + angle_change <= joint_lower_limits[hand_index]): + return curr_opening + + #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 + #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 + +print(claw_control(-0.1, np.full(5,-np.pi), 1)) #lower bound on angle case + +print(claw_control(-0.1, np.full(5,0.3*np.pi), 0)) #lower bound on opening case + +print(claw_control(0.1, np.full(5,0.3*np.pi), 5)) #upper bound on opening case + + + + + + From 644ec9415e3ef123cc70560b3765891425577def Mon Sep 17 00:00:00 2001 From: Ayush Patel Date: Sun, 24 Nov 2024 19:26:47 +0000 Subject: [PATCH 2/2] fixed angle edge cases-ayush --- arm_control/arm_control/human_arm_control.py | 19 ++++++++++++------- 1 file changed, 12 insertions(+), 7 deletions(-) diff --git a/arm_control/arm_control/human_arm_control.py b/arm_control/arm_control/human_arm_control.py index cd36d994..7aee6bfb 100644 --- a/arm_control/arm_control/human_arm_control.py +++ b/arm_control/arm_control/human_arm_control.py @@ -54,15 +54,20 @@ def claw_control(joystick_input, cur_angle, curr_opening): #the limits of the angle must be changed in joint limit angle_change = speed * joystick_input * joint_max_speed[hand_index] - if (cur_angle[hand_index] + angle_change >= joint_upper_limits[hand_index] or - cur_angle[hand_index] + angle_change <= joint_lower_limits[hand_index]): - return curr_opening #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 @@ -73,13 +78,13 @@ def claw_control(joystick_input, cur_angle, curr_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 +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 +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 +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 +print(claw_control(0.1, np.full(5,0.3*np.pi), 5)) #upper bound on opening case, fully opens