diff --git a/PythonExample/AmazingHand_Demo.py b/PythonExample/AmazingHand_Demo.py index ff1beb1..b1d1ba4 100644 --- a/PythonExample/AmazingHand_Demo.py +++ b/PythonExample/AmazingHand_Demo.py @@ -20,7 +20,11 @@ timeout=0.5, ) - +def sid(i): + # logical ID: 1..8 + # right hand: 1..8 + # left hand: 11..18 + return i + 10 if Side == 2 else i def main(): @@ -232,49 +236,48 @@ def Fuck(): Move_Ring (90, -90, MaxSpeed) Move_Thumb (75, 0, MaxSpeed) -def Move_Index (Angle_1,Angle_2,Speed): - - c.write_goal_speed(1, Speed) +def Move_Index(Angle_1, Angle_2, Speed): + c.write_goal_speed(sid(1), Speed) time.sleep(0.0002) - c.write_goal_speed(2, Speed) + c.write_goal_speed(sid(2), Speed) time.sleep(0.0002) - Pos_1 = np.deg2rad(MiddlePos[0]+Angle_1) - Pos_2 = np.deg2rad(MiddlePos[1]+Angle_2) - c.write_goal_position(1, Pos_1) - c.write_goal_position(2, Pos_2) + Pos_1 = np.deg2rad(MiddlePos[0] + Angle_1) + Pos_2 = np.deg2rad(MiddlePos[1] + Angle_2) + c.write_goal_position(sid(1), Pos_1) + c.write_goal_position(sid(2), Pos_2) time.sleep(0.005) def Move_Middle(Angle_1,Angle_2,Speed): - c.write_goal_speed(3, Speed) + c.write_goal_speed(sid(3), Speed) time.sleep(0.0002) - c.write_goal_speed(4, Speed) + c.write_goal_speed(sid(4), Speed) time.sleep(0.0002) Pos_1 = np.deg2rad(MiddlePos[2]+Angle_1) Pos_2 = np.deg2rad(MiddlePos[3]+Angle_2) - c.write_goal_position(3, Pos_1) - c.write_goal_position(4, Pos_2) + c.write_goal_position(sid(3), Pos_1) + c.write_goal_position(sid(4), Pos_2) time.sleep(0.005) def Move_Ring(Angle_1,Angle_2,Speed): - c.write_goal_speed(5, Speed) + c.write_goal_speed(sid(5), Speed) time.sleep(0.0002) - c.write_goal_speed(6, Speed) + c.write_goal_speed(sid(6), Speed) time.sleep(0.0002) Pos_1 = np.deg2rad(MiddlePos[4]+Angle_1) Pos_2 = np.deg2rad(MiddlePos[5]+Angle_2) - c.write_goal_position(5, Pos_1) - c.write_goal_position(6, Pos_2) + c.write_goal_position(sid(5), Pos_1) + c.write_goal_position(sid(6), Pos_2) time.sleep(0.005) def Move_Thumb(Angle_1,Angle_2,Speed): - c.write_goal_speed(7, Speed) + c.write_goal_speed(sid(7), Speed) time.sleep(0.0002) - c.write_goal_speed(8, Speed) + c.write_goal_speed(sid(8), Speed) time.sleep(0.0002) Pos_1 = np.deg2rad(MiddlePos[6]+Angle_1) Pos_2 = np.deg2rad(MiddlePos[7]+Angle_2) - c.write_goal_position(7, Pos_1) - c.write_goal_position(8, Pos_2) + c.write_goal_position(sid(7), Pos_1) + c.write_goal_position(sid(8), Pos_2) time.sleep(0.005)