/
chainjnttojacdotsolver.hpp
183 lines (166 loc) · 7.28 KB
/
chainjnttojacdotsolver.hpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
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