Verified Commit 45d604e4 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo: small fix

parent 809dae10
Pipeline #11893 passed with stage
in 135 minutes and 32 seconds
...@@ -46,7 +46,7 @@ namespace pinocchio ...@@ -46,7 +46,7 @@ namespace pinocchio
const SE3Tpl<Scalar,Options> & placement) const SE3Tpl<Scalar,Options> & placement)
{ {
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType; typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
ReturnType res(ReturnType::Zero(6,(model.joints-1)*6)); ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
computeJointKinematicRegressor(model,data,joint_id,rf,placement,res); computeJointKinematicRegressor(model,data,joint_id,rf,placement,res);
...@@ -77,13 +77,14 @@ namespace pinocchio ...@@ -77,13 +77,14 @@ namespace pinocchio
/// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
/// ///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
const DataTpl<Scalar,Options,JointCollectionTpl> & data, computeJointKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const JointIndex joint_id, const DataTpl<Scalar,Options,JointCollectionTpl> & data,
const ReferenceFrame rf) const JointIndex joint_id,
const ReferenceFrame rf)
{ {
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType; typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
ReturnType res(ReturnType::Zero(6,(model.joints-1)*6)); ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
computeJointKinematicRegressor(model,data,joint_id,rf,res); computeJointKinematicRegressor(model,data,joint_id,rf,res);
...@@ -114,13 +115,14 @@ namespace pinocchio ...@@ -114,13 +115,14 @@ namespace pinocchio
/// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD). /// \param[in] rf Reference frame in which the result is expressed (LOCAL, LOCAL_WORLD_ALIGNED or WORLD).
/// ///
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
void computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model, typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x
DataTpl<Scalar,Options,JointCollectionTpl> & data, computeFrameKinematicRegressor(const ModelTpl<Scalar,Options,JointCollectionTpl> & model,
const FrameIndex frame_id, DataTpl<Scalar,Options,JointCollectionTpl> & data,
const ReferenceFrame rf) const FrameIndex frame_id,
const ReferenceFrame rf)
{ {
typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType; typedef typename DataTpl<Scalar,Options,JointCollectionTpl>::Matrix6x ReturnType;
ReturnType res(ReturnType::Zero(6,(model.joints-1)*6)); ReturnType res(ReturnType::Zero(6,(model.njoints-1)*6));
computeFrameKinematicRegressor(model,data,frame_id,rf,res); computeFrameKinematicRegressor(model,data,frame_id,rf,res);
......
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