KatanaKinematics5M180.cpp
Go to the documentation of this file.
1 /***************************************************************************
2  * Copyright (C) 2006 by Tiziano Mueller *
3  * tiziano.mueller@neuronics.ch *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the *
17  * Free Software Foundation, Inc., *
18  * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19  ***************************************************************************/
20 
23 
24 namespace KNI {
25 
26 const double KatanaKinematics5M180::_tolerance = 0.001;
28 
29 void
30 KatanaKinematics5M180::DK(coordinates& solution, encoders const& current_encoders) const {
31  using namespace KNI_MHF;
32  // numbering the angles by 0-4
33  double a, b, c, alpha1, alpha2;
34  coordinates pose(6, 0);
35 
36  // size has to be 5
37  angles current_angles(5);
38  for(unsigned int z = 0; z < current_encoders.size(); ++z) {
39  current_angles[z] = enc2rad(current_encoders[z], _parameters[z].angleOffset, _parameters[z].epc, _parameters[z].encOffset, _parameters[z].rotDir);
40  }
41 
42  a = _length[1]+_length[2];
43  b = _length[0];
44  c = sqrt( pow2(a)+pow2(b)-2*a*b*cos(current_angles[2]));// law of cosinef
45  alpha1 = asin(a*sin(current_angles[2])/c); // law of sine
46  alpha2 = current_angles[1]-alpha1;
47 
48  pose[0] = c*cos(alpha2)*cos(current_angles[0]);
49  pose[1] = c*cos(alpha2)*sin(current_angles[0]);
50  pose[2] = c*sin(alpha2);
51 
52  std::swap(solution, pose);
53 }
54 
55 void
56 KatanaKinematics5M180::init( metrics const& length, parameter_container const& parameters ) {
57  assert( (length.size() == 3) && "You have to provide the metrics for exactly 3 links" ); // we have 3 links
58  assert( (parameters.size() == 5) && "You have to provide exactly 5 motor parameters" ); // 5 motors are used for IK calculations
59  _setLength( length );
60  _setParameters ( parameters );
61 }
62 
63 
64 void
65 KatanaKinematics5M180::IK(encoders::iterator solution, coordinates const& pose, encoders const& current_encoders) const {
66  using namespace KNI_MHF;
67 
68  // pose: Winkel Deg->Rad
69  // Nur eine Lösung
70 
71  // Declarations
72  position p_m;
74 
75  double dist, a, b;
76  double alpha1Px, alpha2Px;
77 
78  // Initialization
79  p_m.x = pose[0];
80  p_m.y = pose[1];
81  p_m.z = pose[2];
82 
83  dist = sqrt(pow2(p_m.x)+pow2(p_m.y)+pow2(p_m.z));
84  alpha2Px = asin(p_m.z/dist);
85 
86  angle[0].theta1 = atan1(p_m.x,p_m.y);
87  if (angle[0].theta1 > (2*M_PI+_parameters[0].angleOffset))
88  angle[0].theta1 -= 2*M_PI;
89 
90  a = _length[1]+_length[2];
91  b = _length[0];
92  angle[0].theta3 = acos((pow2(a)+pow2(b)-pow2(dist))/(2*a*b)); // law of cosinef
93  if (angle[0].theta3 > (2*M_PI+_parameters[2].angleOffset))
94  angle[0].theta3 -= 2*M_PI;
95 
96  alpha1Px = asin(a*sin(angle[0].theta3)/dist);
97  angle[0].theta2 = alpha1Px+alpha2Px;
98  if (angle[0].theta2 > (2*M_PI+_parameters[1].angleOffset))
99  angle[0].theta2 -= 2*M_PI;
100 
101  std::vector<int> temp_solution(5);
102 
103  temp_solution[0] = rad2enc(angle[0].theta1, _parameters[0].angleOffset, _parameters[0].epc, _parameters[0].encOffset, _parameters[0].rotDir);
104  temp_solution[1] = rad2enc(angle[0].theta2, _parameters[1].angleOffset, _parameters[1].epc, _parameters[1].encOffset, _parameters[1].rotDir);
105 
106  temp_solution[2] = rad2enc(angle[0].theta3, _parameters[2].angleOffset, _parameters[2].epc, _parameters[2].encOffset, _parameters[2].rotDir);
107  temp_solution[3] = current_encoders[3]; // copy encoders
108  temp_solution[4] = current_encoders[4]; // copy encoders
109 
110  std::copy(temp_solution.begin(), temp_solution.end(), solution);
111 
112 }
113 
114 
115 
116 } // NAMESPACE: KNI
117 
_encT rad2enc(_angleT const &angle, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
std::vector< double > angles
Being used to store angles (in radian).
static const int _nrOfPossibleSolutions
void _setParameters(parameter_container const &parameters)
void swap(Matrix &A, Matrix &B)
Definition: newmat.h:2159
FloatVector * pose
std::vector< KinematicParameters > parameter_container
_T atan1(_T in1, _T in2)
Definition: kinematics.h:36
FloatVector FloatVector * a
FloatVector * angle
_angleT enc2rad(_encT const &enc, _angleT const &angleOffset, _encT const &epc, _encT const &encOffset, _encT const &rotDir)
void IK(encoders::iterator solution, coordinates const &pose, encoders const &cur_angles) const
void DK(coordinates &solution, encoders const &current_encoders) const
void _setLength(metrics const &length)
std::vector< double > coordinates
To store coordinates.
std::vector< int > encoders
To store encoders.
void init(metrics const &length, parameter_container const &parameters)
std::vector< double > metrics
To store metrics, &#39;aka&#39; the length&#39;s of the different segments of the robot.
std::vector< angles_calc > angles_container
Definition: Timer.h:30


kni
Author(s): Martin Günther
autogenerated on Fri Jan 3 2020 04:01:16