Skip to content
Snippets Groups Projects
Commit 4389280d authored by François Bailly's avatar François Bailly
Browse files

Merge branch 'devel' of gepgitlab.laas.fr:loco-3d/sot-talos-balance into devel

parents 9539dde3 a33f2d9e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
+ 2
− 0
View file @ 4389280d
......@@ -163,6 +163,8 @@ IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_config_feedback_gazebo.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_config_feedback_gazebo.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_static_com.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_static_com.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_static_sensorfeedback.py
python/${SOTTALOSBALANCE_PYNAME}/test/appli_static_sensorfeedback.py
python/${SOTTALOSBALANCE_PYNAME}/test/test_static_feedback_gazebo.py
......
python/sot_talos_balance/create_entities_utils.py
+ 3
− 3
View file @ 4389280d
......@@ -273,8 +273,8 @@ def create_topic(rospub, entity, signalName, robot=None, data_type='vector'):
def create_dummy_dcm_estimator(robot):
from math import sqrt
estimator = DummyDcmEstimator("dummy")
mass = robot.dynamic.data.mass[0]
robot.dynamic.com.recompute(0)
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
......@@ -302,8 +302,8 @@ def create_com_admittance_controller(Kp,dt,robot):
def create_dcm_controller(Kp,Ki,dt,robot,dcmSignal):
from math import sqrt
controller = DcmController("dcmCtrl")
mass = robot.dynamic.data.mass[0]
robot.dynamic.com.recompute(0)
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
......@@ -327,8 +327,8 @@ def create_dcm_controller(Kp,Ki,dt,robot,dcmSignal):
def create_dcm_com_controller(Kp,Ki,dt,robot,dcmSignal):
from math import sqrt
controller = DcmComController("dcmComCtrl")
mass = robot.dynamic.data.mass[0]
robot.dynamic.com.recompute(0)
mass = robot.dynamic.data.mass[0]
h = robot.dynamic.com.value[2]
g = 9.81
omega = sqrt(g/h)
......
python/sot_talos_balance/test/appli_dcmZmpControl.py 0 → 100644
+ 75
− 0
View file @ 4389280d
'''Test CoM admittance control as described in paper.'''
from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_controller
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 time import sleep
from dynamic_graph.tracer_real_time import TracerRealTime
from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
dt = robot.timeStep;
# --- Dummy estimator
robot.estimator = create_dummy_dcm_estimator(robot)
# --- DCM controller
Kp_dcm = [500.0,500.0,500.0]
Ki_dcm = [0.0,0.0,0.0]
robot.dcm_control = create_dcm_controller(Kp_dcm,Ki_dcm,dt,robot,robot.estimator.dcm)
# --- Admittance controller
Kp_adm = [0.0,0.0,0.0]
robot.com_admittance_control = create_com_admittance_controller(Kp_adm,dt,robot)
# --- CONTACTS
#define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(300)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(300)
robot.contactRF.keep()
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)
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control,robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
# --- 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')
# SIMULATION
robot.tracer.start()
python/sot_talos_balance/test/appli_static_com.py 0 → 100644
+ 79
− 0
View file @ 4389280d
from sot_talos_balance.create_entities_utils import *
from sot_talos_balance.meta_task_config import MetaTaskKineConfig
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom, gotoNd
from dynamic_graph import plug
from dynamic_graph.sot.core import SOT
from dynamic_graph.sot.core.operator import Mix_of_vector
from dynamic_graph.ros import RosSubscribe
from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
dt = robot.timeStep;
# --- Subscriber
robot.subscriber = RosSubscribe("base_subscriber")
robot.subscriber.add("vector","position","/sot/base_link/position")
robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
# --- Dynamic pinocchio
robot.rdynamic = DynamicPinocchio("real_dynamics")
robot.rdynamic.setModel(robot.dynamic.model)
robot.rdynamic.setData(robot.rdynamic.model.createData())
plug(robot.device.state,robot.rdynamic.position)
plug(robot.subscriber.position,robot.rdynamic.ffposition)
plug(robot.device.velocity,robot.rdynamic.velocity)
plug(robot.subscriber.velocity,robot.rdynamic.ffvelocity)
robot.rdynamic.acceleration.value = [0.0]*robot.dynamic.getDimension()
# --- Posture
robot.taskPos = MetaTaskKineConfig(robot.dynamic)
robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
robot.taskPos.task.controlGain.value = 100
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 100
# --- CONTACTS
#define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(100)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(100)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
# --- SOLVER
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.sot.push(robot.taskPos.task.name)
robot.device.control.value = [0.0]*robot.dynamic.getDimension()
# --- TRACER
robot.tracer = TracerRealTime("com_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}.com'.format(robot.rdynamic.name))
robot.device.after.addSignal('{0}.com'.format(robot.dynamic.name))
robot.device.after.addSignal('{0}.errorIN'.format(robot.taskCom.featureDes.name))
addTrace(robot.tracer, robot.taskCom.featureDes, 'errorIN')
addTrace(robot.tracer, robot.dynamic, 'com')
addTrace(robot.tracer, robot.rdynamic, 'com')
robot.tracer.start()
python/sot_talos_balance/test/test_dcmZmpControl.py
+ 64
− 136
View file @ 4389280d
'''Test CoM admittance control as described in paper.'''
from sot_talos_balance.create_entities_utils import create_com_admittance_controller, create_dummy_dcm_estimator, create_dcm_controller
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 numpy import eye
from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
from time import sleep
import sys
import os
from dynamic_graph.tracer_real_time import TracerRealTime
from sot_talos_balance.create_entities_utils import addTrace, dump_tracer
from sot_talos_balance.utils.gazebo_utils import apply_force
import matplotlib.pyplot as plt
import numpy as np
from sot_talos_balance.utils.gazebo_utils import apply_force
def main(robot):
dt = robot.timeStep;
# --- COM
robot.taskCom = MetaTaskKineCom(robot.dynamic)
robot.dynamic.com.recompute(0)
robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
robot.taskCom.task.controlGain.value = 0
robot.taskCom.task.setWithDerivative(True)
# --- Dummy estimator
robot.estimator = create_dummy_dcm_estimator(robot)
# --- DCM controller
Kp_dcm = [500.0,500.0,500.0]
Ki_dcm = [0.0,0.0,0.0]
robot.dcm_control = create_dcm_controller(Kp_dcm,Ki_dcm,dt,robot,robot.estimator.dcm)
# --- Admittance controller
Kp_adm = [0.0,0.0,0.0]
robot.com_admittance_control = create_com_admittance_controller(Kp_adm,dt,robot)
# --- CONTACTS
#define contactLF and contactRF
robot.contactLF = MetaTaskKine6d('contactLF',robot.dynamic,'LF',robot.OperationalPointsMap['left-ankle'])
robot.contactLF.feature.frame('desired')
robot.contactLF.gain.setConstant(300)
robot.contactLF.keep()
locals()['contactLF'] = robot.contactLF
robot.contactRF = MetaTaskKine6d('contactRF',robot.dynamic,'RF',robot.OperationalPointsMap['right-ankle'])
robot.contactRF.feature.frame('desired')
robot.contactRF.gain.setConstant(300)
robot.contactRF.keep()
locals()['contactRF'] = robot.contactRF
robot.sot = SOT('sot')
robot.sot.setSize(robot.dynamic.getDimension())
plug(robot.sot.control,robot.device.control)
robot.sot.push(robot.contactRF.task.name)
robot.sot.push(robot.contactLF.task.name)
robot.sot.push(robot.taskCom.task.name)
robot.device.control.recompute(0)
# --- 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))
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')
# SIMULATION
# begin with constant references
plug(robot.com_admittance_control.comRef,robot.taskCom.featureDes.errorIN)
plug(robot.com_admittance_control.dcomRef,robot.taskCom.featureDes.errordotIN)
sleep(1.0)
os.system("rosservice call \start_dynamic_graph")
sleep(2.0)
# connect ZMP control signal and reset controllers
plug(robot.dcm_control.zmpRef,robot.com_admittance_control.zmpDes)
robot.com_admittance_control.setState(robot.dynamic.com.value,[0.0,0.0,0.0])
robot.com_admittance_control.Kp.value = [10.0,10.0,0.0]
robot.dcm_control.resetDcmIntegralError()
robot.dcm_control.Ki.value = [1.0,1.0,0.0]
robot.tracer.start()
sleep(5.0)
# kick the robot on the chest to test its stability
apply_force([-1000.0,0,0],0.01)
sleep(5.0)
dump_tracer(robot.tracer)
# --- DISPLAY
dcm_data = np.loadtxt('/tmp/dg_'+robot.estimator.name+'-dcm.dat')
zmp_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-zmp.dat')
zmpDes_data = np.loadtxt('/tmp/dg_'+robot.dcm_control.name+'-zmpRef.dat')
com_data = np.loadtxt('/tmp/dg_'+robot.dynamic.name+'-com.dat')
comDes_data = np.loadtxt('/tmp/dg_'+robot.com_admittance_control.name+'-comRef.dat')
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-')
plt.plot(comDes_data[:,1],'b--')
plt.plot(com_data[:,2],'r-')
plt.plot(comDes_data[:,2],'r--')
plt.plot(com_data[:,3],'g-')
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.figure()
plt.plot(zmp_data[:,1],'b-')
plt.plot(zmpDes_data[:,1],'b--')
plt.plot(zmp_data[:,2],'r-')
plt.plot(zmpDes_data[:,2],'r--')
plt.title('ZMP real vs desired')
plt.legend(['Real x','Desired x','Real y','Desired y'])
plt.figure()
plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
plt.title('ZMP error')
plt.legend(['Error on x','Error on y'])
plt.show()
run_test('appli_dcmZmpControl.py')
sleep(5.0)
# connect ZMP control signal and reset controllers
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 = [20.0,10.0,0.0]')
runCommandClient('robot.dcm_control.resetDcmIntegralError()')
runCommandClient('robot.dcm_control.Ki.value = [1.0,1.0,0.0]')
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-')
plt.plot(comDes_data[:,1],'b--')
plt.plot(com_data[:,2],'r-')
plt.plot(comDes_data[:,2],'r--')
plt.plot(com_data[:,3],'g-')
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.figure()
plt.plot(zmp_data[:,1],'b-')
plt.plot(zmpDes_data[:,1],'b--')
plt.plot(zmp_data[:,2],'r-')
plt.plot(zmpDes_data[:,2],'r--')
plt.title('ZMP real vs desired')
plt.legend(['Real x','Desired x','Real y','Desired y'])
plt.figure()
plt.plot(zmp_data[:,1] - zmpDes_data[:,1],'b-')
plt.plot(zmp_data[:,2] - zmpDes_data[:,2],'r-')
plt.title('ZMP error')
plt.legend(['Error on x','Error on y'])
raw_input("Wait before leaving the simulation")
python/sot_talos_balance/test/test_static_com.py 0 → 100644
+ 97
− 0
View file @ 4389280d
from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
from sot_talos_balance.utils.gazebo_utils import GazeboLinkStatePublisher
from time import sleep
import matplotlib.pyplot as plt
import numpy as np
pub = GazeboLinkStatePublisher('base_link',5000)
print("Starting Gazebo link state publisher...")
pub.start()
print("Gazebo link state publisher started")
raw_input("Wait before running the test")
run_test('appli_static_com.py')
# wait for sensor values to be ready
raw_input("Wait before plugging the SOT")
# set initial conditions from sensor readings
runCommandClient('robot.rdynamic.com.recompute(0)')
runCommandClient('robot.taskCom.featureDes.errorIN.value = robot.rdynamic.com.value')
runCommandClient('robot.device.set(robot.subscriber.position.value+robot.device.state.value[6:])')
runCommandClient('robot.contactLF.keep()')
runCommandClient('robot.contactRF.keep()')
# plug the SOT
runCommandClient('plug(robot.sot.control,robot.device.control)')
sleep(5.0)
runCommandClient('dump_tracer(robot.tracer)')
# --- DISPLAY
comDes_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.taskCom.featureDes.name') + '-errorIN.dat')
comSot_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.dynamic.name') + '-com.dat')
com_data = np.loadtxt('/tmp/dg_' + evalCommandClient('robot.rdynamic.name') + '-com.dat')
comErr_data = com_data - comDes_data
plt.ion()
plt.figure()
plt.plot(com_data[:,1],'b-')
plt.plot(comDes_data[:,1],'b--')
plt.plot(comSot_data[:,1],'b:')
plt.plot(com_data[:,2],'r-')
plt.plot(comDes_data[:,2],'r--')
plt.plot(comSot_data[:,2],'r:')
plt.plot(com_data[:,3],'g-')
plt.plot(comDes_data[:,3],'g--')
plt.plot(comSot_data[:,3],'g:')
plt.title('COM real vs desired vs SOT')
plt.legend(['Real x','Desired x','SOT x','Real y','Desired y','SOT y','Real z','Desired z','SOT z'])
plt.figure()
plt.plot(comErr_data[:,1],'b-')
plt.plot(comErr_data[:,2],'r-')
plt.plot(comErr_data[:,3],'g-')
plt.title('COM error')
plt.legend(['x','y','z'])
plt.figure()
plt.plot(com_data[:,1],'b-')
plt.title('COM real x')
plt.figure()
plt.plot(comDes_data[:,1],'b--')
plt.title('COM desired x')
plt.figure()
plt.plot(comSot_data[:,1],'b:')
plt.title('COM SOT x')
plt.figure()
plt.plot(com_data[:,2],'r-')
plt.title('COM real y')
plt.figure()
plt.plot(comDes_data[:,2],'r--')
plt.title('COM desired y')
plt.figure()
plt.plot(comSot_data[:,2],'r:')
plt.title('COM SOT y')
plt.figure()
plt.plot(com_data[:,3],'g-')
plt.title('COM real z')
plt.figure()
plt.plot(comDes_data[:,3],'g--')
plt.title('COM desired z')
plt.figure()
plt.plot(comSot_data[:,3],'g:')
plt.title('COM SOT z')
raw_input("Wait before leaving the simulation")
print("Stopping Gazebo link state publisher...")
pub.stop()
sleep(0.1)
print("Gazebo link state publisher stopped")
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment