Skip to content

GitLab

Verified Commit 470d98b1 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/kin-der: add constness and use temporary

parent c6bb4f38
Hide whitespace changes
Inline Side-by-side
src/algorithm/kinematics-derivatives.hpp
View file @ 470d98b1
......@@ -54,7 +54,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
inline void getJointVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -83,7 +83,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -116,7 +116,7 @@ namespace pinocchio
///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......
src/algorithm/kinematics-derivatives.hxx
View file @ 470d98b1
......@@ -110,7 +110,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
const Data &,
const typename Model::JointIndex &,
const ReferenceFrame &,
Matrix6xOut1 &,
......@@ -120,7 +120,7 @@ namespace pinocchio
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
const Model & model,
Data & data,
const Data & data,
const typename Model::JointIndex & jointId,
const ReferenceFrame & rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -132,12 +132,12 @@ namespace pinocchio
const JointIndex & i = jmodel.id();
const JointIndex & parent = model.parents[i];
Motion & vtmp = data.ov[0]; // Temporary variable
Motion vtmp; // Temporary variable
const SE3 & oMlast = data.oMi[jointId];
const Motion & vlast = data.ov[jointId];
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::ConstType ColsBlock;
ColsBlock Jcols = jmodel.jointCols(data.J);
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<Matrix6xOut1>::Type ColsBlockOut1;
......@@ -198,7 +198,7 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2>
inline void getJointVelocityDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -223,9 +223,6 @@ namespace pinocchio
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut1,v_partial_dq),
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut2,v_partial_dv)));
}
// Set back ov[0] to a zero value
data.ov[0].setZero();
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
......@@ -236,7 +233,7 @@ namespace pinocchio
typedef DataTpl<Scalar,Options,JointCollectionTpl> Data;
typedef boost::fusion::vector<const Model &,
Data &,
const Data &,
const typename Model::JointIndex &,
const ReferenceFrame &,
Matrix6xOut1 &,
......@@ -248,7 +245,7 @@ namespace pinocchio
template<typename JointModel>
static void algo(const JointModelBase<JointModel> & jmodel,
const Model & model,
Data & data,
const Data & data,
const typename Model::JointIndex & jointId,
const ReferenceFrame & rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -262,14 +259,14 @@ namespace pinocchio
const JointIndex & i = jmodel.id();
const JointIndex & parent = model.parents[i];
Motion & vtmp = data.ov[0]; // Temporary variable
Motion & atmp = data.oa[0]; // Temporary variable
Motion vtmp; // Temporary variable
Motion atmp; // Temporary variable
const SE3 & oMlast = data.oMi[jointId];
const Motion & vlast = data.ov[jointId];
const Motion & alast = data.oa[jointId];
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::Type ColsBlock;
typedef typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6x>::ConstType ColsBlock;
ColsBlock dJcols = jmodel.jointCols(data.dJ);
ColsBlock Jcols = jmodel.jointCols(data.J);
......@@ -391,7 +388,7 @@ namespace pinocchio
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4>
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......@@ -426,15 +423,11 @@ namespace pinocchio
PINOCCHIO_EIGEN_CONST_CAST(Matrix6xOut4,a_partial_da)));
}
// Set Zero to temporary spatial variables
data.ov[0].setZero();
data.oa[0].setZero();
}
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl, typename Matrix6xOut1, typename Matrix6xOut2, typename Matrix6xOut3, typename Matrix6xOut4, typename Matrix6xOut5>
inline void getJointAccelerationDerivatives(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
DataTpl<Scalar,Options,JointCollectionTpl> & data,
const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const Model::JointIndex jointId,
const ReferenceFrame rf,
const Eigen::MatrixBase<Matrix6xOut1> & v_partial_dq,
......
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