Skip to content

GitLab

Unverified Commit eab7fb4e authored by Noëlie RAMUZAT's avatar Noëlie RAMUZAT Committed by GitHub
Browse files

Add simpleInverseDyn entity making TSID work with SoT (#71) 

* [Filter Utils] Change include of FilterDifferentiator now in sot-core

filter_differentiator have been moved to sot-core

* [Filter Utils] Change include of FilterDifferentiator now in sot-core

filter_differentiator have been moved to sot-core

* Create simple Inverse Dyn entity with posture and com TSID tasks

* [SimpleInvDyn] Add waist and contact6D tasks to CoM and Posture ones

Add outputs (q_des, v_des, u) and enum on output control modes to use the entity in torque or velocity.
q_des and v_des are the integration of the desired acceleration computed by TSID
u is the result of the desired torque computed + PD on posture des.
Works with talos-torque-control package for sinusoid traj on the CoM in velocity control.

* [SimpleInvDyn] Remove update q,v from robot in torque control

They are used in the u signal for a PD+ control (cannot reuse them in the loop)

* [SimpleInvDyn] Cleaning scripts and add comments

* [Documentation] Add Doxygen documentation

* [Documentation] Correction of the talos-torque-control repository url
parent c4776cf6
Expand all Hide whitespace changes
Inline Side-by-side
doc/CMakeLists.txt 0 → 100644
View file @ eab7fb4e
#
# Copyright 2010 CNRS
# Author: Florent Lamiraux
#
# get the python script install path
IF(BUILD_PYTHON_INTERFACE)
INCLUDE(../cmake/sphinx.cmake)
SPHINX_SETUP()
SPHINX_FINALIZE()
INSTALL (FILES
pictures/feature.png
pictures/task.png
DESTINATION ${_PKG_CONFIG_DOXYGENDOCDIR}/pictures)
ENDIF(BUILD_PYTHON_INTERFACE)
doc/Doxyfile.extra.in 0 → 100644
View file @ eab7fb4e
This diff is collapsed.
doc/Overview.md 0 → 100644
View file @ eab7fb4e
# Overview {#index}
<!--
/*
* Copyright 2017, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
-->
\section OverviewIntro Introduction to sot-torque-control
[![License](https://img.shields.io/badge/License-BSD%202--Clause-orange.svg)](https://opensource.org/licenses/BSD-2-Clause)
[![Building Status](https://travis-ci.org/stack-of-tasks/sot-torque-control.svg?branch=master)](https://travis-ci.org/stack-of-tasks/sot-torque-control)
[![Pipeline status](https://gepgitlab.laas.fr/stack-of-tasks/sot-torque-control/badges/master/pipeline.svg)](https://gepgitlab.laas.fr/stack-of-tasks/sot-torque-control/commits/master)
[![Coverage report](https://gepgitlab.laas.fr/stack-of-tasks/sot-torque-control/badges/master/coverage.svg?job=doc-coverage)](http://projects.laas.fr/gepetto/doc/stack-of-tasks/sot-torque-control/master/coverage/)
The library sot-torque-control contains a collection of dynamic-graph entities aimed at implementing torque control on different robots.
You can find a description of the main entities in the following <a href="https://github.com/stack-of-tasks/sot-torque-control/wiki">wiki</a>.
## Dependencies
This project depends on:
* [dynamic-graph](https://github.com/jrl-umi3218/dynamic-graph) >= 3.0.0
* [dynamic-graph-python](https://github.com/stack-of-tasks/dynamic-graph-python) >= 3.0.0
* [sot-core](https://github.com/stack-of-tasks/sot-core) >= 3.0.0
* [pinocchio](https://github.com/stack-of-tasks/pinocchio) >= 1.2
* [tsid](https://github.com/stack-of-tasks/tsid) >= 1.2
* [PinInvDyn](https://github.com/stack-of-tasks/invdyn)
* [parametric-curves](https://github.com/stack-of-tasks/parametric-curves)
* [simple_humanoid_description](https://github.com/laas/simple_humanoid_description)(for unit testing)
All of these packages (except PinInvDyn) can be installed through [robotpkg](http://robotpkg.openrobots.org/).
In particular, you can find them in [robotpkg-wip](http://robotpkg.openrobots.org/robotpkg-wip.html) (work in progress), a subset of robotpkg.
You will need the sot-talos and talos-dev packages:
```
sudo apt-get install robotpkg-py27-sot-talos robotpkg-talos-dev
```
Pay attention not to install ROS using robotpkg though, because it would install the latest version, which may not be what you need.
You can find the full installation procedure in the <a href="md_doc_installation.html">installation page</a>.
Quick instructions on how to run a test can be found <a href="md_doc_running.html">here</a>.
doc/customdoxygen.css 0 → 100644
View file @ eab7fb4e
/* Customizing Doxygen output */
/* Needed to allow line breaks in tables*/
.memberdecls {
table-layout: fixed;
width: 100%;
}
/* Needed to break long template names*/
.memTemplItemLeft {
white-space: normal !important;
word-wrap: break-word;
}
/* Needed to break long template names*/
.memItemLeft {
white-space: normal !important;
word-wrap: break-word;
}
doc/header.html 0 → 100644
View file @ eab7fb4e
<!-- HTML header for doxygen 1.8.11-->
<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen $doxygenversion"/>
<!--BEGIN PROJECT_NAME--><title>$projectname: $title</title><!--END PROJECT_NAME-->
<!--BEGIN !PROJECT_NAME--><title>$title</title><!--END !PROJECT_NAME-->
<link href="$relpath^tabs.css" rel="stylesheet" type="text/css"/>
<link href="$relpath^pinocchio.ico" rel="icon" type="image/x-icon">
<script type="text/javascript" src="$relpath^jquery.js"></script>
<script type="text/javascript" src="$relpath^dynsections.js"></script>
$treeview
$search
$mathjax
<link href="$relpath^$stylesheet" rel="stylesheet" type="text/css" />
$extrastylesheet
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<!--BEGIN TITLEAREA-->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
<tbody>
<tr style="height: 56px;">
<!--BEGIN PROJECT_LOGO-->
<td id="projectlogo"><img alt="Logo" src="$relpath^$projectlogo"/></td>
<!--END PROJECT_LOGO-->
<!--BEGIN PROJECT_NAME-->
<td id="projectalign" style="padding-left: 0.5em;">
<div id="projectname">$projectname
<!--BEGIN PROJECT_NUMBER-->&#160;<span id="projectnumber">$projectnumber</span><!--END PROJECT_NUMBER-->
</div>
<!--BEGIN PROJECT_BRIEF--><div id="projectbrief">$projectbrief</div><!--END PROJECT_BRIEF-->
</td>
<!--END PROJECT_NAME-->
<!--BEGIN !PROJECT_NAME-->
<!--BEGIN PROJECT_BRIEF-->
<td style="padding-left: 0.5em;">
<div id="projectbrief">$projectbrief</div>
</td>
<!--END PROJECT_BRIEF-->
<!--END !PROJECT_NAME-->
<!--BEGIN DISABLE_INDEX-->
<!--BEGIN SEARCHENGINE-->
<td>$searchbox</td>
<!--END SEARCHENGINE-->
<!--END DISABLE_INDEX-->
</tr>
</tbody>
</table>
</div>
<!--END TITLEAREA-->
<!-- end header part -->
doc/installation.md 0 → 100644
View file @ eab7fb4e
# Installation
1. Clone the git repository:
```
git clone --recursive https://github.com/stack-of-tasks/sot-torque-control.git
cd sot-torque-control
```
2. If you need it, switch to the devel branch
```
git checkout devel
```
3. Create the build directory and move there
```
mkdir _build-RELEASE
cd _build-RELEASE
```
4. Run cmake
```
cmake -DCMAKE_BUILD_TYPE=RELEASE -DCMAKE_INSTALL_PREFIX=$DEVEL_DIR/openrobots
```
In Ubuntu 14.04 you may need to add `-DCMAKE_CXX_FLAGS="-std=c++11"`.
5. Build the package
```
make -j4
```
6. Install the package
```
make install
```
doc/pictures/footer.txt 0 → 100644
View file @ eab7fb4e
Copy in this directory the image you wish to use in the footer of the documentation or edit file sot-core/doc/footer.html and
remove this file.
doc/running.md 0 → 100644
View file @ eab7fb4e
# Running a test
In the following, we quickly demonstrate how to run a test with sot-torque-control and <a href="https://gepgitlab.laas.fr/pyrene-dev/talos-torque-control.git">talos-torque-control</a>.
## Start the simulation
First of all, you need to start the simulation.
To start Gazebo, load a scene and spawn Talos, the simplest way is to directly use the launch file provided by PAL:
```
roslaunch talos_gazebo talos_gazebo.launch
```
Notice that this will spawn Talos at configuration zero. This is not always what you want.
The package talos_data offers different launch files to spawn it at different configurations.
Most commonly, you might want to spawn the robot in the half-sitting position:
```
roslaunch talos_data talos_gazebo.launch start_half_sitting:=true
```
## Start the SoT in position mode
To start the SoT in simulation in position mode:
```
roslaunch roscontrol_sot_talos sot_talos_controller_gazebo.launch
```
## Start the SoT in torque mode
To start the SoT in simulation in torque mode:
```
roslaunch roscontrol_sot_talos sot_talos_controller_gazebo_effort.launch
```
## Run the test
First of all, you need to go to the folder where your script is.
For instance, for running the standard tests of talos-torque-control, assuming you are in the root directory:
```
cd script
```
Then, you can just run the chosen test. For instance:
```
python sim_com.py position
```
This will launch a test routine executing a sinusoid on the CoM of the robot in position control (you should have started the SoT in position mode).
It also saves the dynamic graph in /tmp/sot_talos_tsid_com.pdf.
### Graph of the architecture
\image html Sot_torque_control_framework_for_position_control.png
For torque control simply run:
```
python sim_com.py torque
```
### Graph of the architecture
\image html Sot_torque_control_framework_for_torque_control.png
These two examples replicate the ex2 of the TSID jupyter notebooks in position and torque control in Gazebo (see the jupyter <a href="https://github.com/stack-of-tasks/tsid/blob/master/exercizes/notebooks/TSID_ex2.ipynb">here</a>).
## Interacting with the dynamic graph
If you want to dynamically interact with the graph
```
rosrun dynamic_graph_bridge run_command
```
## Other
More information on how to use the SoT and how to work on Talos can be found <a href="https://wiki.laas.fr/robots/Pyrene">in the robot wiki page</a> (you need LAAS permissions to access this).
doc/treeview.dox 0 → 100644
View file @ eab7fb4e
/*
* Copyright 2017, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
//
// This file strutures pages and modules into a convenient hierarchical structure.
//
namespace dynamicgraph {
namespace sot {
namespace torque_control {
//
// Pages/ tutorials organization
//
// \page md_doc_installation
// \page md_doc_running
}
}
}
include/sot/torque_control/simple-inverse-dyn.hh 0 → 100644
View file @ eab7fb4e
/*
* Copyright 2017, Andrea Del Prete, LAAS-CNRS
*
* This file is part of sot-torque-control.
* sot-torque-control is free software: you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public License
* as published by the Free Software Foundation, either version 3 of
* the License, or (at your option) any later version.
* sot-torque-control is distributed in the hope that it will be
* useful, but WITHOUT ANY WARRANTY; without even the implied warranty
* of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-torque-control. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_torque_control_simple_inverse_dyn_H__
#define __sot_torque_control_simple_inverse_dyn_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (simple_inverse_dyn_EXPORTS)
# define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllexport)
# else
# define SOTSIMPLEINVERSEDYN_EXPORT __declspec(dllimport)
# endif
#else
# define SOTSIMPLEINVERSEDYN_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <map>
#include "boost/assign.hpp"
/* Pinocchio */
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/parsers/urdf.hpp>
#include "pinocchio/algorithm/joint-configuration.hpp"
#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/solvers/solver-HQP-base.hpp>
#include <tsid/contacts/contact-6d.hpp>
#include <tsid/formulations/inverse-dynamics-formulation-acc-force.hpp>
#include <tsid/tasks/task-com-equality.hpp>
#include <tsid/tasks/task-joint-posture.hpp>
#include <tsid/trajectories/trajectory-euclidian.hpp>
/* HELPER */
#include <dynamic-graph/signal-helper.h>
#include <sot/core/matrix-geometry.hh>
#include <sot/core/robot-utils.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
enum ControlOutput {
CONTROL_OUTPUT_VELOCITY = 0,
CONTROL_OUTPUT_TORQUE = 1,
CONTROL_OUTPUT_SIZE = 2
};
const std::string ControlOutput_s[] = {
"velocity", "torque"
};
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTSIMPLEINVERSEDYN_EXPORT SimpleInverseDyn
: public::dynamicgraph::Entity {
typedef SimpleInverseDyn EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
SimpleInverseDyn( const std::string & name );
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(posture_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(posture_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_posture, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_posture, double);
DECLARE_SIGNAL_IN(kp_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(com_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_com, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_com, double);
DECLARE_SIGNAL_IN(kp_contact, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_contact, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_forces, double);
DECLARE_SIGNAL_IN(mu, double);
DECLARE_SIGNAL_IN(contact_points, dynamicgraph::Matrix);
DECLARE_SIGNAL_IN(contact_normal, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(f_min, double);
DECLARE_SIGNAL_IN(f_max, double);
DECLARE_SIGNAL_IN(waist_ref_pos, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(waist_ref_vel, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(waist_ref_acc, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kp_waist, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(kd_waist, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(w_waist, double);
DECLARE_SIGNAL_IN(q, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(v, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(active_joints, dynamicgraph::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_INNER(active_joints_checked, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(tau_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dv_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(v_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(q_des, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(u, dynamicgraph::Vector);
/* --- COMMANDS --- */
void init(const double& dt, const std::string& robotRef);
virtual void setControlOutputType(const std::string& type);
void updateComOffset();
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
void sendMsg(const std::string& msg, MsgType t = MSG_TYPE_INFO, const char* file = "", int line = 0) {
Entity::sendMsg("[" + name + "] " + msg, t, file, line);
}
protected:
double m_dt; /// control loop time period
double m_t; /// current time
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_enabled; /// True if controler is enabled
bool m_firstTime; /// True at the first iteration of the controller
/// TSID
/// Robot
tsid::robots::RobotWrapper * m_robot;
/// Solver and problem formulation
tsid::solvers::SolverHQPBase * m_hqpSolver;
tsid::InverseDynamicsFormulationAccForce * m_invDyn;
/// TASKS
tsid::contacts::Contact6d * m_contactRF;
tsid::contacts::Contact6d * m_contactLF;
tsid::tasks::TaskComEquality * m_taskCom;
tsid::tasks::TaskSE3Equality * m_taskWaist;
tsid::tasks::TaskJointPosture * m_taskPosture;
tsid::tasks::TaskJointPosture * m_taskBlockedJoints;
/// Trajectories of the tasks
tsid::trajectories::TrajectorySample m_sampleCom;
tsid::trajectories::TrajectorySample m_sampleWaist;
tsid::trajectories::TrajectorySample m_samplePosture;
/// Weights of the Tasks (which can be changed)
double m_w_com;
double m_w_posture;
double m_w_waist;
/// Computed solutions (accelerations and torques) and their derivatives
tsid::math::Vector m_dv_sot; /// desired accelerations (sot order)
tsid::math::Vector m_dv_urdf; /// desired accelerations (urdf order)
tsid::math::Vector m_v_sot; /// desired velocities (sot order)
tsid::math::Vector m_v_urdf; /// desired and current velocities (urdf order) (close the TSID loop on it)
tsid::math::Vector m_q_sot; /// desired positions (sot order)
tsid::math::Vector m_q_urdf; /// desired and current positions (urdf order) (close the TSID loop on it)
tsid::math::Vector m_tau_sot; /// desired torques (sot order)
tsid::math::Vector3 m_com_offset; /// 3d CoM offset
unsigned int m_timeLast; /// Final time of the control loop
RobotUtilShrPtr m_robot_util; /// Share pointer to the robot utils methods
ControlOutput m_ctrlMode; /// ctrl mode desired for the output (velocity or torque)
}; // class SimpleInverseDyn
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_simple_inverse_dyn_H__
python/dynamic_graph/sot/torque_control/utils/filter_utils.py
View file @ eab7fb4e
......@@ -2,7 +2,6 @@ from dynamic_graph.sot.core.filter_differentiator import FilterDifferentiator
from numpy import arctan2, imag, pi, real, unwrap
from pylab import grid, plot, show, subplot, subplots_adjust, title, xlabel, ylabel, ylim
def create_butter_lp_filter_Wn_05_N_2(name, dt, size):
lp_filter = FilterDifferentiator(name)
# from scipy.signal import butter
......
src/CMakeLists.txt
View file @ eab7fb4e
......@@ -59,6 +59,7 @@ SET(plugins
free-flyer-locator
imu_offset_compensation
inverse-dynamics-balance-controller
simple-inverse-dyn
joint-torque-controller
joint-trajectory-generator
numerical-difference
......
src/inverse-dynamics-balance-controller.cpp
View file @ eab7fb4e
......@@ -84,32 +84,106 @@ typedef SolverHQuadProgRT<48, 30, 17> SolverHQuadProgRT48x30x17;
#define ZERO_FORCE_THRESHOLD 1e-3
#define INPUT_SIGNALS \
m_com_ref_posSIN << m_com_ref_velSIN << m_com_ref_accSIN << m_rf_ref_posSIN << m_rf_ref_velSIN << m_rf_ref_accSIN \
<< m_lf_ref_posSIN << m_lf_ref_velSIN << m_lf_ref_accSIN << m_rh_ref_posSIN << m_rh_ref_velSIN \
<< m_rh_ref_accSIN << m_lh_ref_posSIN << m_lh_ref_velSIN << m_lh_ref_accSIN \
<< m_posture_ref_posSIN << m_posture_ref_velSIN << m_posture_ref_accSIN \
<< m_base_orientation_ref_posSIN << m_base_orientation_ref_velSIN << m_base_orientation_ref_accSIN \
<< m_f_ref_right_footSIN << m_f_ref_left_footSIN << m_kp_base_orientationSIN \
<< m_kd_base_orientationSIN << m_kp_constraintsSIN << m_kd_constraintsSIN << m_kp_comSIN \
<< m_kd_comSIN << m_kp_feetSIN << m_kd_feetSIN << m_kp_handsSIN << m_kd_handsSIN \
<< m_kp_postureSIN << m_kd_postureSIN << m_kp_posSIN << m_kd_posSIN << m_w_comSIN << m_w_feetSIN \
<< m_w_handsSIN << m_w_postureSIN << m_w_base_orientationSIN << m_w_torquesSIN << m_w_forcesSIN \
<< m_weight_contact_forcesSIN << m_muSIN << m_contact_pointsSIN << m_contact_normalSIN \
<< m_f_minSIN << m_f_max_right_footSIN << m_f_max_left_footSIN << m_rotor_inertiasSIN \
<< m_gear_ratiosSIN << m_tau_maxSIN << m_q_minSIN << m_q_maxSIN << m_dq_maxSIN << m_ddq_maxSIN \
<< m_dt_joint_pos_limitsSIN << m_tau_estimatedSIN << m_qSIN << m_vSIN << m_wrench_baseSIN \
<< m_wrench_left_footSIN << m_wrench_right_footSIN << m_active_jointsSIN
#define OUTPUT_SIGNALS \
m_tau_desSOUT << m_MSOUT << m_dv_desSOUT << m_f_des_right_footSOUT << m_f_des_left_footSOUT \
<< m_zmp_des_right_footSOUT << m_zmp_des_left_footSOUT << m_zmp_des_right_foot_localSOUT \
<< m_zmp_des_left_foot_localSOUT << m_zmp_desSOUT << m_zmp_refSOUT << m_zmp_right_footSOUT \
<< m_zmp_left_footSOUT << m_zmpSOUT << m_comSOUT << m_com_velSOUT << m_com_accSOUT \
<< m_com_acc_desSOUT << m_base_orientationSOUT << m_left_foot_posSOUT << m_right_foot_posSOUT \
<< m_left_hand_posSOUT << m_right_hand_posSOUT << m_left_foot_velSOUT << m_right_foot_velSOUT \
<< m_left_hand_velSOUT << m_right_hand_velSOUT << m_left_foot_accSOUT << m_right_foot_accSOUT \
<< m_left_hand_accSOUT << m_right_hand_accSOUT << m_right_foot_acc_desSOUT << m_left_foot_acc_desSOUT
#define INPUT_SIGNALS m_com_ref_posSIN \
<< m_com_ref_velSIN \
<< m_com_ref_accSIN \
<< m_rf_ref_posSIN \
<< m_rf_ref_velSIN \
<< m_rf_ref_accSIN \
<< m_lf_ref_posSIN \
<< m_lf_ref_velSIN \
<< m_lf_ref_accSIN \
<< m_rh_ref_posSIN \
<< m_rh_ref_velSIN \
<< m_rh_ref_accSIN \
<< m_lh_ref_posSIN \
<< m_lh_ref_velSIN \
<< m_lh_ref_accSIN \
<< m_posture_ref_posSIN \
<< m_posture_ref_velSIN \
<< m_posture_ref_accSIN \
<< m_base_orientation_ref_posSIN \
<< m_base_orientation_ref_velSIN \
<< m_base_orientation_ref_accSIN \
<< m_f_ref_right_footSIN \
<< m_f_ref_left_footSIN \
<< m_kp_base_orientationSIN \
<< m_kd_base_orientationSIN \
<< m_kp_constraintsSIN \
<< m_kd_constraintsSIN \
<< m_kp_comSIN \
<< m_kd_comSIN \
<< m_kp_feetSIN \
<< m_kd_feetSIN \
<< m_kp_handsSIN \
<< m_kd_handsSIN \
<< m_kp_postureSIN \
<< m_kd_postureSIN \
<< m_kp_posSIN \
<< m_kd_posSIN \
<< m_w_comSIN \
<< m_w_feetSIN \
<< m_w_handsSIN \
<< m_w_postureSIN \
<< m_w_base_orientationSIN \
<< m_w_torquesSIN \