KDL  1.4.0
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  // Intertial representation ref Frame: base, ref Point: base
60  static const int INTERTIAL = 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;
169  std::vector<bool> locked_joints_;
180 };
181 
182 }
183 #endif
KDL::ChainJntToJacDotSolver::F_bs_ee_
Frame F_bs_ee_
Definition: chainjnttojacdotsolver.hpp:176
chain.hpp
frames.hpp
KDL::Jacobian::columns
unsigned int columns() const
Definition: jacobian.cpp:75
KDL::ChainJntToJacDotSolver::E_FKSOLVERPOS_FAILED
static const int E_FKSOLVERPOS_FAILED
Definition: chainjnttojacdotsolver.hpp:53
KDL::SolverI::strError
virtual const char * strError(const int error) const
Return a description of the latest error.
Definition: solveri.hpp:125
KDL::ChainJntToJacDotSolver::representation_
int representation_
Definition: chainjnttojacdotsolver.hpp:174
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:723
KDL::ChainJntToJacDotSolver::setLockedJoints
int setLockedJoints(const std::vector< bool > &locked_joints)
Definition: chainjnttojacdotsolver.cpp:219
KDL::Jacobian::setColumn
void setColumn(unsigned int i, const Twist &t)
Definition: jacobian.cpp:149
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::Jacobian::changeBase
friend bool changeBase(const Jacobian &src1, const Rotation &rot, Jacobian &dest)
Definition: jacobian.cpp:104
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
KDL::Rotation::Inverse
Rotation Inverse() const
Gives back the inverse rotation matrix of *this.
Definition: frames.inl:632
KDL::Segment::getJoint
const Joint & getJoint() const
Request the joint of the segment.
Definition: segment.hpp:118
KDL::Frame::p
Vector p
origine of the Frame
Definition: frames.hpp:572
chainfksolverpos_recursive.hpp
KDL::SolverI::E_NOT_UP_TO_DATE
@ E_NOT_UP_TO_DATE
Chain size changed.
Definition: solveri.hpp:97
KDL::ChainJntToJacDotSolver::ChainJntToJacDotSolver
ChainJntToJacDotSolver(const Chain &chain)
Definition: chainjnttojacdotsolver.cpp:26
KDL::ChainJntToJacDotSolver::~ChainJntToJacDotSolver
virtual ~ChainJntToJacDotSolver()
Definition: chainjnttojacdotsolver.cpp:241
jntarrayvel.hpp
KDL
Definition: articulatedbodyinertia.cpp:28
KDL::Chain::getNrOfSegments
unsigned int getNrOfSegments() const
Request the total number of segments in the chain.
Definition: chain.hpp:76
KDL::Jacobian::changeRefPoint
friend bool changeRefPoint(const Jacobian &src1, const Vector &base_AB, Jacobian &dest)
Definition: jacobian.cpp:90
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
Return a description of the latest error.
Definition: chainjnttojacdotsolver.cpp:233
KDL::ChainJntToJacDotSolver::nr_of_unlocked_joints_
unsigned int nr_of_unlocked_joints_
Definition: chainjnttojacdotsolver.hpp:170
KDL::Jacobian::resize
void resize(unsigned int newNrOfColumns)
Allocates memory for new size (can break realtime behavior)
Definition: jacobian.cpp:55
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:720
KDL::SolverI::E_OUT_OF_RANGE
@ E_OUT_OF_RANGE
Requested index out of range.
Definition: solveri.hpp:103
framevel.hpp
KDL::JntArray::rows
unsigned int rows() const
Returns the number of rows (size) of the array.
Definition: jntarray.cpp:72
KDL::ChainJntToJacSolver::JntToJac
virtual int JntToJac(const JntArray &q_in, Jacobian &jac, int segmentNR=-1)
Calculate the jacobian expressed in the base frame of the chain, with reference point at the end effe...
Definition: chainjnttojacsolver.cpp:48
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::Chain::getSegment
const Segment & getSegment(unsigned int nr) const
Request the nr'd segment of the chain.
Definition: chain.cpp:68
KDL::SolverI::E_SIZE_MISMATCH
@ E_SIZE_MISMATCH
Input size does not match internal state.
Definition: solveri.hpp:99
KDL::ChainJntToJacDotSolver::jac_solver_
ChainJntToJacSolver jac_solver_
Definition: chainjnttojacdotsolver.hpp:171
KDL::Frame
Definition: frames.hpp:570
solveri.hpp
KDL::ChainFkSolverPos_recursive::JntToCart
virtual int JntToCart(const JntArray &q_in, Frame &p_out, int segmentNr=-1)
Calculate forward position kinematics for a KDL::Chain, from joint coordinates to cartesian pose.
Definition: chainfksolverpos_recursive.cpp:33
KDL::ChainJntToJacDotSolver::jac_dot_
Jacobian jac_dot_
Definition: chainjnttojacdotsolver.hpp:173
KDL::ChainJntToJacDotSolver::INTERTIAL
static const int INTERTIAL
Definition: chainjnttojacdotsolver.hpp:60
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:47
KDL::ChainJntToJacDotSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacdotsolver.cpp:38
KDL::Joint::None
@ None
Definition: joint.hpp:47
KDL::ChainJntToJacDotSolver::chain
const Chain & chain
Definition: chainjnttojacdotsolver.hpp:168
KDL::ChainJntToJacDotSolver::HYBRID
static const int HYBRID
Definition: chainjnttojacdotsolver.hpp:56
KDL::ChainJntToJacSolver::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainjnttojacsolver.cpp:31
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:722
KDL::JntArrayVel
Definition: jntarrayvel.hpp:45
KDL::ChainJntToJacDotSolver::fk_solver_
ChainFkSolverPos_recursive fk_solver_
Definition: chainjnttojacdotsolver.hpp:175
KDL::ChainJntToJacDotSolver::setInternialRepresentation
void setInternialRepresentation()
JntToJacDot() will compute in the Inertial representation (ref Frame: base, ref Point: base)
Definition: chainjnttojacdotsolver.hpp:103
KDL::SolverI
Solver interface supporting storage and description of the latest error.
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:117
KDL::Frame::M
Rotation M
Orientation of the Frame.
Definition: frames.hpp:573
KDL::JntArrayVel::qdot
JntArray qdot
Definition: jntarrayvel.hpp:49
KDL::ChainJntToJacDotSolver::jac_
Jacobian jac_
Definition: chainjnttojacdotsolver.hpp:172
chainjnttojacdotsolver.hpp
KDL::ChainFkSolverPos_recursive::updateInternalDataStructures
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition: chainfksolverpos_recursive.hpp:45
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:189
KDL::Joint::getType
const JointType & getType() const
Request the type of the joint.
Definition: joint.hpp:159
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:167
KDL::ChainJntToJacSolver
Class to calculate the jacobian of a general KDL::Chain, it is used by other solvers.
Definition: chainjnttojacsolver.hpp:41
KDL::SetToZero
void SetToZero(Jacobian &jac)
Definition: jacobian.cpp:80
KDL::JntArrayVel::q
JntArray q
Definition: jntarrayvel.hpp:48
KDL::SolverI::E_NOERROR
@ E_NOERROR
No error.
Definition: solveri.hpp:91
KDL::ChainFkSolverPos_recursive
Implementation of a recursive forward position kinematics algorithm to calculate the position transfo...
Definition: chainfksolverpos_recursive.hpp:36
KDL::MultiplyJacobian
void MultiplyJacobian(const Jacobian &jac, const JntArray &src, Twist &dest)
Function to multiply a KDL::Jacobian with a KDL::JntArray to get a KDL::Twist, it should not be used ...
Definition: jntarray.cpp:102
jacobian.hpp
KDL::Jacobian::getColumn
Twist getColumn(unsigned int i) const
Definition: jacobian.cpp:145
KDL::ChainJntToJacDotSolver::t_djdq_
Twist t_djdq_
Definition: chainjnttojacdotsolver.hpp:179
KDL::Chain
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:136
KDL::Chain::getNrOfJoints
unsigned int getNrOfJoints() const
Request the total number of joints in the chain.
Definition: chain.hpp:71
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:210
KDL::Jacobian
Definition: jacobian.hpp:36