Skip to content
Snippets Groups Projects
Verified Commit 334fdfde authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/frames: handle LOCAL_WORLD_ALIGNED setup 

Largely simplifies the code
parent 40d22d22
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/algorithm/frames.hpp
+ 7
− 4
View file @ 334fdfde
......@@ -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);
......
src/algorithm/frames.hxx
+ 14
− 58
View file @ 334fdfde
......@@ -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,46 +240,25 @@ 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
......
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