diff --git a/CMakeLists.txt b/CMakeLists.txt index c8983c98967220fc883c4f81a898b8591748ab55..74c00dfb2ebefbc31a0dd7500786db94beeceeb2 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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 diff --git a/python/sot_talos_balance/test/appli_static_com.py b/python/sot_talos_balance/test/appli_static_com.py new file mode 100644 index 0000000000000000000000000000000000000000..902ce8abb5cdabeda44c52441f613e42fca66852 --- /dev/null +++ b/python/sot_talos_balance/test/appli_static_com.py @@ -0,0 +1,79 @@ +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() diff --git a/python/sot_talos_balance/test/test_static_com.py b/python/sot_talos_balance/test/test_static_com.py new file mode 100644 index 0000000000000000000000000000000000000000..67b7c8bbc0353458615c5aa8fbba78c6a1a8229b --- /dev/null +++ b/python/sot_talos_balance/test/test_static_com.py @@ -0,0 +1,97 @@ +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") + +