from sot_talos_balance.utils.gazebo_utils import GazeboLinkStatePublisher from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass pub = GazeboLinkStatePublisher('base_link', 1000) print("Starting Gazebo link state publisher...") pub.start() print("Gazebo link state publisher started") input("Wait before running the test") run_test('appli_ffSubscriber.py') sleep(2.0) runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)') sleep(5.0) runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)') sleep(5.0) runCommandClient('dump_tracer(robot.tracer)') # --- DISPLAY com_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat') pos_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.subscriber.name') + '-position.dat') vel_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.subscriber.name') + '-velocity.dat') plt.ion()
from time import sleep from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_test try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_coupled_ankle_admittance.py') sleep(1.0) RightPitchJoint = 10 LeftPitchJoint = 4 RightRollJoint = 11 LeftRollJoint = 5 input("Wait before evaluation") tauRP = evalCommandClient('robot.device.ptorque.value[RightPitchJoint]') tauLP = evalCommandClient('robot.device.ptorque.value[LeftPitchJoint]') tauRR = evalCommandClient('robot.device.ptorque.value[RightRollJoint]') tauLR = evalCommandClient('robot.device.ptorque.value[LeftRollJoint]') tauDesRP = evalCommandClient('robot.pitchController.tauDesR.value') tauDesLP = evalCommandClient('robot.pitchController.tauDesL.value') tauDesRR = evalCommandClient('robot.rollController.tauDesR.value') tauDesLR = evalCommandClient('robot.rollController.tauDesL.value')
run_ft_calibration, run_test, runCommandClient) try: # Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) use_distribute = ask_for_confirmation("Use output of force distribution?") print(("Using" if use_distribute else "Not using") + " output of force distribution") run_test('appli_dcm_zmp_control_distribute.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') if use_distribute: runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)' ) runCommandClient( 'plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)') else:
from time import sleep from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient run_test('appli_COMTraj.py') sleep(2.0) runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(5.0) runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,2.0)')
'''Test CoM admittance control as described in paper, with pre-loaded movements''' from sys import argv from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, get_file_folder, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) run_test('appli_base_estimator.py') if test_folder is not None: c = ask_for_confirmation('Execute trajectory?') if c: print('Executing the trajectory') runCommandClient('robot.triggerTrajGen.sin.value = 1') else: print('Not executing the trajectory') else: c = ask_for_confirmation("Execute a sinusoid?") if c: print("Putting the robot in position...") runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(1.0) print("Robot is in position!")
# Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) flexi = ask_for_confirmation('Compensate flexibility?') if flexi: print('Compensating flexibility') runCommandClient('flexi = False') else: print('Not compensating flexibility') runCommandClient('flexi = False') run_test('appli_dcm_zmp_control_flex_openloop.py') run_ft_calibration('robot.ftc') if flexi: input("Wait before activating flexibility") cmd_l = 'robot.hipComp.K_l.value = hipFlexCompConfig.flexibility_left' cmd_r = 'robot.hipComp.K_r.value = hipFlexCompConfig.flexibility_right' runCommandClient(cmd_l + '; ' + cmd_r) input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
from time import sleep from sot_talos_balance.utils.run_test_utils import (ask_for_confirmation, get_file_folder, run_ft_calibration, run_test, runCommandClient) try: # Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) run_test('appli_dcm_zmp_control_ffdc.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)') runCommandClient( 'plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])'
from time import sleep from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_test, runCommandClient run_test('appli_admittance_single_joint_velocity_based.py') sleep(1.0) runCommandClient('robot.admittance_control.tauDes.value = [target]') runCommandClient('robot.sot.push(robot.taskJoint.task.name)') sleep(5.0) des_tau = evalCommandClient('target') tau = evalCommandClient('robot.device.ptorque.value[JOINT]') print("Desired torque: %f" % des_tau) print("Current torque: %f" % tau)
'''Test CoM admittance control as described in paper, with pre-loaded movements''' from sys import argv from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, get_file_folder, run_test, runCommandClient test_folder, sot_talos_balance_folder = get_file_folder(argv) try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_base_estimator_integrator.py') if test_folder is not None: c = ask_for_confirmation('Execute trajectory?') if c: print('Executing the trajectory') runCommandClient('robot.triggerTrajGen.sin.value = 1') else: print('Not executing the trajectory') else: c = ask_for_confirmation("Execute a sinusoid?") if c: print("Putting the robot in position...") runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(1.0) print("Robot is in position!")
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmAnkleControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') # runCommandClient('robot.distribute.phase.value = -1') runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)') runCommandClient('robot.rightRollAnkleController.Kp.value = KpRoll') runCommandClient('robot.rightPitchAnkleController.Kp.value = KpPitch') runCommandClient('robot.leftRollAnkleController.Kp.value = KpRoll') runCommandClient('robot.leftPitchAnkleController.Kp.value = KpPitch') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') c = ask_for_confirmation("Execute a sinusoid?")
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmCoupledAndSingleAnkleControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)') runCommandClient('robot.pitchController.kSum.value = [1e-3]') runCommandClient('robot.pitchController.kDiff.value = [0.0]') runCommandClient('robot.rollController.kSum.value = [1e-3]') runCommandClient('robot.rollController.kDiff.value = [0.0]') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') c = ask_for_confirmation("Execute a sinusoid?") if c:
from time import sleep from sot_talos_balance.utils.run_test_utils import (ask_for_confirmation, get_file_folder, run_ft_calibration, run_test, runCommandClient) try: # Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) run_test('appli_dcm_zmp_control_integrator.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])' ) runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.dcm_control.resetDcmIntegralError()')
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_waistControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Running the test') # runCommandClient('plug(robot.baseselec.sout, robot.dynamic.ffposition)') c = ask_for_confirmation("Execute a sinusoid?") if c: print("Putting the robot in position...") runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(1.0) print("Robot is in position!") c2 = ask_for_confirmation("Confirm executing the sinusoid?") if c2: print("Executing the sinusoid...") runCommandClient('robot.comTrajGen.startSinusoid(1,0.025,2.0)')
from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass test_folder = argv[1] if len( argv) > 1 else 'TestKajita2003WalkingOnSpot64/DSP20SSP780' print('Using folder ' + test_folder) runCommandClient('test_folder = "' + test_folder + '"') run_test('appli_dcmZmpControl_file.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])' ) runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.dcm_control.resetDcmIntegralError()')
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmZmpControl_distribute.py') run_ft_calibration('robot.ftc') use_force_distribution = ask_for_confirmation( "Use output of force distribution?") if use_force_distribution: print("Using output of force distribution") else: print("Not using output of force distribution") input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') if use_force_distribution: runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)'
from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, evalCommandClient, run_test, runCommandClient run_test('appli_simple_ankle_admittance.py') sleep(1.0) RightPitchJoint = 10 LeftPitchJoint = 4 RightRollJoint = 11 LeftRollJoint = 5 c = ask_for_confirmation( "Do you want to use the current torques values as reference?") if c: runCommandClient( 'robot.rightPitchAnkleController.tauDes.value = robot.device.ptorque.value[RightPitchJoint]' ) runCommandClient( 'robot.leftPitchAnkleController.tauDes.value = robot.device.ptorque.value[LeftPitchJoint]' ) runCommandClient( 'robot.rightRollAnkleController.tauDes.value = robot.device.ptorque.value[RightRollJoint]' ) runCommandClient( 'robot.leftRollAnkleController.tauDes.value = robot.device.ptorque.value[LeftRollJoint]' ) print("Setting desired torques with current values") else:
'''Test CoM admittance control as described in paper.''' from time import sleep import matplotlib.pyplot as plt import numpy as np from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmZmpControl_feedback.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient('robot.com_admittance_control.setState(comDes,[0.0,0.0,0.0])') runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') sleep(30.0)
from sot_talos_balance.utils.run_test_utils import run_test run_test('appli_kinematics.py')
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmZmpWaistControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(rrobot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])' ) runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.rightAnkleController.gainsXY.value = Kp_ankles') runCommandClient('robot.leftAnkleController.gainsXY.value = Kp_ankles') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm')
from time import sleep from sot_talos_balance.utils.run_test_utils import (ask_for_confirmation, get_file_folder, run_ft_calibration, run_test, runCommandClient) try: # Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) run_test('appli_dcm_zmp_control.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])' ) runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.dcm_control.resetDcmIntegralError()')
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmZmpControl_estimator.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])' ) runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') c = ask_for_confirmation("Execute a sinusoid?") if c:
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmSingleAnkleControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') # runCommandClient('robot.distribute.phase.value = -1') runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)') runCommandClient('robot.rightRollAnkleController.Kp.value = KpRoll') runCommandClient('robot.rightPitchAnkleController.Kp.value = KpPitch') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') c = ask_for_confirmation("Execute a sinusoid?") if c: print("Putting the robot in position...")
from time import sleep import matplotlib.pyplot as plt import numpy as np from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_zmpEstimator.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # plug ZMP emergency signal runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') sleep(2.0) runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(20.0) runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,2.0)') sleep(20.0) runCommandClient('dump_tracer(robot.tracer)') # --- DISPLAY zmpEst_data = np.loadtxt('/tmp/dg_' +
'''Test feet admittance control''' from sot_talos_balance.utils.run_test_utils import run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_feet_admittance.py') run_ft_calibration('robot.ftc') input("Wait before running the test") print('Set saturation value') runCommandClient( 'robot.admBF_dqSaturation.sin.value = [0.0, 0.0, 0.01, 0.0, 0.0, 0.0]') input("Wait before dumping the data") runCommandClient('dump_tracer(robot.tracer)')
'''Test CoM admittance control without using the ZMP, with CoM computed as implemented in reference code.''' from time import sleep import matplotlib.pyplot as plt import numpy as np from sot_talos_balance.utils.gazebo_utils import apply_force from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmComControl.py') sleep(5.0) # Connect CoM reference and reset controllers print('Connect CoM reference') runCommandClient('robot.com_admittance_control.setState(robot.dynamic.com.value,[0.0,0.0,0.0])') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('plug(robot.dcm_control.ddcomRef,robot.com_admittance_control.ddcomDes)') sleep(5.0) print('Kick the robot...') apply_force([-1000.0, 0, 0], 0.01) print('... kick!')
from time import sleep from sot_talos_balance.utils.run_test_utils import run_ft_wrist_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_admittance_end_effector.py') run_ft_wrist_calibration('robot.forceCalibrator') input("Wait before running the test") sleep(10.0) # --- DISPLAY # force_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.controller.name') + '-force.dat') # w_force_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.controller.name') + '-w_force.dat') # plt.ion() # plt.figure() # plt.plot(force_data[:,1],'b-') # plt.plot(force_data[:,2],'r-') # plt.plot(force_data[:,3],'g-') # plt.title('force') # plt.legend(['force x', 'force y', 'force z'])
# Python 2 input = raw_input # noqa except NameError: pass test_folder, sot_talos_balance_folder = get_file_folder(argv) flexi = ask_for_confirmation('Compensate flexibility?') if flexi: print('Compensating flexibility') runCommandClient('flexi = False') else: print('Not compensating flexibility') runCommandClient('flexi = False') run_test('appli_dcm_zmp_control_flex.py') run_ft_calibration('robot.ftc') if flexi: input("Wait before activating flexibility") cmd_l = 'robot.hipComp.K_l.value = hipFlexCompConfig.flexibility_left' cmd_r = 'robot.hipComp.K_r.value = hipFlexCompConfig.flexibility_right' runCommandClient(cmd_l + '; ' + cmd_r) input("Wait before running the test") # Connect ZMP reference and reset controllers print('Connect ZMP reference') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)')
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmZmpFootControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient('plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient('plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)') runCommandClient('plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient('robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])') runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.dcm_control.resetDcmIntegralError()') runCommandClient('robot.dcm_control.Ki.value = Ki_dcm') input("Wait before activating foot force difference control") runCommandClient('robot.ffdc.dfzAdmittance.value = dfzAdmittance') runCommandClient('robot.ffdc.vdcFrequency.value = vdcFrequency') runCommandClient('robot.ffdc.vdcDamping.value = vdcDamping')
'''Test CoM admittance control as described in paper''' from sys import argv from time import sleep from sot_talos_balance.utils.run_test_utils import (ask_for_confirmation, get_file_folder, run_ft_calibration, run_test, runCommandClient) test_folder, sot_talos_balance_folder = get_file_folder(argv) run_test('appli_no_admittance.py') run_ft_calibration('robot.ftc') if test_folder is not None: c = ask_for_confirmation('Execute trajectory?') if c: print('Executing the trajectory') runCommandClient('robot.triggerTrajGen.sin.value = 1') else: print('Not executing the trajectory') else: c = ask_for_confirmation("Execute a sinusoid?") if c: print("Putting the robot in position...") runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(1.0) print("Robot is in position!") c2 = ask_for_confirmation("Confirm executing the sinusoid?")
'''Test CoM admittance control as described in paper.''' from time import sleep from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, run_ft_calibration, run_test, runCommandClient try: # Python 2 input = raw_input # noqa except NameError: pass run_test('appli_dcmZmpCopControl.py') run_ft_calibration('robot.ftc') input("Wait before running the test") # Connect ZMP reference and reset controllers print('Set controller') runCommandClient( 'plug(robot.zmp_estimator.emergencyStop,robot.cm.emergencyStop_zmp)') runCommandClient( 'plug(robot.distribute.emergencyStop,robot.cm.emergencyStop_distribute)') runCommandClient( 'plug(robot.distribute.zmpRef,robot.com_admittance_control.zmpDes)') runCommandClient( 'robot.com_admittance_control.setState(robot.wp.comDes.value,[0.0,0.0,0.0])' ) runCommandClient('robot.com_admittance_control.Kp.value = Kp_adm') runCommandClient('robot.rightAnkleController.gainsXY.value = Kp_ankles') runCommandClient('robot.leftAnkleController.gainsXY.value = Kp_ankles') runCommandClient('robot.dcm_control.resetDcmIntegralError()')