Esempi in Python per run_test

Esempio n. 1

File: test_ffSubscriber.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 2

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

Esempio n. 3

File: test_dcm_zmp_control_distribute.py Progetto: louise-scherrer/sot-talos-balance

                                                    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:

Esempio n. 4

File: test_COMTraj.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 5

File: test_base_estimator.py Progetto: olivier-stasse/sot-talos-balance

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

Esempio n. 6

File: test_dcm_zmp_control_flex_openloop.py Progetto: olivier-stasse/sot-talos-balance

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

Esempio n. 7

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

Esempio n. 8

File: test_admittance_single_joint_velocity_based.py Progetto: louise-scherrer/sot-talos-balance

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)

Esempio n. 9

File: test_base_estimator_integrator.py Progetto: olivier-stasse/sot-talos-balance

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

Esempio n. 10

File: test_dcmAnkleControl.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 11

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

Esempio n. 12

File: test_dcm_zmp_control_integrator.py Progetto: olivier-stasse/sot-talos-balance

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

Esempio n. 13

File: test_waistControl.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 14

File: test_dcmZmpControl_file.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 15

File: test_dcmZmpControl_distribute.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 16

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:

Esempio n. 17

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

Esempio n. 18

from sot_talos_balance.utils.run_test_utils import run_test

run_test('appli_kinematics.py')

Esempio n. 19

File: test_dcmZmpWaistControl.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 20

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

Esempio n. 21

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

Esempio n. 22

File: test_dcmSingleAnkleControl.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 23

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_' +

Esempio n. 24

File: test_feet_admittance.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 25

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

Esempio n. 26

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

Esempio n. 27

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

Esempio n. 28

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

Esempio n. 29

File: test_no_admittance.py Progetto: louise-scherrer/sot-talos-balance

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

Esempio n. 30

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