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

Add a unit test for admittance control

include/sot/core/admittance-control-op-point.hh

+/*
+ * 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__

src/CMakeLists.txt

@@ -65,6 +65,7 @@ SET(plugins
 
   control/control-gr
   control/control-pd
+  control/admittance-control-op-point
   )
 
 # TODO

src/control/admittance-control-op-point.cpp

+/*
+ * 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

tests/CMakeLists.txt

@@ -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

tests/control/test_control_admittance.cpp

+/*
+ * 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));
+  }
+}