test2/intern/itasc/MovingFrame.cpp

/* SPDX-FileCopyrightText: 2009 Benoit Bolsee
 *
 * SPDX-License-Identifier: LGPL-2.1-or-later */

/** \file
 * \ingroup intern_itasc
 */

#include "MovingFrame.hpp"
#include <string.h>
namespace iTaSC {

static const unsigned int frameCacheSize = (sizeof(((Frame *)0)->p.data) +
                                            sizeof(((Frame *)0)->M.data)) /
                                           sizeof(double);

MovingFrame::MovingFrame(const Frame &frame)
    : UncontrolledObject(),
      m_function(NULL),
      m_param(NULL),
      m_velocity(),
      m_poseCCh(-1),
      m_poseCTs(0)
{
  m_internalPose = m_nextPose = frame;
  initialize(6, 1);
  e_matrix &Ju = m_JuArray[0];
  Ju = e_identity_matrix(6, 6);
}

MovingFrame::~MovingFrame() {}

bool MovingFrame::finalize()
{
  updateJacobian();
  return true;
}

void MovingFrame::initCache(Cache *_cache)
{
  m_cache = _cache;
  m_poseCCh = -1;
  if (m_cache) {
    m_poseCCh = m_cache->addChannel(this, "pose", frameCacheSize * sizeof(double));
    // don't store the initial pose, it's causing unnecessary large velocity on the first step
    // pushInternalFrame(0);
  }
}

void MovingFrame::pushInternalFrame(CacheTS timestamp)
{
  if (m_poseCCh >= 0) {
    double buf[frameCacheSize];
    memcpy(buf, m_internalPose.p.data, sizeof(m_internalPose.p.data));
    memcpy(&buf[sizeof(m_internalPose.p.data) / sizeof(double)],
           m_internalPose.M.data,
           sizeof(m_internalPose.M.data));

    m_cache->addCacheVectorIfDifferent(
        this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon);
    m_poseCTs = timestamp;
  }
}

// load pose just preceding timestamp
// return false if no cache position was found
bool MovingFrame::popInternalFrame(CacheTS timestamp)
{
  if (m_poseCCh >= 0) {
    char *item;
    item = (char *)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
    if (item && m_poseCTs != timestamp) {
      memcpy(m_internalPose.p.data, item, sizeof(m_internalPose.p.data));
      item += sizeof(m_internalPose.p.data);
      memcpy(m_internalPose.M.data, item, sizeof(m_internalPose.M.data));
      m_poseCTs = timestamp;
      // changing the starting pose, recompute the jacobian
      updateJacobian();
    }
    return (item) ? true : false;
  }
  // in case of no cache, there is always a previous position
  return true;
}

bool MovingFrame::setFrame(const Frame &frame)
{
  m_internalPose = m_nextPose = frame;
  return true;
}

bool MovingFrame::setCallback(MovingFrameCallback _function, void *_param)
{
  m_function = _function;
  m_param = _param;
  return true;
}

void MovingFrame::updateCoordinates(const Timestamp &timestamp)
{
  // don't compute the velocity during substepping, it is assumed constant.
  if (!timestamp.substep) {
    bool cacheAvail = true;
    if (!timestamp.reiterate) {
      cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
      if (m_function)
        (*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
    }
    // only compute velocity if we have a previous pose
    if (cacheAvail && timestamp.interpolate) {
      unsigned int iXu;
      m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
      for (iXu = 0; iXu < 6; iXu++)
        m_xudot(iXu) = m_velocity(iXu);
    }
    else if (!timestamp.reiterate) {
      // new position is forced, no velocity as we cannot interpolate
      m_internalPose = m_nextPose;
      m_velocity = Twist::Zero();
      m_xudot = e_zero_vector(6);
      // recompute the jacobian
      updateJacobian();
    }
  }
}

void MovingFrame::pushCache(const Timestamp &timestamp)
{
  if (!timestamp.substep && timestamp.cache)
    pushInternalFrame(timestamp.cacheTimestamp);
}

void MovingFrame::updateKinematics(const Timestamp &timestamp)
{
  if (timestamp.interpolate) {
    if (timestamp.substep) {
      // during substepping, update the internal pose from velocity information
      Twist localvel = m_internalPose.M.Inverse(m_velocity);
      m_internalPose.Integrate(localvel, 1.0 / timestamp.realTimestep);
    }
    else {
      m_internalPose = m_nextPose;
    }
    // m_internalPose is updated, recompute the jacobian
    updateJacobian();
  }
  pushCache(timestamp);
}

void MovingFrame::updateJacobian()
{
  Twist m_jac;
  e_matrix &Ju = m_JuArray[0];

  // Jacobian is always identity at position on the object,
  // we ust change the reference to the world.
  // instead of going through complicated jacobian operation, implemented direct formula
  Ju(1, 3) = m_internalPose.p.z();
  Ju(2, 3) = -m_internalPose.p.y();
  Ju(0, 4) = -m_internalPose.p.z();
  Ju(2, 4) = m_internalPose.p.x();
  Ju(0, 5) = m_internalPose.p.y();
  Ju(1, 5) = -m_internalPose.p.x();
  // remember that this object has moved
  m_updated = true;
}

}  // namespace iTaSC