#include <KatanaKinematics6M90G.h>
Classes | |
struct | angles_calc |
struct | position |
Public Member Functions | |
void | DK (coordinates &solution, encoders const ¤t_encoders) const |
void | IK (encoders::iterator solution, coordinates const &pose, encoders const &cur_angles) const |
void | init (metrics const &length, parameter_container const ¶meters) |
Private Types | |
typedef std::vector< angles_calc > | angles_container |
Private Member Functions | |
void | _setLength (metrics const &length) |
void | _setParameters (parameter_container const ¶meters) |
bool | angledef (angles_calc &a) const |
bool | AnglePositionTest (const angles_calc &a) const |
bool | GripperTest (const position &p_gr, const angles_calc &angle) const |
void | IK_b1b2costh3_6MS (angles_calc &a, const position &p) const |
void | IK_theta234theta5 (angles_calc &angle, const position &p_gr) const |
bool | PositionTest6MS (const angles_calc &a, const position &p) const |
void | thetacomp (angles_calc &a, const position &p_m) const |
Private Attributes | |
metrics | _length |
parameter_container | _parameters |
Static Private Attributes | |
static const int | _nrOfPossibleSolutions = 8 |
static const double | _tolerance = 0.001 |
Definition at line 39 of file KatanaKinematics6M90G.h.
typedef std::vector<angles_calc> KNI::KatanaKinematics6M90G::angles_container [private] |
Definition at line 70 of file KatanaKinematics6M90G.h.
void KNI::KatanaKinematics6M90G::_setLength | ( | metrics const & | length | ) | [inline, private] |
Definition at line 78 of file KatanaKinematics6M90G.h.
void KNI::KatanaKinematics6M90G::_setParameters | ( | parameter_container const & | parameters | ) | [inline, private] |
Definition at line 79 of file KatanaKinematics6M90G.h.
bool KNI::KatanaKinematics6M90G::angledef | ( | angles_calc & | a | ) | const [private] |
Definition at line 235 of file KatanaKinematics6M90G.cpp.
bool KNI::KatanaKinematics6M90G::AnglePositionTest | ( | const angles_calc & | a | ) | const [private] |
Definition at line 259 of file KatanaKinematics6M90G.cpp.
void KNI::KatanaKinematics6M90G::DK | ( | coordinates & | solution, |
encoders const & | current_encoders | ||
) | const [virtual] |
Direct Kinematic. Calculates the actual position in cartesian coordinates using the given encoders
solution | This is where the algorithm will store the solution to (in cartesian coordinates) |
current_encoders | The encoder values which are being used for the calculation |
Implements KNI::KatanaKinematics.
Definition at line 34 of file KatanaKinematics6M90G.cpp.
bool KNI::KatanaKinematics6M90G::GripperTest | ( | const position & | p_gr, |
const angles_calc & | angle | ||
) | const [private] |
Definition at line 173 of file KatanaKinematics6M90G.cpp.
void KNI::KatanaKinematics6M90G::IK | ( | encoders::iterator | solution, |
coordinates const & | pose, | ||
encoders const & | cur_angles | ||
) | const [virtual] |
Inverse Kinematic. Calculates one set of encoders (=one solution) for the given cartesian coordinates. You also have to provide the current encoders to allow the algorithm to choose between different valid solutions.
solution | This is where the algorithm will store the solution to (in encoders) |
pose | The target position in cartesian coordinates plus the euler angles for the direction of the gripper |
cur_angles | The current angles (in encoders) of the robot |
Implements KNI::KatanaKinematics.
Definition at line 283 of file KatanaKinematics6M90G.cpp.
void KNI::KatanaKinematics6M90G::IK_b1b2costh3_6MS | ( | angles_calc & | a, |
const position & | p | ||
) | const [private] |
Definition at line 188 of file KatanaKinematics6M90G.cpp.
void KNI::KatanaKinematics6M90G::IK_theta234theta5 | ( | angles_calc & | angle, |
const position & | p_gr | ||
) | const [private] |
Definition at line 128 of file KatanaKinematics6M90G.cpp.
void KNI::KatanaKinematics6M90G::init | ( | metrics const & | length, |
parameter_container const & | parameters | ||
) | [virtual] |
Initialize the parameters for the calculations. This is needed to validate the calculated angles and to choose an appropriate solution You have to provide 5 or 6 length's and parameters, depending on you robot type
Implements KNI::KatanaKinematics.
Definition at line 119 of file KatanaKinematics6M90G.cpp.
bool KNI::KatanaKinematics6M90G::PositionTest6MS | ( | const angles_calc & | a, |
const position & | p | ||
) | const [private] |
Definition at line 219 of file KatanaKinematics6M90G.cpp.
void KNI::KatanaKinematics6M90G::thetacomp | ( | angles_calc & | a, |
const position & | p_m | ||
) | const [private] |
Definition at line 205 of file KatanaKinematics6M90G.cpp.
metrics KNI::KatanaKinematics6M90G::_length [private] |
Definition at line 72 of file KatanaKinematics6M90G.h.
const int KNI::KatanaKinematics6M90G::_nrOfPossibleSolutions = 8 [static, private] |
Definition at line 76 of file KatanaKinematics6M90G.h.
Definition at line 73 of file KatanaKinematics6M90G.h.
const double KNI::KatanaKinematics6M90G::_tolerance = 0.001 [static, private] |
Definition at line 75 of file KatanaKinematics6M90G.h.