control manager has dynamic commands / signals

src/CMakeLists.txt

@@ -62,9 +62,15 @@ FOREACH(plugin ${plugins})
 
   IF(BUILD_PYTHON_INTERFACE)
     STRING(REPLACE - _ PYTHON_LIBRARY_NAME ${LIBRARY_NAME})
-    DYNAMIC_GRAPH_PYTHON_MODULE("${PY_NAME}/${PYTHON_LIBRARY_NAME}"
-      ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
-      MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+    if(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+      DYNAMIC_GRAPH_PYTHON_MODULE("${PY_NAME}/${PYTHON_LIBRARY_NAME}"
+        ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
+        SOURCE_PYTHON_MODULE "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python-module-py.cc")
+    elseif(EXISTS "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+      DYNAMIC_GRAPH_PYTHON_MODULE("${PY_NAME}/${PYTHON_LIBRARY_NAME}"
+        ${LIBRARY_NAME} ${PROJECT_NAME}-${PYTHON_LIBRARY_NAME}-wrap
+        MODULE_HEADER "${CMAKE_CURRENT_SOURCE_DIR}/${plugin}-python.hh")
+    endif()
   ENDIF(BUILD_PYTHON_INTERFACE)
 ENDFOREACH(plugin)
 

src/dynamic_graph/sot_talos_balance/create_entities_utils.py

@@ -86,7 +86,7 @@ def create_extend_mix(n_in, n_out):
 def create_scalar_trajectory_generator(dt, init_value, name):
     trajGen = NdTrajectoryGenerator(name)
     trajGen.initial_value.value = [init_value]
-    trajGen.trigger.value = 1.0
+    trajGen.trigger.value = 1
     trajGen.init(dt, 1)
     return trajGen
 
@@ -94,7 +94,7 @@ def create_scalar_trajectory_generator(dt, init_value, name):
 def create_joint_trajectory_generator(dt, robot):
     jtg = NdTrajectoryGenerator("jtg")
     jtg.initial_value.value = robot.device.state.value[6:]
-    jtg.trigger.value = 1.0
+    jtg.trigger.value = 1
     jtg.init(dt, N_JOINTS)
     return jtg
 
@@ -103,7 +103,7 @@ def create_config_trajectory_generator(dt, robot):
     N_CONFIG = N_JOINTS + 6
     jtg = NdTrajectoryGenerator("jtg")
     jtg.initial_value.value = robot.device.state.value
-    jtg.trigger.value = 1.0
+    jtg.trigger.value = 1
     jtg.init(dt, N_CONFIG)
     return jtg
 
@@ -112,7 +112,7 @@ def create_torque_trajectory_generator(dt, robot):
     N_CONFIG = N_JOINTS + 6
     jtg = NdTrajectoryGenerator("torqueTrajGen")
     jtg.initial_value.value = [0.] * N_CONFIG
-    jtg.trigger.value = 1.0
+    jtg.trigger.value = 1
     jtg.init(dt, N_CONFIG)
     return jtg
 
@@ -120,7 +120,7 @@ def create_torque_trajectory_generator(dt, robot):
 def create_com_trajectory_generator(dt, robot):
     comTrajGen = NdTrajectoryGenerator("comTrajGen")
     comTrajGen.initial_value.value = robot.dynamic.com.value
-    comTrajGen.trigger.value = 1.0
+    comTrajGen.trigger.value = 1
     comTrajGen.init(dt, 3)
     return comTrajGen
 
@@ -129,8 +129,8 @@ def create_zmp_trajectory_generator(dt, robot):
     comTrajGen = NdTrajectoryGenerator("zmpTrajGen")
     zmp = list(robot.dynamic.com.value)
     zmp[2] = 0.0
-    comTrajGen.initial_value.value = zmp
-    comTrajGen.trigger.value = 1.0
+    comTrajGen.initial_value.value = np.array(zmp)
+    comTrajGen.trigger.value = 1
     comTrajGen.init(dt, 3)
     return comTrajGen
 
@@ -142,7 +142,7 @@ def create_position_trajectory_generator(dt, robot, signal_name):
     v = [M[i][3] for i in range(3)]
     trajGen.initial_value.value = v
 
-    trajGen.trigger.value = 1.0
+    trajGen.trigger.value = 1
     trajGen.init(dt, 3)
     return trajGen
 
@@ -152,9 +152,9 @@ def create_orientation_rpy_trajectory_generator(dt, robot, signal_name):
 
     M = robot.dynamic.signal(signal_name).value
     v = list(rotation_matrix_to_rpy(np.array(M)[:3, :3]))
-    trajGen.initial_value.value = v
+    trajGen.initial_value.value = np.array(v)
 
-    trajGen.trigger.value = 1.0
+    trajGen.trigger.value = 1
     trajGen.init(dt, 3)
     return trajGen
 
@@ -166,9 +166,9 @@ def create_pose_rpy_trajectory_generator(dt, robot, signal_name):
     pos = [M[i][3] for i in range(3)]
     euler = list(rotation_matrix_to_rpy(np.array(M)[:3, :3]))
     v = pos + euler
-    trajGen.initial_value.value = v
+    trajGen.initial_value.value = np.array(v)
 
-    trajGen.trigger.value = 1.0
+    trajGen.trigger.value = 1
     trajGen.init(dt, 6)
     return trajGen
 
@@ -316,8 +316,10 @@ def create_be_filters(robot, dt):
 
 def create_ctrl_manager(conf, dt, robot_name='robot'):
     ctrl_manager = TalosControlManager("ctrl_man")
+    ctrl_manager.add_commands()
+    ctrl_manager.add_signals()
     ctrl_manager.init(dt, robot_name)
-    ctrl_manager.u_max.value = conf.NJ * (conf.CTRL_MAX, )
+    ctrl_manager.u_max.value = np.array(conf.NJ * (conf.CTRL_MAX, ))
     # Init should be called before addCtrlMode
     # because the size of state vector must be known.
     return ctrl_manager
@@ -343,8 +345,8 @@ def create_base_estimator(robot, dt, conf, robot_name="robot"):
     # base_estimator.w_lf_in.value = conf.w_lf_in
     # base_estimator.w_rf_in.value = conf.w_rf_in
     base_estimator.set_imu_weight(conf.w_imu)
-    base_estimator.set_stiffness_right_foot(conf.K)
-    base_estimator.set_stiffness_left_foot(conf.K)
+    base_estimator.set_stiffness_right_foot(np.array(conf.K))
+    base_estimator.set_stiffness_left_foot(np.array(conf.K))
     base_estimator.set_zmp_std_dev_right_foot(conf.std_dev_zmp)
     base_estimator.set_zmp_std_dev_left_foot(conf.std_dev_zmp)
     base_estimator.set_normal_force_std_dev_right_foot(conf.std_dev_fz)
@@ -353,8 +355,8 @@ def create_base_estimator(robot, dt, conf, robot_name="robot"):
     base_estimator.set_zmp_margin_left_foot(conf.zmp_margin)
     base_estimator.set_normal_force_margin_right_foot(conf.normal_force_margin)
     base_estimator.set_normal_force_margin_left_foot(conf.normal_force_margin)
-    base_estimator.set_right_foot_sizes(conf.RIGHT_FOOT_SIZES)
-    base_estimator.set_left_foot_sizes(conf.LEFT_FOOT_SIZES)
+    base_estimator.set_right_foot_sizes(np.array(conf.RIGHT_FOOT_SIZES))
+    base_estimator.set_left_foot_sizes(np.array(conf.LEFT_FOOT_SIZES))
 
     return base_estimator
 
@@ -362,7 +364,7 @@ def create_base_estimator(robot, dt, conf, robot_name="robot"):
 def create_imu_filters(robot, dt):
     imu_filter = MadgwickAHRS('imu_filter')
     imu_filter.init(dt)
-    imu_filter.set_imu_quat([0., 1., 0., 0.])  # [w, x, y, z]
+    imu_filter.set_imu_quat(np.array([0., 1., 0., 0.]))  # [w, x, y, z]
     imu_filter.setBeta(1e-3)
     plug(robot.device_filters.acc_filter.x_filtered, imu_filter.accelerometer)  # no IMU compensation
     plug(robot.device_filters.gyro_filter.x_filtered, imu_filter.gyroscope)  # no IMU compensation

src/dynamic_graph/sot_talos_balance/utils/filter_utils.py

@@ -8,9 +8,11 @@ def create_chebi1_checby2_series_filter(name, dt, size):
     # (b,a) = filter_series(b1,a1,b2,a2);
     lp_filter = FilterDifferentiator(name)
 
-    lp_filter.init(dt, size, (2.16439898e-05, 4.43473520e-05, -1.74065002e-05, -8.02197247e-05, -1.74065002e-05,
-                              4.43473520e-05, 2.16439898e-05),
-                   (1., -5.32595322, 11.89749109, -14.26803139, 9.68705647, -3.52968633, 0.53914042))
+    lp_filter.init(
+        dt, size,
+        np.array((2.16439898e-05, 4.43473520e-05, -1.74065002e-05, -8.02197247e-05, -1.74065002e-05, 4.43473520e-05,
+                  2.16439898e-05)),
+        np.array((1., -5.32595322, 11.89749109, -14.26803139, 9.68705647, -3.52968633, 0.53914042)))
     return lp_filter
 
 
@@ -18,7 +20,8 @@ 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))
+    lp_filter.init(dt, size, np.array((0.0036216815, 0.007243363, 0.0036216815)),
+                   np.array((1., -1.8226949252, 0.8371816513)))
 
     return lp_filter
 
@@ -27,7 +30,8 @@ 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))
+    lp_filter.init(dt, size, np.array((0.0035566088, 0.0071132175, 0.0035566088)),
+                   np.array((1., -1.7899455543, 0.8041719893)))
 
     return lp_filter
 

src/talos-control-manager-python-module-py.cc

+#include <dynamic-graph/python/module.hh>
+#include <sot/talos_balance/talos-control-manager.hh>
+
+namespace dg = dynamicgraph;
+
+
+BOOST_PYTHON_MODULE(wrap)
+{
+  bp::import("dynamic_graph");
+
+  dg::python::exposeEntity<dg::sot::talos_balance::TalosControlManager, bp::bases<dg::Entity>, dg::python::AddSignals & dg::python::AddCommands>() ;
+}

src/talos-control-manager-python.hh

-#include <sot/talos_balance/talos-control-manager.hh>
-
-typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::TalosControlManager> entities_t;