Unverified Commit 5f3e8abb authored by Justin Carpentier's avatar Justin Carpentier Committed by GitHub
Browse files

Merge pull request #1220 from jcarpent/devel

Handle LOCAL_WOLRD_ALIGNED in frames algorithms
parents 7ff72445 8cb61e12
......@@ -2,8 +2,8 @@
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_frames_hpp__
#define __pinocchio_frames_hpp__
#ifndef __pinocchio_algorithm_frames_hpp__
#define __pinocchio_algorithm_frames_hpp__
#include "pinocchio/multibody/model.hpp"
#include "pinocchio/multibody/data.hpp"
......@@ -159,11 +159,11 @@ namespace pinocchio
* @param[in] rf Reference frame in which the Jacobian is expressed.
* @param[out] J The Jacobian of the Frame expressed in the coordinates Frame.
*
* @warning The function pinocchio::computeJointJacobians and pinocchio::framesForwardKinematics should have been called first.
* @warning The function pinocchio::computeJointJacobians should have been called first.
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & J);
......@@ -206,6 +206,7 @@ namespace pinocchio
/// \param[in] data The data structure of the rigid body system.
/// \param[in] q The joint configuration vector (dim model.nq).
/// \param[in] frameId The id of the Frame refering to model.frames[frameId].
///
/// \param[out] J A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill J with zero elements, e.g. J.setZero().
///
/// \return The Jacobian of the specific Frame expressed in the LOCAL frame coordinate system (matrix 6 x model.nv).
......@@ -241,7 +242,8 @@ namespace pinocchio
}
///
/// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the world frame (rf = WORLD) or in the local frame (rf = LOCAL).
/// \brief Computes the Jacobian time variation of a specific frame (given by frame_id) expressed either in the WORLD frame (rf = WORLD) or in the LOCAL frame (rf = LOCAL) or in the LOCAL_WORLD_ALIGNED frame (rf = LOCAL_WORLD_ALIGNED).
///
/// \note This jacobian is extracted from data.dJ. You have to run pinocchio::computeJointJacobiansTimeVariation before calling it.
///
/// \tparam JointCollection Collection of Joint types.
......@@ -251,11 +253,12 @@ namespace pinocchio
/// \param[in] data The data structure of the rigid body system.
/// \param[in] frameId The index of the frame.
/// \param[in] rf Reference frame in which the Jacobian is expressed.
///
/// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.).
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
void getFrameJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ);
......@@ -265,4 +268,4 @@ namespace pinocchio
/* --- Details -------------------------------------------------------------------- */
#include "pinocchio/algorithm/frames.hxx"
#endif // ifndef __pinocchio_frames_hpp__
#endif // ifndef __pinocchio_algorithm_frames_hpp__
......@@ -2,8 +2,8 @@
// Copyright (c) 2015-2020 CNRS INRIA
//
#ifndef __pinocchio_frames_hxx__
#define __pinocchio_frames_hxx__
#ifndef __pinocchio_algorithm_frames_hxx__
#define __pinocchio_algorithm_frames_hxx__
#include "pinocchio/algorithm/kinematics.hpp"
#include "pinocchio/algorithm/jacobian.hpp"
......@@ -139,7 +139,7 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
inline void getFrameJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & J)
......@@ -157,34 +157,11 @@ namespace pinocchio
const Frame & frame = model.frames[frame_id];
const JointIndex & joint_id = frame.parent;
if(rf == WORLD)
{
getJointJacobian(model,data,joint_id,WORLD,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J));
return;
}
else if(rf == LOCAL || rf == LOCAL_WORLD_ALIGNED)
{
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
const typename Data::SE3 & oMframe = data.oMf[frame_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::ConstColXpr ConstColXprIn;
const MotionRef<ConstColXprIn> v_in(data.J.col(j));
typedef typename Matrix6xLike::ColXpr ColXprOut;
MotionRef<ColXprOut> v_out(J_.col(j));
if (rf == LOCAL)
v_out = oMframe.actInv(v_in);
else
{
v_out = v_in;
v_out.linear() -= oMframe.translation().cross(v_in.angular());
}
}
}
Matrix6xLike & J_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,J);
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[joint_id] * frame.placement;
details::translateJointJacobian(model,data,joint_id,rf,oMframe,data.J,J_);
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename ConfigVectorType, typename Matrix6xLike>
......@@ -263,48 +240,27 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLike>
void getFrameJacobianTimeVariation(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ)
{
PINOCCHIO_CHECK_INPUT_ARGUMENT( dJ.rows() == data.dJ.rows() );
PINOCCHIO_CHECK_INPUT_ARGUMENT( dJ.cols() == data.dJ.cols() );
assert(model.check(data) && "data is not consistent with model.");
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Model::Frame Frame;
typedef typename Model::SE3 SE3;
const Frame & frame = model.frames[frame_id];
const typename Model::JointIndex & joint_id = frame.parent;
if(rf == WORLD)
{
getJointJacobianTimeVariation(model,data,joint_id,WORLD,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
return;
}
const JointIndex & joint_id = frame.parent;
if(rf == LOCAL)
{
Matrix6xLike & dJ_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ);
const SE3 & oMframe = data.oMf[frame_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::ConstColXpr ConstColXprIn;
const MotionRef<ConstColXprIn> v_in(data.dJ.col(j));
typedef typename Matrix6xLike::ColXpr ColXprOut;
MotionRef<ColXprOut> v_out(dJ_.col(j));
v_out = oMframe.actInv(v_in);
}
return;
}
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[joint_id] * frame.placement;
details::translateJointJacobian(model,data,joint_id,rf,oMframe,
data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
}
} // namespace pinocchio
#endif // ifndef __pinocchio_frames_hxx__
#endif // ifndef __pinocchio_algorithm_frames_hxx__
......@@ -123,19 +123,20 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex jointId,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
const ReferenceFrame rf,
const SE3Tpl<Scalar,Options> & placement,
const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
{
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jin.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.rows() == 6);
PINOCCHIO_CHECK_INPUT_ARGUMENT(Jout.cols() == model.nv);
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
Matrix6xLikeOut & Jout_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLikeOut,Jout);
typedef typename Matrix6xLikeIn::ConstColXpr ConstColXprIn;
......@@ -144,7 +145,7 @@ namespace pinocchio
typedef typename Matrix6xLikeOut::ColXpr ColXprOut;
typedef MotionRef<ColXprOut> MotionOut;
int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1;
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
switch(rf)
{
case WORLD:
......@@ -160,26 +161,24 @@ namespace pinocchio
}
case LOCAL_WORLD_ALIGNED:
{
const typename Data::SE3 & oMjoint = data.oMi[jointId];
for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
{
MotionIn v_in(Jin.col(j));
MotionOut v_out(Jout_.col(j));
v_out = v_in;
v_out.linear() -= oMjoint.translation().cross(v_in.angular());
v_out.linear() -= placement.translation().cross(v_in.angular());
}
break;
}
case LOCAL:
{
const typename Data::SE3 & oMjoint = data.oMi[jointId];
for(Eigen::DenseIndex j=colRef;j>=0;j=data.parents_fromRow[(size_t)j])
{
MotionIn v_in(Jin.col(j));
MotionOut v_out(Jout_.col(j));
v_out = oMjoint.actInv(v_in);
v_out = placement.actInv(v_in);
}
break;
}
......@@ -188,7 +187,21 @@ namespace pinocchio
break;
}
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xLikeIn, typename Matrix6xLikeOut>
void translateJointJacobian(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex joint_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLikeIn> & Jin,
const Eigen::MatrixBase<Matrix6xLikeOut> & Jout)
{
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
const typename Data::SE3 & oMjoint = data.oMi[joint_id];
translateJointJacobian(model,data,joint_id,rf,oMjoint,Jin,Jout);
}
} // namespace details
/* Return the jacobian of the output frame attached to joint <jointId> in the
world frame or in the local frame depending on the template argument. The
......@@ -354,8 +367,6 @@ namespace pinocchio
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xLike> & dJ)
{
assert(model.check(data) && "data is not consistent with model.");
details::translateJointJacobian(model,data,jointId,rf,
data.dJ,PINOCCHIO_EIGEN_CONST_CAST(Matrix6xLike,dJ));
}
......
......@@ -488,26 +488,29 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
const Frame & frame = model.frames[idx];
BOOST_CHECK(frame.placement.isApprox_impl(framePlacement));
Data::Matrix6x J(6,model.nv); J.fill(0.);
Data::Matrix6x dJ(6,model.nv); dJ.fill(0.);
// Regarding to the world origin
// Regarding to the WORLD origin
getFrameJacobian(model,data,idx,WORLD,J);
getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ);
Motion v_idx(J*v);
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);
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);
BOOST_CHECK(a_idx.isApprox(a_ref));
J.fill(0.); dJ.fill(0.);
// Regarding to the local frame
// Regarding to the LOCAL frame
getFrameJacobian(model,data,idx,LOCAL,J);
getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ);
......@@ -517,6 +520,16 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
a_idx = (Motion::Vector6)(J*a + dJ*v);
BOOST_CHECK(a_idx.isApprox(a_ref_local));
// Regarding to the LOCAL_WORLD_ALIGNED frame
getFrameJacobian(model,data,idx,LOCAL_WORLD_ALIGNED,J);
getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ);
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));
// compare to finite differencies
{
Data data_ref(model), data_ref_plus(model);
......@@ -526,42 +539,52 @@ BOOST_AUTO_TEST_CASE ( test_frame_jacobian_time_variation )
q_plus = integrate(model,q,alpha*v);
//data_ref
Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv);
J_ref_world.fill(0.); J_ref_local.fill(0.);
Data::Matrix6x J_ref_world(6,model.nv), J_ref_local(6,model.nv), J_ref_local_world_aligned(6,model.nv);
J_ref_world.fill(0.); J_ref_local.fill(0.); 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);
//data_ref_plus
Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv);
J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.);
Data::Matrix6x J_ref_plus_world(6,model.nv), J_ref_plus_local(6,model.nv), J_ref_plus_local_world_aligned(6,model.nv);
J_ref_plus_world.fill(0.); J_ref_plus_local.fill(0.); J_ref_plus_local_world_aligned.fill(0.);
computeJointJacobians(model,data_ref_plus,q_plus);
updateFramePlacements(model,data_ref_plus);
const SE3 & oMf_qplus = data_ref_plus.oMf[idx];
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_qplus).toActionMatrix()*(J_ref_plus_local);
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_world.fill(0.); dJ_ref_local.fill(0.);
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.); dJ_ref_local.fill(0.); dJ_ref_local_world_aligned.fill(0.);
dJ_ref_world = (J_ref_plus_world - J_ref_world)/alpha;
dJ_ref_local = (J_ref_plus_local - J_ref_local)/alpha;
dJ_ref_local_world_aligned = (J_ref_plus_local_world_aligned - J_ref_local_world_aligned)/alpha;
//data
computeJointJacobiansTimeVariation(model,data,q,v);
forwardKinematics(model,data,q,v);
updateFramePlacements(model,data);
Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv);
dJ_world.fill(0.); dJ_local.fill(0.);
Data::Matrix6x dJ_world(6,model.nv), dJ_local(6,model.nv), dJ_local_world_aligned(6,model.nv);
dJ_world.fill(0.); dJ_local.fill(0.); dJ_local_world_aligned.fill(0.);
getFrameJacobianTimeVariation(model,data,idx,WORLD,dJ_world);
getFrameJacobianTimeVariation(model,data,idx,LOCAL,dJ_local);
getFrameJacobianTimeVariation(model,data,idx,LOCAL_WORLD_ALIGNED,dJ_local_world_aligned);
BOOST_CHECK(dJ_world.isApprox(dJ_ref_world,sqrt(alpha)));
BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));
BOOST_CHECK(dJ_local.isApprox(dJ_ref_local,sqrt(alpha)));
BOOST_CHECK(dJ_local_world_aligned.isApprox(dJ_ref_local_world_aligned,sqrt(alpha)));
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment