static CoM test for comparison

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

python/sot_talos_balance/test/appli_static_com.py

+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_static_com.py

+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")
+
+