Skip to content

Commit b40b500

Browse files
MegMllMegMll
committed
SplineJoint - apply pre-commit
1 parent 02a54e9 commit b40b500

File tree

1 file changed

+84
-80
lines changed

1 file changed

+84
-80
lines changed

‎include/pinocchio/multibody/joint/joint-spline.hpp‎

Lines changed: 84 additions & 80 deletions
Original file line numberDiff line numberDiff line change
@@ -98,7 +98,6 @@ namespace pinocchio
9898
Vector N_der;
9999
Vector N_der2;
100100

101-
102101
JointDataSplineTpl()
103102
: joint_q(ConfigVector_t::Zero())
104103
, joint_v(TangentVector_t::Zero())
@@ -142,7 +141,6 @@ namespace pinocchio
142141

143142
}; // struct JointDataSplinerTpl
144143

145-
146144
PINOCCHIO_JOINT_CAST_TYPE_SPECIALIZATION(JointModelSplineTpl);
147145
template<typename _Scalar, int _Options>
148146
struct JointModelSplineTpl : public JointModelBase<JointModelSplineTpl<_Scalar, _Options>>
@@ -161,13 +159,14 @@ namespace pinocchio
161159
using Base::idx_vExtended;
162160
using Base::setIndexes;
163161

164-
165162
JointModelSplineTpl()
166163
: degree(3)
167164
, nbCtrlFrames(0)
168-
{}
165+
{
166+
}
169167

170-
JointModelSplineTpl(const PINOCCHIO_ALIGNED_STD_VECTOR(SE3) & controlFrames, const int degree=3)
168+
JointModelSplineTpl(
169+
const PINOCCHIO_ALIGNED_STD_VECTOR(SE3) & controlFrames, const int degree = 3)
171170
: degree(degree)
172171
, nbCtrlFrames(controlFrames.size())
173172
, ctrlFrames(controlFrames)
@@ -176,7 +175,7 @@ namespace pinocchio
176175
computeRelativeMotions();
177176
}
178177

179-
void addControlFrame(const SE3 & frame)
178+
void addControlFrame(const SE3 & frame)
180179
{
181180
ctrlFrames.push_back(frame);
182181
}
@@ -217,15 +216,16 @@ namespace pinocchio
217216

218217
// Compute joint transform M
219218
data.M = ctrlFrames[0];
220-
// joint subspace S
219+
// joint subspace S
221220
data.S.matrix().setZero();
222221
for (int i = 0; i < nbCtrlFrames - 1; ++i)
223222
{
224223
const Scalar phi_i = data.N.tail(nbCtrlFrames - (i + 1)).sum();
225224
const Scalar phi_dot_i = data.N_der.tail(nbCtrlFrames - (i + 1)).sum();
226225

227-
data.M = data.M * exp6(relativeMotions[i] * phi_i);
228-
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S) + relativeMotions[i].toVector() * phi_dot_i;
226+
data.M = data.M * exp6(relativeMotions[i] * phi_i);
227+
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S)
228+
+ relativeMotions[i].toVector() * phi_dot_i;
229229
}
230230
}
231231

@@ -259,9 +259,13 @@ namespace pinocchio
259259
const Scalar phi_dot_i = data.N_der.tail(nbCtrlFrames - (i + 1)).sum();
260260
const Scalar phi_ddot_i = data.N_der2.tail(nbCtrlFrames - (i + 1)).sum();
261261

262-
data.M = data.M * exp6(relativeMotions[i] * phi_i);
263-
data.c = relativeMotions[i] * phi_ddot_i + exp6(relativeMotions[i] * phi_i).actInv(data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i);
264-
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S) + relativeMotions[i].toVector() * phi_dot_i;
262+
data.M = data.M * exp6(relativeMotions[i] * phi_i);
263+
data.c =
264+
relativeMotions[i] * phi_ddot_i
265+
+ exp6(relativeMotions[i] * phi_i)
266+
.actInv(data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i);
267+
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S)
268+
+ relativeMotions[i].toVector() * phi_dot_i;
265269
}
266270

267271
data.c = data.c * data.joint_v[0];
@@ -296,8 +300,12 @@ namespace pinocchio
296300
const Scalar phi_dot_i = data.N_der.tail(nbCtrlFrames - (i + 1)).sum();
297301
const Scalar phi_ddot_i = data.N_der2.tail(nbCtrlFrames - (i + 1)).sum();
298302

299-
data.c = relativeMotions[i] * phi_ddot_i + exp6(relativeMotions[i] * phi_i).actInv(data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i);
300-
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S) + relativeMotions[i].toVector() * phi_dot_i;
303+
data.c =
304+
relativeMotions[i] * phi_ddot_i
305+
+ exp6(relativeMotions[i] * phi_i)
306+
.actInv(data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i);
307+
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S)
308+
+ relativeMotions[i].toVector() * phi_dot_i;
301309
}
302310

303311
data.c = data.c * data.joint_v[0];
@@ -337,9 +345,8 @@ namespace pinocchio
337345
typedef JointModelSplineTpl<NewScalar, Options> ReturnType;
338346
ReturnType res;
339347
res.degree = degree;
340-
res.ctrlFrames.resize(ctrlFrames.size());
341-
342-
for(size_t k=0; k<ctrlFrames.size(); ++k)
348+
res.nbCtrlFrames = nbCtrlFrames;
349+
for (size_t k = 0; k < ctrlFrames.size(); k++)
343350
{
344351
res.ctrlFrames.push_back(ctrlFrames[k].template cast<NewScalar>());
345352
}
@@ -356,18 +363,16 @@ namespace pinocchio
356363
knots.head(degree + 1).setZero();
357364
const Scalar denominator = static_cast<Scalar>(nbCtrlFrames - degree + 1);
358365

359-
for (int i = degree + 1; i < nbCtrlFrames; ++i)
360-
knots[i] = static_cast<Scalar>(i - degree) / denominator;
361-
366+
for (int i = degree + 1; i < nbCtrlFrames; i++)
367+
knots[i] = static_cast<Scalar>(i - degree) / denominator;
368+
362369
knots.tail(degree + 1).setOnes();
363370
}
364-
371+
365372
void computeRelativeMotions()
366373
{
367-
relativeMotions.resize(nbCtrlFrames - 1);
368374
for (int i = 0; i < nbCtrlFrames - 1; i++)
369-
relativeMotions[i] = log6(ctrlFrames[i].inverse() * ctrlFrames[i + 1]);
370-
375+
relativeMotions.push_back(log6(ctrlFrames[i].inverse() * ctrlFrames[i + 1]));
371376
}
372377

373378
// attributes
@@ -377,62 +382,61 @@ namespace pinocchio
377382
PINOCCHIO_ALIGNED_STD_VECTOR(SE3) ctrlFrames;
378383
PINOCCHIO_ALIGNED_STD_VECTOR(Motion) relativeMotions;
379384

380-
private:
381-
Scalar bsplineBasis(int i, int k, const Scalar x) const
382-
{
383-
if(k == 0)
384-
return (knots[i] <= x && x <= knots[i + 1]) ? Scalar(1) : Scalar(0);
385-
386-
387-
Scalar left = 0, right = 0;
388-
Scalar den1 = knots[i + k] - knots[i];
389-
if (knots[i + k] > knots[i])
390-
left = (x - knots[i]) / den1 * bsplineBasis(i, k - 1, x);
391-
392-
Scalar den2 = knots[i + k + 1] - knots[i + 1];
393-
if (knots[i + k + 1] > knots[i + 1])
394-
right = (knots[i + k + 1] - x) / den2 * bsplineBasis(i + 1, k - 1, x);
395-
396-
return left + right;
397-
}
398-
399-
Scalar bsplineBasisDerivative(int i, int k, const Scalar x) const
400-
{
401-
if(k == 0)
402-
return Scalar(0);
403-
404-
Scalar term1 = 0, term2 = 0;
405-
Scalar den1 = knots[i + k] - knots[i];
406-
if (den1 > Eigen::NumTraits<Scalar>::dummy_precision())
407-
{
408-
term1 = (static_cast<Scalar>(k) / den1) * bsplineBasis(i, k - 1, x);
409-
}
410-
Scalar den2 = knots[i + k + 1] - knots[i + 1];
411-
if (den2 > Eigen::NumTraits<Scalar>::dummy_precision())
412-
{
413-
term2 = (static_cast<Scalar>(k) / den2) * bsplineBasis(i + 1, k - 1, x);
414-
}
415-
return term1 - term2;
416-
}
417-
418-
Scalar bsplineBasisDerivative2(int i, int k, const Scalar x) const
419-
{
420-
if (k < 2)
421-
return Scalar(0);
422-
423-
Scalar term1 = 0, term2 = 0;
424-
Scalar den1 = knots[i + k] - knots[i];
425-
if (den1 > Eigen::NumTraits<Scalar>::dummy_precision())
426-
{
427-
term1 = (static_cast<Scalar>(k) / den1) * bsplineBasisDerivative(i, k - 1, x);
428-
}
429-
Scalar den2 = knots[i + k + 1] - knots[i + 1];
430-
if (den2 > Eigen::NumTraits<Scalar>::dummy_precision())
431-
{
432-
term2 = (static_cast<Scalar>(k) / den2) * bsplineBasisDerivative(i + 1, k - 1, x);
433-
}
434-
return term1 - term2;
435-
}
385+
private:
386+
Scalar bsplineBasis(int i, int k, const Scalar x) const
387+
{
388+
if (k == 0)
389+
return (knots[i] <= x && x <= knots[i + 1]) ? Scalar(1) : Scalar(0);
390+
391+
Scalar left = 0, right = 0;
392+
Scalar den1 = knots[i + k] - knots[i];
393+
if (knots[i + k] > knots[i])
394+
left = (x - knots[i]) / den1 * bsplineBasis(i, k - 1, x);
395+
396+
Scalar den2 = knots[i + k + 1] - knots[i + 1];
397+
if (knots[i + k + 1] > knots[i + 1])
398+
right = (knots[i + k + 1] - x) / den2 * bsplineBasis(i + 1, k - 1, x);
399+
400+
return left + right;
401+
}
402+
403+
Scalar bsplineBasisDerivative(int i, int k, const Scalar x) const
404+
{
405+
if (k == 0)
406+
return Scalar(0);
407+
408+
Scalar term1 = 0, term2 = 0;
409+
Scalar den1 = knots[i + k] - knots[i];
410+
if (den1 > Eigen::NumTraits<Scalar>::dummy_precision())
411+
{
412+
term1 = (static_cast<Scalar>(k) / den1) * bsplineBasis(i, k - 1, x);
413+
}
414+
Scalar den2 = knots[i + k + 1] - knots[i + 1];
415+
if (den2 > Eigen::NumTraits<Scalar>::dummy_precision())
416+
{
417+
term2 = (static_cast<Scalar>(k) / den2) * bsplineBasis(i + 1, k - 1, x);
418+
}
419+
return term1 - term2;
420+
}
421+
422+
Scalar bsplineBasisDerivative2(int i, int k, const Scalar x) const
423+
{
424+
if (k < 2)
425+
return Scalar(0);
426+
427+
Scalar term1 = 0, term2 = 0;
428+
Scalar den1 = knots[i + k] - knots[i];
429+
if (den1 > Eigen::NumTraits<Scalar>::dummy_precision())
430+
{
431+
term1 = (static_cast<Scalar>(k) / den1) * bsplineBasisDerivative(i, k - 1, x);
432+
}
433+
Scalar den2 = knots[i + k + 1] - knots[i + 1];
434+
if (den2 > Eigen::NumTraits<Scalar>::dummy_precision())
435+
{
436+
term2 = (static_cast<Scalar>(k) / den2) * bsplineBasisDerivative(i + 1, k - 1, x);
437+
}
438+
return term1 - term2;
439+
}
436440
}; // struct JointModelSplineTpl
437441

438442
} // namespace pinocchio

0 commit comments

Comments
 (0)