Commit 38cec6d5 by jcarpent

### [Algo] Add algo to compute inefficiently the time derivative of the joint Jacobian

parent 36d2aa75
 // // Copyright (c) 2015-2016 CNRS // Copyright (c) 2015-2017 CNRS // // This file is part of Pinocchio // Pinocchio is free software: you can redistribute it ... ... @@ -26,7 +26,7 @@ namespace se3 /// \brief Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame. /// The result is accessible through data.J. /// /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Hacobian, it is then possible to easily extract the Jacobian of a specific joint frame. /// \note This Jacobian does not correspond to any specific joint frame Jacobian. From this Jacobian, it is then possible to easily extract the Jacobian of a specific joint frame. /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. ... ... @@ -71,6 +71,41 @@ namespace se3 const Eigen::VectorXd & q, const Model::JointIndex jointId); /// /// \brief Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depends both on q and v /// The result is accessible through data.dJ. /// /// /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] q The joint configuration vector (dim model.nq). /// \param[in] v The joint velocity vector (dim model.nv). /// /// \return The full model Jacobian (matrix 6 x model.nv). /// inline const Data::Matrix6x & computeJacobiansTimeVariation(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v); /// /// \brief Computes the Jacobian time variation of a specific joint frame expressed either in the world frame or in the local frame of the joint. /// \note This jacobian is extracted from data.dJ. You have to run se3::computeJacobiansTimeVariation before calling it. /// /// \param[in] localFrame Expressed the Jacobian in the local frame or world frame coordinates system. /// \param[in] model The model structure of the rigid body system. /// \param[in] data The data structure of the rigid body system. /// \param[in] jointId The id of the joint. /// \param[out] dJ A reference on the Jacobian matrix where the results will be stored in (dim 6 x model.nv). You must fill dJ with zero elements, e.g. dJ.fill(0.). /// template void getJacobian(const Model & model, const Data & data, const Model::JointIndex jointId, Data::Matrix6x & dJ); } // namespace se3 /* --- Details -------------------------------------------------------------------- */ ... ...
 ... ... @@ -140,6 +140,91 @@ namespace se3 return data.J; } struct JacobiansTimeVariationForwardStep : public fusion::JointVisitor { typedef boost::fusion::vector ArgsType; JOINT_VISITOR_INIT(JacobiansTimeVariationForwardStep); template static void algo(const se3::JointModelBase & jmodel, se3::JointDataBase & jdata, const se3::Model & model, se3::Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) { const Model::JointIndex & i = (Model::JointIndex) jmodel.id(); const Model::JointIndex & parent = model.parents[i]; SE3 & oMi = data.oMi[i]; Motion & vJ = data.v[i]; jmodel.calc(jdata.derived(),q,v); vJ = jdata.v(); data.liMi[i] = model.jointPlacements[i]*jdata.M(); if(parent>0) { oMi = data.oMi[parent]*data.liMi[i]; vJ += data.liMi[i].actInv(data.v[parent]); } else { oMi = data.liMi[i]; } jmodel.jointCols(data.J) = oMi.act(jdata.S()); // Spatial velocity of joint i expressed in the global frame o const Motion ov(oMi.act(vJ)); jmodel.jointCols(data.dJ) = (ov.toActionMatrix() * jmodel.jointCols(data.J)); } }; inline const Data::Matrix6x & computeJacobiansTimeVariation(const Model & model, Data & data, const Eigen::VectorXd & q, const Eigen::VectorXd & v) { assert(model.check(data) && "data is not consistent with model."); for(Model::JointIndex i=1; i< (Model::JointIndex) model.njoints;++i) { JacobiansTimeVariationForwardStep::run(model.joints[i],data.joints[i], JacobiansTimeVariationForwardStep::ArgsType(model,data,q,v)); } return data.dJ; } template void getJacobianTimeVariation(const Model & model, const Data & data, const Model::JointIndex jointId, Data::Matrix6x & dJ) { assert( dJ.rows() == data.dJ.rows() ); assert( dJ.cols() == data.dJ.cols() ); assert(model.check(data) && "data is not consistent with model."); const SE3 & oMjoint = data.oMi[jointId]; int colRef = nv(model.joints[jointId])+idx_v(model.joints[jointId])-1; for(int j=colRef;j>=0;j=data.parents_fromRow[(Model::Index)j]) { if(! localFrame ) dJ.col(j) = data.dJ.col(j); else dJ.col(j) = oMjoint.actInv(Motion(data.dJ.col(j))).toVector(); } } } // namespace se3 /// @endcond ... ...
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!