Commit c5561eff by Gabriele Buondonno

### [regressor] Add data.bodyRegressor and avoid allocation

parent a0ed3cfc
 ... ... @@ -63,6 +63,20 @@ namespace pinocchio } } /// /// \brief Computes the regressor for the dynamic parameters of a single rigid body. /// /// The result is such that /// \f$I a + v \times I v = bodyRegressor(v,a) * I.toDynamicParameters() \f$ /// /// \param[in] v Velocity of the rigid body /// \param[in] a Acceleration of the rigid body /// \param[out] regressor The resulting regressor of the body. /// template inline void bodyRegressor(const MotionDense & v, const MotionDense & a, const Eigen::MatrixBase & regressor); /// /// \brief Computes the regressor for the dynamic parameters of a single rigid body. /// ... ... @@ -79,7 +93,8 @@ namespace pinocchio bodyRegressor(const MotionDense & v, const MotionDense & a); /// /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given joint. /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given joint, /// puts the result in data.bodyRegressor and returns it. /// /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects. /// ... ... @@ -94,13 +109,14 @@ namespace pinocchio /// \return The regressor of the body. /// template class JointCollectionTpl> inline Eigen::Matrix inline typename DataTpl::BodyRegressorType & jointBodyRegressor(const ModelTpl & model, DataTpl & data, JointIndex jointId); /// /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given frame. /// \brief Computes the regressor for the dynamic parameters of a rigid body attached to a given frame, /// puts the result in data.bodyRegressor and returns it. /// /// This algorithm assumes RNEA has been run to compute the acceleration and gravitational effects. /// ... ... @@ -115,7 +131,7 @@ namespace pinocchio /// \return The regressor of the body. /// template class JointCollectionTpl> inline Eigen::Matrix inline typename DataTpl::BodyRegressorType & frameBodyRegressor(const ModelTpl & model, DataTpl & data, FrameIndex frameId); ... ... @@ -141,6 +157,8 @@ namespace pinocchio /// /// \return The joint torque regressor of the system. /// /// \warning This function writes temporary information in data.bodyRegressor. This means if you have valuable data in it it will be overwritten. /// template class JointCollectionTpl, typename ConfigVectorType, typename TangentVectorType1, typename TangentVectorType2> inline typename DataTpl::MatrixXs & computeJointTorqueRegressor(const ModelTpl & model, ... ...
 ... ... @@ -69,19 +69,20 @@ namespace pinocchio } } template inline Eigen::Matrix bodyRegressor(const MotionDense & v, const MotionDense & a) template inline void bodyRegressor(const MotionDense & v, const MotionDense & a, const Eigen::MatrixBase & regressor) { PINOCCHIO_ASSERT_MATRIX_SPECIFIC_SIZE(OutputType, params, 6, 10); typedef typename MotionVelocity::Scalar Scalar; enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options }; typedef Symmetric3Tpl Symmetric3; typedef typename Symmetric3::SkewSquare SkewSquare; typedef Eigen::Matrix ReturnType; using ::pinocchio::details::bigL; ReturnType res; Eigen::MatrixBase & res = const_cast< Eigen::MatrixBase & >(regressor); res.template block<3,1>(MotionVelocity::LINEAR,0) = a.linear() + v.angular().cross(v.linear()); const Eigen::Matrix & acc = res.template block<3,1>(MotionVelocity::LINEAR,0); ... ... @@ -93,12 +94,23 @@ namespace pinocchio res.template block<3,1>(MotionVelocity::ANGULAR,0).setZero(); skew(-acc, res.template block<3,3>(MotionVelocity::ANGULAR,1)); res.template block<3,6>(MotionVelocity::ANGULAR,4) = bigL(a.angular()) + cross(v.angular(), bigL(v.angular())); } template inline Eigen::Matrix bodyRegressor(const MotionDense & v, const MotionDense & a) { typedef typename MotionVelocity::Scalar Scalar; enum { Options = PINOCCHIO_EIGEN_PLAIN_TYPE(typename MotionVelocity::Vector3)::Options }; typedef Eigen::Matrix ReturnType; ReturnType res; bodyRegressor(v,a,res); return res; } template class JointCollectionTpl> inline Eigen::Matrix inline typename DataTpl::BodyRegressorType & jointBodyRegressor(const ModelTpl & model, DataTpl & data, JointIndex jointId) ... ... @@ -107,11 +119,12 @@ namespace pinocchio PINOCCHIO_UNUSED_VARIABLE(model); return bodyRegressor(data.v[jointId], data.a_gf[jointId]); bodyRegressor(data.v[jointId], data.a_gf[jointId], data.bodyRegressor); return data.bodyRegressor; } template class JointCollectionTpl> inline Eigen::Matrix inline typename DataTpl::BodyRegressorType & frameBodyRegressor(const ModelTpl & model, DataTpl & data, FrameIndex frameId) ... ... @@ -122,16 +135,13 @@ namespace pinocchio typedef typename Model::Frame Frame; typedef typename Model::JointIndex JointIndex; typedef typename Model::SE3 SE3; typedef typename Model::Motion Motion; const Frame & frame = model.frames[frameId]; const JointIndex & parent = frame.parent; const SE3 & placement = frame.placement; const Motion v = placement.actInv(data.v[parent]); const Motion a = placement.actInv(data.a_gf[parent]); return bodyRegressor(v, a); bodyRegressor(placement.actInv(data.v[parent]), placement.actInv(data.a_gf[parent]), data.bodyRegressor); return data.bodyRegressor; } template class JointCollectionTpl> ... ... @@ -140,11 +150,11 @@ namespace pinocchio { typedef ModelTpl Model; typedef DataTpl Data; typedef typename DataTpl::BodyRegressorType BodyRegressorType; typedef boost::fusion::vector & const JointIndex > ArgsType; template ... ... @@ -152,18 +162,17 @@ namespace pinocchio JointDataBase & jdata, const Model & model, Data & data, const JointIndex col_idx, Eigen::Matrix & A) const JointIndex col_idx) { typedef typename Model::JointIndex JointIndex; const JointIndex i = jmodel.id(); const JointIndex parent = model.parents[i]; data.jointTorqueRegressor.block(jmodel.idx_v(),10*(int(col_idx)-1),jmodel.nv(),10) = jdata.S().transpose()*A; data.jointTorqueRegressor.block(jmodel.idx_v(),10*(int(col_idx)-1),jmodel.nv(),10) = jdata.S().transpose()*data.bodyRegressor; if(parent>0) forceSet::se3Action(data.liMi[i],A,A); forceSet::se3Action(data.liMi[i],data.bodyRegressor,data.bodyRegressor); } }; ... ... @@ -193,12 +202,11 @@ namespace pinocchio } typedef JointTorqueRegressorBackwardStep Pass2; Eigen::Matrix jointRegressor; for(JointIndex i=(JointIndex)model.njoints-1; i>0; --i) { jointRegressor = jointBodyRegressor(model,data,i); jointBodyRegressor(model,data,i); typename Pass2::ArgsType arg2(model,data,i,jointRegressor); typename Pass2::ArgsType arg2(model,data,i); for(JointIndex j=i; j>0; j = model.parents[j]) { Pass2::run(model.joints[j],data.joints[j], ... ...
 ... ... @@ -70,7 +70,10 @@ namespace pinocchio typedef Eigen::Matrix Matrix6; typedef Eigen::Matrix RowMatrix6; typedef Eigen::Matrix RowMatrixXs; /// \brief The type of the body regressor typedef Eigen::Matrix BodyRegressorType; /// \brief Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, /// encapsulated in JointDataAccessor. JointDataVector joints; ... ... @@ -328,6 +331,9 @@ namespace pinocchio // data related to static regressor Matrix3x staticRegressor; /// body regressor BodyRegressorType bodyRegressor; // data related to joint torque regressor MatrixXs jointTorqueRegressor; ... ...
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