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


orocos_kdl
Author(s):
autogenerated on Fri Mar 12 2021 03:05:44