Blender  V3.3
MovingFrame.cpp
Go to the documentation of this file.
1 /* SPDX-License-Identifier: LGPL-2.1-or-later
2  * Copyright 2009 Benoit Bolsee. */
3 
8 #include "MovingFrame.hpp"
9 #include <string.h>
10 namespace iTaSC{
11 
12 static const unsigned int frameCacheSize = (sizeof(((Frame*)0)->p.data)+sizeof(((Frame*)0)->M.data))/sizeof(double);
13 
15  m_function(NULL), m_param(NULL), m_velocity(), m_poseCCh(-1), m_poseCTs(0)
16 {
17  m_internalPose = m_nextPose = frame;
18  initialize(6, 1);
19  e_matrix& Ju = m_JuArray[0];
20  Ju = e_identity_matrix(6,6);
21 }
22 
24 {
25 }
26 
28 {
30  return true;
31 }
32 
34 {
35  m_cache = _cache;
36  m_poseCCh = -1;
37  if (m_cache) {
38  m_poseCCh = m_cache->addChannel(this,"pose",frameCacheSize*sizeof(double));
39  // don't store the initial pose, it's causing unnecessary large velocity on the first step
40  //pushInternalFrame(0);
41  }
42 }
43 
44 void MovingFrame::pushInternalFrame(CacheTS timestamp)
45 {
46  if (m_poseCCh >= 0) {
47  double buf[frameCacheSize];
48  memcpy(buf, m_internalPose.p.data, sizeof(m_internalPose.p.data));
49  memcpy(&buf[sizeof(m_internalPose.p.data)/sizeof(double)], m_internalPose.M.data, sizeof(m_internalPose.M.data));
50 
51  m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, frameCacheSize, KDL::epsilon);
52  m_poseCTs = timestamp;
53  }
54 }
55 
56 // load pose just preceding timestamp
57 // return false if no cache position was found
58 bool MovingFrame::popInternalFrame(CacheTS timestamp)
59 {
60  if (m_poseCCh >= 0) {
61  char *item;
62  item = (char *)m_cache->getPreviousCacheItem(this, m_poseCCh, &timestamp);
63  if (item && m_poseCTs != timestamp) {
64  memcpy(m_internalPose.p.data, item, sizeof(m_internalPose.p.data));
65  item += sizeof(m_internalPose.p.data);
66  memcpy(m_internalPose.M.data, item, sizeof(m_internalPose.M.data));
67  m_poseCTs = timestamp;
68  // changing the starting pose, recompute the jacobian
70  }
71  return (item) ? true : false;
72  }
73  // in case of no cache, there is always a previous position
74  return true;
75 }
76 
77 bool MovingFrame::setFrame(const Frame& frame)
78 {
79  m_internalPose = m_nextPose = frame;
80  return true;
81 }
82 
83 bool MovingFrame::setCallback(MovingFrameCallback _function, void* _param)
84 {
85  m_function = _function;
86  m_param = _param;
87  return true;
88 }
89 
91 {
92  // don't compute the velocity during substepping, it is assumed constant.
93  if (!timestamp.substep) {
94  bool cacheAvail = true;
95  if (!timestamp.reiterate) {
96  cacheAvail = popInternalFrame(timestamp.cacheTimestamp);
97  if (m_function)
98  (*m_function)(timestamp, m_internalPose, m_nextPose, m_param);
99  }
100  // only compute velocity if we have a previous pose
101  if (cacheAvail && timestamp.interpolate) {
102  unsigned int iXu;
103  m_velocity = diff(m_internalPose, m_nextPose, timestamp.realTimestep);
104  for (iXu=0; iXu<6; iXu++)
105  m_xudot(iXu) = m_velocity(iXu);
106  } else if (!timestamp.reiterate) {
107  // new position is forced, no velocity as we cannot interpolate
108  m_internalPose = m_nextPose;
109  m_velocity = Twist::Zero();
110  m_xudot = e_zero_vector(6);
111  // recompute the jacobian
112  updateJacobian();
113  }
114  }
115 }
116 
117 void MovingFrame::pushCache(const Timestamp& timestamp)
118 {
119  if (!timestamp.substep && timestamp.cache)
120  pushInternalFrame(timestamp.cacheTimestamp);
121 }
122 
124 {
125  if (timestamp.interpolate) {
126  if (timestamp.substep) {
127  // during substepping, update the internal pose from velocity information
128  Twist localvel = m_internalPose.M.Inverse(m_velocity);
129  m_internalPose.Integrate(localvel, 1.0/timestamp.realTimestep);
130  } else {
131  m_internalPose = m_nextPose;
132  }
133  // m_internalPose is updated, recompute the jacobian
134  updateJacobian();
135  }
136  pushCache(timestamp);
137 }
138 
140 {
141  Twist m_jac;
142  e_matrix& Ju = m_JuArray[0];
143 
144  //Jacobian is always identity at position on the object,
145  //we ust change the reference to the world.
146  //instead of going through complicated jacobian operation, implemented direct formula
147  Ju(1,3) = m_internalPose.p.z();
148  Ju(2,3) = -m_internalPose.p.y();
149  Ju(0,4) = -m_internalPose.p.z();
150  Ju(2,4) = m_internalPose.p.x();
151  Ju(0,5) = m_internalPose.p.y();
152  Ju(1,5) = -m_internalPose.p.x();
153  // remember that this object has moved
154  m_updated = true;
155 }
156 
157 }
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:526
Rotation M
Orientation of the Frame.
Definition: frames.hpp:529
void Integrate(const Twist &t_this, double frequency)
Definition: frames.inl:628
Vector p
origine of the Frame
Definition: frames.hpp:528
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:641
double data[9]
Definition: frames.hpp:301
represents both translational and rotational velocities.
Definition: frames.hpp:679
static Twist Zero()
Definition: frames.inl:310
double z() const
Definition: frames.inl:85
double y() const
Definition: frames.inl:84
double data[3]
Definition: frames.hpp:145
double x() const
Definition: frames.inl:83
int addChannel(const void *device, const char *name, unsigned int maxItemSize)
Definition: Cache.cpp:201
double * addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold)
Definition: Cache.cpp:599
const void * getPreviousCacheItem(const void *device, int channel, CacheTS *timestamp)
Definition: Cache.cpp:543
virtual void updateKinematics(const Timestamp &timestamp)
virtual void updateJacobian()
MovingFrame(const Frame &frame=F_identity)
Definition: MovingFrame.cpp:14
virtual void updateCoordinates(const Timestamp &timestamp)
Definition: MovingFrame.cpp:90
bool setCallback(MovingFrameCallback _function, void *_param)
Definition: MovingFrame.cpp:83
virtual void pushCache(const Timestamp &timestamp)
virtual void initCache(Cache *_cache)
Definition: MovingFrame.cpp:33
virtual bool finalize()
Definition: MovingFrame.cpp:27
virtual ~MovingFrame()
Definition: MovingFrame.cpp:23
bool setFrame(const Frame &frame)
Definition: MovingFrame.cpp:77
Cache * m_cache
Definition: Object.hpp:27
KDL::Frame m_internalPose
Definition: Object.hpp:28
bool m_updated
Definition: Object.hpp:29
std::vector< e_matrix > m_JuArray
virtual void initialize(unsigned int _nu, unsigned int _nf)
#define e_zero_vector
Definition: eigen_types.hpp:39
#define e_identity_matrix
Definition: eigen_types.hpp:42
#define e_matrix
Definition: eigen_types.hpp:40
#define M
IMETHOD Vector diff(const Vector &a, const Vector &b, double dt=1)
double epsilon
default precision while comparing with Equal(..,..) functions. Initialized at 0.0000001.
Definition: utility.cpp:22
bool(* MovingFrameCallback)(const Timestamp &timestamp, const Frame &_current, Frame &_next, void *param)
Definition: MovingFrame.hpp:17
unsigned int CacheTS
Definition: Cache.hpp:32
static const unsigned int frameCacheSize
Definition: MovingFrame.cpp:12
unsigned int interpolate
Definition: Cache.hpp:44
double realTimestep
Definition: Cache.hpp:37
unsigned int cache
Definition: Cache.hpp:42
CacheTS cacheTimestamp
Definition: Cache.hpp:38
unsigned int reiterate
Definition: Cache.hpp:41
unsigned int substep
Definition: Cache.hpp:40