Skip to content

Commit

Permalink
Merge pull request #2466 from jcarpent/topic/jacobians
Browse files Browse the repository at this point in the history 
Fix bug for get{Joint,Frame}JacobianTimeVariation
  • Loading branch information
@jorisv
jorisv authored Oct 29, 2024
2 parents 68166cc + 831df1f commit 4e13260
Show file tree
Hide file tree
Showing 5 changed files with 171 additions and 46 deletions.
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
- Fix `pinocchio-test-cpp-mjcf` unittest with Boost 1.86 ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
- Fix `pinocchio-test-cpp-constraint-variants` uninitialized values ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
- Fix mixing library symbols between Pinocchio scalar bindings ([#2459](https://github.com/stack-of-tasks/pinocchio/pull/2459))
- Fix bug for get{Joint,Frame}JacobianTimeVariation ([#2466](https://github.com/stack-of-tasks/pinocchio/pull/2466))

### Changed

Expand Down
69 changes: 59 additions & 10 deletions include/pinocchio/algorithm/frames.hxx
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Copyright (c) 2015-2021 CNRS INRIA
// Copyright (c) 2015-2024 CNRS INRIA
//

#ifndef __pinocchio_algorithm_frames_hxx__
Expand Down Expand Up @@ -171,8 +171,7 @@ namespace pinocchio

const typename Data::SE3 oMframe = data.oMi[joint_id] * placement;
details::translateJointJacobian(
model, data, joint_id, reference_frame, oMframe, data.J,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
model, data, joint_id, reference_frame, oMframe, data.J, J.const_cast_derived());
}

template<
Expand Down Expand Up @@ -220,16 +219,15 @@ namespace pinocchio
JointIndex parent = joint_support[k];
Pass::run(
model.joints[parent], data.joints[parent],
typename Pass::ArgsType(
model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
}

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

Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
Matrix6xLike & J_ = J.const_cast_derived();

const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
Expand All @@ -252,8 +250,7 @@ namespace pinocchio
{
Pass::run(
model.joints[i], data.joints[i],
typename Pass::ArgsType(
model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
}
break;
}
Expand All @@ -273,9 +270,11 @@ namespace pinocchio
DataTpl<Scalar, Options, JointCollectionTpl> & data,
const FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ)
const Eigen::MatrixBase<Matrix6xLike> & dJ_)
{
assert(model.check(data) && "data is not consistent with model.");

Matrix6xLike & dJ = dJ_.const_cast_derived();
PINOCCHIO_CHECK_ARGUMENT_SIZE(
dJ.cols(), model.nv,
"The numbers of columns in the Jacobian matrix does not math the "
Expand All @@ -284,6 +283,9 @@ namespace pinocchio
typedef ModelTpl<Scalar, Options, JointCollectionTpl> Model;
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
typedef typename Model::Frame Frame;
typedef typename Data::SE3 SE3;
typedef typename SE3::Vector3 Vector3;
typedef typename Data::Motion Motion;

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

details::translateJointJacobian(
model, data, joint_id, rf, oMframe, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ));
model, data, joint_id, rf, oMframe, data.dJ, dJ.const_cast_derived());

// Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
switch (rf)
{
case LOCAL: {
const Motion & v_joint = data.v[joint_id];
const Motion v_frame = frame.placement.actInv(v_joint);

const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
{
typedef typename Data::Matrix6x::ColXpr ColXprIn;
typedef const MotionRef<ColXprIn> MotionIn;

typedef typename Matrix6xLike::ColXpr ColXprOut;
typedef MotionRef<ColXprOut> MotionOut;
MotionIn v_in(data.J.col(j));
MotionOut v_out(dJ.col(j));

v_out -= v_frame.cross(oMframe.actInv(v_in));
}
break;
}
case LOCAL_WORLD_ALIGNED: {
const Motion & ov_joint = data.ov[joint_id];
const int colRef = nv(model.joints[joint_id]) + idx_v(model.joints[joint_id]) - 1;
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
{
typedef typename Data::Matrix6x::ColXpr ColXprIn;
typedef const MotionRef<ColXprIn> MotionIn;

typedef typename Matrix6xLike::ColXpr ColXprOut;
typedef MotionRef<ColXprOut> MotionOut;
MotionIn v_in(data.J.col(j));
MotionOut v_out(dJ.col(j));

v_out.linear() -=
Vector3(ov_joint.linear() + ov_joint.angular().cross(oMframe.translation()))
.cross(v_in.angular());
}
break;
}

case WORLD:
default:
break;
}
}

template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
Expand Down
80 changes: 67 additions & 13 deletions include/pinocchio/algorithm/jacobian.hxx
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Copyright (c) 2015-2020 CNRS INRIA
// Copyright (c) 2015-2024 CNRS INRIA
//

#ifndef __pinocchio_algorithm_jacobian_hxx__
Expand Down Expand Up @@ -56,7 +56,7 @@ namespace pinocchio
else
data.oMi[i] = data.liMi[i];

Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
Matrix6xLike & J_ = J.const_cast_derived();
jmodel.jointCols(J_) = data.oMi[i].act(jdata.S());
}
};
Expand Down Expand Up @@ -88,7 +88,7 @@ namespace pinocchio
{
Pass::run(
model.joints[i], data.joints[i],
ArgsType(model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6x, data.J)));
ArgsType(model, data, q.derived(), data.J.const_cast_derived()));
}

return data.J;
Expand Down Expand Up @@ -148,7 +148,7 @@ namespace pinocchio
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jin.cols(), Jout.cols());
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);

Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout);
Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();

typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
typedef const MotionRef<ConstColXprIn> MotionIn;
Expand Down Expand Up @@ -189,7 +189,7 @@ namespace pinocchio
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.rows(), 6);
PINOCCHIO_CHECK_ARGUMENT_SIZE(Jout.cols(), model.nv);

Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut, Jout);
Matrix6xLikeOut & Jout_ = Jout.const_cast_derived();

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

::pinocchio::details::translateJointJacobian(
model, data, joint_id, reference_frame, data.J,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J));
model, data, joint_id, reference_frame, data.J, J.const_cast_derived());
}

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

Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J);
Matrix6xLike & J_ = J.const_cast_derived();
jmodel.jointCols(J_) = data.iMf[i].actInv(jdata.S());
}
};
Expand Down Expand Up @@ -352,8 +351,7 @@ namespace pinocchio
{
Pass::run(
model.joints[i], data.joints[i],
typename Pass::ArgsType(
model, data, q.derived(), PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, J)));
typename Pass::ArgsType(model, data, q.derived(), J.const_cast_derived()));
}
}

Expand Down Expand Up @@ -471,10 +469,66 @@ namespace pinocchio
const DataTpl<Scalar, Options, JointCollectionTpl> & data,
const JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ)
const Eigen::MatrixBase<Matrix6xLike> & dJ_)
{
::pinocchio::details::translateJointJacobian(
model, data, jointId, rf, data.dJ, PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike, dJ));
typedef DataTpl<Scalar, Options, JointCollectionTpl> Data;
typedef typename Data::SE3 SE3;
typedef typename SE3::Vector3 Vector3;
typedef typename Data::Motion Motion;

PINOCCHIO_CHECK_INPUT_ARGUMENT(
jointId < JointIndex(model.njoints)
&& "jointId is larger than the number of joints contained in the model");

Matrix6xLike & dJ = dJ_.const_cast_derived();
::pinocchio::details::translateJointJacobian(model, data, jointId, rf, data.dJ, dJ);

// Add contribution for LOCAL and LOCAL_WORLD_ALIGNED
switch (rf)
{
case LOCAL: {
const SE3 & oMjoint = data.oMi[jointId];
const Motion & v_joint = data.v[jointId];
const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1;
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
{
typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
typedef const MotionRef<ConstColXprIn> MotionIn;

typedef typename Matrix6xLike::ColXpr ColXprOut;
typedef MotionRef<ColXprOut> MotionOut;
MotionIn v_in(data.J.col(j));
MotionOut v_out(dJ.col(j));

v_out -= v_joint.cross(oMjoint.actInv(v_in));
}
break;
}
case LOCAL_WORLD_ALIGNED: {
const Motion & ov_joint = data.ov[jointId];
const SE3 & oMjoint = data.oMi[jointId];
const int colRef = nv(model.joints[jointId]) + idx_v(model.joints[jointId]) - 1;
for (Eigen::DenseIndex j = colRef; j >= 0; j = data.parents_fromRow[(size_t)j])
{
typedef typename Data::Matrix6x::ConstColXpr ConstColXprIn;
typedef const MotionRef<ConstColXprIn> MotionIn;

typedef typename Matrix6xLike::ColXpr ColXprOut;
typedef MotionRef<ColXprOut> MotionOut;
MotionIn v_in(data.J.col(j));
MotionOut v_out(dJ.col(j));

v_out.linear() -=
Vector3(ov_joint.linear() + ov_joint.angular().cross(oMjoint.translation()))
.cross(v_in.angular());
}
break;
}

case WORLD:
default:
break;
}
}
} // namespace impl

Expand Down
28 changes: 10 additions & 18 deletions unittest/frames.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
//
// Copyright (c) 2016-2020 CNRS INRIA
// Copyright (c) 2016-2024 CNRS INRIA
//

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

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

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

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

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

a_idx = (Motion::Vector6)(J * a + dJ * v);
BOOST_CHECK(a_idx.isApprox(a_ref_local_world_aligned));
BOOST_CHECK(
a_idx.isApprox(world_v_frame.cross(wMf.act(v_ref_local)) + a_ref_local_world_aligned));

// compare to finite differencies
{
Expand All @@ -617,7 +620,6 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation)
J_ref_local_world_aligned.fill(0.);
computeJointJacobians(model, data_ref, q);
updateFramePlacements(model, data_ref);
const SE3 & oMf_q = data_ref.oMf[idx];
getFrameJacobian(model, data_ref, idx, WORLD, J_ref_world);
getFrameJacobian(model, data_ref, idx, LOCAL, J_ref_local);
getFrameJacobian(model, data_ref, idx, LOCAL_WORLD_ALIGNED, J_ref_local_world_aligned);
Expand All @@ -630,21 +632,11 @@ BOOST_AUTO_TEST_CASE(test_frame_jacobian_time_variation)
J_ref_plus_local_world_aligned.fill(0.);
computeJointJacobians(model, data_ref_plus, q_plus);
updateFramePlacements(model, data_ref_plus);
const SE3 & oMf_q_plus = data_ref_plus.oMf[idx];
getFrameJacobian(model, data_ref_plus, idx, WORLD, J_ref_plus_world);
getFrameJacobian(model, data_ref_plus, idx, LOCAL, J_ref_plus_local);
getFrameJacobian(
model, data_ref_plus, idx, LOCAL_WORLD_ALIGNED, J_ref_plus_local_world_aligned);

// Move J_ref_plus_local to reference frame
J_ref_plus_local = (oMf_q.inverse() * oMf_q_plus).toActionMatrix() * (J_ref_plus_local);

// Move J_ref_plus_local_world_aligned to reference frame
SE3 oMf_translation = SE3::Identity();
oMf_translation.translation() = oMf_q_plus.translation() - oMf_q.translation();
J_ref_plus_local_world_aligned =
oMf_translation.toActionMatrix() * (J_ref_plus_local_world_aligned);

Data::Matrix6x dJ_ref_world(6, model.nv), dJ_ref_local(6, model.nv),
dJ_ref_local_world_aligned(6, model.nv);
dJ_ref_world.fill(0.);
Expand Down
Loading

0 comments on commit 4e13260

Please sign in to comment.