Clean-up command files

python/cmd/cmd_current_ctrl_tuning.py

 create_topic(robot.ros, robot.device.currents,                    'i');
+create_topic(robot.ros, robot.device.control,                    'ctrl');
 create_topic(robot.ros, robot.ctrl_manager.currents_real,         'i_real');
 create_topic(robot.ros, robot.ctrl_manager.pwmDes,                'i_des')
 create_topic(robot.ros, robot.device.robotState,                  'robotState')

python/cmd/cmd_hrp2_balance_ctrl.py

@@ -10,12 +10,13 @@ robot.timeStep=0.0015
 robot = main_v3(robot, startSoT=True, go_half_sitting=0)
 go_to_position(robot.traj_gen, 30*(0.0,), 5.0)
 
+robot.inv_dyn.w_forces.value = 1e-4
 #robot.base_estimator.set_imu_weight(0.0)
 plug(robot.filters.estimator_kin.dx,         robot.base_estimator.joint_velocities);
 #plug(robot.filters.estimator_kin.dx,         robot.current_ctrl.dq);
 #plug(robot.filters.estimator_kin.dx,         robot.torque_ctrl.jointsVelocities);
-robot.inv_dyn.kp_com.value = (30.0, 30.0, 50.0)
-robot.inv_dyn.kd_com.value = (8.0, 8.0, 0.0)
+robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
+robot.inv_dyn.kd_com.value = (0.0, 0.0, 0.0)
 robot.torque_ctrl.KpTorque.value = 30*(0.0,)
 #plug(robot.filters_sg.ft_LF_filter.x_filtered, robot.base_estimator.forceLLEG)
 #plug(robot.filters_sg.ft_RF_filter.x_filtered, robot.base_estimator.forceRLEG)
@@ -58,7 +59,7 @@ go_to_position(robot.traj_gen, robot.halfSitting[6:], 5.0);
 robot.base_estimator.reset_foot_positions();
 robot.inv_dyn.updateComOffset()
 # start torque control on leg joints
-robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lk-lar-lap','torque')
+robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
 
 # enable integral feedback in torque control
 import dynamic_graph.sot.torque_control.hrp2.motors_parameters as motor_params
@@ -108,7 +109,8 @@ create_topic(robot.ros, robot.device.accelerometer,           'accelerometer');
 
 robot.com_traj_gen.move(2, 0.85, 2.0)
 smoothly_set_signal(robot.torque_ctrl.KpTorque,30*(1.,))
-smoothly_set_signal(robot.inv_dyn.kp_posture,30*(5.,))
+smoothly_set_signal(robot.inv_dyn.kp_posture,30*(20.,))
+smoothly_set_signal(robot.inv_dyn.kp_pos,30*(50.,))
 
 robot.com_traj_gen.stop(-1)
 robot.com_traj_gen.move(1, 0.03, 1.5)

python/cmd/cmd_test_admittance_ctrl.py

 from dynamic_graph.sot.torque_control.main import *
 from dynamic_graph.sot.torque_control.utils.sot_utils import smoothly_set_signal,  stop_sot
 from dynamic_graph import plug
+import numpy as np
 robot.timeStep=0.0015
-robot = main_v3(robot, startSoT=True, go_half_sitting=1)
+robot = main_v3(robot, startSoT=1, go_half_sitting=1)
 
-plug(robot.filters.estimator_kin.dx,         robot.base_estimator.joint_velocities);
-robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
-robot.inv_dyn.kd_com.value = (0.0, 0.0, 0.0)
 robot.torque_ctrl.KpTorque.value = 30*(0.0,)
+robot.inv_dyn.kp_com.value = (20.0, 20.0, 20.0)
+robot.inv_dyn.kd_com.value = (5.0, 5.0, 0.0)
+robot.inv_dyn.kp_posture.value = 30*(30.,)
+robot.base_estimator.reset_foot_positions();
+robot.inv_dyn.updateComOffset()
+
+robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lar-lap','torque')
+
+robot.traj_gen.moveJoint('lk', 0.8, 3.0)
+robot.com_traj_gen.move(2, 0.85, 3.0)
 
+create_topic(robot.ros, robot.base_estimator.q,               'q');
+create_topic(robot.ros, robot.adm_ctrl.fRightFootRef,         'f_des_R')
+create_topic(robot.ros, robot.adm_ctrl.fLeftFootRef,          'f_des_L')
+create_topic(robot.ros, robot.adm_ctrl.u,                     'u_adm')
+create_topic(robot.ros, robot.adm_ctrl.dqDes,                 'dq_des')
+create_topic(robot.ros, robot.estimator_ft.contactWrenchLeftSole,  'f_L');
+create_topic(robot.ros, robot.estimator_ft.contactWrenchRightSole, 'f_R');
 create_topic(robot.ros, robot.inv_dyn.com,                      'com')
 create_topic(robot.ros, robot.inv_dyn.com_vel,                  'com_vel')
 create_topic(robot.ros, robot.inv_dyn.com_ref_pos,              'com_ref_pos')
 create_topic(robot.ros, robot.inv_dyn.com_ref_vel,              'com_ref_vel')
-create_topic(robot.ros, robot.inv_dyn.com_acc_des,              'com_acc_des')
-create_topic(robot.ros, robot.base_estimator.q,               'q');
-create_topic(robot.ros, robot.base_estimator.v,               'v');
-create_topic(robot.ros, robot.base_estimator.v_flex,          'v_flex');
-create_topic(robot.ros, robot.base_estimator.v_kin,           'v_kin');
-create_topic(robot.ros, robot.base_estimator.v_gyr,           'v_gyr');
 create_topic(robot.ros, robot.inv_dyn.zmp_des,                  'cop_des')
 create_topic(robot.ros, robot.inv_dyn.zmp,                      'cop')
-create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot,       'cop_des_R')
-create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot,        'cop_des_L')
 create_topic(robot.ros, robot.inv_dyn.zmp_right_foot,           'cop_R')
 create_topic(robot.ros, robot.inv_dyn.zmp_left_foot,            'cop_L')
-create_topic(robot.ros, robot.inv_dyn.dq_admittance, 'dq_adm')
-create_topic(robot.ros, robot.ctrl_manager.u,                        'i_des');	
-create_topic(robot.ros, robot.filters.ft_LF_filter.x_filtered, 'f_LF_filt')
-create_topic(robot.ros, robot.filters_sg.ft_LF_filter.x_filtered, 'f_LF_filt_sg', robot, robot.filters_sg.ft_LF_filter)
+create_topic(robot.ros, robot.inv_dyn.zmp_des_right_foot,       'cop_des_R')
+create_topic(robot.ros, robot.inv_dyn.zmp_des_left_foot,        'cop_des_L')
+create_topic(robot.ros, robot.ctrl_manager.u,                   'i_des');
+create_topic(robot.ros, robot.current_ctrl.i_real,              'i_real');
+
+
+robot.adm_ctrl.force_integral_saturation.value = (0,0,160.0,20.0,20.0,0)
+kp_vel = 12*[0.5,]+18*[0.0,]
+smoothly_set_signal(robot.adm_ctrl.kp_vel, tuple(kp_vel))
+#smoothly_set_signal(robot.adm_ctrl.kp_vel, 30*(0.0,))
+smoothly_set_signal(robot.adm_ctrl.ki_force, (0, 0, 20.0, 2, 0, 0))
 
 b = (0.00554272, 0.01108543, 0.00554272)
 a = (1., -1.77863178, 0.80080265)
+b = (1.0, 0.0)
+a = (1.0, 0.0)
 robot.filters.ft_RF_filter.switch_filter(b, a)
 robot.filters.ft_LF_filter.switch_filter(b, a)
 robot.filters.ft_RH_filter.switch_filter(b, a)
 robot.filters.ft_LH_filter.switch_filter(b, a)
 robot.filters.acc_filter.switch_filter(b, a)
 
-b = (2.16439898e-05, 4.43473520e-05, -1.74065002e-05, -8.02197247e-05,  -1.74065002e-05,   4.43473520e-05, 2.16439898e-05)
-a = (1.,-5.32595322,11.89749109,-14.26803139, 9.68705647,  -3.52968633,   0.53914042)
-robot.filters.gyro_filter.switch_filter(b, a)
-robot.filters.estimator_kin.switch_filter(b, a)
-
-K = (1e10, 1e10, 1e10, 707, 502, 1e10);
-robot.base_estimator.set_stiffness_left_foot(K)
-robot.base_estimator.set_stiffness_right_foot(K)
-
-robot.base_estimator.set_imu_weight(0.0)
-robot.inv_dyn.w_forces.value = 1e-4
-
-robot.base_estimator.reset_foot_positions();
-sleep(1)
-robot.inv_dyn.updateComOffset()
-
-robot.ctrl_manager.setCtrlMode('rhp-rhy-rhr-rk-rar-rap-lhp-lhr-lhy-lk-lar-lap','torque')
-
 robot.com_traj_gen.move(1, 0.03, 1.5)
 robot.com_traj_gen.startSinusoid(1,-0.03,2.0)
-robot.torque_ctrl.KdVel.value = 30*(0.1,)
-
-ki_vel = 30*[0.0,]
-ki_vel[5] = 0.1
-ki_vel[11] = 0.1
-robot.torque_ctrl.KiVel.value = tuple(ki_vel)
 
 def print_cop_stats(robot):
   cop_L = robot.inv_dyn.zmp_left_foot.value