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)
