1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
|
/* 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, ×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;
}
}
|
