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

Simple position controller

parent db6758a4
No related branches found
No related tags found
No related merge requests found
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
* 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
......@@ -14,8 +14,8 @@
* with sot-talos-balance. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __test_example_H__
#define __test_example_H__
#ifndef __sot_talos_balance_example_H__
#define __sot_talos_balance_example_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
......@@ -88,4 +88,4 @@ namespace dynamicgraph {
#endif // #ifndef __test_example_H__
#endif // #ifndef __sot_talos_balance_example_H__
/*
* 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_joint_position_controller_H__
#define __sot_talos_balance_joint_position_controller_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (position_controller_EXPORTS)
# define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllexport)
# else
# define JOINTPOSITIONCONTROLLER_EXPORT __declspec(dllimport)
# endif
#else
# define JOINTPOSITIONCONTROLLER_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 JOINTPOSITIONCONTROLLER_EXPORT JointPositionController
: public ::dynamicgraph::Entity
{
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
JointPositionController( const std::string & name );
void init(const Eigen::VectorXd & Kp);
/* --- SIGNALS --- */
DECLARE_SIGNAL_IN(q, Eigen::VectorXd);
DECLARE_SIGNAL_IN(qDes, Eigen::VectorXd);
DECLARE_SIGNAL_IN(dqDes, Eigen::VectorXd);
DECLARE_SIGNAL_OUT(dqRef, Eigen::VectorXd);
/* --- 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("[JointPositionController-"+name+"] "+msg, t, file, line);
}
protected:
bool m_initSucceeded; /// true if the entity has been successfully initialized
Eigen::VectorXd m_Kp;
}; // class JointPositionController
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_talos_balance_joint_position_controller_H__
......@@ -36,6 +36,7 @@ ENDIF(UNIX)
#This project will create many plugins as shared libraries, listed here
SET(plugins
example
joint-position-controller
)
#set(ADDITIONAL_feature-task_LIBS feature-generic task)
......
/*
* Copyright 2015, Andrea Del Prete, LAAS-CNRS
* 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
......
/*
* 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/joint-position-controller.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_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION "JointPositionController: dqRef computation "
#define INPUT_SIGNALS m_qSIN << 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 JointPositionController EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(JointPositionController,
"JointPositionController");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
JointPositionController::JointPositionController(const std::string& name)
: Entity(name)
, CONSTRUCT_SIGNAL_IN(q, Eigen::VectorXd)
, CONSTRUCT_SIGNAL_IN(qDes, Eigen::VectorXd)
, CONSTRUCT_SIGNAL_IN(dqDes, Eigen::VectorXd)
, CONSTRUCT_SIGNAL_OUT(dqRef, Eigen::VectorXd, INPUT_SIGNALS)
, m_initSucceeded(false)
{
Entity::signalRegistration( INPUT_SIGNALS << OUTPUT_SIGNALS );
/* Commands. */
addCommand("init", makeCommandVoid1(*this, &JointPositionController::init, docCommandVoid1("Initialize the entity.","Control gains")));
}
void JointPositionController::init(const Eigen::VectorXd & Kp)
{
if(!m_qDesSIN.isPlugged())
return SEND_MSG("Init failed: signal firstAddend is not plugged", MSG_TYPE_ERROR);
if(!m_dqDesSIN.isPlugged())
return SEND_MSG("Init failed: signal secondAddend is not plugged", MSG_TYPE_ERROR);
m_Kp = Kp;
m_initSucceeded = true;
}
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
DEFINE_SIGNAL_OUT_FUNCTION(dqRef, Eigen::VectorXd)
{
if(!m_initSucceeded)
{
SEND_WARNING_STREAM_MSG("Cannot compute signal dqRef before initialization!");
return s;
}
getProfiler().start(PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION);
Eigen::VectorXd q = m_qSIN(iter);
Eigen::VectorXd qDes = m_qDesSIN(iter);
Eigen::VectorXd dqDes = m_dqDesSIN(iter);
s = dqDes + m_Kp.cwiseProduct(qDes-q);
getProfiler().stop(PROFILE_JOINTPOSITIONCONTROLLER_DQREF_COMPUTATION);
return s;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void JointPositionController::display(std::ostream& os) const
{
os << "JointPositionController " << getName();
try
{
getProfiler().report_all(3, os);
}
catch (ExceptionSignal e) {}
}
} // namespace talos_balance
} // namespace sot
} // namespace dynamicgraph
from __future__ import print_function
from sot_talos_balance.joint_position_controller import JointPositionController
controller = JointPositionController("ciao")
print("Commands:")
print(controller.commands())
print("\nSignals (at creation):")
controller.displaySignals()
controller.q.value = (0.0,0.0)
controller.qDes.value = (1.0,1.0)
controller.dqDes.value = (0.0,0.0)
Kp = (10.0,10.0)
controller.init(Kp)
controller.dqRef.recompute(1)
print( "\nKp: %s" % (Kp,) )
print( "\nq: %s" % (controller.q.value,) )
print( "qDes: %s" % (controller.qDes.value,) )
print( "dqDes: %s" % (controller.dqDes.value,) )
print( "\ndqRef: %s" % (controller.dqRef.value,) )
#print( "\nInputs: %f, %f" % (ex.firstAddend.value, ex.secondAddend.value) )
#print( "Output: %f" % ex.sum.value )
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