Skip to content

Commit 3a119f8

Browse files
MegMllMegMll
committed
JointSpline - Diverse Changes
1 parent d4b7a1e commit 3a119f8

File tree

2 files changed

+65
-47
lines changed

2 files changed

+65
-47
lines changed

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

Lines changed: 64 additions & 46 deletions
Original file line numberDiff line numberDiff line change
@@ -84,6 +84,9 @@ namespace pinocchio
8484
typedef JointModelSplineTpl<Scalar, Options> JointModelDerived;
8585
// typedef JointMotionSubspace1d Constraint_t;
8686
typedef JointMotionSubspaceTpl<1, Scalar, Options, 1> Constraint_t;
87+
typedef SE3Tpl<Scalar, Options> Transformation_t;
88+
typedef MotionTpl<Scalar, Options> Motion_t;
89+
typedef MotionTpl<Scalar, Options> Bias_t;
8790

8891
// [ABA]
8992
typedef Eigen::Matrix<Scalar, 6, NV, Options> U_t;
@@ -128,7 +131,7 @@ namespace pinocchio
128131
Constraint_t S;
129132
Transformation_t M;
130133
Motion_t v;
131-
Motion_t c;
134+
Bias_t c;
132135

133136
// [ABA] specific data
134137
U_t U;
@@ -146,7 +149,7 @@ namespace pinocchio
146149
, joint_v(TangentVector_t::Zero())
147150
, M(Transformation_t::Identity())
148151
, v(Motion_t::Zero())
149-
, c(Motion_t::Zero())
152+
, c(Bias_t::Zero())
150153
, U(U_t::Zero())
151154
, Dinv(D_t::Zero())
152155
, UDinv(UD_t::Identity())
@@ -162,7 +165,7 @@ namespace pinocchio
162165
, joint_v(TangentVector_t::Zero())
163166
, M(Transformation_t::Identity())
164167
, v(Motion_t::Zero())
165-
, c(Motion_t::Zero())
168+
, c(Bias_t::Zero())
166169
, U(U_t::Zero())
167170
, Dinv(D_t::Zero())
168171
, UDinv(UD_t::Identity())
@@ -217,16 +220,24 @@ namespace pinocchio
217220
JointModelSplineTpl(
218221
const PINOCCHIO_ALIGNED_STD_VECTOR(SE3) & controlFrames, const int degree = 3)
219222
: degree(degree)
223+
, ctrlFrames(controlFrames)
224+
, nbCtrlFrames(controlFrames.size())
225+
{
226+
buildJoint();
227+
}
228+
229+
JointModelSplineTpl(const std::vector<SE3> & controlFrames, const int degree = 3)
230+
: degree(degree)
220231
{
221232
setControlFrames(controlFrames);
222233
}
223234

224235
void setControlFrames(const std::vector<SE3> & controlFrames)
225236
{
226237
nbCtrlFrames = controlFrames.size();
227-
for(size_t i = 0; i < nbCtrlFrames; i++)
228-
ctrlFrames.push_back(controlFrames);
229-
238+
for (size_t i = 0; i < nbCtrlFrames; i++)
239+
ctrlFrames.push_back(controlFrames[i]);
240+
230241
buildJoint();
231242
}
232243

@@ -276,9 +287,11 @@ namespace pinocchio
276287
const Scalar phi_i = data.N.segment(i, indexes.end_idx - i).sum();
277288
const Scalar phi_dot_i = data.N_der.segment(i, indexes.end_idx - i).sum();
278289

279-
data.M = data.M * exp6(relativeMotions[i - 1] * phi_i);
280-
data.S.matrix() = exp6(relativeMotions[i - 1] * phi_i).actInv(data.S)
281-
+ relativeMotions[i - 1].toVector() * phi_dot_i;
290+
const Transformation_t transformation_temp(exp6(relativeMotions[i - 1] * phi_i));
291+
292+
data.M = data.M * transformation_temp;
293+
data.S.matrix() =
294+
transformation_temp.actInv(data.S) + relativeMotions[i - 1].toVector() * phi_dot_i;
282295
}
283296
}
284297

@@ -304,7 +317,6 @@ namespace pinocchio
304317
data.N_der2[i] = bsplineBasisDerivative2(i, degree, data.joint_q[0]);
305318
}
306319

307-
// Compute time derivative of S (for bias acceleration c)
308320
data.S.matrix().setZero();
309321
data.c.setZero();
310322
data.M = ctrlFrames[indexes.start_idx];
@@ -314,24 +326,24 @@ namespace pinocchio
314326
const Scalar phi_dot_i = data.N_der.segment(i, indexes.end_idx - i).sum();
315327
const Scalar phi_ddot_i = data.N_der2.segment(i, indexes.end_idx - i).sum();
316328

317-
data.M = data.M * exp6(relativeMotions[i - 1] * phi_i);
318-
data.c =
319-
relativeMotions[i - 1] * phi_ddot_i
320-
+ exp6(relativeMotions[i - 1] * phi_i)
321-
.actInv(data.c + Motion(data.S.matrix()).cross(relativeMotions[i - 1]) * phi_dot_i);
322-
data.S.matrix() = exp6(relativeMotions[i - 1] * phi_i).actInv(data.S)
323-
+ relativeMotions[i - 1].toVector() * phi_dot_i;
329+
const Transformation_t transformation_temp(exp6(relativeMotions[i - 1] * phi_i));
330+
331+
data.M = data.M * transformation_temp;
332+
data.c = relativeMotions[i - 1] * phi_ddot_i
333+
+ transformation_temp.actInv(
334+
data.c + Motion(data.S.matrix()).cross(relativeMotions[i - 1]) * phi_dot_i);
335+
data.S.matrix() =
336+
transformation_temp.actInv(data.S) + relativeMotions[i - 1].toVector() * phi_dot_i;
324337
}
325338

326339
data.c = data.c * data.joint_v[0];
327340
data.v = data.S * data.joint_v;
328341
}
329342

330343
template<typename TangentVector>
331-
void calc(
332-
JointDataDerived & data,
333-
const Blank not_used,
334-
const Eigen::MatrixBase<TangentVector> & vs) const
344+
void
345+
calc(JointDataDerived & data, const Blank not_used, const Eigen::MatrixBase<TangentVector> & vs)
346+
const
335347
{
336348
data.joint_v = vs.template segment<NV>(idx_v());
337349

@@ -354,12 +366,13 @@ namespace pinocchio
354366
const Scalar phi_dot_i = data.N_der.tail(nbCtrlFrames - (i + 1)).sum();
355367
const Scalar phi_ddot_i = data.N_der2.tail(nbCtrlFrames - (i + 1)).sum();
356368

357-
data.c =
358-
relativeMotions[i] * phi_ddot_i
359-
+ exp6(relativeMotions[i] * phi_i)
360-
.actInv(data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i);
361-
data.S.matrix() = exp6(relativeMotions[i] * phi_i).actInv(data.S)
362-
+ relativeMotions[i].toVector() * phi_dot_i;
369+
const Transformation_t transformation_temp(exp6(relativeMotions[i - 1] * phi_i));
370+
371+
data.c = relativeMotions[i] * phi_ddot_i
372+
+ transformation_temp.actInv(
373+
data.c + Motion(data.S.matrix()).cross(relativeMotions[i]) * phi_dot_i);
374+
data.S.matrix() =
375+
transformation_temp.actInv(data.S) + relativeMotions[i].toVector() * phi_dot_i;
363376
}
364377

365378
data.c = data.c * data.joint_v[0];
@@ -411,8 +424,10 @@ namespace pinocchio
411424

412425
void makeKnots()
413426
{
414-
if(nbCtrlFrames <= degree)
415-
PINOCCHIO_THROW_PRETTY(std::invalid_argument, "JointSpline - Number of control frames must be greater than degree of the spline.")
427+
if (nbCtrlFrames <= degree)
428+
PINOCCHIO_THROW_PRETTY(
429+
std::invalid_argument,
430+
"JointSpline - Number of control frames must be greater than degree of the spline.")
416431
const int n_knots = nbCtrlFrames + degree + 1;
417432
knots.resize(n_knots);
418433
knots.head(degree + 1).setZero();
@@ -446,6 +461,7 @@ namespace pinocchio
446461

447462
Scalar bsplineBasis(int i, int k, const Scalar x) const
448463
{
464+
using internal::if_then_else;
449465
if (k == 0)
450466
{
451467
// clang-format off
@@ -454,29 +470,29 @@ namespace pinocchio
454470
// else
455471
// return 0;
456472
// clang-format on
457-
return pinocchio::internal::if_then_else(
458-
pinocchio::internal::LE, knots[i], x,
459-
pinocchio::internal::if_then_else(
460-
pinocchio::internal::LE, x, knots[i + 1], Scalar(1), Scalar(0)),
461-
Scalar(0));
473+
return if_then_else(
474+
internal::LE, knots[i], x,
475+
if_then_else(internal::LE, x, knots[i + 1], Scalar(1), Scalar(0)), Scalar(0));
462476
}
463477

464478
// Calculate the left term
465479
const Scalar den1 = knots[i + k] - knots[i];
466-
const Scalar left = pinocchio::internal::if_then_else(
467-
pinocchio::internal::GT, den1, Eigen::NumTraits<Scalar>::dummy_precision(),
480+
const Scalar left = if_then_else(
481+
internal::GT, den1, Eigen::NumTraits<Scalar>::dummy_precision(),
468482
(x - knots[i]) / den1 * bsplineBasis(i, k - 1, x), Scalar(0));
469483

470484
// Calculate the right term
471485
const Scalar den2 = knots[i + k + 1] - knots[i + 1];
472-
const Scalar right = pinocchio::internal::if_then_else(
473-
pinocchio::internal::GT, den2, Eigen::NumTraits<Scalar>::dummy_precision(),
486+
const Scalar right = if_then_else(
487+
internal::GT, den2, Eigen::NumTraits<Scalar>::dummy_precision(),
474488
(knots[i + k + 1] - x) / den2 * bsplineBasis(i + 1, k - 1, x), Scalar(0));
475489

476490
return left + right;
477491
}
478492
Scalar bsplineBasisDerivative(int i, int k, const Scalar x) const
479493
{
494+
using internal::if_then_else;
495+
480496
if (k == 0)
481497
{
482498
return Scalar(0);
@@ -485,21 +501,23 @@ namespace pinocchio
485501

486502
// Calculate the first term of the derivative
487503
const Scalar den1 = knots[i + k] - knots[i];
488-
const Scalar term1 = pinocchio::internal::if_then_else(
489-
pinocchio::internal::GT, den1, Eigen::NumTraits<Scalar>::dummy_precision(),
504+
const Scalar term1 = if_then_else(
505+
internal::GT, den1, Eigen::NumTraits<Scalar>::dummy_precision(),
490506
(k_scalar / den1) * bsplineBasis(i, k - 1, x), Scalar(0));
491507

492508
// Calculate the second term of the derivative
493509
const Scalar den2 = knots[i + k + 1] - knots[i + 1];
494-
const Scalar term2 = pinocchio::internal::if_then_else(
495-
pinocchio::internal::GT, den2, Eigen::NumTraits<Scalar>::dummy_precision(),
510+
const Scalar term2 = if_then_else(
511+
internal::GT, den2, Eigen::NumTraits<Scalar>::dummy_precision(),
496512
(k_scalar / den2) * bsplineBasis(i + 1, k - 1, x), Scalar(0));
497513

498514
return term1 - term2;
499515
}
500516

501517
Scalar bsplineBasisDerivative2(int i, int k, const Scalar x) const
502518
{
519+
using internal::if_then_else;
520+
503521
if (k < 2)
504522
{
505523
return Scalar(0);
@@ -509,14 +527,14 @@ namespace pinocchio
509527

510528
// Calculate the first term
511529
const Scalar den1 = knots[i + k] - knots[i];
512-
const Scalar term1 = pinocchio::internal::if_then_else(
513-
pinocchio::internal::GT, den1, Eigen::NumTraits<Scalar>::dummy_precision(),
530+
const Scalar term1 = if_then_else(
531+
internal::GT, den1, Eigen::NumTraits<Scalar>::dummy_precision(),
514532
(k_scalar / den1) * bsplineBasisDerivative(i, k - 1, x), Scalar(0));
515533

516534
// Calculate the second term
517535
const Scalar den2 = knots[i + k + 1] - knots[i + 1];
518-
const Scalar term2 = pinocchio::internal::if_then_else(
519-
pinocchio::internal::GT, den2, Eigen::NumTraits<Scalar>::dummy_precision(),
536+
const Scalar term2 = if_then_else(
537+
internal::GT, den2, Eigen::NumTraits<Scalar>::dummy_precision(),
520538
(k_scalar / den2) * bsplineBasisDerivative(i + 1, k - 1, x), Scalar(0));
521539

522540
return term1 - term2;

‎src/parsers/graph/model-graph-algo.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -236,7 +236,7 @@ namespace pinocchio
236236
{
237237
JointModelSpline jmodel(joint.degree);
238238
jmodel.setControlFrames(joint.ctrlFrames);
239-
239+
240240
return jmodel;
241241
}
242242
ReturnType operator()(const JointComposite & joint) const

0 commit comments

Comments
 (0)