Skip to content
Snippets Groups Projects
Commit 9b9c2c89 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

control manager has dynamic commands / signals

parent 8451a86d
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/CMakeLists.txt
+ 9
− 3
View file @ 9b9c2c89
......@@ -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
+ 20
− 18
View file @ 9b9c2c89
......@@ -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
+ 9
− 5
View file @ 9b9c2c89
......@@ -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 0 → 100644
+ 12
− 0
View file @ 9b9c2c89
#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 deleted 100644 → 0
+ 0
− 3
View file @ 8451a86d
#include <sot/talos_balance/talos-control-manager.hh>
typedef boost::mpl::vector<dynamicgraph::sot::talos_balance::TalosControlManager> entities_t;
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