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')
print("Desired torques: %f, %f, %f, %f" %
(tauDesRP[0], tauDesLP[0], tauDesRR[0], tauDesLR[0]))
print("Current torques: %f, %f, %f, %f" % (tauRP, tauLP, tauRR, tauLR))
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
dcm_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.estimator.name') + '-dcm.dat')
zmp_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-zmp.dat')
zmpDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpRef.dat')
com_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')
comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.com_admittance_control.name') + '-comRef.dat')
plt.ion()
plt.figure()
plt.plot(dcm_data[:, 1], 'b-')
plt.plot(dcm_data[:, 2], 'r-')
plt.title('DCM')
plt.legend(['x', 'y'])
plt.figure()
plt.plot(com_data[:, 1], 'b-')
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') +
'-com.dat')
forceRLEG_data = np.loadtxt('/tmp/dg_' +
evalCommandClient('robot.device.name') +
'-forceRLEG.dat')
forceLLEG_data = np.loadtxt('/tmp/dg_' +
evalCommandClient('robot.device.name') +
'-forceLLEG.dat')
plt.ion()
plt.figure()
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)
comEst_data = np.loadtxt('/tmp/dg_' +
evalCommandClient('robot.cdc_estimator.name') +
'-c.dat') # estimated CoM (not directly employed)
comRef_data = np.loadtxt(
'/tmp/dg_' + evalCommandClient('robot.com_admittance_control.name') +
'-comRef.dat') # reference CoM
comSOT_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') +
'-com.dat') # resulting SOT CoM
dcmDes_data = np.loadtxt('/tmp/dg_' +
evalCommandClient('robot.dcm_control.name') +
'-dcmDes.dat') # desired DCM
dcmEst_data = np.loadtxt('/tmp/dg_' +
evalCommandClient('robot.estimator.name') +
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)
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-')
plt.plot(com_data[:, 3], 'g-')
plt.title('COM')
plt.legend(['x', 'y', 'z'])
plt.figure()
plt.plot(pos_data[:, 1], 'b-')
plt.plot(pos_data[:, 2], 'r-')
