Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Fix bug for get{Joint,Frame}JacobianTimeVariation #2466

Merged
merged 7 commits into from
Oct 29, 2024
Merged

Fix bug for get{Joint,Frame}JacobianTimeVariation #2466

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
c4d2e61
algo/jacobians: use const_cast_derived()
jcarpent Oct 28, 2024
d60c5e3
algo/jacobians: fix bug with getJointJacobianTimeVariation
jcarpent Oct 28, 2024
54d001e
test/jacobians: enforce correct testing of getJointJacobianTimeVariation
jcarpent Oct 28, 2024
eb01753
algo/frames: remove call to PINOCCHIO_EIGEN_CONST_CAST
jcarpent Oct 28, 2024
b2c996b
algo/frames: fix bug with getFrameJacobianTimeVariation
jcarpent Oct 28, 2024
6cb88b2
test/frames: fix testing of getFrameJacobianTimeVariation
jcarpent Oct 28, 2024
831df1f
changelog: sync
jcarpent Oct 28, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions CHANGELOG.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading