Skip to content
Snippets Groups Projects
Commit c9d7ef7c authored by Gabriele Buondonno's avatar Gabriele Buondonno
Browse files

Admittance controller sketch

parent 7a9738c6
No related branches found
No related tags found
No related merge requests found
/*
* 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_admittance_controller_single_joint_H__
#define __sot_talos_balance_admittance_controller_single_joint_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (admittance_controller_single_joint_EXPORTS)
# define ADMITTANCECONTROLLERSINGLEJOINT_EXPORT __declspec(dllexport)
# else
# define ADMITTANCECONTROLLERSINGLEJOINT_EXPORT __declspec(dllimport)
# endif
#else
# define ADMITTANCECONTROLLERSINGLEJOINT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include "utils/signal-helper.hh"
#include "utils/logger.hh"
#include <map>
#include "boost/assign.hpp"
namespace dynamicgraph {
namespace sot {
namespace talos_balance {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class ADMITTANCECONTROLLERSINGLEJOINT_EXPORT AdmittanceControllerSingleJoint
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControllerSingleJoint( const std::string & name );
void init(const unsigned & n);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(state, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(qDes, dynamicgraph::Vector);
DECLARE_SIGNAL_IN(dqDes, dynamicgraph::Vector);
DECLARE_SIGNAL_OUT(dqRef, dynamicgraph::Vector);
/* --- COMMANDS --- */
/* --- 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)
{
getLogger().sendMsg("[AdmittanceControllerSingleJoint-"+name+"] "+msg, t, file, line);
}
protected:
int m_n;
bool m_initSucceeded; /// true if the entity has been successfully initialized
dynamicgraph::Vector m_Kp;
}; // class AdmittanceControllerSingleJoint
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_admittance_controller_single_joint_H__
......@@ -36,6 +36,7 @@ ENDIF(UNIX)
#This project will create many plugins as shared libraries, listed here
SET(plugins
example
admittance-controller-single-joint
joint-position-controller
nd-trajectory-generator
)
......
/*
* 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/>.
*/
#include "sot/talos_balance/admittance-controller-single-joint.hh"
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/command-bind.h>
#include "sot/talos_balance/utils/commands-helper.hh"
#include "sot/talos_balance/utils/stop-watch.hh"
namespace dynamicgraph
{
namespace sot
{
namespace talos_balance
{
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace dg::command;
//Size to be aligned "-------------------------------------------------------"
#define PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION "AdmittanceControllerSingleJoint: dqRef computation "
#define INPUT_SIGNALS m_KpSIN << m_stateSIN << m_qDesSIN << m_dqDesSIN
#define OUTPUT_SIGNALS m_dqRefSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef AdmittanceControllerSingleJoint EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControllerSingleJoint,
"AdmittanceControllerSingleJoint");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
AdmittanceControllerSingleJoint::AdmittanceControllerSingleJoint(const std::string& name)
: Entity(name)
, CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(state, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(qDes, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_IN(dqDes, dynamicgraph::Vector)
, CONSTRUCT_SIGNAL_OUT(dqRef, dynamicgraph::Vector, INPUT_SIGNALS)
, m_initSucceeded(false)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
/* Commands. */
addCommand("init", makeCommandVoid1(*this, &AdmittanceControllerSingleJoint::init, docCommandVoid1("Initialize the entity.","Control gains")));
}
void AdmittanceControllerSingleJoint::init(const unsigned & n)
{
if(n<1)
return SEND_MSG("n must be at least 1", MSG_TYPE_ERROR);
if(!m_KpSIN.isPlugged())
return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
if(!m_stateSIN.isPlugged())
return SEND_MSG("Init failed: signal q is not plugged", MSG_TYPE_ERROR);
if(!m_qDesSIN.isPlugged())
return SEND_MSG("Init failed: signal qDes is not plugged", MSG_TYPE_ERROR);
if(!m_dqDesSIN.isPlugged())
return SEND_MSG("Init failed: signal dqDes is not plugged", MSG_TYPE_ERROR);
m_n = n;
m_initSucceeded = true;
}
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(dqRef, dynamicgraph::Vector)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal dqRef before initialization!");
return s;
}
getProfiler().start(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION);
const Vector & state = m_stateSIN(iter);
const Vector & qDes = m_qDesSIN(iter);
const Vector & dqDes = m_dqDesSIN(iter);
const Vector & Kp = m_KpSIN(iter);
assert(state.size()==m_n+6 && "Unexpected size of signal state");
assert(qDes.size()==m_n && "Unexpected size of signal qDes");
assert(dqDes.size()==m_n && "Unexpected size of signal dqDes");
assert(Kp.size()==m_n && "Unexpected size of signal Kp");
const Vector & q = state.tail(m_n);
s = dqDes + Kp.cwiseProduct(qDes-q);
getProfiler().stop(PROFILE_ADMITTANCECONTROLLERSINGLEJOINT_DQREF_COMPUTATION);
return s;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void AdmittanceControllerSingleJoint::display(std::ostream& os) const
{
os << "AdmittanceControllerSingleJoint " << getName();
try
{
getProfiler().report_all(3, os);
}
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment