Commit fa016a15 authored by NoelieRamuzat's avatar NoelieRamuzat Committed by olivier stasse
Browse files

[Admittance] Move entity Admittance OpPoint in sot-core/control

Add a unit test for admittance control
parent 7100af81
/*
* Copyright 2019
*
* LAAS-CNRS
*
* Noëlie Ramuzat
* This file is part of sot-core.
* See license file.
*/
#ifndef __sot_core_admittance_control_op_point_H__
#define __sot_core_admittance_control_op_point_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined(WIN32)
# if defined(admittance_control_op_point_EXPORTS)
# define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllexport)
# else
# define ADMITTANCECONTROLOPPOINT_EXPORT __declspec(dllimport)
# endif
#else
# define ADMITTANCECONTROLOPPOINT_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#include <dynamic-graph/signal-helper.h>
#include "pinocchio/spatial/se3.hpp"
#include "pinocchio/spatial/motion.hpp"
#include "pinocchio/spatial/force.hpp"
#include <sot/core/matrix-geometry.hh>
namespace dynamicgraph {
namespace sot {
namespace core {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/**
* @brief Admittance controller for an operational point wrt to a force sensor.
* It can be a point of the model (hand) or not (created operational point: an object in the hand of the robot)
* Which is closed to a force sensor (for instance the right or left wrist ft)
*
* This entity computes a velocity reference for an operational point based
* on the force error in the world frame :
* w_dq = integral(Kp(w_forceDes-w_force)) + Kd (w_dq)
*
*/
class ADMITTANCECONTROLOPPOINT_EXPORT AdmittanceControlOpPoint
: public ::dynamicgraph::Entity {
DYNAMIC_GRAPH_ENTITY_DECL();
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/* --- CONSTRUCTOR ---- */
AdmittanceControlOpPoint(const std::string &name);
/**
* @brief Initialize the entity
*
* @param[in] dt Time step of the control
*/
void init(const double &dt);
/* --- SIGNALS --- */
/// \brief Gain (6d) for the integration of the error on the force
DECLARE_SIGNAL_IN(Kp, dynamicgraph::Vector);
/// \brief Derivative gain (6d) for the error on the force
DECLARE_SIGNAL_IN(Kd, dynamicgraph::Vector);
/// \brief Value of the saturation to apply on the velocity output
DECLARE_SIGNAL_IN(dqSaturation, dynamicgraph::Vector);
/// \brief 6d force given by the sensor in its local frame
DECLARE_SIGNAL_IN(force, dynamicgraph::Vector);
/// \brief 6d desired force of the end-effector in the world frame
DECLARE_SIGNAL_IN(w_forceDes, dynamicgraph::Vector);
/// \brief Current position (matrixHomogeneous) of the given operational point
DECLARE_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous);
/// \brief Current position (matrixHomogeneous) of the given force sensor
DECLARE_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous);
/// \brief 6d force given by the sensor in the world frame
DECLARE_SIGNAL_INNER(w_force, dynamicgraph::Vector);
/// \brief Internal intergration computed in the world frame
DECLARE_SIGNAL_INNER(w_dq, dynamicgraph::Vector);
/// \brief Velocity reference for the end-effector in the local frame
DECLARE_SIGNAL_OUT(dq, dynamicgraph::Vector);
/* --- COMMANDS --- */
/**
* @brief Reset the velocity
*/
void resetDq();
/* --- ENTITY INHERITANCE --- */
virtual void display(std::ostream &os) const;
protected:
/// Dimension of the force signals and of the output
int m_n;
/// True if the entity has been successfully initialized
bool m_initSucceeded;
/// Internal state
dynamicgraph::Vector m_w_dq;
/// Time step of the control
double m_dt;
// Weight of the end-effector
double m_mass;
}; // class AdmittanceControlOpPoint
} // namespace core
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_core_admittance_control_op_point_H__
......@@ -65,6 +65,7 @@ SET(plugins
control/control-gr
control/control-pd
control/admittance-control-op-point
)
# TODO
......
/*
* Copyright 2019
*
* LAAS-CNRS
*
* Noëlie Ramuzat
* This file is part of sot-core.
* See license file.
*/
#include "sot/core/admittance-control-op-point.hh"
#include <sot/core/debug.hh>
#include <dynamic-graph/factory.h>
#include <dynamic-graph/all-commands.h>
#include <sot/core/stop-watch.hh>
namespace dynamicgraph {
namespace sot {
namespace core {
namespace dg = ::dynamicgraph;
using namespace dg;
using namespace pinocchio;
using namespace dg::command;
#define PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION \
"AdmittanceControlOpPoint: w_force computation "
#define PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION \
"AdmittanceControlOpPoint: w_dq computation "
#define PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION \
"AdmittanceControlOpPoint: dq computation "
#define INPUT_SIGNALS m_KpSIN << m_KdSIN << m_dqSaturationSIN << m_forceSIN << m_w_forceDesSIN << \
m_opPoseSIN << m_sensorPoseSIN
#define INNER_SIGNALS m_w_forceSINNER << m_w_dqSINNER
#define OUTPUT_SIGNALS m_dqSOUT
/// Define EntityClassName here rather than in the header file
/// so that it can be used by the macros DEFINE_SIGNAL_**_FUNCTION.
typedef AdmittanceControlOpPoint EntityClassName;
/* --- DG FACTORY ---------------------------------------------------- */
DYNAMICGRAPH_FACTORY_ENTITY_PLUGIN(AdmittanceControlOpPoint,
"AdmittanceControlOpPoint");
/* ------------------------------------------------------------------- */
/* --- CONSTRUCTION -------------------------------------------------- */
/* ------------------------------------------------------------------- */
AdmittanceControlOpPoint::AdmittanceControlOpPoint(const std::string &name)
: Entity(name),
CONSTRUCT_SIGNAL_IN(Kp, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(Kd, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(dqSaturation, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(force, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(w_forceDes, dynamicgraph::Vector),
CONSTRUCT_SIGNAL_IN(opPose, dynamicgraph::sot::MatrixHomogeneous),
CONSTRUCT_SIGNAL_IN(sensorPose, dynamicgraph::sot::MatrixHomogeneous),
CONSTRUCT_SIGNAL_INNER(w_force, dynamicgraph::Vector, m_forceSIN),
CONSTRUCT_SIGNAL_INNER(w_dq, dynamicgraph::Vector, INPUT_SIGNALS << m_w_forceSINNER),
CONSTRUCT_SIGNAL_OUT(dq, dynamicgraph::Vector, m_w_dqSINNER),
m_initSucceeded(false) {
Entity::signalRegistration(INPUT_SIGNALS << INNER_SIGNALS << OUTPUT_SIGNALS);
/* Commands. */
addCommand("init", makeCommandVoid1(*this,
&AdmittanceControlOpPoint::init,
docCommandVoid1("Initialize the entity.",
"time step")));
addCommand("resetDq", makeCommandVoid0(*this,
&AdmittanceControlOpPoint::resetDq,
docCommandVoid0("resetDq")));
}
void AdmittanceControlOpPoint::init(const double &dt) {
if (!m_dqSaturationSIN.isPlugged())
return SEND_MSG("Init failed: signal dqSaturation is not plugged", MSG_TYPE_ERROR);
if (!m_KpSIN.isPlugged())
return SEND_MSG("Init failed: signal Kp is not plugged", MSG_TYPE_ERROR);
if (!m_KdSIN.isPlugged())
return SEND_MSG("Init failed: signal Kd is not plugged", MSG_TYPE_ERROR);
if (!m_forceSIN.isPlugged())
return SEND_MSG("Init failed: signal force is not plugged", MSG_TYPE_ERROR);
if (!m_w_forceDesSIN.isPlugged())
return SEND_MSG("Init failed: signal w_forceDes is not plugged", MSG_TYPE_ERROR);
if (!m_opPoseSIN.isPlugged())
return SEND_MSG("Init failed: signal opPose is not plugged", MSG_TYPE_ERROR);
if (!m_sensorPoseSIN.isPlugged())
return SEND_MSG("Init failed: signal sensorPose is not plugged", MSG_TYPE_ERROR);
m_n = 6;
m_dt = dt;
m_w_dq.setZero(m_n);
m_initSucceeded = true;
}
void AdmittanceControlOpPoint::resetDq() {
m_w_dq.setZero(m_n);
return;
}
/* ------------------------------------------------------------------- */
/* --- SIGNALS ------------------------------------------------------- */
/* ------------------------------------------------------------------- */
DEFINE_SIGNAL_INNER_FUNCTION(w_force, dynamicgraph::Vector) {
if (!m_initSucceeded) {
SEND_WARNING_STREAM_MSG("Cannot compute signal w_force before initialization!");
return s;
}
if (s.size() != 6)
s.resize(6);
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION);
const Vector &force = m_forceSIN(iter);
const MatrixHomogeneous &sensorPose = m_sensorPoseSIN(iter);
assert(force.size() == m_n && "Unexpected size of signal force");
pinocchio::SE3 sensorPlacement(sensorPose.matrix()); // homogeneous matrix to SE3
s = sensorPlacement.act(pinocchio::Force(force)).toVector();
getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_WFORCE_COMPUTATION);
return s;
}
DEFINE_SIGNAL_INNER_FUNCTION(w_dq, dynamicgraph::Vector) {
if (!m_initSucceeded) {
SEND_WARNING_STREAM_MSG("Cannot compute signal w_dq before initialization!");
return s;
}
if (s.size() != 6)
s.resize(6);
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION);
const Vector &w_forceDes = m_w_forceDesSIN(iter);
const Vector &w_force = m_w_forceSINNER(iter);
const Vector &Kp = m_KpSIN(iter);
const Vector &Kd = m_KdSIN(iter);
const Vector &dqSaturation = m_dqSaturationSIN(iter);
assert(w_force.size() == m_n && "Unexpected size of signal force");
assert(w_forceDes.size() == m_n && "Unexpected size of signal w_forceDes");
assert(Kp.size() == m_n && "Unexpected size of signal Kp");
assert(Kd.size() == m_n && "Unexpected size of signal Kd");
assert(dqSaturation.size() == m_n && "Unexpected size of signal dqSaturation");
m_w_dq = m_w_dq + m_dt * (Kp.cwiseProduct(w_forceDes - w_force)) - Kd.cwiseProduct(m_w_dq);
for (int i = 0; i < m_n; i++) {
if (m_w_dq[i] > dqSaturation[i])
m_w_dq[i] = dqSaturation[i];
if (m_w_dq[i] < -dqSaturation[i])
m_w_dq[i] = -dqSaturation[i];
}
s = m_w_dq;
getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_WDQ_COMPUTATION);
return s;
}
DEFINE_SIGNAL_OUT_FUNCTION(dq, dynamicgraph::Vector) {
if (!m_initSucceeded) {
SEND_WARNING_STREAM_MSG("Cannot compute signal dq before initialization!");
return s;
}
if (s.size() != 6)
s.resize(6);
getProfiler().start(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION);
const Vector &w_dq = m_w_dqSINNER(iter);
const MatrixHomogeneous &opPose = m_opPoseSIN(iter);
assert(w_dq.size() == m_n && "Unexpected size of signal w_dq");
pinocchio::SE3 opPointPlacement(opPose.matrix()); // homogeneous matrix to SE3
s = opPointPlacement.actInv(pinocchio::Motion(w_dq)).toVector();
getProfiler().stop(PROFILE_ADMITTANCECONTROLOPPOINT_DQ_COMPUTATION);
return s;
}
/* --- COMMANDS ---------------------------------------------------------- */
/* ------------------------------------------------------------------- */
/* --- ENTITY -------------------------------------------------------- */
/* ------------------------------------------------------------------- */
void AdmittanceControlOpPoint::display(std::ostream &os) const {
os << "AdmittanceControlOpPoint " << getName();
try {
getProfiler().report_all(3, os);
} catch (ExceptionSignal e) {
}
}
} // namespace core
} // namespace sot
} // namespace dynamicgraph
......@@ -39,6 +39,9 @@ SET(TEST_test_mailbox_LIBS
SET(TEST_test_control_pd_LIBS
control-pd)
SET(TEST_test_control_admittance_LIBS
admittance-control-op-point)
SET(TEST_test_feature_generic_EXT_LIBS
pinocchio::pinocchio)
......@@ -56,6 +59,7 @@ SET(tests
dummy
control/test_control_pd
control/test_control_admittance
features/test_feature_point6d
features/test_feature_generic
......
/*
* Copyright 2019,
* Noëlie Ramuzat,
*
*
*/
#include <iostream>
#include <sot/core/debug.hh>
#ifndef WIN32
#include <unistd.h>
#endif
using namespace std;
#include <dynamic-graph/entity.h>
#include <dynamic-graph/factory.h>
#include <sot/core/admittance-control-op-point.hh>
#include <sstream>
using namespace dynamicgraph;
using namespace dynamicgraph::sot;
#define BOOST_TEST_MODULE debug - control - admittance
#include <boost/test/output_test_stream.hpp>
#include <boost/test/unit_test.hpp>
BOOST_AUTO_TEST_CASE(control_admittance) {
sot::core::AdmittanceControlOpPoint *aControlAdm = new sot::core::AdmittanceControlOpPoint("acontrol_admittance");
std::istringstream Kp("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
std::istringstream Kd("[6](0.0,0.0,0.0,0.0,0.0,0.0)");
aControlAdm->m_KpSIN.set(Kp);
aControlAdm->m_KdSIN.set(Kd);
std::istringstream dqSaturation("[6](10.0,10.0,10.0,10.0,10.0,10.0)");
aControlAdm->m_dqSaturationSIN.set(dqSaturation);
std::istringstream w_forceDes("[6](100.0,0.0,0.0,0.0,0.0,0.0)");
aControlAdm->m_w_forceDesSIN.set(w_forceDes);
std::istringstream force("[6](10.0,0.0,10.0,0.0,0.0,0.0)");
aControlAdm->m_forceSIN.set(force);
MatrixHomogeneous opPose;
opPose.translation() << 0.3, 0.0, 0.0;
opPose.linear() << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
aControlAdm->m_opPoseSIN = opPose;
MatrixHomogeneous sensorPose;
sensorPose.translation() << 0.3, 0.0, 0.0;
sensorPose.linear() << 0.0, 0.0, -1.0, 0.0, 1.0, 0.0, 1.0, 0.0, 0.0;
aControlAdm->m_sensorPoseSIN = sensorPose;
aControlAdm->init(0.001);
aControlAdm->m_dqSOUT.recompute(0);
{
dynamicgraph::Vector expected(6);
expected << 1.1, 0.0, -0.109, 0.0, 0.03, 0.0;
BOOST_CHECK(aControlAdm->m_dqSOUT(0).isApprox(expected));
}
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment