KDL 1.5.1
Loading...
Searching...
No Matches
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
33namespace KDL
34{
367{
368 typedef std::vector<Twist> Twists;
369 typedef std::vector<Frame> Frames;
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
374public:
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
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 */
443private:
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
465private:
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
530 std::vector<segment_info, Eigen::aligned_allocator<segment_info> > results;
531
532};
533}
534
535#endif // KDL_CHAINHDSOLVER_VERESHCHAGIN_HPP
6D Inertia of a articulated body
Definition articulatedbodyinertia.hpp:40
Abstract: Acceleration constrained hybrid dynamics calculations for a chain, based on Vereshchagin 19...
Definition chainhdsolver_vereshchagin.hpp:367
std::vector< segment_info, Eigen::aligned_allocator< segment_info > > results
Definition chainhdsolver_vereshchagin.hpp:530
void final_upwards_sweep(JntArray &q_dotdot, JntArray &constraint_torques)
This method puts all acceleration contributions (constraint, bias, nullspace and parent accelerations...
Definition chainhdsolver_vereshchagin.cpp:299
Eigen::Matrix< double, 6, 1 > Vector6d
Definition chainhdsolver_vereshchagin.hpp:370
void getTotalTorque(JntArray &total_tau)
Definition chainhdsolver_vereshchagin.cpp:361
unsigned int nj
Definition chainhdsolver_vereshchagin.hpp:467
Eigen::Matrix< double, 6, 6 > Matrix6d
Definition chainhdsolver_vereshchagin.hpp:371
Eigen::VectorXd Sm
Definition chainhdsolver_vereshchagin.hpp:479
Wrench qdotdot_sum
Definition chainhdsolver_vereshchagin.hpp:482
const Chain & chain
Definition chainhdsolver_vereshchagin.hpp:466
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)
This method calculates joint space constraint torques and accelerations.
Definition chainhdsolver_vereshchagin.cpp:56
~ChainHdSolver_Vereshchagin()
Definition chainhdsolver_vereshchagin.hpp:384
Eigen::VectorXd tmpm
Definition chainhdsolver_vereshchagin.hpp:480
Jacobian alfa_N2
Definition chainhdsolver_vereshchagin.hpp:472
Frame F_total
Definition chainhdsolver_vereshchagin.hpp:483
unsigned int nc
Definition chainhdsolver_vereshchagin.hpp:469
void getContraintForceMagnitude(Eigen::VectorXd &nu_)
Definition chainhdsolver_vereshchagin.cpp:368
Eigen::VectorXd total_torques
Definition chainhdsolver_vereshchagin.hpp:481
std::vector< Frame > Frames
Definition chainhdsolver_vereshchagin.hpp:369
void constraint_calculation(const JntArray &beta)
This method calculates constraint force magnitudes.
Definition chainhdsolver_vereshchagin.cpp:264
void getTransformedLinkAcceleration(Twists &x_dotdot)
Definition chainhdsolver_vereshchagin.cpp:352
Jacobian alfa_N
Definition chainhdsolver_vereshchagin.hpp:471
Eigen::VectorXd nu
Definition chainhdsolver_vereshchagin.hpp:477
Twist acc_root
Definition chainhdsolver_vereshchagin.hpp:470
void initial_upwards_sweep(const JntArray &q, const JntArray &q_dot, const JntArray &q_dotdot, const Wrenches &f_ext)
This method calculates all cartesian space poses, twists, bias accelerations.
Definition chainhdsolver_vereshchagin.cpp:77
Eigen::MatrixXd Vm
Definition chainhdsolver_vereshchagin.hpp:475
Eigen::MatrixXd Um
Definition chainhdsolver_vereshchagin.hpp:474
unsigned int ns
Definition chainhdsolver_vereshchagin.hpp:468
virtual void updateInternalDataStructures()
Update the internal data structures.
Definition chainhdsolver_vereshchagin.cpp:49
Eigen::VectorXd nu_sum
Definition chainhdsolver_vereshchagin.hpp:478
JntArray beta_N
Definition chainhdsolver_vereshchagin.hpp:476
Eigen::Matrix< double, 6, Eigen::Dynamic > Matrix6Xd
Definition chainhdsolver_vereshchagin.hpp:372
Eigen::MatrixXd M_0_inverse
Definition chainhdsolver_vereshchagin.hpp:473
std::vector< Twist > Twists
Definition chainhdsolver_vereshchagin.hpp:368
void downwards_sweep(const Jacobian &alfa, const JntArray &ff_torques)
This method is a force balance sweep.
Definition chainhdsolver_vereshchagin.cpp:138
Definition chain.hpp:35
Definition frames.hpp:570
Definition jacobian.hpp:37
Definition jntarray.hpp:70
Solver interface supporting storage and description of the latest error.
Definition solveri.hpp:85
represents both translational and rotational velocities.
Definition frames.hpp:720
represents both translational and rotational acceleration.
Definition frames.hpp:879
Definition articulatedbodyinertia.cpp:26
std::map< std::string, Twist > Twists
Definition treeiksolver.hpp:18
std::vector< Wrench > Wrenches
Definition chainfdsolver.hpp:34
Definition chainhdsolver_vereshchagin.hpp:486
double D
Definition chainhdsolver_vereshchagin.hpp:502
Twist v
Definition chainhdsolver_vereshchagin.hpp:490
double constAccComp
Definition chainhdsolver_vereshchagin.hpp:509
ArticulatedBodyInertia H
Definition chainhdsolver_vereshchagin.hpp:497
Wrench R
Definition chainhdsolver_vereshchagin.hpp:493
Frame F
Definition chainhdsolver_vereshchagin.hpp:487
double biasAccComp
Definition chainhdsolver_vereshchagin.hpp:510
Matrix6Xd E
Definition chainhdsolver_vereshchagin.hpp:503
double u
Definition chainhdsolver_vereshchagin.hpp:512
Twist acc
Definition chainhdsolver_vereshchagin.hpp:491
Eigen::MatrixXd M
Definition chainhdsolver_vereshchagin.hpp:505
Wrench PC
Definition chainhdsolver_vereshchagin.hpp:501
Frame F_base
Definition chainhdsolver_vereshchagin.hpp:488
Wrench R_tilde
Definition chainhdsolver_vereshchagin.hpp:494
Twist Z
Definition chainhdsolver_vereshchagin.hpp:489
Eigen::VectorXd EZ
Definition chainhdsolver_vereshchagin.hpp:507
Twist A
Definition chainhdsolver_vereshchagin.hpp:496
segment_info(unsigned int nc)
Definition chainhdsolver_vereshchagin.hpp:514
double totalBias
Definition chainhdsolver_vereshchagin.hpp:511
Wrench PZ
Definition chainhdsolver_vereshchagin.hpp:500
ArticulatedBodyInertia P_tilde
Definition chainhdsolver_vereshchagin.hpp:499
double nullspaceAccComp
Definition chainhdsolver_vereshchagin.hpp:508
Matrix6Xd E_tilde
Definition chainhdsolver_vereshchagin.hpp:504
Twist C
Definition chainhdsolver_vereshchagin.hpp:495
Eigen::VectorXd G
Definition chainhdsolver_vereshchagin.hpp:506
ArticulatedBodyInertia P
Definition chainhdsolver_vereshchagin.hpp:498
Wrench U
Definition chainhdsolver_vereshchagin.hpp:492