Initial commit
This commit is contained in:
zhangshun
183
KDL/install/include/kdl/chainjnttojacdotsolver.hpp
Normal file
183
KDL/install/include/kdl/chainjnttojacdotsolver.hpp
Normal file
@@ -0,0 +1,183 @@
|
||||
/*
|
||||
Computes the Jacobian time derivative
|
||||
Copyright (C) 2015 Antoine Hoarau <hoarau [at] isir.upmc.fr>
|
||||
|
||||
This library is free software; you can redistribute it and/or
|
||||
modify it under the terms of the GNU Lesser General Public
|
||||
License as published by the Free Software Foundation; either
|
||||
version 2.1 of the License, or (at your option) any later version.
|
||||
|
||||
This library is distributed in the hope that it will be useful,
|
||||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||||
Lesser General Public License for more details.
|
||||
|
||||
You should have received a copy of the GNU Lesser General Public
|
||||
License along with this library; if not, write to the Free Software
|
||||
Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
|
||||
*/
|
||||
|
||||
|
||||
#ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
|
||||
#define KDL_CHAINJNTTOJACDOTSOLVER_HPP
|
||||
|
||||
#include "solveri.hpp"
|
||||
#include "frames.hpp"
|
||||
#include "jntarrayvel.hpp"
|
||||
#include "jacobian.hpp"
|
||||
#include "chain.hpp"
|
||||
#include "framevel.hpp"
|
||||
#include "chainjnttojacsolver.hpp"
|
||||
#include "chainfksolverpos_recursive.hpp"
|
||||
|
||||
namespace KDL
|
||||
{
|
||||
|
||||
/**
|
||||
* @brief Computes the Jacobian time derivative (Jdot) by calculating the
|
||||
* partial derivatives regarding to a joint angle, in the Hybrid, Body-fixed
|
||||
* or Inertial representation.
|
||||
*
|
||||
* This work is based on :
|
||||
* Symbolic differentiation of the velocity mapping for a serial kinematic chain
|
||||
* H. Bruyninckx, J. De Schutter
|
||||
* doi:10.1016/0094-114X(95)00069-B
|
||||
*
|
||||
* url : http://www.sciencedirect.com/science/article/pii/0094114X9500069B
|
||||
*/
|
||||
class ChainJntToJacDotSolver : public SolverI
|
||||
{
|
||||
public:
|
||||
static const int E_JAC_DOT_FAILED = -100;
|
||||
static const int E_JACSOLVER_FAILED = -101;
|
||||
static const int E_FKSOLVERPOS_FAILED = -102;
|
||||
|
||||
// Hybrid representation ref Frame: base, ref Point: end-effector
|
||||
static const int HYBRID = 0;
|
||||
// Body-fixed representation ref Frame: end-effector, ref Point: end-effector
|
||||
static const int BODYFIXED = 1;
|
||||
// Inertial representation ref Frame: base, ref Point: base
|
||||
static const int INERTIAL = 2;
|
||||
|
||||
explicit ChainJntToJacDotSolver(const Chain& chain);
|
||||
virtual ~ChainJntToJacDotSolver();
|
||||
/**
|
||||
* @brief Computes \f$ {}_{bs}\dot{J}^{ee}.\dot{q} \f$
|
||||
*
|
||||
* @param q_in Current joint positions and velocities
|
||||
* @param jac_dot_q_dot The twist representing Jdot*qdot
|
||||
* @param seg_nr The final segment to compute
|
||||
* @return int 0 if no errors happened
|
||||
*/
|
||||
virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
|
||||
/**
|
||||
* @brief Computes \f$ {}_{bs}\dot{J}^{ee} \f$
|
||||
*
|
||||
* @param q_in Current joint positions and velocities
|
||||
* @param jdot The jacobian time derivative in the configured representation
|
||||
* (HYBRID, BODYFIXED or INERTIAL)
|
||||
* @param seg_nr The final segment to compute
|
||||
* @return int 0 if no errors happened
|
||||
*/
|
||||
virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
|
||||
int setLockedJoints(const std::vector<bool>& locked_joints);
|
||||
|
||||
/**
|
||||
* @brief JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
|
||||
*
|
||||
*
|
||||
* @return void
|
||||
*/
|
||||
void setHybridRepresentation(){setRepresentation(HYBRID);}
|
||||
/**
|
||||
* @brief JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector, ref Point: end-effector)
|
||||
*
|
||||
* @return void
|
||||
*/
|
||||
void setBodyFixedRepresentation(){setRepresentation(BODYFIXED);}
|
||||
/**
|
||||
* @brief JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
|
||||
*
|
||||
* @return void
|
||||
*/
|
||||
void setInertialRepresentation(){setRepresentation(INERTIAL);}
|
||||
/**
|
||||
* @brief Sets the internal variable for the representation (with a check on the value)
|
||||
*
|
||||
* @param representation The representation for Jdot : HYBRID,BODYFIXED or INERTIAL
|
||||
* @return void
|
||||
*/
|
||||
void setRepresentation(const int& representation);
|
||||
|
||||
/// @copydoc KDL::SolverI::updateInternalDataStructures
|
||||
virtual void updateInternalDataStructures();
|
||||
|
||||
/// @copydoc KDL::SolverI::strError()
|
||||
virtual const char* strError(const int error) const;
|
||||
|
||||
|
||||
protected:
|
||||
/**
|
||||
* @brief Computes \f$ \frac{\partial {}_{bs}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
|
||||
*
|
||||
* @param bs_J_ee The Jacobian expressed in the base frame with the end effector as the reference point (default in KDL Jacobian Solver)
|
||||
* @param joint_idx The index of the current joint (j in the formula)
|
||||
* @param column_idx The index of the current column (i in the formula)
|
||||
* @return Twist The twist representing dJi/dqj .qdotj
|
||||
*/
|
||||
const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
|
||||
const unsigned int& joint_idx,
|
||||
const unsigned int& column_idx);
|
||||
/**
|
||||
* @brief Computes \f$ \frac{\partial {}_{ee}J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
|
||||
*
|
||||
* @param bs_J_ee The Jacobian expressed in the end effector frame with the end effector as the reference point
|
||||
* @param joint_idx The indice of the current joint (j in the formula)
|
||||
* @param column_idx The indice of the current column (i in the formula)
|
||||
* @return Twist The twist representing dJi/dqj .qdotj
|
||||
*/
|
||||
const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
|
||||
const unsigned int& joint_idx,
|
||||
const unsigned int& column_idx);
|
||||
/**
|
||||
* @brief Computes \f$ \frac{\partial {}_{bs}J^{i,bs}}{\partial q^{j}}.\dot{q}^{j} \f$
|
||||
*
|
||||
* @param ee_J_ee The Jacobian expressed in the base frame with the base as the reference point
|
||||
* @param joint_idx The indice of the current joint (j in the formula)
|
||||
* @param column_idx The indice of the current column (i in the formula)
|
||||
* @return Twist The twist representing dJi/dqj .qdotj
|
||||
*/
|
||||
const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
|
||||
const unsigned int& joint_idx,
|
||||
const unsigned int& column_idx);
|
||||
/**
|
||||
* @brief Computes \f$ \frac{\partial J^{i,ee}}{\partial q^{j}}.\dot{q}^{j} \f$
|
||||
*
|
||||
* @param bs_J_bs The Jacobian expressed in the base frame with the end effector as the reference point
|
||||
* @param joint_idx The indice of the current joint (j in the formula)
|
||||
* @param column_idx The indice of the current column (i in the formula)
|
||||
* @param representation The representation (Hybrid,Body-fixed,Inertial) in which you want to get dJ/dqj .qdotj
|
||||
* @return Twist The twist representing dJi/dqj .qdotj
|
||||
*/
|
||||
const Twist& getPartialDerivative(const Jacobian& J,
|
||||
const unsigned int& joint_idx,
|
||||
const unsigned int& column_idx,
|
||||
const int& representation);
|
||||
private:
|
||||
|
||||
const Chain& chain;
|
||||
std::vector<bool> locked_joints_;
|
||||
unsigned int nr_of_unlocked_joints_;
|
||||
ChainJntToJacSolver jac_solver_;
|
||||
Jacobian jac_;
|
||||
Jacobian jac_dot_;
|
||||
int representation_;
|
||||
ChainFkSolverPos_recursive fk_solver_;
|
||||
Frame F_bs_ee_;
|
||||
Twist jac_dot_k_;
|
||||
Twist jac_j_, jac_i_;
|
||||
Twist t_djdq_;
|
||||
};
|
||||
|
||||
}
|
||||
#endif
|
||||
Reference in New Issue
Block a user