Verified Commit 5a1cbda5 authored by Justin Carpentier's avatar Justin Carpentier
Browse files

algo/center-of-mass-derivatives: improve efficiency

Number of operations are reduced and it also avoid memory allocations
parent 98335a94
//
// Copyright (c) 2019 CNRS
// Copyright (c) 2019 CNRS INRIA
//
#ifndef __pinocchio_center_of_mass_derivatives_hxx__
......@@ -37,17 +37,20 @@ namespace pinocchio
typedef typename Data::Motion Motion;
const JointIndex & i = jmodel.id();
const JointIndex & parent = model.parents[i];
Matrix3xOut & dvcom_dq = PINOCCHIO_EIGEN_CONST_CAST(Matrix3xOut,vcom_partial_dq);
typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix3x>::Type
dvcom_dqi = jmodel.jointCols(dvcom_dq);
Motion & vpc = data.v[0] = (model.parents[i]>0) ? (data.v[i]-(Motion)jdata.v()) : Motion::Zero();
Motion & vpc = data.v[0] = (parent>0) ? (data.v[i]-(Motion)jdata.v()) : Motion::Zero();
vpc.linear() -= data.vcom[i]; // vpc = v_{parent+c} = [ v_parent+vc; w_parent ]
const Eigen::Matrix<Scalar,6,JointModel::NV,Options> & vxS = vpc.cross(jdata.S());
dvcom_dqi = data.mass[i]*data.oMi[i].rotation()
//*( vxS.template topRows<3>() - cross( data.com[i], vxS.template bottomRows<3>()) );
typename SizeDepType<JointModel::NV>::template ColsReturn<typename Data::Matrix6>::Type
vxS = data.M6tmp.template leftCols<JointModel::NV>(jmodel.nv());
vxS = vpc.cross(jdata.S());
dvcom_dqi.noalias() = (data.mass[i]/data.mass[0])*data.oMi[i].rotation()
*( vxS.template middleRows<3>(Motion::LINEAR) - cross( data.com[i], vxS.template middleRows<3>(Motion::ANGULAR)) );
}
};
......@@ -74,7 +77,7 @@ namespace pinocchio
Pass1::run(model.joints[i],data.joints[i],
typename Pass1::ArgsType(model,data,dvcom_dq));
}
dvcom_dq /= data.mass[0];
data.v[0].setZero();
}
......
......@@ -67,6 +67,7 @@ namespace pinocchio
/// \brief The 3d jacobian type (temporary)
typedef Eigen::Matrix<Scalar,3,Eigen::Dynamic,Options> Matrix3x;
typedef Eigen::Matrix<Scalar,6,6,Options> Matrix6;
typedef Eigen::Matrix<Scalar,6,6,Eigen::RowMajor | Options> RowMatrix6;
typedef Eigen::Matrix<Scalar,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor | Options> RowMatrixXs;
......@@ -181,6 +182,7 @@ namespace pinocchio
typename Inertia::Matrix6 Itmp;
/// \brief Temporary for derivative algorithms
Matrix6 M6tmp;
RowMatrix6 M6tmpR;
RowMatrix6 M6tmpR2;
......
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