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

CMakeLists.txt

@@ -161,6 +161,10 @@ IF(BUILD_PYTHON_INTERFACE)
                 DESTINATION ${PYTHON_SITELIB}/${SOTTALOSBALANCE_PYNAME}/talos)
   IF(BUILD_TEST)
     INSTALL(FILES python/${SOTTALOSBALANCE_PYNAME}/test/__init__.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_feedback_gazebo.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj_feedback_gazebo.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_sensorfeedback.py
+                  python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj_sensorfeedback.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_COMTraj_checkfeedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/appli_COMTraj_checkfeedback_gazebo.py
                   python/${SOTTALOSBALANCE_PYNAME}/test/test_ffSubscriber.py

python/sot_talos_balance/create_entities_utils.py

@@ -25,6 +25,7 @@ from sot_talos_balance.utils.filter_utils                     import create_cheb
 from sot_talos_balance.utils.sot_utils                        import Bunch
 
 from dynamic_graph import plug
+from dynamic_graph.ros import RosPublish
 
 N_JOINTS = 32;
 
@@ -102,6 +103,9 @@ def create_device_filters(robot, dt):
     filters.joints_kin    = create_chebi1_checby2_series_filter("joints_kin", dt, N_JOINTS);
     filters.ft_RF_filter  = create_chebi1_checby2_series_filter("ft_RF_filter", dt, 6);
     filters.ft_LF_filter  = create_chebi1_checby2_series_filter("ft_LF_filter", dt, 6);
+    filters.ft_RH_filter  = create_chebi1_checby2_series_filter("ft_RH_filter", dt, 6);
+    filters.ft_LH_filter  = create_chebi1_checby2_series_filter("ft_LH_filter", dt, 6);
+    filters.torque_filter = create_chebi1_checby2_series_filter("ptorque_filter", dt, N_JOINTS);
     filters.acc_filter    = create_chebi1_checby2_series_filter("acc_filter", dt, 3);
     filters.gyro_filter   = create_chebi1_checby2_series_filter("gyro_filter", dt, 3);
     filters.estimator_kin = create_chebi1_checby2_series_filter("estimator_kin", dt, N_JOINTS);
@@ -109,6 +113,9 @@ def create_device_filters(robot, dt):
     plug(robot.device.joint_angles,                       filters.estimator_kin.x);  # device.state, device.joint_angles or device.motor_angles ?
     plug(robot.device.forceRLEG,                          filters.ft_RF_filter.x);
     plug(robot.device.forceLLEG,                          filters.ft_LF_filter.x);
+    plug(robot.device.forceRARM,                          filters.ft_RH_filter.x);
+    plug(robot.device.forceLARM,                          filters.ft_LH_filter.x);
+    plug(robot.device.ptorque,                            filters.torque_filter.x);
     
     # switch following lines if willing to use imu offset compensation
     #~ plug(robot.imu_offset_compensation.accelerometer_out, filters.acc_filter.x);
@@ -222,12 +229,13 @@ def addSignalsToTracer(tracer, device, outputs):
          addTrace(tracer,device,sign);
     return
     
-def create_tracer(robot,entity,tracer_name, outputs):
+def create_tracer(robot,entity,tracer_name, outputs=None):
     tracer = TracerRealTime(tracer_name)
     tracer.setBufferSize(80*(2**20))
     tracer.open('/tmp','dg_','.dat')
     robot.device.after.addSignal('{0}.triger'.format(tracer.name))
-    addSignalsToTracer(tracer, entity, outputs)
+    if outputs is not None:
+        addSignalsToTracer(tracer, entity, outputs)
     return tracer
 	
 def reset_tracer(device,tracer):
@@ -247,6 +255,21 @@ def dump_tracer(tracer):
     sleep(0.2);
     tracer.close();
 
+def create_rospublish(robot, name):
+    rospub = RosPublish(name)
+    robot.device.after.addSignal(rospub.name+'.trigger')
+    return rospub
+
+def create_topic(rospub, entity, signalName, robot=None, data_type='vector'):
+    if not entity.hasSignal(signalName): # check needed to prevent creation of broken topic
+        raise AttributeError( 'Entity %s does not have signal %s' % (entity.name, signalName) )
+    rospub_signalName = '{0}_{1}'.format(entity.name, signalName)
+    topicname = '/sot/{0}/{1}'.format(entity.name, signalName)
+    rospub.add(data_type,rospub_signalName,topicname)
+    plug(entity.signal(signalName), rospub.signal(rospub_signalName))
+    if robot is not None:
+        robot.device.after.addSignal( '{0}.{1}'.format(entity.name, signalName) )
+
 def create_dummy_dcm_estimator(robot):
     from math import sqrt
     estimator = DummyDcmEstimator("dummy")

python/sot_talos_balance/meta_task_config.py

 from dynamic_graph.sot.core.meta_task_6d import toFlags
 from dynamic_graph import plug
 from dynamic_graph.sot.core import *
-# from dynamic_graph.sot.core.meta_tasks import setGain
-# from dynamic_graph.sot.dyninv import *
 from dynamic_graph.sot.core.matrix_util import matrixToTuple, vectorToTuple,rotate
 from numpy import matrix, identity, zeros, eye
 
 
 class MetaTaskConfig(object):
-    def __init__(self,dyn,config,name="config"):
+    def __init__(self,dyn,config=None,name="config"):
         self.dyn=dyn
         self.name=name
         self.config = config
@@ -18,10 +16,11 @@ class MetaTaskConfig(object):
         self.gain = GainAdaptive('gain'+name)
 
         plug(dyn.position,self.feature.errorIN)
-        robotDim = len(dyn.position.value)
+        robotDim = dyn.getDimension()
         self.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
         self.feature.setReference(self.featureDes.name)
-        self.feature.selec.value = toFlags(self.config)
+        if config is not None:
+            self.feature.selec.value = toFlags(self.config)
 
     def plugTask(self):
         self.task.add(self.feature.name)
@@ -37,7 +36,7 @@ class MetaTaskConfig(object):
         self.featureDes.errorIN.value = v
 
 class MetaTaskKineConfig(MetaTaskConfig):
-    def __init__(self,dyn,config,name="config"):
+    def __init__(self,dyn,config=None,name="config"):
         MetaTaskConfig.__init__(self,dyn,config,name)
         self.task = Task('task'+name)
         self.plugTask()

python/sot_talos_balance/test/appli_COMTraj_feedback_gazebo.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, Selec_of_vector
+
+from dynamic_graph.ros import RosSubscribe
+from dynamic_graph.sot.dynamics_pinocchio import DynamicPinocchio
+
+dt = robot.timeStep;
+
+# --- COM trajectory generator
+robot.comTrajGen = create_com_trajectory_generator(dt,robot);
+
+# --- Subscriber
+robot.subscriber = RosSubscribe("base_subscriber")
+robot.subscriber.add("vector","position","/sot/base_link/position")
+robot.subscriber.add("vector","velocity","/sot/base_link/velocity")
+
+# --- Merger
+robot.stateselec = Selec_of_vector("stateselec")
+robot.stateselec.selec(6,robot.dynamic.getDimension())
+plug(robot.device.robotState,robot.stateselec.sin)
+
+robot.stateselec = Selec_of_vector("stateselec")
+robot.stateselec.selec(6,robot.dynamic.getDimension())
+plug(robot.device.robotState,robot.stateselec.sin)
+
+robot.statemix = Mix_of_vector("statemix")
+
+robot.statemix.setSignalNumber(3)
+
+robot.statemix.addSelec(1,0,6)
+robot.statemix.addSelec(2,6,robot.dynamic.getDimension()-6)
+
+robot.statemix.default.value=[0.0]*robot.dynamic.getDimension()
+plug(robot.subscriber.position,robot.statemix.signal("sin1"))
+plug(robot.stateselec.sout,robot.statemix.signal("sin2"))
+
+# --- Dynamic pinocchio
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+
+plug(robot.device.robotState,robot.rdynamic.position)
+plug(robot.subscriber.position,robot.rdynamic.ffposition)
+
+plug(robot.device.robotVelocity,robot.rdynamic.velocity)
+plug(robot.subscriber.velocity,robot.rdynamic.ffvelocity)
+
+robot.rdynamic.acceleration.value = [0.0]*robot.dynamic.getDimension()
+
+# --- Posture
+robot.taskPos = MetaTaskKineConfig(robot.rdynamic)
+robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
+robot.taskPos.task.controlGain.value = 10
+plug(robot.statemix.sout,robot.taskPos.feature.errorIN)
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.rdynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 10
+plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN)
+
+# --- CONTACTS
+#define contactLF and contactRF
+robot.contactLF = MetaTaskKine6d('contactLF',robot.rdynamic,'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.rdynamic,'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()

python/sot_talos_balance/test/appli_COMTraj_sensorfeedback.py

+from sot_talos_balance.create_entities_utils import *
+from sot_talos_balance.meta_task_config import MetaTaskKineConfig
+import sot_talos_balance.control_manager_conf as param_server_conf
+from sot_talos_balance.talos import base_estimator_conf
+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.dynamics_pinocchio import DynamicPinocchio
+
+dt = robot.timeStep;
+
+# --- COM trajectory generator
+robot.comTrajGen = create_com_trajectory_generator(dt,robot);
+
+# --- Estimators
+robot.param_server            = create_parameter_server(param_server_conf,dt)
+robot.device_filters          = create_device_filters(robot, dt)
+robot.imu_filters             = create_imu_filters(robot, dt)
+robot.base_estimator          = create_base_estimator(robot, dt, base_estimator_conf)
+robot.be_filters              = create_be_filters(robot, dt)
+robot.dcm_estimator           = create_dcm_estimator(robot, dt)
+
+# --- Dynamic pinocchio
+robot.rdynamic = DynamicPinocchio("real_dynamics")
+robot.rdynamic.setModel(robot.dynamic.model)
+robot.rdynamic.setData(robot.rdynamic.model.createData())
+
+plug(robot.base_estimator.q,robot.rdynamic.position)
+
+robot.rdynamic.velocity.value = [0.0]*robot.dynamic.getDimension()
+
+robot.rdynamic.acceleration.value = [0.0]*robot.dynamic.getDimension()
+
+# --- Posture
+robot.taskPos = MetaTaskKineConfig(robot.rdynamic)
+robot.taskPos.featureDes.errorIN.value = robot.dynamic.position.value
+robot.taskPos.task.controlGain.value = 10
+plug(robot.base_estimator.q,robot.taskPos.feature.errorIN)
+
+# --- COM
+robot.taskCom = MetaTaskKineCom(robot.rdynamic)
+robot.dynamic.com.recompute(0)
+robot.taskCom.featureDes.errorIN.value = robot.dynamic.com.value
+robot.taskCom.task.controlGain.value = 10
+plug(robot.dcm_estimator.c, robot.taskCom.feature.errorIN)
+plug(robot.comTrajGen.x, robot.taskCom.featureDes.errorIN)
+
+# --- CONTACTS
+#define contactLF and contactRF
+robot.contactLF = MetaTaskKine6d('contactLF',robot.rdynamic,'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.rdynamic,'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()

python/sot_talos_balance/test/test_COMTraj_feedback_gazebo.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
+
+pub = GazeboLinkStatePublisher('base_link',1000)
+print("Starting Gazebo link state publisher...")
+pub.start()
+print("Gazebo link state publisher started")
+raw_input("Wait before running the test")
+
+run_test('appli_COMTraj_feedback_gazebo.py')
+
+# wait for sensor values to be ready
+sleep(evalCommandClient('robot.timeStep'))
+
+# set initial conditions from sensor readings
+runCommandClient('robot.rdynamic.com.recompute(0)')
+runCommandClient('robot.comTrajGen.initial_value.value = robot.rdynamic.com.value')
+runCommandClient('robot.contactLF.keep()')
+runCommandClient('robot.contactRF.keep()')
+
+# plug the SOT
+runCommandClient('plug(robot.sot.control,robot.device.control)')
+
+# execute rest of the commands
+sleep(2.0)
+runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+sleep(5.0)
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+

python/sot_talos_balance/test/test_COMTraj_sensorfeedback.py

+from sot_talos_balance.utils.run_test_utils import run_test, runCommandClient, evalCommandClient
+from time import sleep
+
+run_test('appli_COMTraj_sensorfeedback.py')
+
+# wait for sensor values to be ready
+sleep(evalCommandClient('robot.timeStep'))
+
+# set initial conditions from sensor readings
+runCommandClient('robot.dcm_estimator.c.recompute(0)')
+runCommandClient('robot.comTrajGen.initial_value.value = robot.dcm_estimator.c.value')
+runCommandClient('robot.contactLF.keep()')
+runCommandClient('robot.contactRF.keep()')
+
+# plug the SOT
+runCommandClient('plug(robot.sot.control,robot.device.control)')
+
+# execute rest of the commands
+sleep(2.0)
+runCommandClient('robot.comTrajGen.move(1,-0.025,4.0)')
+sleep(5.0)
+runCommandClient('robot.comTrajGen.startSinusoid(1,0.05,8.0)')
+

python/sot_talos_balance/utils/filter_utils.py

@@ -11,4 +11,22 @@ def create_chebi1_checby2_series_filter(name, dt, size):
                   (1.,-5.32595322,11.89749109,-14.26803139, 9.68705647,  -3.52968633,   0.53914042))
    return lp_filter;
 
+def create_butter_lp_filter_Wn_04_N_2(name, dt, size):
+    # (b,a) =  butter(N=2, Wn=0.04)
+    lp_filter = FilterDifferentiator(name)
 
+    lp_filter.init(dt, size,
+                   (0.0036216815,   0.007243363, 0.0036216815),
+                   (          1., -1.8226949252, 0.8371816513))
+
+    return lp_filter
+
+def create_bessel_lp_filter_Wn_04_N_2(name, dt, size):
+    # (b,a) =  bessel(N=2, Wn=0.04)
+    lp_filter = FilterDifferentiator(name)
+
+    lp_filter.init(dt, size,
+                   (0.0035566088,  0.0071132175, 0.0035566088),
+                   (          1., -1.7899455543, 0.8041719893))
+
+    return lp_filter