Skip to content

Commit 4e13260

Browse files
jorisvjorisv
authored
Merge pull request #2466 from jcarpent/topic/jacobians
Fix bug for get{Joint,Frame}JacobianTimeVariation
2 parents 68166cc + 831df1f commit 4e13260

File tree

5 files changed

+171
-46
lines changed

5 files changed

+171
-46
lines changed

‎CHANGELOG.md‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -30,6 +30,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
3030
- Fix `pinocchio-test-cpp-mjcf` unittest with Boost 1.86 ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
3131
- Fix `pinocchio-test-cpp-constraint-variants` uninitialized values ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
3232
- Fix mixing library symbols between Pinocchio scalar bindings ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
33+
- Fix bug for get{Joint,Frame}JacobianTimeVariation ([#2466](https://github.com/stack-of-tasks/pinocchio/pull/2466))
3334

3435
### Changed
3536

‎include/pinocchio/algorithm/frames.hxx‎

Lines changed: 59 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
//
2-
// Copyright (c) 2015-2021 CNRS INRIA
2+
// Copyright (c) 2015-2024 CNRS INRIA
33
//
44

55
#ifndef __pinocchio_algorithm_frames_hxx__
@@ -171,8 +171,7 @@ namespace pinocchio
171171

172172
const typename Data::SE3 oMframe = data.oMi[joint_id] * placement;
173173
details::translateJointJacobian(
174-
model, data, joint_id, reference_frame, oMframe, data.J,
175-
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
174+
model, data, joint_id, reference_frame, oMframe, data.J, J.const_cast_derived());
176175
}
177176

178177
template<
@@ -220,16 +219,15 @@ namespace pinocchio
220219
JointIndex parent = joint_support[k];
221220
Pass::run(
222221
model.joints[parent], data.joints[parent],
223-
typename Pass::ArgsType(
224-
model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
222+
typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
225223
}
226224

227225
if (reference_frame == LOCAL_WORLD_ALIGNED)
228226
{
229227
typename Data::SE3 & oMframe = data.oMf[frameId];
230228
oMframe = data.oMi[joint_id] * frame.placement;
231229

232-
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
230+
Matrix6xLike & J_ = J.const_cast_derived();
233231

234232
const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
235233
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
@@ -252,8 +250,7 @@ namespace pinocchio
252250
{
253251
Pass::run(
254252
model.joints[i], data.joints[i],
255-
typename Pass::ArgsType(
256-
model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
253+
typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
257254
}
258255
break;
259256
}
@@ -273,9 +270,11 @@ namespace pinocchio
273270
DataTpl<Scalar, Options, JointCollectionTpl> & data,
274271
const FrameIndex frame_id,
275272
const ReferenceFrame rf,
276-
const Eigen::MatrixBase<Matrix6xLike> & dJ)
273+
const Eigen::MatrixBase<Matrix6xLike> & dJ_)
277274
{
278275
assert(model.check(data) && "data is not consistent with model.");
276+
277+
Matrix6xLike & dJ = dJ_.const_cast_derived();
279278
PINOCCHIO_CHECK_ARGUMENT_SIZE(
280279
dJ.cols(), model.nv,
281280
"The numbers of columns in the Jacobian matrix does not math the "
@@ -284,6 +283,9 @@ namespace pinocchio
284283
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
285284
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
286285
typedef typename Model::Frame Frame;
286+
typedef typename Data::SE3 SE3;
287+
typedef typename SE3::Vector3 Vector3;
288+
typedef typename Data::Motion Motion;
287289

288290
const Frame & frame = model.frames[frame_id];
289291
const JointIndex & joint_id = frame.parentJoint;
@@ -292,7 +294,54 @@ namespace pinocchio
292294
oMframe = data.oMi[joint_id] * frame.placement;
293295

294296
details::translateJointJacobian(
295-
model, data, joint_id, rf, oMframe, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ));
297+
model, data, joint_id, rf, oMframe, data.dJ, dJ.const_cast_derived());
298+
299+
// Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
300+
switch (rf)
301+
{
302+
case LOCAL: {
303+
const Motion & v_joint = data.v[joint_id];
304+
const Motion v_frame = frame.placement.actInv(v_joint);
305+
306+
const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
307+
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
308+
{
309+
typedef typename Data::Matrix6x::ColXpr ColXprIn;
310+
typedef const MotionRef<ColXprIn> MotionIn;
311+
312+
typedef typename Matrix6xLike::ColXpr ColXprOut;
313+
typedef MotionRef<ColXprOut> MotionOut;
314+
MotionIn v_in(data.J.col(j));
315+
MotionOut v_out(dJ.col(j));
316+
317+
v_out -= v_frame.cross(oMframe.actInv(v_in));
318+
}
319+
break;
320+
}
321+
case LOCAL_WORLD_ALIGNED: {
322+
const Motion & ov_joint = data.ov[joint_id];
323+
const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
324+
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
325+
{
326+
typedef typename Data::Matrix6x::ColXpr ColXprIn;
327+
typedef const MotionRef<ColXprIn> MotionIn;
328+
329+
typedef typename Matrix6xLike::ColXpr ColXprOut;
330+
typedef MotionRef<ColXprOut> MotionOut;
331+
MotionIn v_in(data.J.col(j));
332+
MotionOut v_out(dJ.col(j));
333+
334+
v_out.linear() -=
335+
Vector3(ov_joint.linear() + ov_joint.angular().cross(oMframe.translation()))
336+
.cross(v_in.angular());
337+
}
338+
break;
339+
}
340+
341+
case WORLD:
342+
default:
343+
break;
344+
}
296345
}
297346

298347
template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>

‎include/pinocchio/algorithm/jacobian.hxx‎

Lines changed: 67 additions & 13 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
//
2-
// Copyright (c) 2015-2020 CNRS INRIA
2+
// Copyright (c) 2015-2024 CNRS INRIA
33
//
44

55
#ifndef __pinocchio_algorithm_jacobian_hxx__
@@ -56,7 +56,7 @@ namespace pinocchio
5656
else
5757
data.oMi[i] = data.liMi[i];
5858

59-
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
59+
Matrix6xLike & J_ = J.const_cast_derived();
6060
jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
6161
}
6262
};
@@ -88,7 +88,7 @@ namespace pinocchio
8888
{
8989
Pass::run(
9090
model.joints[i], data.joints[i],
91-
ArgsType(model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6x, data.J)));
91+
ArgsType(model, data, q.derived(), data.J.const_cast_derived()));
9292
}
9393

9494
return data.J;
@@ -148,7 +148,7 @@ namespace pinocchio
148148
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
149149
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
150150

151-
Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout);
151+
Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();
152152

153153
typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
154154
typedef const MotionRef<ConstColXprIn> MotionIn;
@@ -189,7 +189,7 @@ namespace pinocchio
189189
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
190190
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);
191191

192-
Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout);
192+
Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();
193193

194194
typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
195195
typedef const MotionRef<ConstColXprIn> MotionIn;
@@ -277,8 +277,7 @@ namespace pinocchio
277277
assert(model.check(data) && "data is not consistent with model.");
278278

279279
::pinocchio::details::translateJointJacobian(
280-
model, data, joint_id, reference_frame, data.J,
281-
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
280+
model, data, joint_id, reference_frame, data.J, J.const_cast_derived());
282281
}
283282

284283
template<
@@ -319,7 +318,7 @@ namespace pinocchio
319318
data.liMi[i] = model.jointPlacements[i] * jdata.M();
320319
data.iMf[parent] = data.liMi[i] * data.iMf[i];
321320

322-
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
321+
Matrix6xLike & J_ = J.const_cast_derived();
323322
jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
324323
}
325324
};
@@ -352,8 +351,7 @@ namespace pinocchio
352351
{
353352
Pass::run(
354353
model.joints[i], data.joints[i],
355-
typename Pass::ArgsType(
356-
model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
354+
typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
357355
}
358356
}
359357

@@ -471,10 +469,66 @@ namespace pinocchio
471469
const DataTpl<Scalar, Options, JointCollectionTpl> & data,
472470
const JointIndex jointId,
473471
const ReferenceFrame rf,
474-
const Eigen::MatrixBase<Matrix6xLike> & dJ)
472+
const Eigen::MatrixBase<Matrix6xLike> & dJ_)
475473
{
476-
::pinocchio::details::translateJointJacobian(
477-
model, data, jointId, rf, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ));
474+
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
475+
typedef typename Data::SE3 SE3;
476+
typedef typename SE3::Vector3 Vector3;
477+
typedef typename Data::Motion Motion;
478+
479+
PINOCCHIO_CHECK_INPUT_ARGUMENT(
480+
jointId < JointIndex(model.njoints)
481+
&& "jointId is larger than the number of joints contained in the model");
482+
483+
Matrix6xLike & dJ = dJ_.const_cast_derived();
484+
::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ);
485+
486+
// Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
487+
switch (rf)
488+
{
489+
case LOCAL: {
490+
const SE3 & oMjoint = data.oMi[jointId];
491+
const Motion & v_joint = data.v[jointId];
492+
const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1;
493+
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
494+
{
495+
typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
496+
typedef const MotionRef<ConstColXprIn> MotionIn;
497+
498+
typedef typename Matrix6xLike::ColXpr ColXprOut;
499+
typedef MotionRef<ColXprOut> MotionOut;
500+
MotionIn v_in(data.J.col(j));
501+
MotionOut v_out(dJ.col(j));
502+
503+
v_out -= v_joint.cross(oMjoint.actInv(v_in));
504+
}
505+
break;
506+
}
507+
case LOCAL_WORLD_ALIGNED: {
508+
const Motion & ov_joint = data.ov[jointId];
509+
const SE3 & oMjoint = data.oMi[jointId];
510+
const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1;
511+
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
512+
{
513+
typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
514+
typedef const MotionRef<ConstColXprIn> MotionIn;
515+
516+
typedef typename Matrix6xLike::ColXpr ColXprOut;
517+
typedef MotionRef<ColXprOut> MotionOut;
518+
MotionIn v_in(data.J.col(j));
519+
MotionOut v_out(dJ.col(j));
520+
521+
v_out.linear() -=
522+
Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation()))
523+
.cross(v_in.angular());
524+
}
525+
break;
526+
}
527+
528+
case WORLD:
529+
default:
530+
break;
531+
}
478532
}
479533
} // namespace impl
480534

‎unittest/frames.cpp‎

Lines changed: 10 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,5 @@
11
//
2-
// Copyright (c) 2016-2020 CNRS INRIA
2+
// Copyright (c) 2016-2024 CNRS INRIA
33
//
44

55
#include "pinocchio/multibody/model.hpp"
@@ -569,14 +569,14 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation)
569569
const Motion & v_ref_local = frame.placement.actInv(data_ref.v[parent_idx]);
570570
const Motion & v_ref = data_ref.oMf[idx].act(v_ref_local);
571571

572-
const SE3 & wMf = SE3(data_ref.oMf[idx].rotation(), SE3::Vector3::Zero());
573-
const Motion & v_ref_local_world_aligned = wMf.act(v_ref_local);
572+
const SE3 wMf = SE3(data_ref.oMf[idx].rotation(), SE3::Vector3::Zero());
573+
const Motion v_ref_local_world_aligned = wMf.act(v_ref_local);
574574
BOOST_CHECK(v_idx.isApprox(v_ref));
575575

576576
Motion a_idx(J * a + dJ * v);
577-
const Motion & a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
578-
const Motion & a_ref = data_ref.oMf[idx].act(a_ref_local);
579-
const Motion & a_ref_local_world_aligned = wMf.act(a_ref_local);
577+
const Motion a_ref_local = frame.placement.actInv(data_ref.a[parent_idx]);
578+
const Motion a_ref = data_ref.oMf[idx].act(a_ref_local);
579+
const Motion a_ref_local_world_aligned = wMf.act(a_ref_local);
580580
BOOST_CHECK(a_idx.isApprox(a_ref));
581581

582582
J.fill(0.);
@@ -594,12 +594,15 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation)
594594
// Regarding to the LOCAL_WORLD_ALIGNED frame
595595
getFrameJacobian(model, data, idx, LOCAL_WORLD_ALIGNED, J);
596596
getFrameJacobianTimeVariation(model, data, idx, LOCAL_WORLD_ALIGNED, dJ);
597+
Data::Motion world_v_frame = data.ov[parent_idx];
598+
world_v_frame.linear().setZero();
597599

598600
v_idx = (Motion::Vector6)(J * v);
599601
BOOST_CHECK(v_idx.isApprox(v_ref_local_world_aligned));
600602

601603
a_idx = (Motion::Vector6)(J * a + dJ * v);
602-
BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned));
604+
BOOST_CHECK(
605+
a_idx.isApprox(world_v_frame.cross(wMf.act(v_ref_local)) + a_ref_local_world_aligned));
603606

604607
// compare to finite differencies
605608
{
@@ -617,7 +620,6 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation)
617620
J_ref_local_world_aligned.fill(0.);
618621
computeJointJacobians(model, data_ref, q);
619622
updateFramePlacements(model, data_ref);
620-
const SE3 & oMf_q = data_ref.oMf[idx];
621623
getFrameJacobian(model, data_ref, idx, WORLD, J_ref_world);
622624
getFrameJacobian(model, data_ref, idx, LOCAL, J_ref_local);
623625
getFrameJacobian(model, data_ref, idx, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
@@ -630,21 +632,11 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation)
630632
J_ref_plus_local_world_aligned.fill(0.);
631633
computeJointJacobians(model, data_ref_plus, q_plus);
632634
updateFramePlacements(model, data_ref_plus);
633-
const SE3 & oMf_q_plus = data_ref_plus.oMf[idx];
634635
getFrameJacobian(model, data_ref_plus, idx, WORLD, J_ref_plus_world);
635636
getFrameJacobian(model, data_ref_plus, idx, LOCAL, J_ref_plus_local);
636637
getFrameJacobian(
637638
model, data_ref_plus, idx, LOCAL_WORLD_ALIGNED, J_ref_plus_local_world_aligned);
638639

639-
// Move J_ref_plus_local to reference frame
640-
J_ref_plus_local = (oMf_q.inverse() * oMf_q_plus).toActionMatrix() * (J_ref_plus_local);
641-
642-
// Move J_ref_plus_local_world_aligned to reference frame
643-
SE3 oMf_translation = SE3::Identity();
644-
oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation();
645-
J_ref_plus_local_world_aligned =
646-
oMf_translation.toActionMatrix() * (J_ref_plus_local_world_aligned);
647-
648640
Data::Matrix6x dJ_ref_world(6, model.nv), dJ_ref_local(6, model.nv),
649641
dJ_ref_local_world_aligned(6, model.nv);
650642
dJ_ref_world.fill(0.);

0 commit comments

Comments
 (0)