orocos_kdl
treeiksolvervel_wdls.cpp
Go to the documentation of this file.
1 /*
2  * TreeIkSolverVel_wdls.cpp
3  *
4  * Created on: Nov 28, 2008
5  * Author: rubensmits
6  */
7 
10 
11 namespace KDL {
13  tree(tree_in), jnttojacsolver(tree),
14  J(Eigen::MatrixXd::Zero(6 * endpoints.size(), tree.getNrOfJoints())),
15  Wy(Eigen::MatrixXd::Identity(J.rows(),J.rows())),
16  Wq(Eigen::MatrixXd::Identity(J.cols(),J.cols())),
17  J_Wq(J.rows(),J.cols()),Wy_J_Wq(J.rows(),J.cols()),
18  U(Eigen::MatrixXd::Identity(J.rows(),J.cols())),
19  V(Eigen::MatrixXd::Identity(J.cols(),J.cols())),
20  Wy_U(J.rows(),J.rows()),Wq_V(J.cols(),J.cols()),
21  t(Eigen::VectorXd::Zero(J.rows())), Wy_t(Eigen::VectorXd::Zero(J.rows())),
22  qdot(Eigen::VectorXd::Zero(J.cols())),
23  tmp(Eigen::VectorXd::Zero(J.cols())),S(Eigen::VectorXd::Zero(J.cols())),
24  lambda(0)
25  {
26 
27  for (size_t i = 0; i < endpoints.size(); ++i) {
28  jacobians.insert(Jacobians::value_type(endpoints[i], Jacobian(tree.getNrOfJoints())));
29  }
30 
31  }
32 
34  }
35 
36  void TreeIkSolverVel_wdls::setWeightJS(const Eigen::MatrixXd& Mq) {
37  Wq = Mq;
38  }
39 
40  void TreeIkSolverVel_wdls::setWeightTS(const Eigen::MatrixXd& Mx) {
41  Wy = Mx;
42  }
43 
44  void TreeIkSolverVel_wdls::setLambda(const double& lambda_in) {
45  lambda = lambda_in;
46  }
47 
48  double TreeIkSolverVel_wdls::CartToJnt(const JntArray& q_in, const Twists& v_in, JntArray& qdot_out) {
49 
50  //First check if we are configured for this Twists:
51  for (Twists::const_iterator v_it = v_in.begin(); v_it != v_in.end(); ++v_it) {
52  if (jacobians.find(v_it->first) == jacobians.end())
53  return -2;
54  }
55  //Check if q_in has the right size
56  if (q_in.rows() != tree.getNrOfJoints())
57  return -1;
58 
59  //Lets get all the jacobians we need:
60  unsigned int k = 0;
61  for (Jacobians::iterator jac_it = jacobians.begin(); jac_it
62  != jacobians.end(); ++jac_it) {
63  int ret = jnttojacsolver.JntToJac(q_in, jac_it->second, jac_it->first);
64  if (ret < 0)
65  return ret;
66  else {
67  //lets put the jacobian in the big matrix and put the twist in the big t:
68  J.block(6*k,0, 6,tree.getNrOfJoints()) = jac_it->second.data;
69  const Twist& twist=v_in.find(jac_it->first)->second;
70  t.segment(6*k,3) = Eigen::Map<const Eigen::Vector3d>(twist.vel.data);
71  t.segment(6*k+3,3) = Eigen::Map<const Eigen::Vector3d>(twist.rot.data);
72  }
73  ++k;
74  }
75 
76  //Lets use the wdls algorithm to find the qdot:
77  // Create the Weighted jacobian
78  J_Wq.noalias() = J * Wq;
79  Wy_J_Wq.noalias() = Wy * J_Wq;
80 
81  // Compute the SVD of the weighted jacobian
82  int ret = svd_eigen_HH(Wy_J_Wq, U, S, V, tmp);
83  if (ret < 0 )
84  return E_SVD_FAILED;
85  //Pre-multiply U and V by the task space and joint space weighting matrix respectively
86  Wy_t.noalias() = Wy * t;
87  Wq_V.noalias() = Wq * V;
88 
89  // tmp = (Si*Wy*U'*y),
90  for (unsigned int i = 0; i < J.cols(); i++) {
91  double sum = 0.0;
92  for (unsigned int j = 0; j < J.rows(); j++) {
93  if (i < Wy_t.rows())
94  sum += U(j, i) * Wy_t(j);
95  else
96  sum += 0.0;
97  }
98  tmp( i) = sum * ((S(i) / (S(i) * S(i) + lambda * lambda)));
99  }
100 
101  // x = Lx^-1*V*tmp + x
102  qdot_out.data.noalias() = Wq_V * tmp;
103 
104  return Wy_t.norm();
105  }
106 }
KDL::Vector::data
double data[3]
Definition: frames.hpp:165
KDL::Tree::getNrOfJoints
unsigned int getNrOfJoints() const
Definition: tree.hpp:159
KDL::TreeIkSolverVel_wdls::tmp
Eigen::VectorXd tmp
Definition: treeiksolvervel_wdls.hpp:86
KDL::Twist::rot
Vector rot
The rotational velocity of that point.
Definition: frames.hpp:726
KDL::TreeIkSolverVel_wdls::Wq_V
Eigen::MatrixXd Wq_V
Definition: treeiksolvervel_wdls.hpp:85
KDL::JntArray
Definition: jntarray.hpp:69
KDL::TreeIkSolverVel_wdls::CartToJnt
virtual double CartToJnt(const JntArray &q_in, const Twists &v_in, JntArray &qdot_out)
Definition: treeiksolvervel_wdls.cpp:48
KDL::TreeIkSolverVel_wdls::J_Wq
Eigen::MatrixXd J_Wq
Definition: treeiksolvervel_wdls.hpp:85
std::vector< std::string >
std::map::find
T find(T... args)
std::vector::size
T size(T... args)
KDL::TreeJntToJacSolver::JntToJac
int JntToJac(const JntArray &q_in, Jacobian &jac, const std::string &segmentname)
Definition: treejnttojacsolver.cpp:21
KDL::svd_eigen_HH
int svd_eigen_HH(const Eigen::MatrixXd &A, Eigen::MatrixXd &U, Eigen::VectorXd &S, Eigen::MatrixXd &V, Eigen::VectorXd &tmp, int maxiter, double epsilon)
Definition: svd_eigen_HH.cpp:26
KDL::TreeIkSolverVel_wdls::setLambda
void setLambda(const double &lambda)
Definition: treeiksolvervel_wdls.cpp:44
KDL::TreeIkSolverVel_wdls::J
Eigen::MatrixXd J
Definition: treeiksolvervel_wdls.hpp:85
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::TreeIkSolverVel_wdls::~TreeIkSolverVel_wdls
virtual ~TreeIkSolverVel_wdls()
Definition: treeiksolvervel_wdls.cpp:33
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::TreeIkSolverVel_wdls::setWeightJS
void setWeightJS(const Eigen::MatrixXd &Mq)
Definition: treeiksolvervel_wdls.cpp:36
KDL::JntArray::rows
unsigned int rows() const
Definition: jntarray.cpp:70
treeiksolvervel_wdls.hpp
KDL::TreeIkSolverVel_wdls::Wy_J_Wq
Eigen::MatrixXd Wy_J_Wq
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::lambda
double lambda
Definition: treeiksolvervel_wdls.hpp:87
KDL::TreeIkSolverVel_wdls::jnttojacsolver
TreeJntToJacSolver jnttojacsolver
Definition: treeiksolvervel_wdls.hpp:82
std::map< std::string, Twist >
KDL::TreeIkSolverVel_wdls::E_SVD_FAILED
static const int E_SVD_FAILED
Definition: treeiksolvervel_wdls.hpp:19
KDL::JntArray::data
Eigen::VectorXd data
Definition: jntarray.hpp:72
KDL::Tree
This class encapsulates a tree kinematic interconnection structure. It is built out of segments.
Definition: tree.hpp:99
KDL::TreeIkSolverVel_wdls::setWeightTS
void setWeightTS(const Eigen::MatrixXd &Mx)
Definition: treeiksolvervel_wdls.cpp:40
KDL::Twist::vel
Vector vel
The velocity of that point.
Definition: frames.hpp:725
KDL::TreeIkSolverVel_wdls::S
Eigen::VectorXd S
Definition: treeiksolvervel_wdls.hpp:86
svd_eigen_HH.hpp
KDL::TreeIkSolverVel_wdls::Wy
Eigen::MatrixXd Wy
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::t
Eigen::VectorXd t
Definition: treeiksolvervel_wdls.hpp:86
std::map::begin
T begin(T... args)
KDL::TreeIkSolverVel_wdls::U
Eigen::MatrixXd U
Definition: treeiksolvervel_wdls.hpp:85
std::map::insert
T insert(T... args)
KDL::TreeIkSolverVel_wdls::Wq
Eigen::MatrixXd Wq
Definition: treeiksolvervel_wdls.hpp:85
KDL::TreeIkSolverVel_wdls::tree
Tree tree
Definition: treeiksolvervel_wdls.hpp:81
std::map::end
T end(T... args)
KDL::TreeIkSolverVel_wdls::TreeIkSolverVel_wdls
TreeIkSolverVel_wdls(const Tree &tree, const std::vector< std::string > &endpoints)
Child SVD failed.
Definition: treeiksolvervel_wdls.cpp:12
KDL::TreeIkSolverVel_wdls::Wy_t
Eigen::VectorXd Wy_t
Definition: treeiksolvervel_wdls.hpp:86
KDL::TreeIkSolverVel_wdls::jacobians
Jacobians jacobians
Definition: treeiksolvervel_wdls.hpp:83
KDL::TreeIkSolverVel_wdls::V
Eigen::MatrixXd V
Definition: treeiksolvervel_wdls.hpp:85
KDL::Jacobian
Definition: jacobian.hpp:36