Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Olivier Stasse
sot-torque-control
Commits
72b0f9fe
Commit
72b0f9fe
authored
Mar 07, 2019
by
Guilhem Saurel
Browse files
Merge tag 'v1.3.1'
Release of version 1.3.1.
parents
90409a65
1166d6a7
Changes
61
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_
include/sot/torque_control/control-manager.hh