Implements the kinematics for the Katana6M180. More...
#include <kinematics6M180.h>
Classes | |
struct | angles_calc |
struct | position |
structs, type and constants used in inverse kinematics calculation More... | |
Public Member Functions | |
bool | directKinematics (std::vector< double > &aPosition, const std::vector< double > aAngles) |
bool | enc2rad (std::vector< double > &aAngles, const std::vector< int > aEncoders) |
std::vector< double > | getAngMax () |
get angle max More... | |
std::vector< double > | getAngMin () |
get angle min More... | |
std::vector< double > | getAngOff () |
get angle offset More... | |
std::vector< double > | getAngRange () |
get angle range More... | |
std::vector< double > | getAngStop () |
get angle stop More... | |
std::vector< int > | getDir () |
get direction More... | |
std::vector< int > | getEncOff () |
get encoder offset More... | |
std::vector< int > | getEpc () |
get encoders per cycle More... | |
std::vector< double > | getLinkLength () |
get link length More... | |
bool | inverseKinematics (std::vector< double > &aAngles, const std::vector< double > aPosition, const std::vector< double > aStartingAngles) |
Kinematics6M180 () | |
Constructor. More... | |
bool | rad2enc (std::vector< int > &aEncoders, const std::vector< double > aAngles) |
bool | setAngOff (const std::vector< double > aAngOff) |
set angle offset More... | |
bool | setAngStop (const std::vector< double > aAngStop) |
set angle stop More... | |
bool | setLinkLength (const std::vector< double > aLengths) |
set link length More... | |
~Kinematics6M180 () | |
Destructor. More... | |
Public Member Functions inherited from AnaGuess::Kinematics | |
virtual | ~Kinematics () |
Virtual destructor. More... | |
Private Types | |
typedef std::vector< angles_calc > | angles_container |
Private Member Functions | |
bool | angledef (angles_calc &a) const |
bool | AnglePositionTest (const angles_calc &a) const |
void | IK_b1b2costh3_6M180 (angles_calc &a, const position &p) const |
helper functions More... | |
bool | initialize () |
initialization routine More... | |
bool | PositionTest6M180 (const angles_calc &a, const position &p) const |
void | thetacomp (angles_calc &a, const position &p_m) const |
Private Attributes | |
std::vector< double > | mAngleOffset |
Angle offset vector [rad]. More... | |
std::vector< double > | mAngleStop |
Angle stop vector [rad]. More... | |
std::vector< int > | mEncoderOffset |
Encoder offset vector. More... | |
std::vector< int > | mEncodersPerCycle |
Encoders per cycle vector. More... | |
bool | mIsInitialized |
Initialized flag. More... | |
int | mNumberOfMotors |
Number of motors of the robot. More... | |
int | mNumberOfSegments |
Number of segments of the robot. More... | |
std::vector< int > | mRotationDirection |
Rotation direction vector [1|-1]. More... | |
std::vector< double > | mSegmentLength |
Effector segment lengths vector [m]. More... | |
Static Private Attributes | |
static const int | cNrOfPossibleSolutions = 8 |
Implements the kinematics for the Katana6M180.
This class implemets the abstract kinematics interface for the Katana6M180
Definition at line 30 of file kinematics6M180.h.
|
private |
Definition at line 68 of file kinematics6M180.h.
AnaGuess::Kinematics6M180::Kinematics6M180 | ( | ) |
Constructor.
Definition at line 11 of file kinematics6M180.cpp.
AnaGuess::Kinematics6M180::~Kinematics6M180 | ( | ) |
Destructor.
Definition at line 15 of file kinematics6M180.cpp.
|
private |
Definition at line 473 of file kinematics6M180.cpp.
|
private |
Definition at line 494 of file kinematics6M180.cpp.
|
virtual |
calculates the direct kinematics
aPosition | empty vector to store the position |
aAngles | the angle vector |
Implements AnaGuess::Kinematics.
Definition at line 140 of file kinematics6M180.cpp.
|
virtual |
calculates the angles corresponding to the given encoders
aAngles | empty vector to store the angles |
aEncoders | the encoder vector |
Implements AnaGuess::Kinematics.
Definition at line 125 of file kinematics6M180.cpp.
|
virtual |
|
virtual |
|
virtual |
get angle offset
Implements AnaGuess::Kinematics.
Definition at line 39 of file kinematics6M180.cpp.
|
virtual |
|
virtual |
|
virtual |
|
virtual |
get encoder offset
Implements AnaGuess::Kinematics.
Definition at line 29 of file kinematics6M180.cpp.
|
virtual |
get encoders per cycle
Implements AnaGuess::Kinematics.
Definition at line 24 of file kinematics6M180.cpp.
|
virtual |
|
private |
helper functions
Definition at line 439 of file kinematics6M180.cpp.
|
privatevirtual |
initialization routine
Implements AnaGuess::Kinematics.
Definition at line 382 of file kinematics6M180.cpp.
|
virtual |
caltulates the inverse kinematics
aAngles | empty vector to store the angles |
aPosition | the position vector |
aStartingAngles | starting angle vector to find the best (nearest) solution |
NoSolutionException | if no solutios exists |
Implements AnaGuess::Kinematics.
Definition at line 202 of file kinematics6M180.cpp.
|
private |
Definition at line 459 of file kinematics6M180.cpp.
|
virtual |
calculates the encoders corresponding to the given angles
aEncoders | empty vector to store the encoders |
aAngles | the angle vector |
Implements AnaGuess::Kinematics.
Definition at line 132 of file kinematics6M180.cpp.
|
virtual |
set angle offset
Implements AnaGuess::Kinematics.
Definition at line 100 of file kinematics6M180.cpp.
|
virtual |
|
virtual |
|
private |
Definition at line 448 of file kinematics6M180.cpp.
|
staticprivate |
Definition at line 69 of file kinematics6M180.h.
|
private |
Angle offset vector [rad].
Definition at line 39 of file kinematics6M180.h.
|
private |
Angle stop vector [rad].
Definition at line 41 of file kinematics6M180.h.
|
private |
Encoder offset vector.
Definition at line 45 of file kinematics6M180.h.
|
private |
Encoders per cycle vector.
Definition at line 43 of file kinematics6M180.h.
|
private |
Initialized flag.
Definition at line 49 of file kinematics6M180.h.
|
private |
Number of motors of the robot.
Definition at line 33 of file kinematics6M180.h.
|
private |
Number of segments of the robot.
Definition at line 35 of file kinematics6M180.h.
|
private |
Rotation direction vector [1|-1].
Definition at line 47 of file kinematics6M180.h.
|
private |
Effector segment lengths vector [m].
Definition at line 37 of file kinematics6M180.h.