Skip to content

GitLab

Commit 72b0f9fe authored by Guilhem Saurel's avatar Guilhem Saurel
Browse files

Merge tag 'v1.3.1' 

Release of version 1.3.1.
parents 90409a65 1166d6a7
Hide whitespace changes
Inline Side-by-side
.gitlab-ci.yml
View file @ 72b0f9fe
variables:
GIT_SUBMODULE_STRATEGY: "recursive"
GIT_DEPTH: "3"
CCACHE_BASEDIR: "${CI_PROJECT_DIR}"
CCACHE_DIR: "${CI_PROJECT_DIR}/ccache"
cache:
paths:
- ccache
.robotpkg-py-sot-torque-control: &robotpkg-py-sot-torque-control
except:
- gh-pages
script:
- mkdir -p ccache
- cd /root/robotpkg/wip/py-sot-torque-control
- git pull
- make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
- make install
- cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
- make test
robotpkg-py-sot-torque-control-14.04-release:
<<: *robotpkg-py-sot-torque-control
image: eur0c.laas.fr:5000/stack-of-tasks/sot-torque-control/py-sot-torque-control:14.04
robotpkg-py-sot-torque-control-16.04-release:
<<: *robotpkg-py-sot-torque-control
image: eur0c.laas.fr:5000/stack-of-tasks/sot-torque-control/py-sot-torque-control:16.04
.robotpkg-sot-torque-control: &robotpkg-sot-torque-control
except:
- gh-pages
script:
- mkdir -p ccache
- cd /root/robotpkg/wip/sot-torque-control
- git pull
- make checkout MASTER_REPOSITORY="dir ${CI_PROJECT_DIR}"
- make install
- cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
- make test
robotpkg-sot-torque-control-14.04-release:
<<: *robotpkg-sot-torque-control
image: eur0c.laas.fr:5000/stack-of-tasks/sot-torque-control/sot-torque-control:14.04
robotpkg-sot-torque-control-16.04-release:
<<: *robotpkg-sot-torque-control
image: eur0c.laas.fr:5000/stack-of-tasks/sot-torque-control/sot-torque-control:16.04
doc-coverage:
<<: *robotpkg-py-sot-torque-control
image: eur0c.laas.fr:5000/stack-of-tasks/sot-torque-control/py-sot-torque-control:16.04
before_script:
- echo -e 'CXXFLAGS+= --coverage\nLDFLAGS+= --coverage\nPKG_DEFAULT_OPTIONS= debug' >> /opt/openrobots/etc/robotpkg.conf
after_script:
- cd /root/robotpkg/wip/py-sot-torque-control
- cd work.$(hostname)/$(make show-var VARNAME=DISTNAME)
- make doc
- mv doc/doxygen-html ${CI_PROJECT_DIR}
- mkdir -p ${CI_PROJECT_DIR}/coverage/
- gcovr -r .
- gcovr -r . --html --html-details -o ${CI_PROJECT_DIR}/coverage/index.html
artifacts:
expire_in: 1 day
paths:
- doxygen-html/
- coverage/
include: http://rainboard.laas.fr/project/sot-torque-control/.gitlab-ci.yml
CMakeLists.txt
View file @ 72b0f9fe
# Copyright 2014, Andrea Del Prete, LAAS/CNRS
# Copyright 2014-2018, Andrea Del Prete, LAAS/CNRS
# Thomas Flayols, LAAS/CNRS
# Olivier Stasse, LAAS/CNRS
# Florent Forget, LAAS/CNRS
# Paul Dandignac, LAAS/CNRS
#
# This file is part of sot-torque-control.
# sot-torque-control is free software: you can redistribute it and/or
......@@ -17,15 +21,14 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/test.cmake)
SET(PROJECT_NAMESPACE stack-of-tasks)
SET(PROJECT_NAME sot-torque-control)
SET(PROJECT_DESCRIPTION "Collection of dynamic-graph entities aimed at implementing torque control on different robots.")
SET(PROJECT_URL "https://github.com/stack-of-tasks/sot-torque-control")
SET(PROJECT_URL "https://github.com/${PROJECT_NAMESPACE}/${PROJECT_NAME}")
SET(CUSTOM_HEADER_DIR "sot/torque-control")
......@@ -77,6 +80,7 @@ ADD_REQUIRED_DEPENDENCY("tsid")
ADD_REQUIRED_DEPENDENCY("parametric-curves")
ADD_REQUIRED_DEPENDENCY("simple_humanoid_description")
ADD_OPTIONAL_DEPENDENCY("ddp-actuator-solver")
SET(SOTTORQUECONTROL_LIB_NAME ${PROJECT_NAME})
SET(LIBRARY_NAME ${SOTTORQUECONTROL_LIB_NAME})
......@@ -95,15 +99,12 @@ SET(${LIBRARY_NAME}_HEADERS
include/sot/torque_control/control-manager.hh
include/sot/torque_control/current-controller.hh
include/sot/torque_control/commands-helper.hh
include/sot/torque_control/signal-helper.hh
include/sot/torque_control/common.hh
include/sot/torque_control/madgwickahrs.hh
include/sot/torque_control/device-torque-ctrl.hh
include/sot/torque_control/trace-player.hh
include/sot/torque_control/torque-offset-estimator.hh
include/sot/torque_control/imu_offset_compensation.hh
include/sot/torque_control/admittance-controller.hh
include/sot/torque_control/utils/logger.hh
include/sot/torque_control/utils/trajectory-generators.hh
include/sot/torque_control/utils/lin-estimator.hh
include/sot/torque_control/utils/poly-estimator.hh
......@@ -119,7 +120,6 @@ SET(${LIBRARY_NAME}_HEADERS
# PERMISSIONS OWNER_READ GROUP_READ WORLD_READ OWNER_WRITE)
SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_HEADERS}
src/logger.cpp
src/trajectory-generators.cpp
src/lin-estimator.cpp
src/poly-estimator.cpp
......@@ -127,7 +127,6 @@ SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_HEADERS}
src/causal-filter.cpp
src/stop-watch.cpp
src/motor-model.cpp
src/common.cpp
)
......@@ -189,13 +188,9 @@ ADD_SUBDIRECTORY(src)
ADD_SUBDIRECTORY(unitTesting)
# *****************************
# Common-sot-py PYTHON module *
# PYTHON interface *
# *****************************
IF(BUILD_PYTHON_INTERFACE)
PYTHON_ADD_MODULE(common_sot_py src/common-py.cpp)
TARGET_LINK_LIBRARIES(common_sot_py ${Boost_LIBRARIES} ${PYTHON_LIBRARIES} ${LIBRARY_NAME})
TARGET_LINK_BOOST_PYTHON(common_sot_py)
INSTALL(TARGETS common_sot_py DESTINATION ${PYTHON_INSTALL_DIR})
IF(TALOS_DATA_FOUND)
FOREACH(py_filename test_torque_offset_estimator)
......@@ -207,6 +202,7 @@ IF(BUILD_PYTHON_INTERFACE)
INSTALL(FILES ${PROJECT_BINARY_DIR}/python/dynamic_graph/sot/torque_control/tests/${py_filename}.py
DESTINATION ${PYTHON_SITELIB}/dynamic_graph/sot/torque_control/tests)
ENDFOREACH(py_filename)
ENDIF(TALOS_DATA_FOUND)
......@@ -222,5 +218,5 @@ IF(BUILD_PYTHON_INTERFACE)
ENDIF(SIMPLE_HUMANOID_DESCRIPTION_FOUND)
ENDIF(BUILD_PYTHON_INTERFACE)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
README.md
View file @ 72b0f9fe
......@@ -16,6 +16,7 @@ This project depends on:
* pinocchio >= 1.2
* [PinInvDyn](https://github.com/stack-of-tasks/invdyn)
* [parametric-curves](https://github.com/stack-of-tasks/parametric-curves)
* (for unit testing)[simple_humanoid_description](https://github.com/laas/simple_humanoid_description)
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.
......
cmake @ 1d9aeca2
Compare 8e7bedfc...1d9aeca2
Subproject commit 8e7bedfcbd8524c0401a58fd74edc07c3d4308d0
Subproject commit 1d9aeca25e970d2d967fd5be0fb93fe961db121b
include/sot/torque_control/admittance-controller.hh
View file @ 72b0f9fe
......@@ -36,16 +36,19 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <sot/torque_control/common.hh>
#include <map>
#include "boost/assign.hpp"
#include <tsid/robots/robot-wrapper.hpp>
#include <tsid/tasks/task-se3-equality.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 {
......@@ -105,7 +108,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("["+name+"] "+msg, t, file, line);
sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
......@@ -122,7 +125,7 @@ namespace dynamicgraph {
/// tsid
tsid::robots::RobotWrapper * m_robot;
se3::Data* m_data;
pinocchio::Data* m_data;
tsid::math::Vector6 m_f_RF; /// desired 6d wrench right foot
tsid::math::Vector6 m_f_LF; /// desired 6d wrench left foot
......@@ -135,7 +138,7 @@ namespace dynamicgraph {
tsid::math::Vector m_dq_fd; /// joint velocities computed with finite differences
tsid::math::Vector m_qPrev; /// previous value of encoders
typedef se3::Data::Matrix6x Matrix6x;
typedef pinocchio::Data::Matrix6x Matrix6x;
Matrix6x m_J_RF;
Matrix6x m_J_LF;
Eigen::ColPivHouseholderQR<Matrix6x> m_J_RF_QR;
......
include/sot/torque_control/base-estimator.hh
View file @ 72b0f9fe
......@@ -36,10 +36,6 @@
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <sot/torque_control/common.hh>
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <sot/torque_control/utils/logger.hh>
#include <map>
#include "boost/assign.hpp"
//#include <boost/random/normal_distribution.hpp>
......@@ -50,6 +46,13 @@
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.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 {
......@@ -60,7 +63,7 @@ namespace dynamicgraph {
/** Compute s12 as an intermediate transform between s1 and s2 SE3 transforms**/
void se3Interp(const se3::SE3 & s1, const se3::SE3 & s2, const double alpha, se3::SE3 & s12);
void se3Interp(const pinocchio::SE3 & s1, const pinocchio::SE3 & s2, const double alpha, pinocchio::SE3 & s12);
/** Convert from Roll, Pitch, Yaw to transformation Matrix. */
void rpyToMatrix(double r, double p, double y, Eigen::Matrix3d & R);
......@@ -81,12 +84,12 @@ namespace dynamicgraph {
:public::dynamicgraph::Entity
{
typedef BaseEstimator EntityClassName;
typedef se3::SE3 SE3;
typedef pinocchio::SE3 SE3;
typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3;
typedef Eigen::Vector4d Vector4;
typedef Eigen::Vector6d Vector6;
typedef Eigen::Vector7d Vector7;
typedef dynamicgraph::sot::Vector6d Vector6;
typedef dynamicgraph::sot::Vector7d Vector7;
typedef Eigen::Matrix3d Matrix3;
typedef boost::math::normal normal;
......@@ -171,7 +174,7 @@ namespace dynamicgraph {
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("["+name+"] "+msg, t, file, line);
sendMsg("["+name+"] "+msg, t, file, line);
}
protected:
......@@ -217,10 +220,10 @@ namespace dynamicgraph {
double m_w_lf_filtered; /// filtered weight of the estimation coming from the left foot
double m_w_rf_filtered; /// filtered weight of the estimation coming from the right foot
se3::Model m_model; /// Pinocchio robot model
se3::Data *m_data; /// Pinocchio robot data
se3::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
se3::SE3 m_oMff_rf; /// world-to-base transformation obtained through right foot
pinocchio::Model m_model; /// Pinocchio robot model
pinocchio::Data *m_data; /// Pinocchio robot data
pinocchio::SE3 m_oMff_lf; /// world-to-base transformation obtained through left foot
pinocchio::SE3 m_oMff_rf; /// world-to-base transformation obtained through right foot
SE3 m_oMlfs; /// transformation from world to left foot sole
SE3 m_oMrfs; /// transformation from world to right foot sole
Vector7 m_oMlfs_xyzquat;
......@@ -233,9 +236,9 @@ namespace dynamicgraph {
SE3 m_sole_M_ftSens; /// foot sole to F/T sensor transformation
se3::FrameIndex m_right_foot_id;
se3::FrameIndex m_left_foot_id;
se3::FrameIndex m_IMU_body_id;
pinocchio::FrameIndex m_right_foot_id;
pinocchio::FrameIndex m_left_foot_id;
pinocchio::FrameIndex m_IMU_body_id;
Eigen::VectorXd m_q_pin; /// robot configuration according to pinocchio convention
Eigen::VectorXd m_q_sot; /// robot configuration according to SoT convention
......
include/sot/torque_control/commands-helper.hh
View file @ 72b0f9fe
......@@ -17,13 +17,14 @@
#ifndef __sot_torquecontrol_commands_helper_H__
#define __sot_torquecontrol_commands_helper_H__
#include <boost/function.hpp>
/* --- COMMON INCLUDE -------------------------------------------------- */
#include <dynamic-graph/command.h>
#include <dynamic-graph/command-direct-setter.h>
#include <dynamic-graph/command-direct-getter.h>
#include <dynamic-graph/command-bind.h>
#include <boost/function.hpp>
/* --- HELPER ---------------------------------------------------------- */
namespace dynamicgraph {
......
include/sot/torque_control/common.hh deleted 100644 → 0
View file @ 90409a65
/*
* Copyright 2017, A. Del Prete, T. Flayols, O. Stasse, 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_common_H__
#define __sot_torque_control_common_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (hrp2_common_EXPORTS)
# define HRP2COMMON_EXPORT __declspec(dllexport)
# else
# define HRP2COMMON_EXPORT __declspec(dllimport)
# endif
#else
# define HRP2COMMON_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/linear-algebra.h>
#include <sot/torque_control/signal-helper.hh>
#include <sot/torque_control/utils/vector-conversions.hh>
#include <map>
#include "boost/assign.hpp"
#include <sot/torque_control/utils/logger.hh>
namespace dg = ::dynamicgraph;
using namespace dg;
namespace dynamicgraph {
namespace sot {
namespace torque_control {
struct JointLimits
{
double upper;
double lower;
JointLimits():
upper(0.0),
lower(0.0)
{}
JointLimits(double l, double u):
upper(u),
lower(l)
{}
};
typedef Eigen::VectorXd::Index Index;
struct ForceLimits
{
Eigen::VectorXd upper;
Eigen::VectorXd lower;
ForceLimits():
upper(Eigen::Vector6d::Zero()),
lower(Eigen::Vector6d::Zero())
{}
ForceLimits(const Eigen::VectorXd& l, const Eigen::VectorXd& u):
upper(u),
lower(l)
{}
void display(std::ostream &os) const;
};
struct ForceUtil
{
std::map<Index,ForceLimits> m_force_id_to_limits;
std::map<std::string,Index> m_name_to_force_id;
std::map<Index,std::string> m_force_id_to_name;
Index m_Force_Id_Left_Hand, m_Force_Id_Right_Hand,
m_Force_Id_Left_Foot, m_Force_Id_Right_Foot;
void set_name_to_force_id(const std::string & name,
const Index &force_id);
void set_force_id_to_limits(const Index &force_id,
const dg::Vector &lf,
const dg::Vector &uf);
void create_force_id_to_name_map();
Index get_id_from_name(const std::string &name);
const std::string & get_name_from_id(Index idx);
std::string cp_get_name_from_id(Index idx);
const ForceLimits & get_limits_from_id(Index force_id);
ForceLimits cp_get_limits_from_id(Index force_id);
Index get_force_id_left_hand()
{
return m_Force_Id_Left_Hand;
}
void set_force_id_left_hand(Index anId)
{
m_Force_Id_Left_Hand = anId;
}
Index get_force_id_right_hand()
{
return m_Force_Id_Right_Hand;
}
void set_force_id_right_hand( Index anId)
{
m_Force_Id_Right_Hand = anId;
}
Index get_force_id_left_foot()
{
return m_Force_Id_Left_Foot;
}
void set_force_id_left_foot(Index anId)
{
m_Force_Id_Left_Foot = anId;
}
Index get_force_id_right_foot()
{
return m_Force_Id_Right_Foot;
}
void set_force_id_right_foot( Index anId)
{
m_Force_Id_Right_Foot = anId;
}
void display(std::ostream & out) const;
}; // struct ForceUtil
struct FootUtil
{
/// Position of the foot soles w.r.t. the frame of the foot
dynamicgraph::Vector m_Right_Foot_Sole_XYZ;
/// Position of the force/torque sensors w.r.t. the frame of the hosting link
dynamicgraph::Vector m_Right_Foot_Force_Sensor_XYZ;
std::string m_Left_Foot_Frame_Name;
std::string m_Right_Foot_Frame_Name;
void display(std::ostream & os) const;
};
struct RobotUtil
{
public:
RobotUtil();
/// Forces data
ForceUtil m_force_util;
/// Foot information
FootUtil m_foot_util;
/// Map from the urdf index to the SoT index.
std::vector<Index> m_urdf_to_sot;
/// Nb of Dofs for the robot.
long unsigned int m_nbJoints;
/// Map from the name to the id.
std::map<std::string,Index> m_name_to_id;
/// The map between id and name
std::map<Index,std::string> m_id_to_name;
/// The joint limits map.
std::map<Index,JointLimits> m_limits_map;
/// The name of the joint IMU is attached to
std::string m_imu_joint_name;
/// This method creates the map between id and name.
/// It is called each time a new link between id and name is inserted
/// (i.e. when set_name_to_id is called).
void create_id_to_name_map();
/// URDF file path
std::string m_urdf_filename;
dynamicgraph::Vector m_dgv_urdf_to_sot;
/** Given a joint name it finds the associated joint id.
* If the specified joint name is not found it returns -1;
* @param name Name of the joint to find.
* @return The id of the specified joint, -1 if not found. */
const Index get_id_from_name(const std::string &name);
/** Given a joint id it finds the associated joint name.
* If the specified joint is not found it returns "Joint name not found";
* @param id Id of the joint to find.
* @return The name of the specified joint, "Joint name not found" if not found. */
/// Get the joint name from its index
const std::string & get_name_from_id(Index id);
/// Set relation between the name and the SoT id
void set_name_to_id(const std::string &jointName,
const Index & jointId);
/// Set the map between urdf index and sot index
void set_urdf_to_sot(const std::vector<Index> &urdf_to_sot);
void set_urdf_to_sot(const dg::Vector &urdf_to_sot);
/// Set the limits (lq,uq) for joint idx
void set_joint_limits_for_id(const Index &idx,
const double &lq,
const double &uq);
bool joints_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool joints_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool velocity_urdf_to_sot(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_urdf, Eigen::RefVector v_sot);
bool velocity_sot_to_urdf(Eigen::ConstRefVector q_urdf,
Eigen::ConstRefVector v_sot, Eigen::RefVector v_urdf);
bool config_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool config_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
bool base_urdf_to_sot(Eigen::ConstRefVector q_urdf, Eigen::RefVector q_sot);
bool base_sot_to_urdf(Eigen::ConstRefVector q_sot, Eigen::RefVector q_urdf);
/** Given a joint id it finds the associated joint limits.
* If the specified joint is not found it returns JointLimits(0,0).
* @param id Id of the joint to find.
* @return The limits of the specified joint, JointLimits(0,0) if not found. */
const JointLimits & get_joint_limits_from_id(Index id);
JointLimits cp_get_joint_limits_from_id(Index id);
void sendMsg(const std::string& msg,
MsgType t=MSG_TYPE_INFO,
const char* file="", int line=0)
{
getLogger().sendMsg("[FromURDFToSoT] "+msg, t, file, line);
}
void display(std::ostream &os) const;
}; // struct RobotUtil
RobotUtil * RefVoidRobotUtil();
RobotUtil * getRobotUtil(std::string &robotName);
bool isNameInRobotUtil(std::string &robotName);
RobotUtil * createRobotUtil(std::string &robotName);
bool base_se3_to_sot(Eigen::ConstRefVector pos,
Eigen::ConstRefMatrix R,
Eigen::RefVector q_sot);
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // sot_torque_control_common_h_