Commit 92a734c1 authored by andreadelprete's avatar andreadelprete
Browse files

Commit all files

parent 155e941f
[submodule "cmake"]
path = cmake
url = git@github.com:jrl-umi3218/jrl-cmakemodules.git
[submodule "hrp2-torque-control"]
path = hrp2-torque-control
url = ssh://git@redmine.laas.fr/laas/sot-torque-control/hrp2-torque-control.git
hrp2-torque-control @ 46ba1cb5
Subproject commit 46ba1cb507c2a661e8ffefcf7438be5d8721449b
# Copyright 2014, 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
# General Lesser 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/>.
/*
* Copyright 2014, 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_admittance_controller_H__
#define __sot_torque_control_admittance_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (sot_admittance_controller_EXPORTS)
# define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllexport)
# else
# define SOTADMITTANCECONTROLLER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTADMITTANCECONTROLLER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- 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/hrp2-common.hh>
#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
/* Metapod */
#include <metapod/models/hrp2_14/hrp2_14.hh>
#include <metapod/algos/rnea.hh>
#include <metapod/algos/jac.hh>
#include <metapod/tools/jcalc.hh>
#include <metapod/tools/bcalc.hh>
#include <metapod/tools/print.hh>
#include <metapod/tools/initconf.hh>
namespace dynamicgraph {
namespace sot {
namespace torque_control {
Eigen::VectorXd svdSolveWithDamping(const Eigen::JacobiSVD<Eigen::MatrixXd>& A, const Eigen::VectorXd &b, double damping);
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTADMITTANCECONTROLLER_EXPORT AdmittanceController
:public::dynamicgraph::Entity
{
typedef AdmittanceController EntityClassName;
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceController( const std::string & name );
void init(const double& dt);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(base6d_encoders, ml::Vector);
DECLARE_SIGNAL_IN(jointsVelocities, ml::Vector);
DECLARE_SIGNAL_IN(Kd, ml::Vector);
DECLARE_SIGNAL_IN(Kf, ml::Vector);
DECLARE_SIGNAL_IN(fRightFootRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftFootRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightHandRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fLeftHandRef, ml::Vector); /// 6d reference force
DECLARE_SIGNAL_IN(fRightFoot, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftFoot, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fRightHand, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(fLeftHand, ml::Vector); /// 6d estimated force
DECLARE_SIGNAL_IN(controlledJoints, ml::Vector); /// mask with 1 for controlled joints, 0 otherwise
DECLARE_SIGNAL_IN(damping, ml::Vector); /// damping factors used for the 4 end-effectors
DECLARE_SIGNAL_OUT(qDes, ml::Vector); /// integral of dqDes
// DEBUG SIGNALS
DECLARE_SIGNAL_OUT(dqDes, ml::Vector); /// dqDes = J^+ * Kf * (fRef-f)
DECLARE_SIGNAL_OUT(fRightFootError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fLeftFootError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fRightHandError, ml::Vector); /// fRef-f
DECLARE_SIGNAL_OUT(fLeftHandError, ml::Vector); /// fRef-f
/* --- COMMANDS --- */
/* --- ENTITY INHERITANCE --- */
virtual void display( std::ostream& os ) const;
virtual void commandLine(const std::string& cmdLine,
std::istringstream& cmdArgs,
std::ostream& os);
void sendMsg(const std::string& msg, MsgType t=MSG_TYPE_INFO, const char* file="", int line=0)
{
getLogger().sendMsg("[AdmittanceController-"+name+"] "+msg, t, file, line);
}
protected:
Eigen::VectorXd m_qDes; /// desired joint positions
Eigen::VectorXd m_dqDes; /// desired joint velocities
bool m_initSucceeded; /// true if the entity has been successfully initialized
bool m_firstIter; /// true only at the first iteration
bool m_useJacobianTranspose; /// if true it uses the Jacobian transpose rather than the pseudoinverse
double m_dt; /// control loop time period
/// robot geometric/inertial data
typedef metapod::hrp2_14<double> Hrp2_14;
typedef metapod::Nodes<Hrp2_14, Hrp2_14::r_ankle>::type RightFootNode;
typedef metapod::Nodes<Hrp2_14, Hrp2_14::l_ankle>::type LeftFootNode;
typedef metapod::Nodes<Hrp2_14, Hrp2_14::r_wrist>::type RightHandNode;
typedef metapod::Nodes<Hrp2_14, Hrp2_14::l_wrist>::type LeftHandNode;
typedef Eigen::Matrix<double, 6*Hrp2_14::NBBODIES, Hrp2_14::NBDOF> AllJacobian;
typedef Eigen::Matrix<double, 6, 6> FootJacobian;
typedef Eigen::Matrix<double, 6, 9> HandJacobian;
typedef metapod::Spatial::TransformT<double, metapod::Spatial::RotationMatrixIdentityTpl<double> > Transform;
// nodes corresponding to the four end-effectors
RightFootNode& m_node_right_foot;
LeftFootNode& m_node_left_foot;
RightHandNode& m_node_right_hand;
LeftHandNode& m_node_left_hand;
AllJacobian m_J_all;
FootJacobian m_J_right_foot;
FootJacobian m_J_left_foot;
HandJacobian m_J_right_hand;
HandJacobian m_J_left_hand;
Eigen::JacobiSVD<Eigen::MatrixXd> m_J_right_foot_svd; /// svd of the jacobian matrix
Eigen::JacobiSVD<Eigen::MatrixXd> m_J_left_foot_svd; /// svd of the jacobian matrix
Eigen::JacobiSVD<Eigen::MatrixXd> m_J_right_hand_svd; /// svd of the jacobian matrix
Eigen::JacobiSVD<Eigen::MatrixXd> m_J_left_hand_svd; /// svd of the jacobian matrix
/// Transformation from foot frame to sole frame
Transform m_sole_X_RF;
Transform m_sole_X_LF;
/// Transformation from hand frame to gripper frame
Transform m_gripper_X_RH;
Transform m_gripper_X_LH;
Hrp2_14 m_robot;
Hrp2_14::confVector m_q, m_dq;
}; // class AdmittanceController
} // namespace torque_control
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_torque_control_admittance_controller_H__
/*
* Copyright 2011, Nicolas Mansard, 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_torquecontrol_commands_helper_H__
#define __sot_torquecontrol_commands_helper_H__
/* --- 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 {
namespace sot {
namespace torquecontrol {
using ::dynamicgraph::command::makeDirectGetter;
using ::dynamicgraph::command::docDirectGetter;
using ::dynamicgraph::command::makeDirectSetter;
using ::dynamicgraph::command::docDirectSetter;
using ::dynamicgraph::command::makeCommandVoid0;
using ::dynamicgraph::command::docCommandVoid0;
using ::dynamicgraph::command::makeCommandVoid1;
using ::dynamicgraph::command::docCommandVoid1;
using ::dynamicgraph::command::makeCommandVoid2;
using ::dynamicgraph::command::docCommandVoid2;
using ::dynamicgraph::command::makeCommandVerbose;
using ::dynamicgraph::command::docCommandVerbose;
} // namespace torquecontrol
} // namespace sot
} // namespace dynamicgraph
namespace dynamicgraph {
namespace command {
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
struct CommandVoid5
: public Command
{
typedef boost::function<void(const T1&,const T2&,const T3&,const T4&,const T5&)> function_t;
typedef boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&)> memberFunction_t;
typedef void (E::*memberFunction_ptr_t) (const T1&,const T2&,const T3&,const T4&,const T5&);
CommandVoid5(E& entity, function_t function,
const std::string& docString)
:Command(entity,
boost::assign::list_of
(ValueHelper<T1>::TypeID)
(ValueHelper<T2>::TypeID)
(ValueHelper<T3>::TypeID)
(ValueHelper<T4>::TypeID)
(ValueHelper<T5>::TypeID)
, docString)
,fptr(function)
{}
protected:
virtual Value doExecute()
{
assert( getParameterValues().size() == 5 );
T1 val1 = getParameterValues()[0].value();
T2 val2 = getParameterValues()[1].value();
T3 val3 = getParameterValues()[2].value();
T4 val4 = getParameterValues()[3].value();
T5 val5 = getParameterValues()[4].value();
fptr(val1,val2,val3,val4,val5);
return Value(); // void
}
private:
function_t fptr;
};
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
CommandVoid5<E,T1,T2,T3,T4,T5>*
makeCommandVoid5(E& entity,
typename CommandVoid5<E,T1,T2,T3,T4,T5>::function_t function ,
const std::string& docString)
{
return new CommandVoid5<E,T1,T2,T3,T4,T5>( entity,function,docString );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
CommandVoid5<E,T1,T2,T3,T4,T5>*
makeCommandVoid5(E& entity,
boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&)> function,
const std::string& docString)
{
return new CommandVoid5<E,T1,T2,T3,T4,T5>( entity,
boost::bind(function,&entity,_1,_2,_3,_4,_5),docString );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5>
CommandVoid5<E,T1,T2,T3,T4,T5>*
makeCommandVoid5(E& entity,
void (E::*function) (const T1&,const T2&,const T3&,const T4&,const T5&),
const std::string& docString)
{
return new CommandVoid5<E,T1,T2,T3,T4,T5>( entity,
boost::bind(function,&entity,_1,_2,_3,_4,_5),
docString );
return NULL;
}
inline std::string docCommandVoid5( const std::string& doc,
const std::string& type1,
const std::string& type2,
const std::string& type3,
const std::string& type4,
const std::string& type5)
{
return (std::string("\n")+doc+"\n\n"
+"Input:\n - A "+type1+".\n"
+"Input:\n - A "+type2+".\n"
+"Input:\n - A "+type3+".\n"
+"Input:\n - A "+type4+".\n"
+"Input:\n - A "+type5+".\n"
+"Void return.\n\n" );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
struct CommandVoid6
: public Command
{
typedef boost::function<void(const T1&,const T2&,const T3&,const T4&,const T5&,const T6&)> function_t;
typedef boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&)> memberFunction_t;
typedef void (E::*memberFunction_ptr_t) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&);
CommandVoid6(E& entity, function_t function,
const std::string& docString)
:Command(entity,
boost::assign::list_of
(ValueHelper<T1>::TypeID)
(ValueHelper<T2>::TypeID)
(ValueHelper<T3>::TypeID)
(ValueHelper<T4>::TypeID)
(ValueHelper<T5>::TypeID)
(ValueHelper<T6>::TypeID)
, docString)
,fptr(function)
{}
protected:
virtual Value doExecute()
{
assert( getParameterValues().size() == 6 );
T1 val1 = getParameterValues()[0].value();
T2 val2 = getParameterValues()[1].value();
T3 val3 = getParameterValues()[2].value();
T4 val4 = getParameterValues()[3].value();
T5 val5 = getParameterValues()[4].value();
T6 val6 = getParameterValues()[5].value();
fptr(val1,val2,val3,val4,val5,val6);
return Value(); // void
}
private:
function_t fptr;
};
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
CommandVoid6<E,T1,T2,T3,T4,T5,T6>*
makeCommandVoid6(E& entity,
typename CommandVoid6<E,T1,T2,T3,T4,T5,T6>::function_t function ,
const std::string& docString)
{
return new CommandVoid6<E,T1,T2,T3,T4,T5,T6>( entity,function,docString );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
CommandVoid6<E,T1,T2,T3,T4,T5,T6>*
makeCommandVoid6(E& entity,
boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&)> function,
const std::string& docString)
{
return new CommandVoid6<E,T1,T2,T3,T4,T5,T6>( entity,
boost::bind(function,&entity,_1,_2,_3,_4,_5,_6),docString );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6 >
CommandVoid6<E,T1,T2,T3,T4,T5,T6>*
makeCommandVoid6(E& entity,
void (E::*function) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&),
const std::string& docString)
{
return new CommandVoid6<E,T1,T2,T3,T4,T5,T6>( entity,
boost::bind(function,&entity,_1,_2,_3,_4,_5,_6),
docString );
return NULL;
}
inline std::string docCommandVoid6( const std::string& doc,
const std::string& type1,
const std::string& type2,
const std::string& type3,
const std::string& type4,
const std::string& type5,
const std::string& type6)
{
return (std::string("\n")+doc+"\n\n"
+"Input:\n - A "+type1+".\n"
+"Input:\n - A "+type2+".\n"
+"Input:\n - A "+type3+".\n"
+"Input:\n - A "+type4+".\n"
+"Input:\n - A "+type5+".\n"
+"Input:\n - A "+type6+".\n"
+"Void return.\n\n" );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
struct CommandVoid7
: public Command
{
typedef boost::function<void(const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&)> function_t;
typedef boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&)> memberFunction_t;
typedef void (E::*memberFunction_ptr_t) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&);
CommandVoid7(E& entity, function_t function,
const std::string& docString)
:Command(entity,
boost::assign::list_of
(ValueHelper<T1>::TypeID)
(ValueHelper<T2>::TypeID)
(ValueHelper<T3>::TypeID)
(ValueHelper<T4>::TypeID)
(ValueHelper<T5>::TypeID)
(ValueHelper<T6>::TypeID)
(ValueHelper<T7>::TypeID)
, docString)
,fptr(function)
{}
protected:
virtual Value doExecute()
{
assert( getParameterValues().size() == 7 );
T1 val1 = getParameterValues()[0].value();
T2 val2 = getParameterValues()[1].value();
T3 val3 = getParameterValues()[2].value();
T4 val4 = getParameterValues()[3].value();
T5 val5 = getParameterValues()[4].value();
T6 val6 = getParameterValues()[5].value();
T7 val7 = getParameterValues()[6].value();
fptr(val1,val2,val3,val4,val5,val6,val7);
return Value(); // void
}
private:
function_t fptr;
};
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>*
makeCommandVoid7(E& entity,
typename CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>::function_t function ,
const std::string& docString)
{
return new CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>( entity,function,docString );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>*
makeCommandVoid7(E& entity,
boost::function<void(E*,const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&)> function,
const std::string& docString)
{
return new CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>( entity,
boost::bind(function,&entity,_1,_2,_3,_4,_5,_6,_7),docString );
}
template <class E,typename T1,typename T2,typename T3,typename T4,typename T5,typename T6,typename T7 >
CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>*
makeCommandVoid7(E& entity,
void (E::*function) (const T1&,const T2&,const T3&,const T4&,const T5&,const T6&,const T7&),
const std::string& docString)
{
return new CommandVoid7<E,T1,T2,T3,T4,T5,T6,T7>( entity,
boost::bind(function,&entity,_1,_2,_3,_4,_5,_6,_7),
docString );
return NULL;
}
inline std::string docCommandVoid7( const std::string& doc,
const std::string& type1,
const std::string& type2,
const std::string& type3,
const std::string& type4,
const std::string& type5,
const std::string& type6,
const std::string& type7)
{
return (std::string("\n")+doc+"\n\n"
+"Input:\n - A "+type1+".\n"
+"Input:\n - A "+type2+".\n"
+"Input:\n - A "+type3+".\n"
+"Input:\n - A "+type4+".\n"
+"Input:\n - A "+type5+".\n"
+"Input:\n - A "+type6+".\n"
+"Input:\n - A "+type7+".\n"
+"Void return.\n\n" );
}
} // namespace command
} // namespace dynamicgraph
#endif // __sot_torquecontrol_commands_helper_H__
/*
* Copyright 2015, 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_control_manager_H__
#define __sot_torque_control_control_manager_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (__sot_torque_control_control_manager_H__)
# define SOTCONTROLMANAGER_EXPORT __declspec(dllexport)
# else
# define SOTCONTROLMANAGER_EXPORT __declspec(dllimport)
# endif
#else
# define SOTCONTROLMANAGER_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- 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/hrp2-common.hh>
#include <map>
#include <initializer_list>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace torque_control {
/* --------------------------------------------------------------------- */