bdk-blender/intern/itasc/MovingFrame.cpp
Campbell Barton 5035fbdd23 License headers: use SPDX identifiers in intern/itasc
Added license headers based on the original LGPL files from:
gitlab.kuleuven.be/rob-itasc
2022-03-18 10:27:01 +11:00

158 lines
4.3 KiB
C++

/* SPDX-License-Identifier: LGPL-2.1-or-later
* Copyright 2009 Benoit Bolsee. */
/** \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;
}
}