orocos_kdl
chainjnttojacdotsolver.hpp
Go to the documentation of this file.
1 /*
2  Computes the Jacobian time derivative
3  Copyright (C) 2015 Antoine Hoarau <hoarau [at] isir.upmc.fr>
4 
5  This library is free software; you can redistribute it and/or
6  modify it under the terms of the GNU Lesser General Public
7  License as published by the Free Software Foundation; either
8  version 2.1 of the License, or (at your option) any later version.
9 
10  This library is distributed in the hope that it will be useful,
11  but WITHOUT ANY WARRANTY; without even the implied warranty of
12  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
13  Lesser General Public License for more details.
14 
15  You should have received a copy of the GNU Lesser General Public
16  License along with this library; if not, write to the Free Software
17  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
18 */
19 
20 
21 #ifndef KDL_CHAINJNTTOJACDOTSOLVER_HPP
22 #define KDL_CHAINJNTTOJACDOTSOLVER_HPP
23 
24 #include "solveri.hpp"
25 #include "frames.hpp"
26 #include "jntarrayvel.hpp"
27 #include "jacobian.hpp"
28 #include "chain.hpp"
29 #include "framevel.hpp"
30 #include "chainjnttojacsolver.hpp"
32 
33 namespace KDL
34 {
35 
49 {
50 public:
51  static const int E_JAC_DOT_FAILED = -100;
52  static const int E_JACSOLVER_FAILED = -101;
53  static const int E_FKSOLVERPOS_FAILED = -102;
54 
55  // Hybrid representation ref Frame: base, ref Point: end-effector
56  static const int HYBRID = 0;
57  // Body-fixed representation ref Frame: end-effector, ref Point: end-effector
58  static const int BODYFIXED = 1;
59  // Inertial representation ref Frame: base, ref Point: base
60  static const int INERTIAL = 2;
61 
62  explicit ChainJntToJacDotSolver(const Chain& chain);
63  virtual ~ChainJntToJacDotSolver();
72  virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Twist& jac_dot_q_dot, int seg_nr = -1);
82  virtual int JntToJacDot(const KDL::JntArrayVel& q_in, KDL::Jacobian& jdot, int seg_nr = -1);
83  int setLockedJoints(const std::vector<bool>& locked_joints);
84 
110  void setRepresentation(const int& representation);
111 
113  virtual void updateInternalDataStructures();
114 
116  virtual const char* strError(const int error) const;
117 
118 
119 protected:
128  const Twist& getPartialDerivativeHybrid(const Jacobian& bs_J_ee,
129  const unsigned int& joint_idx,
130  const unsigned int& column_idx);
139  const Twist& getPartialDerivativeBodyFixed(const Jacobian& ee_J_ee,
140  const unsigned int& joint_idx,
141  const unsigned int& column_idx);
150  const Twist& getPartialDerivativeInertial(const Jacobian& bs_J_bs,
151  const unsigned int& joint_idx,
152  const unsigned int& column_idx);
162  const Twist& getPartialDerivative(const Jacobian& J,
163  const unsigned int& joint_idx,
164  const unsigned int& column_idx,
165  const int& representation);
166 private:
167 
168  const Chain& chain;
180 };
181 
182 }
183 #endif
KDL::ChainJntToJacDotSolver::F_bs_ee_
Frame F_bs_ee_
Definition: chainjnttojacdotsolver.hpp:176
chain.hpp
frames.hpp
KDL::ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED
static const int E_FKSOLVERPOS_FAILED
Definition: chainjnttojacdotsolver.hpp:53
KDL::ChainJntToJacDotSolver::representation_
int representation_
Definition: chainjnttojacdotsolver.hpp:174
KDL::ChainJntToJacDotSolver::setLockedJoints
int setLockedJoints(const std::vector< bool > &locked_joints)
Definition: chainjnttojacdotsolver.cpp:227
KDL::ChainJntToJacDotSolver::jac_j_
Twist jac_j_
Definition: chainjnttojacdotsolver.hpp:178
KDL::ChainJntToJacDotSolver::E_JACSOLVER_FAILED
static const int E_JACSOLVER_FAILED
Definition: chainjnttojacdotsolver.hpp:52
KDL::ChainJntToJacDotSolver::setBodyFixedRepresentation
void setBodyFixedRepresentation()
JntToJacDot() will compute in the Body-fixed representation (ref Frame: end-effector,...
Definition: chainjnttojacdotsolver.hpp:97
KDL::ChainJntToJacDotSolver::BODYFIXED
static const int BODYFIXED
Definition: chainjnttojacdotsolver.hpp:58
KDL::ChainJntToJacDotSolver::jac_i_
Twist jac_i_
Definition: chainjnttojacdotsolver.hpp:178
std::vector< bool >
chainfksolverpos_recursive.hpp
KDL::ChainJntToJacDotSolver::ChainJntToJacDotSolver
ChainJntToJacDotSolver(const Chain &chain)
Definition: chainjnttojacdotsolver.cpp:34
KDL::ChainJntToJacDotSolver::~ChainJntToJacDotSolver
virtual ~ChainJntToJacDotSolver()
Definition: chainjnttojacdotsolver.cpp:249
jntarrayvel.hpp
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::ChainJntToJacDotSolver::setHybridRepresentation
void setHybridRepresentation()
JntToJacDot() will compute in the Hybrid representation (ref Frame: base, ref Point: end-effector)
Definition: chainjnttojacdotsolver.hpp:91
KDL::ChainJntToJacDotSolver::strError
virtual const char * strError(const int error) const
Definition: chainjnttojacdotsolver.cpp:241
KDL::ChainJntToJacDotSolver::nr_of_unlocked_joints_
unsigned int nr_of_unlocked_joints_
Definition: chainjnttojacdotsolver.hpp:170
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
framevel.hpp
KDL::ChainJntToJacDotSolver::locked_joints_
std::vector< bool > locked_joints_
Definition: chainjnttojacdotsolver.hpp:169
KDL::ChainJntToJacDotSolver
Computes the Jacobian time derivative (Jdot) by calculating the partial derivatives regarding to a jo...
Definition: chainjnttojacdotsolver.hpp:48
KDL::ChainJntToJacDotSolver::jac_solver_
ChainJntToJacSolver jac_solver_
Definition: chainjnttojacdotsolver.hpp:171
KDL::ChainJntToJacDotSolver::INERTIAL
static const int INERTIAL
Definition: chainjnttojacdotsolver.hpp:60
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
solveri.hpp
KDL::ChainJntToJacDotSolver::setInertialRepresentation
void setInertialRepresentation()
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
Definition: chainjnttojacdotsolver.hpp:103
KDL::ChainJntToJacDotSolver::jac_dot_
Jacobian jac_dot_
Definition: chainjnttojacdotsolver.hpp:173
KDL::ChainJntToJacDotSolver::JntToJacDot
virtual int JntToJacDot(const KDL::JntArrayVel &q_in, KDL::Twist &jac_dot_q_dot, int seg_nr=-1)
Computes .
Definition: chainjnttojacdotsolver.cpp:55
KDL::ChainJntToJacDotSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainjnttojacdotsolver.cpp:46
KDL::ChainJntToJacDotSolver::chain
const Chain & chain
Definition: chainjnttojacdotsolver.hpp:168
KDL::ChainJntToJacDotSolver::HYBRID
static const int HYBRID
Definition: chainjnttojacdotsolver.hpp:56
KDL::JntArrayVel
Definition: jntarrayvel.hpp:45
KDL::ChainJntToJacDotSolver::fk_solver_
ChainFkSolverPos_recursive fk_solver_
Definition: chainjnttojacdotsolver.hpp:175
KDL::SolverI
Definition: solveri.hpp:84
KDL::ChainJntToJacDotSolver::jac_dot_k_
Twist jac_dot_k_
Definition: chainjnttojacdotsolver.hpp:177
KDL::ChainJntToJacDotSolver::getPartialDerivative
const Twist & getPartialDerivative(const Jacobian &J, const unsigned int &joint_idx, const unsigned int &column_idx, const int &representation)
Computes .
Definition: chainjnttojacdotsolver.cpp:125
KDL::ChainJntToJacDotSolver::jac_
Jacobian jac_
Definition: chainjnttojacdotsolver.hpp:172
KDL::ChainJntToJacDotSolver::getPartialDerivativeInertial
const Twist & getPartialDerivativeInertial(const Jacobian &bs_J_bs, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:197
chainjnttojacsolver.hpp
KDL::ChainJntToJacDotSolver::getPartialDerivativeBodyFixed
const Twist & getPartialDerivativeBodyFixed(const Jacobian &ee_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:175
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:38
KDL::ChainFkSolverPos_recursive
Definition: chainfksolverpos_recursive.hpp:36
jacobian.hpp
KDL::ChainJntToJacDotSolver::t_djdq_
Twist t_djdq_
Definition: chainjnttojacdotsolver.hpp:179
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::ChainJntToJacDotSolver::getPartialDerivativeHybrid
const Twist & getPartialDerivativeHybrid(const Jacobian &bs_J_ee, const unsigned int &joint_idx, const unsigned int &column_idx)
Computes .
Definition: chainjnttojacdotsolver.cpp:144
KDL::ChainJntToJacDotSolver::E_JAC_DOT_FAILED
static const int E_JAC_DOT_FAILED
Definition: chainjnttojacdotsolver.hpp:51
KDL::SolverI::error
int error
Latest error, initialized to E_NOERROR in constructor.
Definition: solveri.hpp:149
KDL::ChainJntToJacDotSolver::setRepresentation
void setRepresentation(const int &representation)
Sets the internal variable for the representation (with a check on the value)
Definition: chainjnttojacdotsolver.cpp:218
KDL::Jacobian
Definition: jacobian.hpp:36