Skip to content
GitLab
Commit f90d9096 authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge branch 'devel' into cmake-export

parents 924bf57d 4133ba32
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ f90d9096
# Copyright 2018, Gepetto team, LAAS-CNRS
#
CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
set(CMAKE_CXX_STANDARD 11)
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
SET(PROJECT_NAMESPACE loco-3d)
SET(PROJECT_NAME sot-talos-balance)
......@@ -16,15 +16,10 @@ SET(PROJECT_USE_CMAKE_EXPORT TRUE)
SET(CXX_DISABLE_WERROR True)
INCLUDE(cmake/base.cmake)
# Specify the project.
cmake_policy(SET CMP0048 NEW)
PROJECT(${PROJECT_NAME}
LANGUAGES
CXX
VERSION
${PROJECT_VERSION_MAJOR}.${PROJECT_VERSION_MINOR}.${PROJECT_VERSION_PATCH}
)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
# So that generated headers are in same place as standard ones
SET(CUSTOM_HEADER_DIR "sot/talos_balance")
......@@ -40,12 +35,8 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
plugindir
)
find_package (Boost REQUIRED
python filesystem system thread program_options unit_test_framework)
find_package (Eigen3 REQUIRED NO_MODULE)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
CMAKE_POLICY(SET CMP0048 OLD)
PROJECT(${PROJECT_NAME} CXX)
IF(WIN32)
SET(LINK copy_if_different)
......@@ -89,7 +80,9 @@ ADD_PROJECT_DEPENDENCY(sot-core REQUIRED ) #4.5.0
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.1")
ADD_REQUIRED_DEPENDENCY("parametric-curves")
ADD_REQUIRED_DEPENDENCY("eigen-quadprog")
ADD_PROJECT_DEPENDENCY(talos_data)
IF(BUILD_TEST)
ADD_REQUIRED_DEPENDENCY("talos_data")
ENDIF(BUILD_TEST)
SET(SOTTALOSBALANCE_LIB_NAME ${PROJECT_NAME})
SET(LIBRARY_NAME ${SOTTALOSBALANCE_LIB_NAME})
......@@ -206,6 +199,6 @@ IF(BUILD_ROS_PACKAGES AND NOT INSTALL_PYTHON_INTERFACE_ONLY)
ADD_SUBDIRECTORY(ros)
ENDIF(BUILD_ROS_PACKAGES AND NOT INSTALL_PYTHON_INTERFACE_ONLY)
ADD_SUBDIRECTORY(unittest)
SETUP_PROJECT_FINALIZE()
IF(BUILD_TEST)
ADD_SUBDIRECTORY(unittest)
ENDIF(BUILD_TEST)
cmake @ 54ece258
Compare 6ccdbd0d...54ece258
Subproject commit 6ccdbd0d13d29e94d1ff21bc7f9f5ef643f31094
Subproject commit 54ece258eed16da94c7f10979588fcb47b744eb8
include/sot/talos_balance/dcm-controller.hh
View file @ f90d9096
......@@ -66,6 +66,7 @@ namespace dynamicgraph {
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Ki, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(Kz, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(decayFactor, double);
DECLARE_SIGNAL_IN(omega, double);
DECLARE_SIGNAL_IN(mass, double);
......@@ -73,6 +74,7 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(dcm, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dcmDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmpDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(zmp, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(zmpRef, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(wrenchRef, dynamicgraph::Vector);
......
include/sot/talos_balance/distribute-wrench.hh
View file @ f90d9096
......@@ -46,7 +46,7 @@
#include <pinocchio/multibody/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <eigen-quadprog/QuadProg.h>
#include <eiquadprog/eiquadprog-fast.hpp>
namespace dynamicgraph {
namespace sot {
......@@ -133,8 +133,8 @@ namespace dynamicgraph {
void computeWrenchFaceMatrix(const double mu);
Eigen::Matrix<double, 16, 6> m_wrenchFaceMatrix; // for modelling contact
Eigen::QuadProgDense m_qp1; // saturate wrench
Eigen::QuadProgDense m_qp2; // distribute wrench
eiquadprog::solvers::EiquadprogFast m_qp1; // saturate wrench
eiquadprog::solvers::EiquadprogFast m_qp2; // distribute wrench
// QP problem matrices -- SSP
Eigen::MatrixXd m_Q1;
......@@ -146,6 +146,8 @@ namespace dynamicgraph {
Eigen::MatrixXd m_Aineq1;
Eigen::VectorXd m_Bineq1;
Eigen::VectorXd m_result1;
// QP problem matrices -- DSP
Eigen::MatrixXd m_Q2;
Eigen::VectorXd m_C2;
......@@ -156,6 +158,8 @@ namespace dynamicgraph {
Eigen::MatrixXd m_Aineq2;
Eigen::VectorXd m_Bineq2;
Eigen::VectorXd m_result2;
double m_wSum;
double m_wNorm;
double m_wRatio;
......
include/sot/talos_balance/foot-force-difference-controller.hh
View file @ f90d9096
......@@ -75,6 +75,8 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(vdcFrequency, double);
DECLARE_SIGNAL_IN(vdcDamping, double);
DECLARE_SIGNAL_IN(swingAdmittance, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchRightDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchLeftDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(wrenchRight, dynamicgraph::Vector);
......@@ -99,6 +101,9 @@ namespace dynamicgraph {
virtual void display( std::ostream& os ) const;
protected:
Eigen::Vector3d calcSwingAdmittance(const dynamicgraph::Vector & wrench, const dynamicgraph::Vector & swingAdmittance);
double m_eps;
bool m_initSucceeded; /// true if the entity has been successfully initialized
}; // class FootForceDifferenceController
......
include/sot/talos_balance/hip-flexibility-compensation.hh
View file @ f90d9096
......@@ -60,6 +60,9 @@ class HIPFLEXIBILITYCOMPENSATION_EXPORT HipFlexibilityCompensation
HipFlexibilityCompensation(const std::string& name);
/* --- SIGNALS --- */
/// \brief Walking phase
DECLARE_SIGNAL_IN(phase, int);
/// \brief Desired joint configuration of the robot
DECLARE_SIGNAL_IN(q_des, dynamicgraph::Vector);
/// \brief Current torque mesured at each joint
......@@ -106,6 +109,7 @@ class HIPFLEXIBILITYCOMPENSATION_EXPORT HipFlexibilityCompensation
double m_rate_limiter;
dynamicgraph::Vector m_previous_delta_q;
dynamicgraph::Vector m_previous_tau;
dynamicgraph::Vector m_limitedSignal;
RobotUtilShrPtr m_robot_util;
......@@ -115,4 +119,4 @@ class HIPFLEXIBILITYCOMPENSATION_EXPORT HipFlexibilityCompensation
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_hip_flexibility_compensation_H__
\ No newline at end of file
#endif // #ifndef __sot_talos_balance_hip_flexibility_compensation_H__
include/sot/talos_balance/simple-state-integrator.hh 0 → 100644
View file @ f90d9096
/*
* Copyright 2018, Gepetto team, LAAS-CNRS
*
* This file is part of sot-talos-balance.
* sot-talos-balance 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-talos-balance 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-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_talos_balance_simple_state_integrator_H__
#define __sot_talos_balance_simple_state_integrator_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (simple_state_integrator_EXPORTS)
# define SIMPLE_STATE_INTEGRATOR_EXPORT __declspec(dllexport)
# else
# define SIMPLE_STATE_INTEGRATOR_EXPORT __declspec(dllimport)
# endif
#else
# define SIMPLE_STATE_INTEGRATOR_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include <map>
#include "boost/assign.hpp"
#include <sot/core/matrix-geometry.hh>
#include <dynamic-graph/linear-algebra.h>
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SIMPLE_STATE_INTEGRATOR_EXPORT SimpleStateIntegrator
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
protected:
/// \brief Current integration step.
double timestep_;
/// \name Vectors related to the state.
///@{
/// Position of the robot wrt pinocchio.
Eigen::VectorXd state_;
/// Velocity of the robot wrt pinocchio.
Eigen::VectorXd velocity_;
///@}
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
SimpleStateIntegrator( const std::string & name );
void init(const double& step);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(control, ::dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(state, ::dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(velocity, ::dynamicgraph::Vector);
public:
void setState(const ::dynamicgraph::Vector& st);
void setVelocity(const ::dynamicgraph::Vector& vel);
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
protected:
/// Integrate the freeflyer state (to obtain position).
/// Compute roll pitch yaw angles
void integrateRollPitchYaw(::dynamicgraph::Vector& state, const ::dynamicgraph::Vector& control, double dt);
// Computes Euler angles in good range : [-pi:pi]x[-pi/2:pi/2]x[-pi:pi]
void rotationMatrixToEuler(const Eigen::Matrix3d& rotationMatrix, Eigen::Vector3d& rollPitchYaw);
}; // class SimpleStateIntegrator
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_simple_state_integrator_H__
plot/plot_base_estimator.xml 0 → 100644
View file @ f90d9096
<?xml version='1.0' encoding='UTF-8'?>
<root version="2.3.0">
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix columns="3" rows="2" tab_name="plot">
<plot col="0" row="0">
<range top="0.001005" bottom="-0.041205" right="53.261000" left="3.261000"/>
<limitY/>
<curve R="31" custom_transform="noTransform" G="119" name="/gazebo/model_states/pose.1/position/x" B="180"/>
<curve R="241" custom_transform="noTransform" G="76" name="/sot/PYRENE/state/data.0" B="193"/>
<curve R="188" custom_transform="noTransform" G="189" name="/sot/base_estimator/q/data.0" B="34"/>
<transform value="noTransform"/>
</plot>
<plot col="0" row="1">
<range top="0.000454" bottom="-0.000255" right="53.261000" left="3.261000"/>
<limitY/>
<curve R="26" custom_transform="noTransform" G="201" name="/gazebo/model_states/pose.1/orientation/x" B="56"/>
<curve R="148" custom_transform="noTransform" G="103" name="/sot/PYRENE/state/data.3" B="189"/>
<curve R="31" custom_transform="noTransform" G="119" name="/sot/base_estimator/q/data.3" B="180"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="0">
<range top="0.000082" bottom="-0.003373" right="53.261000" left="3.261000"/>
<limitY/>
<curve R="214" custom_transform="noTransform" G="39" name="/gazebo/model_states/pose.1/position/y" B="40"/>
<curve R="148" custom_transform="noTransform" G="103" name="/sot/PYRENE/state/data.1" B="189"/>
<curve R="31" custom_transform="noTransform" G="119" name="/sot/base_estimator/q/data.1" B="180"/>
<transform value="noTransform"/>
</plot>
<plot col="1" row="1">
<range top="0.000205" bottom="-0.008397" right="53.261000" left="3.261000"/>
<limitY/>
<curve R="255" custom_transform="noTransform" G="127" name="/gazebo/model_states/pose.1/orientation/y" B="14"/>
<curve R="23" custom_transform="noTransform" G="190" name="/sot/PYRENE/state/data.4" B="207"/>
<curve R="214" custom_transform="noTransform" G="39" name="/sot/base_estimator/q/data.4" B="40"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="0">
<range top="1.021203" bottom="1.016997" right="53.261000" left="3.261000"/>
<limitY/>
<curve R="255" custom_transform="noTransform" G="127" name="/gazebo/model_states/pose.1/position/z" B="14"/>
<curve R="23" custom_transform="noTransform" G="190" name="/sot/PYRENE/state/data.2" B="207"/>
<curve R="214" custom_transform="noTransform" G="39" name="/sot/base_estimator/q/data.2" B="40"/>
<transform value="noTransform"/>
</plot>
<plot col="2" row="1">
<range top="0.001018" bottom="-0.000117" right="53.261000" left="3.261000"/>
<limitY/>
<curve R="241" custom_transform="noTransform" G="76" name="/gazebo/model_states/pose.1/orientation/z" B="193"/>
<curve R="188" custom_transform="noTransform" G="189" name="/sot/PYRENE/state/data.5" B="34"/>
<curve R="26" custom_transform="noTransform" G="201" name="/sot/base_estimator/q/data.5" B="56"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="0"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
<!-- - - - - - - - - - - - - - - -->
<Plugins>
<plugin ID="DataLoad_CSV">
<default time_axis=""/>
</plugin>
<plugin ID="DataLoad_ROS_bags">
<use_header_stamp value="false"/>
<use_renaming_rules value="true"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="DataLoad_ULog"/>
<plugin ID="ROS_Topic_Streamer">
<use_header_stamp value="false"/>
<use_renaming_rules value="true"/>
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="RosoutPublisherROS" status="idle"/>
<plugin ID="TopicPublisherROS" status="idle"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
<previouslyLoaded_Streamer name="ROS_Topic_Streamer"/>
<!-- - - - - - - - - - - - - - - -->
<customMathEquations/>
<snippets>
<snippet name="1st_derivative">
<global>var prevX = 0
var prevY = 0</global>
<equation>dx = time - prevX
dy = value - prevY
prevX = time
prevY = value
return dy/dx</equation>
</snippet>
<snippet name="1st_order_lowpass">
<global>var prevY = 0
var alpha = 0.1</global>
<equation>prevY = alpha * value + (1.-alpha) * prevY
return prevY</equation>
</snippet>
<snippet name="sum_A_B">
<global></global>
<equation>return $$PLOT_A$$ + $$PLOT_B$$</equation>
</snippet>
<snippet name="yaw_from_quaternion">
<global>// source: https://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles
function quaternionToYaw(x, y, z, w)
{
// yaw (z-axis rotation)
t1 = 2.0 * (w * z + x * y);
t2 = 1.0 - 2.0 * (y * y + z * z);
yaw = Math.atan2(t1, t2);
return yaw
}</global>
<equation>return quaternionToYaw(x, y, z, w);</equation>
</snippet>
</snippets>
<!-- - - - - - - - - - - - - - - -->
</root>
plot/plot_wrench_distrib.xml
View file @ f90d9096
<?xml version='1.0' encoding='UTF-8'?>
<root version="2.3.0">
<tabbed_widget name="Main Window" parent="main_window">
<tabbed_widget parent="main_window" name="Main Window">
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot row="0" col="0">
<range top="-1.926198" bottom="-2.704815" left="0.000000" right="11.113000"/>
<range bottom="-64.965137" top="117.614916" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve name="/sot/dcmCtrl/wrenchRef/data.0" R="20" G="100" custom_transform="noTransform" B="160"/>
<curve name="/sot/distribute/wrenchRef/data.0" R="247" G="0" custom_transform="noTransform" B="247"/>
<curve R="20" B="160" G="100" name="/sot/dcmCtrl/wrenchRef/data.0" custom_transform="noTransform"/>
<curve R="247" B="247" G="0" name="/sot/distribute/wrenchRef/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range top="-0.947004" bottom="-1.368506" left="0.000000" right="11.113000"/>
<range bottom="-64.965137" top="117.614916" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve name="/sot/distribute/wrenchLeft/data.0" R="80" G="180" custom_transform="noTransform" B="127"/>
<curve name="/sot/distribute/wrenchRight/data.0" R="20" G="100" custom_transform="noTransform" B="160"/>
<curve R="80" B="127" G="180" name="/sot/distribute/wrenchLeft/data.0" custom_transform="noTransform"/>
<curve R="20" B="160" G="100" name="/sot/distribute/wrenchRight/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range top="1.292420" bottom="1.087894" left="0.000000" right="11.113000"/>
<range bottom="-70.960893" top="83.557378" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve name="/sot/dcmCtrl/wrenchRef/data.1" R="5" G="116" custom_transform="noTransform" B="13"/>
<curve name="/sot/distribute/wrenchRef/data.1" R="0" G="51" custom_transform="noTransform" B="238"/>
<curve R="5" B="13" G="116" name="/sot/dcmCtrl/wrenchRef/data.1" custom_transform="noTransform"/>
<curve R="0" B="238" G="51" name="/sot/distribute/wrenchRef/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range top="0.994414" bottom="0.195436" left="0.000000" right="11.113000"/>
<range bottom="-70.960893" top="83.557378" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve name="/sot/distribute/wrenchLeft/data.1" R="244" G="83" custom_transform="noTransform" B="29"/>
<curve name="/sot/distribute/wrenchRight/data.1" R="5" G="116" custom_transform="noTransform" B="13"/>
<curve R="244" B="29" G="83" name="/sot/distribute/wrenchLeft/data.1" custom_transform="noTransform"/>
<curve R="5" B="13" G="116" name="/sot/distribute/wrenchRight/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range top="885.570204" bottom="885.570203" left="0.000000" right="11.113000"/>
<range bottom="885.570204" top="885.570204" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve name="/sot/dcmCtrl/wrenchRef/data.2" R="255" G="19" custom_transform="noTransform" B="24"/>
<curve name="/sot/distribute/wrenchRef/data.2" R="0" G="119" custom_transform="noTransform" B="119"/>
<curve R="255" B="24" G="19" name="/sot/dcmCtrl/wrenchRef/data.2" custom_transform="noTransform"/>
<curve R="0" B="119" G="119" name="/sot/distribute/wrenchRef/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range top="442.786545" bottom="442.783658" left="0.000000" right="11.113000"/>
<range bottom="-22.139255" top="907.709459" left="-21.733000" right="-11.738000"/>
<limitY/>
<curve name="/sot/distribute/wrenchLeft/data.2" R="142" G="52" custom_transform="noTransform" B="136"/>
<curve name="/sot/distribute/wrenchRight/data.2" R="255" G="19" custom_transform="noTransform" B="24"/>
<curve R="142" B="136" G="52" name="/sot/distribute/wrenchLeft/data.2" custom_transform="noTransform"/>
<curve R="255" B="24" G="19" name="/sot/distribute/wrenchRight/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="0"/>
<plotmatrix rows="2" columns="3" tab_name="plot">
<plot row="0" col="0">
<range bottom="58.812999" top="59.369892" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="241" B="193" G="76" name="/sot/ftc/left_foot_force_out/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="0">
<range bottom="-59.654389" top="-59.188734" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="188" B="34" G="189" name="/sot/ftc/right_foot_force_out/data.0" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="1">
<range bottom="-46.926824" top="-45.127294" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="148" B="189" G="103" name="/sot/ftc/left_foot_force_out/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="1">
<range bottom="45.588683" top="47.350587" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="31" B="180" G="119" name="/sot/ftc/right_foot_force_out/data.1" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="0" col="2">
<range bottom="416.204087" top="417.069832" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="23" B="207" G="190" name="/sot/ftc/left_foot_force_out/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
<plot row="1" col="2">
<range bottom="445.899472" top="446.756994" left="24.225000" right="34.222000"/>
<limitY/>
<curve R="214" B="40" G="39" name="/sot/ftc/right_foot_force_out/data.2" custom_transform="noTransform"/>
<transform value="noTransform"/>
</plot>
</plotmatrix>
<currentPlotMatrix index="1"/>
</tabbed_widget>
<use_relative_time_offset enabled="1"/>
<!-- - - - - - - - - - - - - - - -->
......@@ -67,8 +105,8 @@
<discard_large_arrays value="true"/>
<max_array_size value="100"/>
</plugin>
<plugin ID="RosoutPublisherROS" status="idle"/>
<plugin ID="TopicPublisherROS" status="idle"/>
<plugin status="idle" ID="RosoutPublisherROS"/>
<plugin status="idle" ID="TopicPublisherROS"/>
</Plugins>
<!-- - - - - - - - - - - - - - - -->
<previouslyLoaded_Datafiles/>
......
python/sot_talos_balance/CMakeLists.txt
View file @ f90d9096
......@@ -5,6 +5,7 @@ SET(${PROJECT_NAME}_PYTHON_FILES
create_entities_utils.py
meta_task_config.py
meta_task_joint.py
meta_task_pose.py
motor_parameters.py
)
......
python/sot_talos_balance/extract_torque_motorenc_joint_enc_batteries.py
View file @ f90d9096
import subprocess
import rosbag
import rospy
import subprocess, yaml
import yaml
bagname= '2017-07-24-08-04-30.bag'
bagname = '2017-07-24-08-04-30.bag'
info_dict = yaml.load(subprocess.Popen(['rosbag', 'info', '--yaml', bagname], stdout=subprocess.PIPE).communicate()[0])
bag = rosbag.Bag(bagname)
ps=bag.read_messages(topics=['/power_status'])
introdata=bag.read_messages(topics=['/data'])
start_time=rospy.Time()
ps = bag.read_messages(topics=['/power_status'])
introdata = bag.read_messages(topics=['/data'])
start_time = rospy.Time()
class message:
"""Store torque, motor encoder, joint encoder"""
motorenc = 0
jointenc = 0
torquesensor = 0
current = 0
def extract_data():
jointnames = ('torso_1','torso_2', 'head_1','head_2', \
'arm_left_1','arm_left_2','arm_left_3','arm_left_4','arm_left_5','arm_left_6','arm_left_7', \
'arm_right_1','arm_right_2','arm_right_3','arm_right_4','arm_right_5','arm_right_6','arm_right_7', \
'leg_left_1','leg_left_2','leg_left_3','leg_left_4','leg_left_5','leg_left_6','leg_left_7', \
'leg_right_1','leg_right_2','leg_right_3','leg_right_4','leg_right_5','leg_right_6','leg_right_7')
"""Store torque, motor encoder, joint encoder"""
motorenc = 0
jointenc = 0
torquesensor = 0
current = 0
filesnames={}
files={}
for jn in jointnames:
filesnames[jn] = '/BackupFiles/devel-src/data/'+ jn + '_me_je.dat'
files[jn] = open(filesnames[jn],'w')
def extract_data():
jointnames = ('torso_1', 'torso_2', 'head_1', 'head_2', 'arm_left_1', 'arm_left_2', 'arm_left_3', 'arm_left_4',
'arm_left_5', 'arm_left_6', 'arm_left_7', 'arm_right_1', 'arm_right_2', 'arm_right_3', 'arm_right_4',
'arm_right_5', 'arm_right_6', 'arm_right_7', 'leg_left_1', 'leg_left_2', 'leg_left_3', 'leg_left_4',
'leg_left_5', 'leg_left_6', 'leg_left_7', 'leg_right_1', 'leg_right_2', 'leg_right_3', 'leg_right_4',
'leg_right_5', 'leg_right_6', 'leg_right_7')
start=1
for apw in introdata:
messages={}
if start==1:
start_time=apw.timestamp