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_' + evalCommandClient('robot.zmp_estimator.name') + '-zmp.dat') zmpDyn_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-zmp.dat') com_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') +
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) runCommandClient('dump_tracer(robot.tracer)') # --- DISPLAY comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-dcmDes.dat') # desired CoM (workaround)
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)')
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']) # plt.figure() # plt.plot(w_force_data[:,1],'b-') # plt.plot(w_force_data[:,2],'r-') # plt.plot(w_force_data[:,3],'g-') # plt.title('w_force') # plt.legend(['w_force x', 'w_force y', 'w_force z']) input("Wait before leaving the simulation") runCommandClient('dump_tracer(robot.tracer)')
def handleRunCommandClient(code): out = runCommandClient(code) if out.standarderror: print("standarderror: " + out.standarderror) sys.exit(-1)
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: 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')
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() plt.figure() plt.plot(com_data[:, 1], 'b-') plt.plot(com_data[:, 2], 'r-')
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)
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: c2 = ask_for_confirmation( "Do you want to use nul torques values as reference?")
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: 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, with pre-loaded movements''' from sys import argv from sot_talos_balance.utils.run_test_utils import ask_for_confirmation, 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_base_estimator.py') c = ask_for_confirmation('Execute trajectory?') if c: print('Executing the trajectory') runCommandClient('robot.triggerTrajGen.sin.value = 1') else: print('Not executing the trajectory') input("Wait before dumping the data")
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_dcmCoupledAnkleControl.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 = [0.0]') runCommandClient('robot.pitchController.kDiff.value = [0.0]') runCommandClient('robot.rollController.kSum.value = [0.0]') 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: print("Putting the robot in position...") runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(1.0) print("Robot is in position!")
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)') print("Sinusoid started!") else: print("Not executing the sinusoid") c3 = ask_for_confirmation("Put the robot back?") if c3: print("Stopping the robot...") runCommandClient('robot.comTrajGen.stop(1)')
'''Test CoM admittance control as described in paper, with pre-loaded movements''' from sys import argv 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])' )
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!') sleep(5.0) runCommandClient('dump_tracer(robot.tracer)') # --- DISPLAY
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...") runCommandClient('robot.comTrajGen.move(1,-0.025,1.0)') sleep(1.0) print("Robot is in position!")
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) 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")
'''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)')
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?") if c2: print("Executing the sinusoid...") runCommandClient('robot.comTrajGen.startSinusoid(1,0.025,2.0)') print("Sinusoid started!")
from time import sleep from sot_talos_balance.utils.run_test_utils import evalCommandClient, run_test, runCommandClient run_test('appli_admittance_single_joint.py') sleep(1.0) runCommandClient('robot.admittance_control.tauDes.value = [target]') runCommandClient('robot.admittance_control.setPosition([ robot.device.state.value[QJOINT] ])') 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)