184 lines
7.3 KiB
C++
184 lines
7.3 KiB
C++
/*
|
|
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
|