From 9a22d667dab4081fe19159598a86684156b9e553 Mon Sep 17 00:00:00 2001 From: giusebar Date: Fri, 30 Oct 2020 10:57:55 +0000 Subject: [PATCH] temp branch with changes for hand C hybrid controller --- .../host/lh/sr_edc_hybrid_controller_PWM.yaml | 204 +++++++++++++++++ .../host/rh/sr_edc_hybrid_controller_PWM.yaml | 205 ++++++++++++++++++ .../sr_edc_default_controllers.launch | 1 + 3 files changed, 410 insertions(+) create mode 100644 sr_ethercat_hand_config/controls/host/lh/sr_edc_hybrid_controller_PWM.yaml create mode 100644 sr_ethercat_hand_config/controls/host/rh/sr_edc_hybrid_controller_PWM.yaml diff --git a/sr_ethercat_hand_config/controls/host/lh/sr_edc_hybrid_controller_PWM.yaml b/sr_ethercat_hand_config/controls/host/lh/sr_edc_hybrid_controller_PWM.yaml new file mode 100644 index 00000000..c773a4ab --- /dev/null +++ b/sr_ethercat_hand_config/controls/host/lh/sr_edc_hybrid_controller_PWM.yaml @@ -0,0 +1,204 @@ +sh_lh_hybrid_controller: + gains: + lh_FFJ0: + d: 150.0 + deadband: 0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 9500.0 + position_deadband: 0.002 + lh_FFJ3: + d: -270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -5700.0 + position_deadband: 0.002 + lh_FFJ4: + d: 310.0 + deadband: 0.02 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: 7500.0 + position_deadband: 0.003 + lh_LFJ0: + d: -150.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -8000.0 + position_deadband: 0.002 + lh_LFJ3: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 5700.0 + position_deadband: 0.002 + lh_LFJ4: + d: -310.0 + deadband: 0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: -7100.0 + position_deadband: 0.003 + lh_LFJ5: + d: -270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: -7300.0 + position_deadband: 0.003 + lh_MFJ0: + d: 150.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 9500.0 + position_deadband: 0.002 + lh_MFJ3: + d: -270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -5900.0 + position_deadband: 0.002 + lh_MFJ4: + d: 310.0 + deadband: 0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: 7100.0 + position_deadband: 0.003 + lh_RFJ0: + d: 190.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 7500.0 + position_deadband: 0.002 + lh_RFJ3: + d: -270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -5900.0 + position_deadband: 0.002 + lh_RFJ4: + d: 310.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: 7300.0 + position_deadband: 0.003 + lh_THJ1: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: 5900.0 + position_deadband: 0.002 + lh_THJ2: + d: -270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: -6500.0 + position_deadband: 0.002 + lh_THJ3: + d: 390.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: 7700.0 + position_deadband: 0.003 + lh_THJ4: + d: -310.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: -5900.0 + position_deadband: 0.003 + lh_THJ5: + d: 330.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: 6100.0 + position_deadband: 0.003 + lh_WRJ1: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 7500.0 + position_deadband: 0.003 + lh_WRJ2: + d: 110.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: 9100.0 + position_deadband: 0.003 + joints: + - lh_RFJ4 + - lh_LFJ4 + - lh_RFJ3 + - lh_FFJ4 + - lh_LFJ0 + - lh_MFJ3 + - lh_LFJ5 + - lh_THJ1 + - lh_MFJ4 + - lh_THJ5 + - lh_WRJ1 + - lh_THJ2 + - lh_FFJ0 + - lh_MFJ0 + - lh_WRJ2 + - lh_LFJ3 + - lh_THJ4 + - lh_THJ3 + - lh_RFJ0 + - lh_FFJ3 + type: sr_hybrid_controllers/SrHybridController diff --git a/sr_ethercat_hand_config/controls/host/rh/sr_edc_hybrid_controller_PWM.yaml b/sr_ethercat_hand_config/controls/host/rh/sr_edc_hybrid_controller_PWM.yaml new file mode 100644 index 00000000..563feca2 --- /dev/null +++ b/sr_ethercat_hand_config/controls/host/rh/sr_edc_hybrid_controller_PWM.yaml @@ -0,0 +1,205 @@ +sh_rh_hybrid_controller: + gains: + rh_FFJ0: + d: 0.0 + deadband: 0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -4900.0 + position_deadband: 0.002 + rh_FFJ3: + d: 290.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 6300.0 + position_deadband: 0.002 + rh_FFJ4: + d: -270.0 + deadband: 0.02 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: -6700.0 + position_deadband: 0.003 + rh_LFJ0: + d: 0.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 4300.0 + position_deadband: 0.002 + rh_LFJ3: + d: -290.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -5900.0 + position_deadband: 0.002 + rh_LFJ4: + d: 270.0 + deadband: 0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: 6900.0 + position_deadband: 0.003 + rh_LFJ5: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: 6300.0 + position_deadband: 0.003 + rh_MFJ0: + d: 0.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -4400.0 + position_deadband: 0.002 + rh_MFJ3: + d: 300.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 5500.0 + position_deadband: 0.002 + rh_MFJ4: + d: -270.0 + deadband: 0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: -6700.0 + position_deadband: 0.003 + rh_RFJ0: + d: 0.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -4300.0 + position_deadband: 0.002 + rh_RFJ3: + d: 200.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 4300.0 + position_deadband: 0.002 + rh_RFJ4: + d: -270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: -6100.0 + position_deadband: 0.003 + rh_THJ1: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 5300.0 + position_deadband: 0.002 + rh_THJ2: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 5700.0 + position_deadband: 0.002 + rh_THJ3: + d: -370.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 400.0 + p: -7900.0 + position_deadband: 0.003 + rh_THJ4: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: 5100.0 + position_deadband: 0.003 + rh_THJ5: + d: 330.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 500.0 + p: 5900.0 + position_deadband: 0.003 + rh_WRJ1: + d: 270.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: 6700.0 + position_deadband: 0.003 + rh_WRJ2: + d: -120.0 + deadband: 0.0 + friction_deadband: 2000.0 + i: 0.0 + i_clamp: 0.0 + max_force: 600.0 + p: -8100.0 + position_deadband: 0.003 + joints: + - rh_LFJ5 + - rh_THJ4 + - rh_MFJ0 + - rh_LFJ4 + - rh_RFJ3 + - rh_LFJ3 + - rh_WRJ1 + - rh_FFJ4 + - rh_MFJ4 + - rh_FFJ3 + - rh_THJ1 + - rh_THJ3 + - rh_THJ2 + - rh_RFJ4 + - rh_WRJ2 + - rh_MFJ3 + - rh_RFJ0 + - rh_FFJ0 + - rh_LFJ0 + - rh_THJ5 + type: sr_hybrid_controllers/SrHybridController + torque_to_pwm_scaling: [10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0] diff --git a/sr_ethercat_hand_config/controls/sr_edc_default_controllers.launch b/sr_ethercat_hand_config/controls/sr_edc_default_controllers.launch index 866cc645..5ef4aa5e 100644 --- a/sr_ethercat_hand_config/controls/sr_edc_default_controllers.launch +++ b/sr_ethercat_hand_config/controls/sr_edc_default_controllers.launch @@ -20,4 +20,5 @@ +