diff --git a/python/sot_talos_balance/talos/__init__.py b/python/sot_talos_balance/talos/__init__.py new file mode 100644 index 0000000000000000000000000000000000000000..e69de29bb2d1d6434b8b29ae775ad8c2e48c5391 diff --git a/python/sot_talos_balance/talos/admittance_ctrl_conf.py b/python/sot_talos_balance/talos/admittance_ctrl_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..87dd8caa12d92371603e5eb118fc425210ba7811 --- /dev/null +++ b/python/sot_talos_balance/talos/admittance_ctrl_conf.py @@ -0,0 +1,9 @@ +from math import sqrt +import numpy as np + +kp_force = 6*(0.0,) +ki_force = 6*(0.0,) +kp_vel = 32*(0.0,) +ki_vel = 32*(0.0,) +force_integral_saturation = (0.0, 0.0, 160.0, 20.0, 20.0, 0.0) +force_integral_deadzone = (5.0, 5.0, 5.0, 0.5, 0.5, 0.5) diff --git a/python/sot_talos_balance/talos/balance_ctrl_conf.py b/python/sot_talos_balance/talos/balance_ctrl_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..f8bedbb32b74e404b209ce060afd590370e1fa1d --- /dev/null +++ b/python/sot_talos_balance/talos/balance_ctrl_conf.py @@ -0,0 +1,52 @@ +from math import sqrt +import numpy as np + +''' *********************** USER-PARAMETERS *********************** ''' + +COM_DES = (0.00, 0.0, 0.81); + +# CONTROLLER GAINS +NJ = 32; +kp_posture = NJ*(100.0,); # proportional gain of postural task +kd_posture = NJ*(0*sqrt(kp_posture[0]),); +kp_pos = NJ*(0.0,); # proportional gain of position controller +kd_pos = NJ*(0*sqrt(kp_pos[0]),); +kp_constr = .0*1.0; # constraint proportional feedback gain +kd_constr = 0*sqrt(kp_constr); # constraint derivative feedback gain +kp_com = .0*30.0; +kd_com = 0.0; +kp_feet = .0*30.0; +kd_feet = 0.0; +kp_admittance = (0,0,0,.0*1,.0*1,0.); +ki_admittance = 6*(0.0,); +constraint_mask = np.array([True, True, True, True, True, True]).T; +ee_mask = np.array([True, True, True, True, True, True]).T; + +# CONTROLLER WEIGTHS +w_com = .0*1.0; +w_feet = .0*1.0; +w_posture = 1e-3; # weight of postural task +w_forces = .0*1e-6; +w_base_orientation = 0.0; +w_torques = 0.0; + +# CONTACT PARAMETERS +RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.075); # pos x, neg x, pos y, neg y size +LEFT_FOOT_SIZES = (0.130, -0.100, 0.075, -0.056); # pos x, neg x, pos y, neg y size + +RIGHT_FOOT_SIZES = (0.130, -0.100, 0.056, -0.056); # pos x, neg x, pos y, neg y size +RIGHT_FOOT_CONTACT_POINTS = ((RIGHT_FOOT_SIZES[0], RIGHT_FOOT_SIZES[0], RIGHT_FOOT_SIZES[1], RIGHT_FOOT_SIZES[1]), + (RIGHT_FOOT_SIZES[3], RIGHT_FOOT_SIZES[2], RIGHT_FOOT_SIZES[3], RIGHT_FOOT_SIZES[2]), + (-0.105, -0.105, -0.105, -0.105)); # contact points in local reference frame + +LEFT_FOOT_CONTACT_POINTS = np.matrix([[LEFT_FOOT_SIZES[0], LEFT_FOOT_SIZES[3], -0.105], + [LEFT_FOOT_SIZES[0], LEFT_FOOT_SIZES[2], -0.105], + [LEFT_FOOT_SIZES[1], LEFT_FOOT_SIZES[3], -0.105], + [LEFT_FOOT_SIZES[1], LEFT_FOOT_SIZES[2], -0.105]]).T # contact points in local reference frame +FOOT_CONTACT_NORMAL = (0.0, 0.0, 1.0); +mu = np.array([0.3, 0.1]); # force and moment friction coefficient +fMin = 1.0; # minimum normal force +fMax = 1e3; # maximum normal force + +RF_FORCE_DES = (0, 0, 300, 0, 0, 0.); +LF_FORCE_DES = (0, 0, 300, 0, 0, 0.); diff --git a/python/sot_talos_balance/talos/balance_ctrl_sim_conf.py b/python/sot_talos_balance/talos/balance_ctrl_sim_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..2d3cede9c2d63d77b5563b7e50639ecccc6b45af --- /dev/null +++ b/python/sot_talos_balance/talos/balance_ctrl_sim_conf.py @@ -0,0 +1,9 @@ + +from balance_ctrl_conf import * + +# CONTROLLER GAINS +kp_posture = NJ*(400.0,); +kd_posture = NJ*(sqrt(kp_posture[0]),); +kd_constr = 0.0*2*sqrt(kp_constr); # constraint derivative feedback gain +kd_com = 0.0*2*sqrt(kp_com); +kd_feet = 0.0*2*sqrt(kp_feet); diff --git a/python/sot_talos_balance/talos_conf.py b/python/sot_talos_balance/talos/base_estimator_conf.py similarity index 100% rename from python/sot_talos_balance/talos_conf.py rename to python/sot_talos_balance/talos/base_estimator_conf.py diff --git a/python/sot_talos_balance/talos/base_estimator_sim_conf.py b/python/sot_talos_balance/talos/base_estimator_sim_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..707820c99840bb6416348c2a7278e3a4c4d0a6cc --- /dev/null +++ b/python/sot_talos_balance/talos/base_estimator_sim_conf.py @@ -0,0 +1,4 @@ + +''' *********************** USER-PARAMETERS OF BASE ESTIMATOR *********************** ''' +from base_estimator_conf import * +K = 6*(1e7,); diff --git a/python/sot_talos_balance/talos/control_manager_conf.py b/python/sot_talos_balance/talos/control_manager_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..3e2f956581f395beb91712f2761f95f52b8225c1 --- /dev/null +++ b/python/sot_talos_balance/talos/control_manager_conf.py @@ -0,0 +1,114 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" +import numpy as np + +NJ = 32; +TAU_MAX = 1.*1e2; # max joint torques (security check of ControlManager) +CURRENT_MAX = 20.0; # max motor current (security check of ControlManager) +CTRL_MAX = 20.0; # max desired current (security check of ControlManager) +model_path = ["/opt/openrobots/share"]; +urdfFileName = model_path[0] + "/talos_data/robots/talos_reduced.urdf"; +ImuJointName = "imu_joint"; + +mapJointNameToID={ + 'lhy' : 0, + 'lhr' : 1, + 'lhp' : 2, + 'lk' : 3, + 'lap' : 4, + 'lar' : 5, + 'rhy' : 6, + 'rhr' : 7, + 'rhp' : 8, + 'rk' : 9, + 'rap' : 10, + 'rar' : 11, + 'ty' : 12, + 'tp' : 13, + 'lsy' : 14, + 'lsr' : 15, + 'lay' : 16, + 'le' : 17, + 'lwy' : 18, + 'lwp' : 19, + 'lwr' : 20, + 'lh' : 21, + 'rsy' : 22, + 'rsr' : 23, + 'ray' : 24, + 're' : 25, + 'rwy' : 26, + 'rwp' : 27, + 'rwr' : 28, + 'rh' : 29, + 'hp' : 30, + 'hy' : 31 +} + + + +mapJointLimits={ + 0 : [-0.349065850399, 1.57079632679], + 1 : [-0.5236, 0.5236], + 2 : [-2.095, 0.7], + 3 : [0.0, 2.618], + 4 : [-1.309, 0.768], + 5 : [-0.5236, 0.5236], + 6 : [-1.57079632679, 0.349065850399], + 7 : [-0.5236, 0.5236], + 8 : [-2.095, 0.7], + 9 : [0.0, 2.618], + 10 : [-1.309, 0.768], + 11 : [-0.5236, 0.5236], + 12 : [-1.308996939, 1.308996939], + 13 : [-0.261799387799, 0.785398163397], + 14 : [-1.57079632679, 0.523598775598], + 15 : [0.0, 2.87979326579], + 16 : [-2.44346095279, 2.44346095279], + 17 : [-2.35619449019, 0.0], + 18 : [-2.53072741539, 2.53072741539], + 19 : [-1.3962634016, 1.3962634016], + 20 : [-0.698131700798, 0.698131700798], + 21 : [-1.0471975512, 0.0], + 22 : [-0.523598775598, 1.57079632679], + 23 : [-2.87979326579, 0.0], + 24 : [-2.44346095279, 2.44346095279], + 25 : [-2.35619449019, 0.0], + 26 : [-2.53072741539, 2.53072741539], + 27 : [-1.3962634016, 1.3962634016], + 28 : [-0.698131700798, 0.698131700798], + 29 : [-1.0471975512, 0.0], + 30 : [-0.261799387799, 0.785398163397], + 31 : [-1.308996939, 1.308996939] +} + +vfMax=np.array([100.0,100.0,300.0,80.0,80.0,30.0]) +vfMin=-vfMax +mapForceIdToForceLimits={ + 0: [vfMin,vfMax], + 1: [vfMin,vfMax], + 2: [vfMin,vfMax], + 3: [vfMin,vfMax] + } + +mapNameToForceId={ + "rf": 0, + "lf": 1, + "rh": 2, + "lh": 3 + } + +indexOfForceSensors= () + +footFrameNames= { + "Right": "leg_right_6_joint", + "Left" : "leg_left_6_joint" +} + +rightFootSensorXYZ = (0.0,0.0,-0.085) +rightFootSoleXYZ = (0.0,0.0,-0.105) + +urdftosot=(0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19,20,21,22,23,24,25,26,27,28,29,30,31) diff --git a/python/sot_talos_balance/talos/control_manager_sim_conf.py b/python/sot_talos_balance/talos/control_manager_sim_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..51ce2f4592afa5acd30e4a03e24568cd40cd0245 --- /dev/null +++ b/python/sot_talos_balance/talos/control_manager_sim_conf.py @@ -0,0 +1,11 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" + +from control_manager_conf import * + +TAU_MAX = 1e6; # security check of ControlManager +CURRENT_MAX = 1e6; # security check of ControlManager +CTRL_MAX = 1e6; # max desired current (security check of ControlManager) diff --git a/python/sot_talos_balance/talos/current_controller_conf.py b/python/sot_talos_balance/talos/current_controller_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..52fed568f0ae8b6e989627b3adc82f2739395788 --- /dev/null +++ b/python/sot_talos_balance/talos/current_controller_conf.py @@ -0,0 +1,70 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" +import numpy as np + +NJ = 32; +CURRENT_OFFSET_ITERS = 200; # Number of itertion while control is disabled to calibrate current sensors +CURRENT_MAX = 8.0; # max motor current (security check of ControlManager) +CTRL_MAX = 10.0; # max desired current (security check of ControlManager) +CTRL_SATURATION = 2047; # saturation of the control signal +IN_OUT_GAIN = 102.4; # factor to convert from a [-20.0 ; 20.0] Ampers value to the [-2048 ; 2048] 12bit DAC register +percentage_dead_zone_compensation = NJ*[0.,]; # percentage of dead zone to compensate (used by ControlManager) +i_max_dz_comp = NJ*[0.05,]; # value of current tracking error at which deadzone is completely compensated +percentage_bemf_compensation = NJ*[0.,] +current_sensor_offsets_low_level = NJ*[0.,] +kp_current = NJ*[0.,]; +ki_current = np.array(NJ*[0.,]); + +percentage_bemf_compensation[0] = 0.9 +percentage_bemf_compensation[1] = 0.9 # a bit unstable +percentage_bemf_compensation[2] = 0.9 +percentage_bemf_compensation[3] = 0.9 +percentage_bemf_compensation[4] = 1.0 +percentage_bemf_compensation[5] = 0.9 +percentage_bemf_compensation[6] = 0.9 +percentage_bemf_compensation[7] = 0.9 +percentage_bemf_compensation[8] = 0.9 +percentage_bemf_compensation[9] = 0.9 +percentage_bemf_compensation[10] = 0.9 +percentage_bemf_compensation[11] = 0.9 + +percentage_dead_zone_compensation[0] = 0.8 +percentage_dead_zone_compensation[1] = 0.8 +percentage_dead_zone_compensation[2] = 0.8 # could go up to 1.2 +percentage_dead_zone_compensation[3] = 0.8 +percentage_dead_zone_compensation[4] = 0.8 # a bit unstable +percentage_dead_zone_compensation[5] = 0.8 +percentage_dead_zone_compensation[6] = 0.8 +percentage_dead_zone_compensation[7] = 0.8 +percentage_dead_zone_compensation[8] = 0.8 +percentage_dead_zone_compensation[9] = 0.8 +percentage_dead_zone_compensation[10] = 0.8 # very asymettric error, a bit unstable on negative currents, may need investigation +percentage_dead_zone_compensation[11] = 0.8 # getting some vibrations for negative errors, may need investigation + +i_max_dz_comp[0] = 0.03 + +current_sensor_offsets_low_level[0] = 0.04 +current_sensor_offsets_low_level[2] = -0.05 +current_sensor_offsets_low_level[5] = 0.02 +current_sensor_offsets_low_level[6] = 0.01 +current_sensor_offsets_low_level[7] = 0.01 +current_sensor_offsets_low_level[8] = -0.01 +current_sensor_offsets_low_level[10] = -0.11 +current_sensor_offsets_low_level[11] = -0.04 + +ki_current[0] = 3e-3 # could go higher +ki_current[1] = 3e-3 # could go higher +ki_current[2] = 1e-3 # could probably go higher +ki_current[3] = 3e-3 +ki_current[4] = 1e-3 # could probably go higher +ki_current[5] = 3e-3 # could probably go higher +ki_current[6] = 1e-3 +ki_current[7] = 1e-3 +ki_current[8] = 1e-3 +ki_current[9] = 1e-3 +ki_current[10] = 1e-3 +ki_current[11] = 1e-3 +#ki_current[:] = 0.0 diff --git a/python/sot_talos_balance/talos/current_controller_sim_conf.py b/python/sot_talos_balance/talos/current_controller_sim_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..ca0bb4b35d255ffdd1ff7a0883212d4715ba1240 --- /dev/null +++ b/python/sot_talos_balance/talos/current_controller_sim_conf.py @@ -0,0 +1,15 @@ +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" +from current_controller_conf import * + +CURRENT_OFFSET_ITERS = 0; # Number of itertion while control is disabled to calibrate current sensors +CURRENT_MAX = 1e6; # max motor current (security check of ControlManager) +CTRL_MAX = 1e6; # max desired current (security check of ControlManager) +CTRL_SATURATION = 1e6; # saturation of the control signal +IN_OUT_GAIN = 1.; # factor to convert from a [-20.0 ; 20.0] Ampers value to the [-2048 ; 2048] 12bit DAC register +percentage_dead_zone_compensation = NJ*[0.,]; # percentage of dead zone to compensate (used by ControlManager) +percentage_bemf_compensation = NJ*[0.,] +kp_current = NJ*[0.,]; +ki_current = NJ*[0.,]; diff --git a/python/sot_talos_balance/talos/force_torque_estimator_conf.py b/python/sot_talos_balance/talos/force_torque_estimator_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..a07c60fc09a46eb244abddf59537b9a85732e86f --- /dev/null +++ b/python/sot_talos_balance/talos/force_torque_estimator_conf.py @@ -0,0 +1,13 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" + +CURRENT_TORQUE_ESTIMATION_TRUST = 0.0; # weight for current-base torque estimation (in [0,1]) of ForceTorqueEstimator +SATURATION_CURRENT = 10.0; # Saturation of current sensor (used by ForceTorqueEstimator) +DELAY_ENC = 40; # estimation delay expressed relative to the timestep (i.e. delay = DELAY_ENC*dt) +DELAY_ACC = 40; # estimation delay expressed relative to the timestep (i.e. delay = DELAY_ACC*dt) +DELAY_GYRO = 40; # estimation delay expressed relative to the timestep (i.e. delay = DELAY_ACC*dt) +DELAY_FORCE = 40; # estimation delay expressed relative to the timestep (i.e. delay = DELAY_ACC*dt) +DELAY_CURRENT = 40; # estimation delay expressed relative to the timestep (i.e. delay = DELAY_ACC*dt) diff --git a/python/sot_talos_balance/talos/joint_torque_controller_conf.py b/python/sot_talos_balance/talos/joint_torque_controller_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..48c26d019627a2373d049d48c3f025cd66593a15 --- /dev/null +++ b/python/sot_talos_balance/talos/joint_torque_controller_conf.py @@ -0,0 +1,40 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" +import numpy as np + +NJ = 32; + +COULOMB_FRICTION_COMPENSATION_PERCENTAGE = 0.0; +k_p_torque = np.array(NJ*[0.5,]); # torque control proportional gains +k_d_torque = np.array(NJ*[0.0,]); # torque control derivative gains +k_i_torque = np.array(NJ*[3.0,]); # torque control integral gains +k_d_vel = np.array(NJ*[0.0,]); # velocity feedback gains +k_i_vel = np.array(NJ*[0.0,]); # velocity feedback gains +torque_integral_saturation = np.array(NJ*[0.0,]); +poly_sign_dq = 3; # order of polynomial to approximate Coulomb friction around zero velocity +alpha_leaking = np.array([75.,75.,75.,75.,75.,75., + 75.,75.,75.,75.,75.,75., + 0.0,0.0, + 0.0,0.0,0.0,0.0,0.0,0.0,0.0, + 0.0, + 0.0,0.0,0.0,0.0,0.0,0.0,0.0, + 0.0, + 0.0,0.0]); + +## PARAMETERS OF R_hip_y JOINT 0 +#k_p_torque[0] = 12.0; +## PARAMETERS OF R_hip_r JOINT 1 +#k_p_torque[1] = 5; #15.0 # could easily go up to 20, but it's a bit less stable +## PARAMETERS OF R_hip_p JOINT 2 +#k_p_torque[2] = 6; #with delay 30 ms +## PARAMETERS OF R_knee JOINT 3 +#k_p_torque[3] = 10.0; # with 12 it starts vibrating at low velocity +## PARAMETERS OF R_ankle pitch JOINT 4 +#k_p_torque[4] = 10.0; # 10 feels good, but maybe i could go higher +## PARAMETERS OF R_ankle roll JOINT 5 +#k_p_torque[5] = 15.0; # could go higher, but it feels already good +## PARAMETERS OF Left hip pitch JOINT 8 +#k_p_torque[8] = 6.0; # with delay 20 ms diff --git a/python/sot_talos_balance/talos/joint_torque_controller_sim_conf.py b/python/sot_talos_balance/talos/joint_torque_controller_sim_conf.py new file mode 100644 index 0000000000000000000000000000000000000000..326be151cb0056ac9f097676fcb3473e4337d469 --- /dev/null +++ b/python/sot_talos_balance/talos/joint_torque_controller_sim_conf.py @@ -0,0 +1,39 @@ +# -*- coding: utf-8 -*- +""" +Created on Mon Feb 9 13:55:16 2015 +@author: adelpret +""" +import numpy as np + +NJ = 32; + +COULOMB_FRICTION_COMPENSATION_PERCENTAGE = 0.0; +k_p_torque = np.array(NJ*[0.5,]); # torque control proportional gains +k_d_torque = np.array(NJ*[0.0,]); # torque control derivative gains +k_i_torque = np.array(NJ*[3.0,]); # torque control integral gains +torque_integral_saturation = np.array(NJ*[0.0,]); +poly_sign_dq = 3; # order of polynomial to approximate Coulomb friction around zero velocity +k_d_velocity = np.array(NJ*[0.0,]); # torque control derivative gains +alpha_leaking = np.array([75.,75.,75.,75.,75.,75., + 75.,75.,75.,75.,75.,75., + 0.0,0.0, + 0.0,0.0,0.0,0.0,0.0,0.0,0.0, + 0.0, + 0.0,0.0,0.0,0.0,0.0,0.0,0.0, + 0.0, + 0.0,0.0]); + +## PARAMETERS OF R_hip_y JOINT 0 +#k_p_torque[0] = 12.0; +## PARAMETERS OF R_hip_r JOINT 1 +#k_p_torque[1] = 5; #15.0 # could easily go up to 20, but it's a bit less stable +## PARAMETERS OF R_hip_p JOINT 2 +#k_p_torque[2] = 6; #with delay 30 ms +## PARAMETERS OF R_knee JOINT 3 +#k_p_torque[3] = 10.0; # with 12 it starts vibrating at low velocity +## PARAMETERS OF R_ankle pitch JOINT 4 +#k_p_torque[4] = 10.0; # 10 feels good, but maybe i could go higher +## PARAMETERS OF R_ankle roll JOINT 5 +#k_p_torque[5] = 15.0; # could go higher, but it feels already good +## PARAMETERS OF Left hip pitch JOINT 8 +#k_p_torque[8] = 6.0; # with delay 20 ms