#include <FalconKinematicStamper.h>
Inherits libnifalcon::FalconKinematic.
Public Member Functions | |
FalconKinematicStamper (bool init_now=true) | |
~FalconKinematicStamper () | |
void | initialize () |
virtual bool | getForces (const boost::array< double, 3 >(&position), const boost::array< double, 3 >(&cart_force), boost::array< int, 3 >(&enc_force)) |
virtual bool | getAngles (boost::array< double, 3 >(&position), boost::array< double, 3 >(&angles)) |
virtual bool | getPosition (boost::array< int, 3 >(&angles), boost::array< double, 3 >(&position)) |
virtual void | getWorkspaceOrigin (boost::array< double, 3 >(&origin)) |
void | FK (const gmtl::Vec3d &theta0, gmtl::Vec3d &pos) |
gmtl::Matrix33d | jacobian (const StamperKinematicImpl::Angle &angles) |
void | IK (StamperKinematicImpl::Angle &angles, const gmtl::Vec3d &worldPosition) |
Public Attributes | |
gmtl::Vec3d | pos_ |
http://docs.nonpolynomial.com/libnifalcon/pdf/StamperThesis.pdf
This implementation was written by Alastair Barrow. The original code is available in the barrow_mechanics example.
libnifalcon::FalconKinematicStamper::FalconKinematicStamper | ( | bool | init_now = true |
) |
Constructor.
init_now | If true, runs initialize() function on construction (can block). Defaults to true. |
libnifalcon::FalconKinematicStamper::~FalconKinematicStamper | ( | ) | [inline] |
Destructor
void libnifalcon::FalconKinematicStamper::FK | ( | const gmtl::Vec3d & | theta0, | |
gmtl::Vec3d & | pos | |||
) |
Implementation of Forward Kinematics equation for kinematics model, by Alastair Barrow
theta0 | Vector of joint angles to calculate end effector position from | |
pos | Vector to store calculated cartesian end effector position to |
bool libnifalcon::FalconKinematicStamper::getAngles | ( | boost::array< double, 3 > & | position, | |
boost::array< double, 3 > & | angles | |||
) | [virtual] |
Given a caretesian position (in meters), return the angle of the legs requires to achieve the positions
position | Position to get the angles for (in cartesian coordinates, meters) | |
angles | Array to write result into |
Implements libnifalcon::FalconKinematic.
bool libnifalcon::FalconKinematicStamper::getForces | ( | const boost::array< double, 3 > & | position, | |
const boost::array< double, 3 > & | cart_force, | |||
boost::array< int, 3 > & | enc_force | |||
) | [virtual] |
Given a caretesian position (in meters), and force vector (in newtons), return the force values that need to be sent to the firmware. Values are capped at 4096.
position | Current position of the end effector | |
cart_force | Force vector to apply to the end effector | |
enc_force | Force to be sent to the firmware |
Implements libnifalcon::FalconKinematic.
bool libnifalcon::FalconKinematicStamper::getPosition | ( | boost::array< int, 3 > & | angles, | |
boost::array< double, 3 > & | position | |||
) | [virtual] |
Given a set of encoder values, return the cartesian position (in meters) of the end effector in relation to the origin. Note: Origin subject to change based on kinematics system. Use the workspaceOrigin() function to get what the system thinks its origin is.
angles | Encoder values for the 3 legs | |
position | Array to write result into |
Implements libnifalcon::FalconKinematic.
virtual void libnifalcon::FalconKinematicStamper::getWorkspaceOrigin | ( | boost::array< double, 3 > & | origin | ) | [inline, virtual] |
Returns the center point of the workspace. May not always be [0,0,0].
origin | Array to store values in |
Implements libnifalcon::FalconKinematic.
void libnifalcon::FalconKinematicStamper::IK | ( | StamperKinematicImpl::Angle & | angles, | |
const gmtl::Vec3d & | worldPosition | |||
) |
Implementation of Inverse Kinematics equation for kinematics model, by Alastair Barrow
angles | Angle structure to store calculated joint angles to | |
worldPosition | Current cartesian position of end effector |
void libnifalcon::FalconKinematicStamper::initialize | ( | ) |
Initializes lookup tables for kinematics (can block)
gmtl::Matrix33d libnifalcon::FalconKinematicStamper::jacobian | ( | const StamperKinematicImpl::Angle & | angles | ) |
Implementation of jacobian for kinematics model, by Alastair Barrow
angles | Current joint angles |
gmtl::Vec3d libnifalcon::FalconKinematicStamper::pos_ |
Internal position state