Skip to content
Snippets Groups Projects
Commit 14e8b914 authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

rework test_dcmZmpControl

parent cdcac9eb
No related branches found
No related tags found
No related merge requests found
from sot_talos_balance.create_entities_utils import *
import sot_talos_balance.control_manager_conf as param_server_conf
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from math import sqrt
from dynamic_graph.tracer_real_time import TracerRealTime
robot.timeStep = robot.device.getTimeStep()
dt = robot.timeStep;
# --- Dummy estimator
robot.estimator = create_dummy_dcm_estimator(robot)
# -------------------------- DESIRED TRAJECTORY --------------------------
# --- Desired values
robot.dynamic.com.recompute(0)
comDes = robot.dynamic.com.value
dcmDes = comDes
zmpDes = comDes[:2] + (0.0,)
ddcomDes = (0.0,0.0,0.0)
# --- Pendulum parameters
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
# -------------------------- ESTIMATION --------------------------
# --- General Estimation
robot.param_server = create_parameter_server(param_server_conf,dt)
robot_name='robot'
cdc_estimator = DcmEstimator('dcm_estimator')
cdc_estimator.init(dt, robot_name)
plug(robot.device.state, cdc_estimator.q)
plug(robot.device.velocity, cdc_estimator.v)
robot.cdc_estimator = cdc_estimator # cdc_estimator.c == robot.dynamic.com
# --- DCM Estimation
robot.estimator = create_cdc_dcm_estimator(robot)
# --- ZMP estimation
zmp_estimator = SimpleZmpEstimator("zmpEst")
robot.dynamic.createOpPoint('LF',robot.OperationalPointsMap['left-ankle'])
robot.dynamic.createOpPoint('RF',robot.OperationalPointsMap['right-ankle'])
plug(robot.dynamic.LF,zmp_estimator.poseLeft)
plug(robot.dynamic.RF,zmp_estimator.poseRight)
# plug(robot.device_filters.ft_LF_filter.x_filtered,zmp_estimator.wrenchLeft)
# plug(robot.device_filters.ft_RF_filter.x_filtered,zmp_estimator.wrenchRight)
plug(robot.device.forceLLEG,zmp_estimator.wrenchLeft)
plug(robot.device.forceRLEG,zmp_estimator.wrenchRight)
zmp_estimator.init()
robot.zmp_estimator = zmp_estimator
# -------------------------- ADMITTANCE CONTROL --------------------------
# --- DCM controller
Kp_dcm = [500.0,500.0,500.0]
Ki_dcm = [0.0,0.0,0.0] # zero (to be set later)
gamma_dcm = 0.2
dcm_controller = DcmController("dcmCtrl")
dcm_controller.Kp.value = Kp_dcm
dcm_controller.Ki.value = Ki_dcm
dcm_controller.decayFactor.value = gamma_dcm
dcm_controller.mass.value = mass
dcm_controller.omega.value = omega
plug(robot.cdc_estimator.c,dcm_controller.com)
plug(robot.estimator.dcm,dcm_controller.dcm)
dcm_controller.zmpDes.value = zmpDes # plug a signal here
dcm_controller.dcmDes.value = dcmDes # plug a signal here
dcm_controller.init(dt)
robot.dcm_control = dcm_controller
Ki_dcm = [1.0,1.0,0.0] # to be set later
robot.dcm_control = create_dcm_controller(Kp_dcm,[0.0]*3,dt,robot,robot.estimator.dcm)
# --- Admittance controller
# --- CoM admittance controller
Kp_adm = [0.0,0.0,0.0] # zero (to be set later)
com_admittance_control = ComAdmittanceController("comAdmCtrl")
com_admittance_control.Kp.value = Kp_adm
plug(robot.zmp_estimator.zmp,com_admittance_control.zmp)
com_admittance_control.zmpDes.value = zmpDes # should be plugged to robot.dcm_control.zmpRef
com_admittance_control.ddcomDes.value = ddcomDes # plug a signal here
com_admittance_control.init(dt)
com_admittance_control.setState(comDes,[0.0,0.0,0.0])
robot.com_admittance_control = com_admittance_control
Kp_adm = [20.0,10.0,0.0] # to be set later
robot.com_admittance_control = create_com_admittance_controller([0.0]*3,dt,robot)
# -------------------------- SOT CONTROL --------------------------
# --- CONTACTS
#define contactLF and contactRF
......@@ -37,12 +114,12 @@ locals()['contactRF'] = robot.contactRF
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef,robot.taskCom.featureDes.errordotIN)
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)
# --- SOT solver
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control,robot.device.control)
......@@ -52,30 +129,45 @@ robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
# -------------------------- PLOTS --------------------------
# --- ROS PUBLISHER
robot.publisher = create_rospublish(robot, 'robot_publisher')
rospub_signalName = '{0}_{1}'.format('fake', 'comDes')
topicname = '/sot/{0}/{1}'.format('fake', 'comDes')
robot.publisher.add('vector',rospub_signalName,topicname)
plug(robot.dcm_control.dcmDes, robot.publisher.signal(rospub_signalName)) # desired CoM (workaround)
create_topic(robot.publisher, robot.cdc_estimator, 'c', robot = robot, data_type='vector') # estimated CoM
create_topic(robot.publisher, robot.com_admittance_control, 'comRef', robot = robot, data_type='vector') # reference CoM
create_topic(robot.publisher, robot.dynamic, 'com', robot = robot, data_type='vector') # resulting SOT CoM
create_topic(robot.publisher, robot.dcm_control, 'dcmDes', robot = robot, data_type='vector') # desired DCM
create_topic(robot.publisher, robot.estimator, 'dcm', robot = robot, data_type='vector') # estimated DCM
create_topic(robot.publisher, robot.dcm_control, 'zmpDes', robot = robot, data_type='vector') # desired ZMP
create_topic(robot.publisher, robot.zmp_estimator, 'zmp', robot = robot, data_type='vector') # estimated ZMP
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot = robot, data_type='vector') # resulting SOT CoM
# --- TRACER
robot.tracer = TracerRealTime("zmp_tracer")
robot.tracer.setBufferSize(80*(2**20))
robot.tracer.open('/tmp','dg_','.dat')
robot.device.after.addSignal('{0}.triger'.format(robot.tracer.name))
robot.device.after.addSignal('{0}.zmpRef'.format(robot.dcm_control.name))
robot.device.after.addSignal('{0}.zmp'.format(robot.dynamic.name)) # why needed?
robot.device.after.addSignal('{0}.comRef'.format(robot.com_admittance_control.name)) # why needed?
addTrace(robot.tracer, robot.dynamic, 'zmp')
addTrace(robot.tracer, robot.dcm_control, 'zmpRef')
addTrace(robot.tracer, robot.estimator, 'dcm')
addTrace(robot.tracer, robot.dynamic, 'com')
addTrace(robot.tracer, robot.com_admittance_control, 'comRef')
addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired CoM (workaround)
addTrace(robot.tracer, robot.cdc_estimator, 'c') # estimated CoM
addTrace(robot.tracer, robot.com_admittance_control, 'comRef') # reference CoM
addTrace(robot.tracer, robot.dynamic, 'com') # resulting SOT CoM
# --- ROS PUBLISHER
robot.publisher = create_rospublish(robot, 'robot_publisher')
create_topic(robot.publisher, robot.dynamic, 'zmp', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.dcm_control, 'zmpRef', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.estimator, 'dcm', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.dynamic, 'com', robot = robot, data_type='vector')
create_topic(robot.publisher, robot.com_admittance_control, 'comRef', robot = robot, data_type='vector')
# SIMULATION
# addTrace(robot.tracer, robot.dcm_control, 'dcmDes') # desired DCM (already added)
addTrace(robot.tracer, robot.estimator, 'dcm') # estimated DCM
addTrace(robot.tracer, robot.dcm_control, 'zmpDes') # desired ZMP
addTrace(robot.tracer, robot.zmp_estimator, 'zmp') # estimated ZMP
addTrace(robot.tracer, robot.dcm_control, 'zmpRef') # reference ZMP
# -------------------------- SIMULATION --------------------------
robot.tracer.start()
......@@ -13,11 +13,11 @@ sleep(5.0)
# Connect ZMP reference and reset controllers
print('Connect ZMP reference')
runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
runCommandClient('robot.com_admittance_control.setState(robot.dynamic.com.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')
#runCommandClient('plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)')
#runCommandClient('robot.com_admittance_control.setState(robot.dynamic.com.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')
sleep(5.0)
......@@ -30,41 +30,61 @@ 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')
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
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
plt.ion()
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') + '-dcm.dat') # estimated DCM
plt.figure()
plt.plot(dcm_data[:,1],'b-')
plt.plot(dcm_data[:,2],'r-')
plt.title('DCM')
plt.legend(['x','y'])
zmpDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpDes.dat') # desired ZMP
zmpEst_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.zmp_estimator.name') + '-zmp.dat') # estimated ZMP
zmpRef_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dcm_control.name') + '-zmpRef.dat') # reference ZMP
plt.ion()
plt.figure()
plt.plot(com_data[:,1],'b-')
plt.plot(comDes_data[:,1],'b--')
plt.plot(com_data[:,2],'r-')
plt.plot(comEst_data[:,1],'b-')
plt.plot(comRef_data[:,1],'b:')
plt.plot(comSOT_data[:,1],'b-.')
plt.plot(comDes_data[:,2],'r--')
plt.plot(com_data[:,3],'g-')
plt.plot(comEst_data[:,2],'r-')
plt.plot(comRef_data[:,2],'r:')
plt.plot(comSOT_data[:,2],'r-.')
plt.plot(comDes_data[:,3],'g--')
plt.title('COM real vs desired')
plt.legend(['Real x','Desired x','Real y','Desired y','Real z','Desired z'])
plt.plot(comEst_data[:,3],'g-')
plt.plot(comRef_data[:,3],'g:')
plt.plot(comSOT_data[:,3],'g-.')
plt.title('COM')
plt.legend(['Desired x', 'Estimated x', 'Reference x', 'SOT x',
'Desired y', 'Estimated y', 'Reference y', 'SOT y',
'Desired z', 'Estimated z', 'Reference z', 'SOT z'])
plt.figure()
plt.plot(dcmDes_data[:,1],'b--')
plt.plot(dcmEst_data[:,1],'b-')
plt.plot(dcmDes_data[:,2],'r--')
plt.plot(dcmEst_data[:,2],'r-')
plt.title('DCM')
plt.legend(['Desired x', 'Estimated x', 'Desired y', 'Estimated y'])
plt.figure()
plt.plot(zmp_data[:,1],'b-')
plt.plot(zmpDes_data[:,1],'b--')
plt.plot(zmp_data[:,2],'r-')
plt.plot(zmpEst_data[:,1],'b-')
plt.plot(zmpRef_data[:,1],'b:')
plt.plot(zmpDes_data[:,2],'r--')
plt.title('ZMP real vs desired')
plt.legend(['Real x','Desired x','Real y','Desired y'])
plt.plot(zmpEst_data[:,2],'r-')
plt.plot(zmpRef_data[:,2],'r:')
plt.title('ZMP')
plt.legend(['Desired x', 'Estimated x', 'Reference x',
'Desired y', 'Estimated y', 'Reference y'])
plt.figure()
plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
plt.plot(zmpEst_data[:,1] - zmpDes_data[:,1],'b-')
plt.plot(zmpEst_data[:,2] - zmpDes_data[:,2],'r-')
plt.title('ZMP error')
plt.legend(['Error on x','Error on y'])
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment