test2/intern/itasc/ConstraintSet.cpp

/* SPDX-FileCopyrightText: 2009 Ruben Smits
 *
 * SPDX-License-Identifier: LGPL-2.1-or-later */

/** \file
 * \ingroup intern_itasc
 */

#include "ConstraintSet.hpp"
#include "kdl/utilities/svd_eigen_HH.hpp"

namespace iTaSC {

ConstraintSet::ConstraintSet(unsigned int _nc, double accuracy, unsigned int maximum_iterations)
    : m_nc(_nc),
      m_Cf(e_zero_matrix(m_nc, 6)),
      m_Wy(e_scalar_vector(m_nc, 1.0)),
      m_y(m_nc),
      m_ydot(e_zero_vector(m_nc)),
      m_chi(e_zero_vector(6)),
      m_S(6),
      m_temp(6),
      m_tdelta(6),
      m_Jf(e_identity_matrix(6, 6)),
      m_U(e_identity_matrix(6, 6)),
      m_V(e_identity_matrix(6, 6)),
      m_B(e_zero_matrix(6, 6)),
      m_Jf_inv(e_zero_matrix(6, 6)),
      m_internalPose(F_identity),
      m_externalPose(F_identity),
      m_constraintCallback(NULL),
      m_constraintParam(NULL),
      m_toggle(false),
      m_substep(false),
      m_threshold(accuracy),
      m_maxIter(maximum_iterations)
{
  m_maxDeltaChi = e_scalar(0.52);
}

ConstraintSet::ConstraintSet()
    : m_nc(0),
      m_internalPose(F_identity),
      m_externalPose(F_identity),
      m_constraintCallback(NULL),
      m_constraintParam(NULL),
      m_toggle(false),
      m_substep(false),
      m_threshold(0.0),
      m_maxIter(0)
{
  m_maxDeltaChi = e_scalar(0.52);
}

void ConstraintSet::reset(unsigned int _nc, double accuracy, unsigned int maximum_iterations)
{
  m_nc = _nc;
  m_Jf = e_identity_matrix(6, 6);
  m_Cf = e_zero_matrix(m_nc, 6);
  m_U = e_identity_matrix(6, 6);
  m_V = e_identity_matrix(6, 6);
  m_B = e_zero_matrix(6, 6);
  m_Jf_inv = e_zero_matrix(6, 6), m_Wy = e_scalar_vector(m_nc, 1.0), m_chi = e_zero_vector(6);
  m_chidot = e_zero_vector(6);
  m_y = e_zero_vector(m_nc);
  m_ydot = e_zero_vector(m_nc);
  m_S = e_zero_vector(6);
  m_temp = e_zero_vector(6);
  m_tdelta = e_zero_vector(6);
  m_threshold = accuracy;
  m_maxIter = maximum_iterations;
}

ConstraintSet::~ConstraintSet() {}

void ConstraintSet::modelUpdate(Frame &_external_pose, const Timestamp &timestamp)
{
  m_chi += m_chidot * timestamp.realTimestep;
  m_externalPose = _external_pose;

  // update the internal pose and Jf
  updateJacobian();
  // check if loop is already closed, if not update the pose and Jf
  unsigned int iter = 0;
  while (iter < 5 && !closeLoop())
    iter++;
}

double ConstraintSet::getMaxTimestep(double &timestep)
{
  e_scalar maxChidot = m_chidot.array().abs().maxCoeff();
  if (timestep * maxChidot > m_maxDeltaChi) {
    timestep = m_maxDeltaChi / maxChidot;
  }
  return timestep;
}

bool ConstraintSet::initialise(Frame &init_pose)
{
  m_externalPose = init_pose;
  // get current Jf
  updateJacobian();

  unsigned int iter = 0;
  while (iter < m_maxIter && !closeLoop()) {
    iter++;
  }
  if (iter < m_maxIter)
    return true;
  else
    return false;
}

bool ConstraintSet::setControlParameter(int id,
                                        ConstraintAction action,
                                        double data,
                                        double timestep)
{
  ConstraintValues values;
  ConstraintSingleValue value;
  values.values = &value;
  values.number = 0;
  values.action = action;
  values.id = id;
  value.action = action;
  value.id = id;
  switch (action) {
    case ACT_NONE:
      return true;
    case ACT_VALUE:
      value.yd = data;
      values.number = 1;
      break;
    case ACT_VELOCITY:
      value.yddot = data;
      values.number = 1;
      break;
    case ACT_TOLERANCE:
      values.tolerance = data;
      break;
    case ACT_FEEDBACK:
      values.feedback = data;
      break;
    case ACT_ALPHA:
      values.alpha = data;
      break;
    default:
      assert(action == ACT_NONE);
      break;
  }
  return setControlParameters(&values, 1, timestep);
}

bool ConstraintSet::closeLoop()
{
  // Invert Jf
  // TODO: svd_boost_Macie has problems if Jf contains zero-rows
  // toggle=!toggle;
  // svd_boost_Macie(Jf,U,S,V,B,temp,1e-3*threshold,toggle);
  int ret = KDL::svd_eigen_HH(m_Jf, m_U, m_S, m_V, m_temp);
  if (ret < 0)
    return false;

  // the reference point and frame of the jacobian is the base frame
  // m_externalPose-m_internalPose is the twist to extend the end effector
  // to get the required pose => change the reference point to the base frame
  Twist twist_delta(diff(m_internalPose, m_externalPose));
  twist_delta = twist_delta.RefPoint(-m_internalPose.p);
  for (unsigned int i = 0; i < 6; i++)
    m_tdelta(i) = twist_delta(i);
  // TODO: use damping in constraintset inversion?
  for (unsigned int i = 0; i < 6; i++) {
    if (m_S(i) < m_threshold) {
      m_B.row(i).setConstant(0.0);
    }
    else {
      m_B.row(i) = m_U.col(i) / m_S(i);
    }
  }

  m_Jf_inv.noalias() = m_V * m_B;

  m_chi.noalias() += m_Jf_inv * m_tdelta;
  updateJacobian();
  // m_externalPose-m_internalPose in end effector frame
  // this is just to compare the pose, a different formula would work too
  return Equal(m_internalPose.Inverse() * m_externalPose, F_identity, m_threshold);
}
}  // namespace iTaSC