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-')