[removed talos_conf] and add talos directory that contains all config files

python/sot_talos_balance/talos/admittance_ctrl_conf.py

+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)

python/sot_talos_balance/talos/balance_ctrl_conf.py

+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.);

python/sot_talos_balance/talos/balance_ctrl_sim_conf.py

+
+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);

python/sot_talos_balance/talos/base_estimator_sim_conf.py

+
+''' *********************** USER-PARAMETERS OF BASE ESTIMATOR *********************** '''
+from base_estimator_conf import *
+K                   = 6*(1e7,);

python/sot_talos_balance/talos/control_manager_conf.py

+# -*- 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)

python/sot_talos_balance/talos/control_manager_sim_conf.py

+# -*- 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)

python/sot_talos_balance/talos/current_controller_conf.py

+# -*- 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

python/sot_talos_balance/talos/current_controller_sim_conf.py

+"""
+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.,];

python/sot_talos_balance/talos/force_torque_estimator_conf.py

+# -*- 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)

python/sot_talos_balance/talos/joint_torque_controller_conf.py

+# -*- 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

python/sot_talos_balance/talos/joint_torque_controller_sim_conf.py

+# -*- 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