From 735f702fa052fc448ea473a59948194300d8a2e3 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 10 Feb 2022 10:31:37 +0000 Subject: [PATCH 1/6] no filters for demohand_E --- .../host/pwm/sr_edc_joint_position_controllers_PWM.yaml | 4 ++-- .../4106/controls/motors/motor_board_effort_controllers.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml index 63f1ccf1..3709566f 100644 --- a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml +++ b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml @@ -161,7 +161,7 @@ sh_lh_thj1_position_controller: i_clamp: 0.0 max_force: 500.0 p: -5700.0 - position_deadband: 0.002 + position_deadband: 0.00000000000001 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj2_position_controller: joint: lh_THJ2 @@ -173,7 +173,7 @@ sh_lh_thj2_position_controller: i_clamp: 0.0 max_force: 500.0 p: -6000.0 - position_deadband: 0.002 + position_deadband: 0.00000000000001 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj3_position_controller: joint: lh_THJ3 diff --git a/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml b/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml index 8485a0d5..afabf323 100644 --- a/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml +++ b/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml @@ -263,7 +263,7 @@ thj1: torque_limit: 400 torque_limiter_gain: 64 pos_filter: - tau: 0.05 + tau: 0.00000000000001 thj2: backlash_compensation: true pid: @@ -282,7 +282,7 @@ thj2: torque_limit: 400 torque_limiter_gain: 64 pos_filter: - tau: 0.05 + 0.00000000000001 thj3: backlash_compensation: true pid: From 0b0d1eef18aa758af0bdbfaef06c41cad5457950 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 10 Feb 2022 10:54:37 +0000 Subject: [PATCH 2/6] testing --- .../host/torque/sr_edc_joint_position_controllers.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sr_hand_config/4106/controls/host/torque/sr_edc_joint_position_controllers.yaml b/sr_hand_config/4106/controls/host/torque/sr_edc_joint_position_controllers.yaml index 527d5dd8..4b543175 100644 --- a/sr_hand_config/4106/controls/host/torque/sr_edc_joint_position_controllers.yaml +++ b/sr_hand_config/4106/controls/host/torque/sr_edc_joint_position_controllers.yaml @@ -161,7 +161,7 @@ sh_lh_thj1_position_controller: i_clamp: 3000.0 max_force: 0.0 p: 5000.0 - position_deadband: 0.015 + position_deadband: 0.000000000078 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj2_position_controller: joint: lh_THJ2 @@ -173,7 +173,7 @@ sh_lh_thj2_position_controller: i_clamp: 6000.0 max_force: 0.0 p: -7000.0 - position_deadband: 0.015 + position_deadband: 0.000000000078 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj3_position_controller: joint: lh_THJ3 From 0ccfa1485f0b2bb526b79e5e5a26c301638d5941 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 10 Feb 2022 13:10:01 +0000 Subject: [PATCH 3/6] restore position deadbands --- .../host/pwm/sr_edc_joint_position_controllers_PWM.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml index 3709566f..63f1ccf1 100644 --- a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml +++ b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml @@ -161,7 +161,7 @@ sh_lh_thj1_position_controller: i_clamp: 0.0 max_force: 500.0 p: -5700.0 - position_deadband: 0.00000000000001 + position_deadband: 0.002 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj2_position_controller: joint: lh_THJ2 @@ -173,7 +173,7 @@ sh_lh_thj2_position_controller: i_clamp: 0.0 max_force: 500.0 p: -6000.0 - position_deadband: 0.00000000000001 + position_deadband: 0.002 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj3_position_controller: joint: lh_THJ3 From 4e77f1db38743bfcecc25ee85ba4654236e4ea7b Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 10 Feb 2022 13:10:25 +0000 Subject: [PATCH 4/6] fix thj2 tau param, also disable filter on thj3 --- .../4106/controls/motors/motor_board_effort_controllers.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml b/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml index afabf323..9f61536e 100644 --- a/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml +++ b/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml @@ -282,7 +282,7 @@ thj2: torque_limit: 400 torque_limiter_gain: 64 pos_filter: - 0.00000000000001 + tau: 0.00000000000001 thj3: backlash_compensation: true pid: @@ -301,7 +301,7 @@ thj3: torque_limit: 350 torque_limiter_gain: 64 pos_filter: - tau: 0.05 + tau: 0.00000000000001 thj4: backlash_compensation: false pid: From ec5e6350623b48aa73a0ca9ba7d86c037a9828c2 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 28 Apr 2022 12:55:49 +0000 Subject: [PATCH 5/6] copying to server --- .../4106/calibrations/calibration.yaml | 96 +++++++++---------- ...sr_edc_joint_position_controllers_PWM.yaml | 2 +- sr_hand_config/4106/general_info.yaml | 4 +- 3 files changed, 51 insertions(+), 51 deletions(-) diff --git a/sr_hand_config/4106/calibrations/calibration.yaml b/sr_hand_config/4106/calibrations/calibration.yaml index ed8d8341..54945026 100644 --- a/sr_hand_config/4106/calibrations/calibration.yaml +++ b/sr_hand_config/4106/calibrations/calibration.yaml @@ -1,52 +1,52 @@ sr_calibrations: [ -["FFJ1", [[3468.0, 0.0], [2792.0, 22.5], [1961.0, 45.0], [1208.0, 67.5], [460.0, 90.0]]], -["FFJ2", [[3041.0, 0.0], [2689.0, 22.5], [2285.0, 45.0], [1849.0, 67.5], [1458.0, 90.0]]], -["FFJ3", [[1721.0, 0.0], [2064.0, 22.5], [2401.0, 45.0], [2726.0, 67.5], [2951.0, 90.0]]], -["FFJ4", [[1094.0, -20.0], [1396.0, -10.0], [1685.0, 0.0], [2024.0, 10.0], [2307.0, 20.0]]], -["MFJ1", [[2892.0, 0.0], [2235.0, 22.5], [1501.0, 45.0], [794.0, 67.5], [219.0, 90.0]]], -["MFJ2", [[2957.0, 0.0], [2601.0, 22.5], [2197.0, 45.0], [1772.0, 67.5], [1381.0, 90.0]]], -["MFJ3", [[1824.0, 0.0], [2116.0, 22.5], [2432.0, 45.0], [2697.0, 67.5], [2897.0, 90.0]]], -["MFJ4", [[1246.0, -20.0], [1587.0, -10.0], [1956.0, 0.0], [2315.0, 10.0], [2642.0, 20.0]]], -["RFJ1", [[3137.0, 0.0], [2496.0, 22.5], [1748.0, 45.0], [972.0, 67.5], [313.0, 90.0]]], -["RFJ2", [[2842.0, 0.0], [2510.0, 22.5], [2157.0, 45.0], [1784.0, 67.5], [1452.0, 90.0]]], -["RFJ3", [[1793.0, 0.0], [2058.0, 22.5], [2326.0, 45.0], [2580.0, 67.5], [2751.0, 90.0]]], -["RFJ4", [[2223.0, -20.0], [1895.0, -10.0], [1586.0, 0.0], [1270.0, 10.0], [1080.0, 20.0]]], -["LFJ1", [[3232.0, 0.0], [2721.0, 22.5], [2087.0, 45.0], [1428.0, 67.5], [805.0, 90.0]]], -["LFJ2", [[2893.0, 0.0], [2652.0, 22.5], [2320.0, 45.0], [1937.0, 67.5], [1590.0, 90.0]]], -["LFJ3", [[1831.0, 0.0], [2202.0, 22.5], [2512.0, 45.0], [2754.0, 67.5], [2892.0, 90.0]]], -["LFJ4", [[2483.0, -20.0], [2121.0, -10.0], [1760.0, 0.0], [1408.0, 10.0], [1143.0, 20.0]]], -["LFJ5", [[299.0, 0.0], [1193.0, 22.5], [1651.0, 45.0], [2083.0, 67.5]]], -["THJ3", [[854.0, -15.0], [1538.0, 0.0], [2610.0, 15.0]]], -["THJ4", [[1526.0, 0.0], [1834.0, 22.5], [2137.0, 45.0], [2407.0, 67.5]]], -["THJ5", [[551.0, -60.0], [774.0, -45.0], [1029.0, -30.0], [1264.0, -15.0], [1565.0, 0.0], [1928.0, 15.0], [2287.0, 30.0], [2631.0, 45.0], [2932.0, 60.0]]], -["WRJ1", [[2532.19, -45.0], [2402.81, -22.5], [2206.98, 0.0], [2060.335, 15.0], [1951.305, 30.0]]], -["WRJ2", [[2483.0, -30.0], [1013.0, 0.0], [374.0, 10.0]]], +["FFJ1", [[3432.0, 0.0], [2730.0, 22.5], [1881.0, 45.0], [1155.0, 67.5], [388.0, 90.0]]], +["FFJ2", [[3069.0, 0.0], [2743.0, 22.5], [2326.0, 45.0], [1840.0, 67.5], [1472.0, 90.0]]], +["FFJ3", [[1734.0, 0.0], [2106.0, 22.5], [2433.0, 45.0], [2750.0, 67.5], [2945.0, 90.0]]], +["FFJ4", [[1071.0, -20.0], [1361.0, -10.0], [1675.0, 0.0], [1991.0, 10.0], [2286.0, 20.0]]], +["MFJ1", [[2892.0, 0.0], [2161.0, 22.5], [1398.0, 45.0], [770.0, 67.5], [196.0, 90.0]]], +["MFJ2", [[2968.0, 0.0], [2617.0, 22.5], [2206.0, 45.0], [1775.0, 67.5], [1399.0, 90.0]]], +["MFJ3", [[1853.0, 0.0], [2124.0, 22.5], [2450.0, 45.0], [2689.0, 67.5], [2866.0, 90.0]]], +["MFJ4", [[1245.0, -20.0], [1578.0, -10.0], [1946.0, 0.0], [2313.0, 10.0], [2640.0, 20.0]]], +["RFJ1", [[3124.0, 0.0], [2406.0, 22.5], [1640.0, 45.0], [881.0, 67.5], [279.0, 90.0]]], +["RFJ2", [[2872.0, 0.0], [2536.0, 22.5], [2138.0, 45.0], [1765.0, 67.5], [1426.0, 90.0]]], +["RFJ3", [[1813.51, 0.0], [2055.0, 22.5], [2318.0, 45.0], [2571.0, 67.5], [2733.0, 90.0]]], +["RFJ4", [[2224.0, -20.0], [1897.0, -10.0], [1572.0, 0.0], [1282.0, 10.0], [1020.0, 20.0]]], +["LFJ1", [[3250.0, 0.0], [2674.0, 22.5], [2005.0, 45.0], [1368.0, 67.5], [768.0, 90.0]]], +["LFJ2", [[2891.0, 0.0], [2639.0, 22.5], [2303.0, 45.0], [1944.0, 67.5], [1569.0, 90.0]]], +["LFJ3", [[1833.0, 0.0], [2190.0, 22.5], [2450.0, 45.0], [2737.0, 67.5], [2885.0, 90.0]]], +["LFJ4", [[2496.0, -20.0], [2139.0, -10.0], [1768.0, 0.0], [1458.0, 10.0], [1191.0, 20.0]]], +["LFJ5", [[367.0, 0.0], [1072.0, 22.5], [1669.0, 45.0], [2153.0, 67.5]]], +["THJ3", [[1101.0, -15.0], [1596.0, 0.0], [2591.0, 15.0]]], +["THJ4", [[1693.0, 0.0], [1902.0, 22.5], [2136.0, 45.0], [2382.0, 67.5]]], +["THJ5", [[480.0, -60.0], [694.0, -45.0], [941.0, -30.0], [1189.0, -15.0], [1423.0, 0.0], [1767.0, 15.0], [2137.0, 30.0], [2557.0, 45.0], [2921.0, 60.0]]], +["WRJ1", [[2525.0, -45.0], [2416.0, -22.5], [2237.0, 0.0], [2098.0, 15.0], [1983.0, 30.0]]], +["WRJ2", [[2491.0, -30.0], [1067.0, 0.0], [445.0, 10.0]]], ] sr_calibrations_coupled: [ -[["THJ1", "THJ2"], [[[2618.0, 2309.0], 0.0, 40.0], - [[2606.0, 2156.0], 0.0, 20.0], - [[2590.0, 1985.0], 0.0, 0.0], - [[2575.0, 1838.0], 0.0, -20.0], - [[2563.0, 1709.0], 0.0, -40.0], - [[2252.0, 2277.0], 22.5, 40.0], - [[2238.0, 2129.0], 22.5, 20.0], - [[2221.0, 1962.0], 22.5, 0.0], - [[2207.0, 1820.0], 22.5, -20.0], - [[2196.0, 1689.0], 22.5, -40.0], - [[1863.0, 2253.0], 45.0, 40.0], - [[1847.0, 2091.0], 45.0, 20.0], - [[1832.0, 1930.0], 45.0, 0.0], - [[1819.0, 1783.0], 45.0, -20.0], - [[1808.0, 1665.0], 45.0, -40.0], - [[1484.0, 2233.0], 67.5, 40.0], - [[1467.0, 2066.0], 67.5, 20.0], - [[1452.0, 1910.0], 67.5, 0.0], - [[1438.0, 1748.0], 67.5, -20.0], - [[1429.0, 1640.0], 67.5, -40.0], - [[1201.0, 2211.0], 90.0, 40.0], - [[1181.0, 2054.0], 90.0, 20.0], - [[1165.0, 1889.0], 90.0, 0.0], - [[1152.0, 1740.0], 90.0, -20.0], - [[1145.0, 1628.0], 90.0, -40.0]]] -] +[["THJ1", "THJ2"], [[[2615.0, 2322.0], 0.0, 40.0], + [[2600.0, 2165.0], 0.0, 20.0], + [[2583.0, 2000.0], 0.0, 0.0], + [[2567.0, 1844.0], 0.0, -20.0], + [[2554.0, 1718.0], 0.0, -40.0], + [[2265.0, 2298.0], 22.5, 40.0], + [[2250.0, 2147.0], 22.5, 20.0], + [[2233.0, 1972.0], 22.5, 0.0], + [[2220.0, 1822.0], 22.5, -20.0], + [[2207.0, 1692.0], 22.5, -40.0], + [[1879.0, 2272.0], 45.0, 40.0], + [[1864.0, 2112.0], 45.0, 20.0], + [[1849.0, 1948.0], 45.0, 0.0], + [[1836.0, 1795.0], 45.0, -20.0], + [[1825.0, 1663.0], 45.0, -40.0], + [[1504.0, 2248.0], 67.5, 40.0], + [[1491.0, 2096.0], 67.5, 20.0], + [[1473.0, 1928.0], 67.5, 0.0], + [[1461.0, 1774.0], 67.5, -20.0], + [[1452.0, 1642.0], 67.5, -40.0], + [[1224.0, 2233.0], 90.0, 40.0], + [[1207.0, 2075.0], 90.0, 20.0], + [[1191.0, 1910.0], 90.0, 0.0], + [[1179.0, 1757.0], 90.0, -20.0], + [[1169.0, 1627.0], 90.0, -40.0]]] +] \ No newline at end of file diff --git a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml index 63f1ccf1..75943b02 100644 --- a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml +++ b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml @@ -161,7 +161,7 @@ sh_lh_thj1_position_controller: i_clamp: 0.0 max_force: 500.0 p: -5700.0 - position_deadband: 0.002 + position_deadband: 0.0000000002 type: sr_mechanism_controllers/SrhJointPositionController sh_lh_thj2_position_controller: joint: lh_THJ2 diff --git a/sr_hand_config/4106/general_info.yaml b/sr_hand_config/4106/general_info.yaml index db826932..e592f915 100644 --- a/sr_hand_config/4106/general_info.yaml +++ b/sr_hand_config/4106/general_info.yaml @@ -3,11 +3,11 @@ type: 'hand_e' version: 'E3M5' fingers: [th, ff, mf, rf, lf] sensors: - tip: {th: pst, ff: pst, mf: pst, rf: pst, lf: pat} + tip: {th: pst, ff: pst, mf: pst, rf: pst, lf: pst} mid: {} prox: {} palm: {} mapping_path: package_name: sr_edc_launch relative_path: mappings/default_mappings/lh_E_v4.yaml -biotac: false # deprecated, to be removed \ No newline at end of file +biotac: false # deprecated, to be removed From 150ced85b3a5f0b975144ef9f42260469aad4a6a Mon Sep 17 00:00:00 2001 From: carebare47 Date: Fri, 13 May 2022 09:20:05 +0100 Subject: [PATCH 6/6] disable effort filter --- .../host/pwm/sr_edc_joint_position_controllers_PWM.yaml | 2 +- .../4106/controls/motors/motor_board_effort_controllers.yaml | 2 ++ 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml index 75943b02..28683b4d 100644 --- a/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml +++ b/sr_hand_config/4106/controls/host/pwm/sr_edc_joint_position_controllers_PWM.yaml @@ -159,7 +159,7 @@ sh_lh_thj1_position_controller: friction_deadband: 2000.0 i: 0.0 i_clamp: 0.0 - max_force: 500.0 + max_force: 600.0 p: -5700.0 position_deadband: 0.0000000002 type: sr_mechanism_controllers/SrhJointPositionController diff --git a/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml b/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml index 9f61536e..f1c13136 100644 --- a/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml +++ b/sr_hand_config/4106/controls/motors/motor_board_effort_controllers.yaml @@ -264,6 +264,8 @@ thj1: torque_limiter_gain: 64 pos_filter: tau: 0.00000000000001 + effort_filter: + tau: 0.00000000000001 thj2: backlash_compensation: true pid: