diff options
author | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-25 01:22:24 +0400 |
---|---|---|
committer | Benoit Bolsee <benoit.bolsee@online.be> | 2009-09-25 01:22:24 +0400 |
commit | 1483fafd1372a3d3e025d08634e798adb7da512f (patch) | |
tree | 9191765749e29866339f4c31d892603f5f8b334d /intern/itasc/MovingFrame.cpp | |
parent | c995c605f640d8d688e6e58e0fe247ca83f91696 (diff) | |
parent | 222fe6b1a5d49f67177cbb762f55a0e482145f5d (diff) |
Merge of itasc branch. Project files, scons and cmake should be working. Makefile updated but not tested. Comes with Eigen2 2.0.6 C++ matrix library.
Diffstat (limited to 'intern/itasc/MovingFrame.cpp')
-rw-r--r-- | intern/itasc/MovingFrame.cpp | 157 |
1 files changed, 157 insertions, 0 deletions
diff --git a/intern/itasc/MovingFrame.cpp b/intern/itasc/MovingFrame.cpp new file mode 100644 index 00000000000..545f9bd38e9 --- /dev/null +++ b/intern/itasc/MovingFrame.cpp @@ -0,0 +1,157 @@ +/* $Id: MovingFrame.cpp 20853 2009-06-13 12:29:46Z ben2610 $ + * MovingFrame.cpp + * + * Created on: Feb 10, 2009 + * Author: benoitbolsee + */ + +#include "MovingFrame.hpp" +#include <malloc.h> +#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() +{ +} + +void MovingFrame::finalize() +{ + updateJacobian(); +} + +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 preceeding 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, ×tamp); + 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; +} + +} |