orocos_kdl
chainhdsolver_vereshchagin.hpp
Go to the documentation of this file.
1 // Copyright (C) 2009 Ruben Smits <ruben dot smits at intermodalics dot eu>
2 
3 // Version: 1.0
4 // Author: Ruben Smits <ruben dot smits at intermodalics dot eu>
5 // Author: Herman Bruyninckx
6 // Author: Azamat Shakhimardanov
7 // Maintainer: Ruben Smits <ruben dot smits at intermodalics dot eu>
8 // URL: http://www.orocos.org/kdl
9 
10 // This library is free software; you can redistribute it and/or
11 // modify it under the terms of the GNU Lesser General Public
12 // License as published by the Free Software Foundation; either
13 // version 2.1 of the License, or (at your option) any later version.
14 
15 // This library is distributed in the hope that it will be useful,
16 // but WITHOUT ANY WARRANTY; without even the implied warranty of
17 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
18 // Lesser General Public License for more details.
19 
20 // You should have received a copy of the GNU Lesser General Public
21 // License along with this library; if not, write to the Free Software
22 // Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
23 
24 #ifndef KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
25 #define KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
26 
27 #include "chainidsolver.hpp"
28 #include "frames.hpp"
30 
31 #include<Eigen/StdVector>
32 
33 namespace KDL
34 {
367 {
370  typedef Eigen::Matrix<double, 6, 1 > Vector6d;
371  typedef Eigen::Matrix<double, 6, 6 > Matrix6d;
372  typedef Eigen::Matrix<double, 6, Eigen::Dynamic> Matrix6Xd;
373 
374 public:
382  ChainHdSolver_Vereshchagin(const Chain& chain, const Twist &root_acc, const unsigned int nc);
383 
385  {
386  };
387 
405  int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian& alfa, const JntArray& beta, const Wrenches& f_ext, const JntArray &ff_torques, JntArray &constraint_torques);
406 
408  virtual void updateInternalDataStructures();
409 
410  //Returns cartesian acceleration of links in base coordinates
411  void getTransformedLinkAcceleration(Twists& x_dotdot);
412 
413  // Returns total torque acting on each joint (constraints + nature + external forces)
414  void getTotalTorque(JntArray &total_tau);
415 
416  // Returns magnitude of the constraint forces acting on the end-effector: Lagrange Multiplier
417  void getContraintForceMagnitude(Eigen::VectorXd &nu_);
418 
419  /*
420  //Returns cartesian positions of links in base coordinates
421  void getLinkCartesianPose(Frames& x_base);
422  //Returns cartesian velocities of links in base coordinates
423  void getLinkCartesianVelocity(Twists& xDot_base);
424  //Returns cartesian acceleration of links in base coordinates
425  void getLinkCartesianAcceleration(Twists& xDotDot_base);
426  //Returns cartesian positions of links in link tip coordinates
427  void getLinkPose(Frames& x_local);
428  //Returns cartesian velocities of links in link tip coordinates
429  void getLinkVelocity(Twists& xDot_local);
430  //Returns cartesian acceleration of links in link tip coordinates
431  void getLinkAcceleration(Twists& xDotdot_local);
432  //Acceleration energy due to unit constraint forces at the end-effector
433  void getLinkUnitForceAccelerationEnergy(Eigen::MatrixXd& M);
434  //Acceleration energy due to arm configuration: bias force plus input joint torques
435  void getLinkBiasForceAcceleratoinEnergy(Eigen::VectorXd& G);
436 
437  void getLinkUnitForceMatrix(Matrix6Xd& E_tilde);
438 
439  void getLinkBiasForceMatrix(Wrenches& R_tilde);
440 
441  void getJointBiasAcceleration(JntArray &bias_q_dotdot);
442  */
443 private:
448  void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches& f_ext);
453  void downwards_sweep(const Jacobian& alfa, const JntArray& ff_torques);
458  void constraint_calculation(const JntArray& beta);
463  void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques);
464 
465 private:
466  const Chain& chain;
467  unsigned int nj;
468  unsigned int ns;
469  unsigned int nc;
473  Eigen::MatrixXd M_0_inverse;
474  Eigen::MatrixXd Um;
475  Eigen::MatrixXd Vm;
477  Eigen::VectorXd nu;
478  Eigen::VectorXd nu_sum;
479  Eigen::VectorXd Sm;
480  Eigen::VectorXd tmpm;
481  Eigen::VectorXd total_torques; // all the contributions that are felt at the joint: constraints + nature + external forces
484 
486  {
487  Frame F; //local pose with respect to previous link in segments coordinates
488  Frame F_base; // pose of a segment in root coordinates
489  Twist Z; //Unit twist
490  Twist v; //twist
491  Twist acc; //acceleration twist
492  Wrench U; //wrench p of the bias forces (in cartesian space)
493  Wrench R; //wrench p of the bias forces
494  Wrench R_tilde; //vector of wrench p of the bias forces (new) in matrix form
495  Twist C; //constraint
496  Twist A; //constraint
497  ArticulatedBodyInertia H; //I (expressed in 6*6 matrix)
498  ArticulatedBodyInertia P; //I (expressed in 6*6 matrix)
499  ArticulatedBodyInertia P_tilde; //I (expressed in 6*6 matrix)
500  Wrench PZ; //vector U[i] = I_A[i]*S[i]
501  Wrench PC; //vector E[i] = I_A[i]*c[i]
502  double D; //vector D[i] = S[i]^T*U[i]
503  Matrix6Xd E; //matrix with virtual unit constraint force due to acceleration constraints
505  Eigen::MatrixXd M; //acceleration energy already generated at link i
506  Eigen::VectorXd G; //magnitude of the constraint forces already generated at link i
507  Eigen::VectorXd EZ; //K[i] = Etiltde'*Z
508  double nullspaceAccComp; //Azamat: constribution of joint space u[i] forces to joint space acceleration
509  double constAccComp; //Azamat: constribution of joint space constraint forces to joint space acceleration
510  double biasAccComp; //Azamat: constribution of joint space bias forces to joint space acceleration
511  double totalBias; //Azamat: R+PC (centrepital+coriolis) in joint subspace
512  double u; //vector u[i] = torques(i) - S[i]^T*(p_A[i] + I_A[i]*C[i]) in joint subspace. Azamat: In code u[i] = torques(i) - s[i].totalBias
513 
514  segment_info(unsigned int nc):
516  {
517  E.resize(6, nc);
518  E_tilde.resize(6, nc);
519  G.resize(nc);
520  M.resize(nc, nc);
521  EZ.resize(nc);
522  E.setZero();
523  E_tilde.setZero();
524  M.setZero();
525  G.setZero();
526  EZ.setZero();
527  };
528  };
529 
531 
532 };
533 }
534 
535 #endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
KDL::ChainHdSolver_Vereshchagin::Frames
std::vector< Frame > Frames
Definition: chainhdsolver_vereshchagin.hpp:369
KDL::ChainHdSolver_Vereshchagin::segment_info::A
Twist A
Definition: chainhdsolver_vereshchagin.hpp:496
frames.hpp
KDL::ChainHdSolver_Vereshchagin::segment_info::E
Matrix6Xd E
Definition: chainhdsolver_vereshchagin.hpp:503
KDL::ChainHdSolver_Vereshchagin::downwards_sweep
void downwards_sweep(const Jacobian &alfa, const JntArray &ff_torques)
Definition: chainhdsolver_vereshchagin.cpp:138
KDL::JntArray
Definition: jntarray.hpp:69
KDL::ChainHdSolver_Vereshchagin::ChainHdSolver_Vereshchagin
ChainHdSolver_Vereshchagin(const Chain &chain, const Twist &root_acc, const unsigned int nc)
Definition: chainhdsolver_vereshchagin.cpp:31
KDL::ChainHdSolver_Vereshchagin::nc
unsigned int nc
Definition: chainhdsolver_vereshchagin.hpp:469
KDL::ChainHdSolver_Vereshchagin::initial_upwards_sweep
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
Definition: chainhdsolver_vereshchagin.cpp:77
KDL::ChainHdSolver_Vereshchagin::segment_info::C
Twist C
Definition: chainhdsolver_vereshchagin.hpp:495
KDL::ChainHdSolver_Vereshchagin::acc_root
Twist acc_root
Definition: chainhdsolver_vereshchagin.hpp:470
KDL::ChainHdSolver_Vereshchagin::alfa_N
Jacobian alfa_N
Definition: chainhdsolver_vereshchagin.hpp:471
std::vector
KDL::ChainHdSolver_Vereshchagin::segment_info::PC
Wrench PC
Definition: chainhdsolver_vereshchagin.hpp:501
KDL::ChainHdSolver_Vereshchagin::chain
const Chain & chain
Definition: chainhdsolver_vereshchagin.hpp:466
KDL::ChainHdSolver_Vereshchagin::qdotdot_sum
Wrench qdotdot_sum
Definition: chainhdsolver_vereshchagin.hpp:482
KDL::ChainHdSolver_Vereshchagin::segment_info
Definition: chainhdsolver_vereshchagin.hpp:485
KDL::ChainHdSolver_Vereshchagin::segment_info::acc
Twist acc
Definition: chainhdsolver_vereshchagin.hpp:491
KDL::ArticulatedBodyInertia
6D Inertia of a articulated body
Definition: articulatedbodyinertia.hpp:40
KDL::ChainHdSolver_Vereshchagin::total_torques
Eigen::VectorXd total_torques
Definition: chainhdsolver_vereshchagin.hpp:481
KDL::ChainHdSolver_Vereshchagin::Twists
std::vector< Twist > Twists
Definition: chainhdsolver_vereshchagin.hpp:368
KDL::ChainHdSolver_Vereshchagin::nu_sum
Eigen::VectorXd nu_sum
Definition: chainhdsolver_vereshchagin.hpp:478
KDL::ChainHdSolver_Vereshchagin::~ChainHdSolver_Vereshchagin
~ChainHdSolver_Vereshchagin()
Definition: chainhdsolver_vereshchagin.hpp:384
KDL::ChainHdSolver_Vereshchagin::getTotalTorque
void getTotalTorque(JntArray &total_tau)
Definition: chainhdsolver_vereshchagin.cpp:361
KDL::ChainHdSolver_Vereshchagin::Matrix6Xd
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition: chainhdsolver_vereshchagin.hpp:372
KDL::ChainHdSolver_Vereshchagin::segment_info::constAccComp
double constAccComp
Definition: chainhdsolver_vereshchagin.hpp:509
KDL
Definition: kukaLWR_DHnew.cpp:25
KDL::ChainHdSolver_Vereshchagin::segment_info::PZ
Wrench PZ
Definition: chainhdsolver_vereshchagin.hpp:500
KDL::ChainHdSolver_Vereshchagin::Sm
Eigen::VectorXd Sm
Definition: chainhdsolver_vereshchagin.hpp:479
KDL::ChainHdSolver_Vereshchagin::segment_info::biasAccComp
double biasAccComp
Definition: chainhdsolver_vereshchagin.hpp:510
KDL::ChainHdSolver_Vereshchagin::updateInternalDataStructures
virtual void updateInternalDataStructures()
Definition: chainhdsolver_vereshchagin.cpp:49
KDL::Twist
represents both translational and rotational velocities.
Definition: frames.hpp:723
KDL::ChainHdSolver_Vereshchagin::segment_info::Z
Twist Z
Definition: chainhdsolver_vereshchagin.hpp:489
KDL::ChainHdSolver_Vereshchagin::segment_info::R_tilde
Wrench R_tilde
Definition: chainhdsolver_vereshchagin.hpp:494
KDL::ChainHdSolver_Vereshchagin::M_0_inverse
Eigen::MatrixXd M_0_inverse
Definition: chainhdsolver_vereshchagin.hpp:473
KDL::ChainHdSolver_Vereshchagin
Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 19...
Definition: chainhdsolver_vereshchagin.hpp:366
KDL::ChainHdSolver_Vereshchagin::getContraintForceMagnitude
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
Definition: chainhdsolver_vereshchagin.cpp:368
KDL::ChainHdSolver_Vereshchagin::segment_info::v
Twist v
Definition: chainhdsolver_vereshchagin.hpp:490
KDL::Wrench
represents both translational and rotational acceleration.
Definition: frames.hpp:881
KDL::ChainHdSolver_Vereshchagin::constraint_calculation
void constraint_calculation(const JntArray &beta)
Definition: chainhdsolver_vereshchagin.cpp:264
KDL::ChainHdSolver_Vereshchagin::segment_info::D
double D
Definition: chainhdsolver_vereshchagin.hpp:502
chainidsolver.hpp
KDL::Frame
represents a frame transformation in 3D space (rotation + translation)
Definition: frames.hpp:572
KDL::ChainHdSolver_Vereshchagin::Matrix6d
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition: chainhdsolver_vereshchagin.hpp:371
KDL::ChainHdSolver_Vereshchagin::segment_info::segment_info
segment_info(unsigned int nc)
Definition: chainhdsolver_vereshchagin.hpp:514
KDL::ChainHdSolver_Vereshchagin::segment_info::EZ
Eigen::VectorXd EZ
Definition: chainhdsolver_vereshchagin.hpp:507
KDL::ChainHdSolver_Vereshchagin::segment_info::U
Wrench U
Definition: chainhdsolver_vereshchagin.hpp:492
KDL::ChainHdSolver_Vereshchagin::segment_info::R
Wrench R
Definition: chainhdsolver_vereshchagin.hpp:493
KDL::ChainHdSolver_Vereshchagin::nj
unsigned int nj
Definition: chainhdsolver_vereshchagin.hpp:467
KDL::ChainHdSolver_Vereshchagin::Vm
Eigen::MatrixXd Vm
Definition: chainhdsolver_vereshchagin.hpp:475
std::map< std::string, Twist >
KDL::ChainHdSolver_Vereshchagin::segment_info::M
Eigen::MatrixXd M
Definition: chainhdsolver_vereshchagin.hpp:505
KDL::ChainHdSolver_Vereshchagin::tmpm
Eigen::VectorXd tmpm
Definition: chainhdsolver_vereshchagin.hpp:480
KDL::ChainHdSolver_Vereshchagin::results
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Definition: chainhdsolver_vereshchagin.hpp:530
KDL::SolverI
Definition: solveri.hpp:84
KDL::ChainHdSolver_Vereshchagin::segment_info::P
ArticulatedBodyInertia P
Definition: chainhdsolver_vereshchagin.hpp:498
KDL::ChainHdSolver_Vereshchagin::F_total
Frame F_total
Definition: chainhdsolver_vereshchagin.hpp:483
KDL::ChainHdSolver_Vereshchagin::Um
Eigen::MatrixXd Um
Definition: chainhdsolver_vereshchagin.hpp:474
articulatedbodyinertia.hpp
KDL::ChainHdSolver_Vereshchagin::final_upwards_sweep
void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques)
Definition: chainhdsolver_vereshchagin.cpp:299
KDL::ChainHdSolver_Vereshchagin::segment_info::P_tilde
ArticulatedBodyInertia P_tilde
Definition: chainhdsolver_vereshchagin.hpp:499
KDL::ChainHdSolver_Vereshchagin::segment_info::H
ArticulatedBodyInertia H
Definition: chainhdsolver_vereshchagin.hpp:497
KDL::ChainHdSolver_Vereshchagin::segment_info::F_base
Frame F_base
Definition: chainhdsolver_vereshchagin.hpp:488
KDL::ChainHdSolver_Vereshchagin::segment_info::nullspaceAccComp
double nullspaceAccComp
Definition: chainhdsolver_vereshchagin.hpp:508
KDL::ChainHdSolver_Vereshchagin::segment_info::G
Eigen::VectorXd G
Definition: chainhdsolver_vereshchagin.hpp:506
KDL::ChainHdSolver_Vereshchagin::alfa_N2
Jacobian alfa_N2
Definition: chainhdsolver_vereshchagin.hpp:472
KDL::ChainHdSolver_Vereshchagin::ns
unsigned int ns
Definition: chainhdsolver_vereshchagin.hpp:468
KDL::ChainHdSolver_Vereshchagin::segment_info::E_tilde
Matrix6Xd E_tilde
Definition: chainhdsolver_vereshchagin.hpp:504
KDL::ChainHdSolver_Vereshchagin::Vector6d
Eigen::Matrix< double, 6, 1 > Vector6d
Definition: chainhdsolver_vereshchagin.hpp:370
KDL::ChainHdSolver_Vereshchagin::beta_N
JntArray beta_N
Definition: chainhdsolver_vereshchagin.hpp:476
KDL::ChainHdSolver_Vereshchagin::segment_info::u
double u
Definition: chainhdsolver_vereshchagin.hpp:512
KDL::Chain
This class encapsulates a serial kinematic interconnection structure. It is built out of segments.
Definition: chain.hpp:35
KDL::ChainHdSolver_Vereshchagin::CartToJnt
int CartToJnt(const JntArray &q, const JntArray &q_dot, JntArray &q_dotdot, const Jacobian &alfa, const JntArray &beta, const Wrenches &f_ext, const JntArray &ff_torques, JntArray &constraint_torques)
Definition: chainhdsolver_vereshchagin.cpp:56
KDL::ChainHdSolver_Vereshchagin::segment_info::totalBias
double totalBias
Definition: chainhdsolver_vereshchagin.hpp:511
KDL::ChainHdSolver_Vereshchagin::nu
Eigen::VectorXd nu
Definition: chainhdsolver_vereshchagin.hpp:477
KDL::ChainHdSolver_Vereshchagin::getTransformedLinkAcceleration
void getTransformedLinkAcceleration(Twists &x_dotdot)
Definition: chainhdsolver_vereshchagin.cpp:352
KDL::Jacobian
Definition: jacobian.hpp:36
KDL::ChainHdSolver_Vereshchagin::segment_info::F
Frame F
Definition: chainhdsolver_vereshchagin.hpp:487