Verified Commit e3fa2be2 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/frames-der: add getFrameAccelerationDerivatives

parent c57f79a3
......@@ -34,6 +34,73 @@ namespace pinocchio
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv);
/**
* @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a.
* You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.
* It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da.
*
* @tparam JointCollection Collection of Joint types.
* @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
* @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
* @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
* @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[in] rf Reference frame in which the velocity is expressed.
* @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
void
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da);
/**
* @brief Computes the partial derivatives of the frame acceleration quantity with respect to q, v and a.
* You must first call pinocchio::computeForwardKinematicsDerivatives to compute all the required quantities.
* It is important to notice that a direct outcome (for free) of this algo is v_partial_dq and v_partial_dv which is equal to a_partial_da.
*
* @tparam JointCollection Collection of Joint types.
* @tparam Matrix6xOut1 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint configuration vector.
* @tparam Matrix6xOut2 Matrix6x containing the partial derivatives of the frame spatial velocity with respect to the joint velocity vector.
* @tparam Matrix6xOut3 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint configuration vector.
* @tparam Matrix6xOut4 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint velocity vector.
* @tparam Matrix6xOut5 Matrix6x containing the partial derivatives of the frame spatial acceleration with respect to the joint acceleration vector.
*
* @param[in] model The kinematic model
* @param[in] data Data associated to model
* @param[in] frame_id Id of the operational Frame
* @param[in] rf Reference frame in which the velocity is expressed.
* @param[out] v_partial_dq Partial derivative of the joint spatial velocity w.r.t. \f$ q \f$.
* @param[out] v_partial_dv Partial derivative of the joint spatial velociy w.r.t. \f$ \dot{q} \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ q \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \dot{q} \f$.
* @param[out] a_partial_dq Partial derivative of the joint spatial acceleration w.r.t. \f$ \ddot{q} \f$.
*
*/
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
void
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da);
}
#include "pinocchio/algorithm/frames-derivatives.hxx"
......
......@@ -86,6 +86,123 @@ namespace pinocchio
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
void
getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_da)
{
typedef ModelTpl<Scalar,Options,JointCollectionTpl> Model;
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef typename Data::Matrix6x Matrix6x;
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut1,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut2,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut3,Matrix6x);
EIGEN_STATIC_ASSERT_SAME_MATRIX_SIZE(Matrix6xOut4,Matrix6x);
PINOCCHIO_CHECK_INPUT_ARGUMENT(v_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_dq.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_dv.cols() == model.nv);
PINOCCHIO_CHECK_INPUT_ARGUMENT(a_partial_da.cols() == model.nv);
assert(model.check(data) && "data is not consistent with model.");
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame_id <= model.frames.size(),"frame_id is larger than the number of frames");
typedef typename Model::Frame Frame;
typedef typename Data::Motion Motion;
const Frame & frame = model.frames[frame_id];
const JointIndex joint_id = frame.parent;
Matrix6xOut1 & v_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq);
Matrix6xOut2 & a_partial_dq_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,a_partial_dq);
Matrix6xOut3 & a_partial_dv_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dv);
Matrix6xOut4 & a_partial_da_ = PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da);
getJointAccelerationDerivatives(model,data,joint_id,rf,
v_partial_dq_,a_partial_dq_,a_partial_dv_,a_partial_da_);
// Update frame placement
typename Data::SE3 & oMframe = data.oMf[frame_id];
oMframe = data.oMi[joint_id] * frame.placement;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1;
typedef MotionRef<ColsBlockOut1> MotionOut1;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut2>::Type ColsBlockOut2;
typedef MotionRef<ColsBlockOut2> MotionOut2;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut3>::Type ColsBlockOut3;
typedef MotionRef<ColsBlockOut3> MotionOut3;
typedef typename SizeDepType<1>::template ColsReturn<Matrix6xOut4>::Type ColsBlockOut4;
typedef MotionRef<ColsBlockOut4> MotionOut4;
Motion v_tmp;
const typename SE3::Vector3 trans = data.oMi[joint_id].rotation() * frame.placement.translation();
const int colRef = nv(model.joints[joint_id])+idx_v(model.joints[joint_id])-1;
switch (rf)
{
case WORLD:
// Do nothing
break;
case LOCAL_WORLD_ALIGNED:
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id])
{
MotionOut1 m1(v_partial_dq_.col(col_id));
m1.linear() -= trans.cross(m1.angular());
MotionOut2 m2(a_partial_dq_.col(col_id));
m2.linear() -= trans.cross(m2.angular());
MotionOut3 m3(a_partial_dv_.col(col_id));
m3.linear() -= trans.cross(m3.angular());
MotionOut4 m4(a_partial_da_.col(col_id));
m4.linear() -= trans.cross(m4.angular());
}
break;
case LOCAL:
for(Eigen::DenseIndex col_id=colRef;col_id>=0;col_id=data.parents_fromRow[(size_t)col_id])
{
v_tmp = v_partial_dq_.col(col_id);
MotionOut1(v_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = a_partial_dq_.col(col_id);
MotionOut2(a_partial_dq_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = a_partial_dv_.col(col_id);
MotionOut3(a_partial_dv_.col(col_id)) = frame.placement.actInv(v_tmp);
v_tmp = a_partial_da_.col(col_id);
MotionOut4(a_partial_da_.col(col_id)) = frame.placement.actInv(v_tmp);
}
break;
default:
break;
}
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
inline void getFrameAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::FrameIndex frame_id,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
const Eigen::MatrixBase<Matrix6xOut2> & v_partial_dv,
const Eigen::MatrixBase<Matrix6xOut3> & a_partial_dq,
const Eigen::MatrixBase<Matrix6xOut4> & a_partial_dv,
const Eigen::MatrixBase<Matrix6xOut5> & a_partial_da)
{
getFrameAccelerationDerivatives(model,data,
frame_id,rf,
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut3,a_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_dv),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut5,a_partial_da));
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv) = a_partial_da;
}
}
#endif // ifndef __pinocchio_algorithm_frames_derivatives_hxx__
Markdown is supported
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