Kinematics Class Reference#include <Kinematics.h>
Inheritance diagram for Kinematics:
[legend]List of all members.
Detailed Description
Provides access to the mathematical functionality of the ROBOOP package using Tekkotsu data structures.
You should read the Kinematics tutorial to get a general understanding of the math involved and diagrams for usage with supported robots.
This class involves all aspects of the forward kinematics: calculations concerning locations and orientations in space given a known set of joint configurations. There is a global instantiation of Kinematics named kine, which can be used to perform these calculations regarding the joint positions currently in state.
To perform kinematics on a hypothetical set of joint values, use PostureEngine or one of its subclasses. PostureEngine also adds inverse kinematic functions, which will allow you to determine joint angles in order to reach a given point.
The underlying ROBOOP library does not currently handle branching kinematic chains - in other words, each limb of the robot is a separate ROBOOP::Robot as far as ROBOOP is concerned. This Kinematics class interfaces the Tekkotsu array index approach to referencing joints with ROBOOP's chain based hierarchy.
Thus, wherever a reference frame index is requested, you can simply supply one of the output indexes in the usual manner:
However, there are also a number of points on the body which are not joints, but should have their own reference frames, such as the base frame, or the camera. These frames have their own indices, listed in the robot info file for the model in question (such as ERS7Info.h), with names ending in FrameOffset . Note that for these non-joint-associated reference frames, the link and joint frames are always identical, so you can use either version of the corresponding functions.
Since newmat matrix library is used by ROBOOP, you will need to pass and receive information in newmat matrices. Kinematics class provides static pack and unpack functions which can convert individual x,y,z variables into a NEWMAT::ColumnVector, and vice versa. However, for readability of your code and long-term ease of use, we recommend embracing the newmat data structures directly when appropriate.
Finally, for each model we have created a database of "interest points" -- locations of notable interest on the body of the robot. These may be of use to people attempting to use the limbs to manipulate objects. To access these interest points, call either getLinkInterestPoint or getJointInterestPoint with the name of the interest point, obtained from the diagrams.
Note that you can pass a comma separated list of interest point names and the result will be the midpoint of those interest points:
- See also:
- PostureEngine for inverse kinematics
Configuration File Format
The file is actually read by ROBOOP::Config, and thus the syntax of the file is defined by that class. However, Tekkotsu will look for some additional sections beyond what is expected by ROBOOP.
In any give link section, a tekkotsu_output field may appear, which specifies the index (aka offset) of the corresponding joint in Tekkotsu, as defined by the model's Info.h file (e.g. ERS7Info.h). Alternatively, tekkotsu_frame may be specified, which should specify the offset of an abstract reference frame, which does not correspond to any joint. Typically this is used for things such as the camera, or un-actuated joints, such as the spring-loaded ankles.
Additionally, Kinematics will look for an custom InterestPoints section, which should contain a Length field for specifying the number of interest points. Kinematics will then attempt to read InterestPoint N for 1 through Length.
Each InterestPoint N section should contain:
name - (string) name which will be passed to get*InterestPoint() to retrieve this IPchain - (string) name of the chain the IP is in, must match one of the kinematic chains loaded from the filelink - (unsigned int) the index of the link the IP is connected tox - (float) the x location of the point, in link-relative coordinatesy - (float) the y location of the point, in link-relative coordinatesz - (float) the z location of the point, in link-relative coordinates
Definition at line 115 of file Kinematics.h.
|
Public Member Functions |
| Kinematics () |
| Constructor, pass the full path to the kinematics configuration file.
|
virtual | ~Kinematics () |
| Destructor.
|
NEWMAT::ReturnMatrix | linkToBase (unsigned int link) |
| Returns a matrix for transforming from link frame j to base frame.
|
NEWMAT::ReturnMatrix | jointToBase (unsigned int joint) |
| Returns a matrix for transforming from joint frame j to base frame.
|
NEWMAT::ReturnMatrix | baseToLink (unsigned int link) |
| Returns a matrix for transforming from the base frame to link j frame.
|
NEWMAT::ReturnMatrix | baseToJoint (unsigned int joint) |
| Returns a matrix for transforming from the base frame to joint j frame.
|
NEWMAT::ReturnMatrix | linkToLink (unsigned int iL, unsigned int oL) |
| Returns a matrix for transforming from link ij to link oj.
|
NEWMAT::ReturnMatrix | linkToJoint (unsigned int iL, unsigned int oJ) |
| Returns a matrix for transforming from link frame ij to joint frame oj.
|
NEWMAT::ReturnMatrix | jointToLink (unsigned int iJ, unsigned int oL) |
| Returns a matrix for transforming from joint frame ij to link frame oj.
|
NEWMAT::ReturnMatrix | jointToJoint (unsigned int iJ, unsigned int oJ) |
| Returns a matrix for transforming from joint ij to joint oj.
|
void | getInterestPoint (const std::string &name, unsigned int &link, NEWMAT::Matrix &ip) |
| Returns the location of a named point and the link it is attached to.
|
NEWMAT::ReturnMatrix | getJointInterestPoint (unsigned int joint, const std::string &name) |
| Returns the location of a named point, relative to any desired joint reference frame.
|
NEWMAT::ReturnMatrix | getLinkInterestPoint (unsigned int link, const std::string &name) |
| Returns the location of a named point, relative to any desired reference frame.
|
LegOrder_t | findUnusedLeg (const NEWMAT::ColumnVector &down) |
| Find the leg which is in least contact with ground (as best we can anyway).
|
NEWMAT::ReturnMatrix | calculateGroundPlane () |
| Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).
|
NEWMAT::ReturnMatrix | calculateGroundPlane (const NEWMAT::ColumnVector &down) |
| Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).
|
NEWMAT::ReturnMatrix | projectToPlane (unsigned int j, const NEWMAT::ColumnVector &r_j, unsigned int b, const NEWMAT::ColumnVector &p_b, unsigned int f) |
| Find the point of intersection between a ray and a plane.
|
Static Public Member Functions |
static NEWMAT::ReturnMatrix | pack (float x, float y, float z, float h=1) |
| A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector.
|
static void | unpack (NEWMAT::Matrix m, float &ox, float &oy, float &oz) |
| A simple utility function, pulls the first 3 rows of the first column, divides each by the fourth row, and stores into ox, oy, and oz.
|
static void | unpack (NEWMAT::Matrix m, float &ox, float &oy, float &oz, float &oh) |
| A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh.
|
static ROBOOP::Config * | getConfig () |
| returns the global ROBOOP::Config object which Kinematics classes initialize themselves from (roconfig)
|
Protected Types |
typedef __gnu_cxx::hash_map<
const string, InterestPoint,
hashstring > | InterestPointMap |
| we'll be using the hash_map to store named interest points
|
Protected Member Functions |
void | init () |
| Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig.
|
void | getInterestPoint (const std::string &name, unsigned int &c, unsigned int &l, NEWMAT::Matrix &ip) |
| Returns the location of a named point, relative to the link it is attached to.
|
virtual void | update (unsigned int c, unsigned int l) |
| Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures.
|
bool | lookup (unsigned int tkout, unsigned int &c, unsigned int &l) |
| converts a Tekkotsu output index to a chain and link
|
Protected Attributes |
std::vector< ROBOOP::Robot * > | chains |
| A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well).
|
std::vector< std::vector<
unsigned int > > | chainMaps |
| holds mapping for each chain's links back to the tekkotsu outputs and reference frames they represent
|
JointMap | jointMaps [NumReferenceFrames] |
| holds mapping from tekkotsu output index to chain and link indicies
|
Static Protected Attributes |
static ROBOOP::Config * | roconfig = NULL |
| cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk)
|
static InterestPointMap * | ips = NULL |
| these interest points are shared by all Kinematics classes (i.e. all PostureEngines)
|
Classes |
struct | hashstring |
| allows us to use the STL hash_map with strings More...
|
struct | InterestPoint |
| holds the position and attached link of a given interest point More...
|
struct | JointMap |
| Allows mapping from tekkotsu output index to chain and link indicies. More...
|
Member Typedef Documentation
|
we'll be using the hash_map to store named interest points
Definition at line 338 of file Kinematics.h. |
Constructor & Destructor Documentation
Kinematics::Kinematics |
( |
|
) |
[inline] |
|
|
Constructor, pass the full path to the kinematics configuration file.
Definition at line 118 of file Kinematics.h. |
virtual Kinematics::~Kinematics |
( |
|
) |
[inline, virtual] |
|
Member Function Documentation
|
Returns a matrix for transforming from the base frame to joint j frame.
- Parameters:
-
[in] | joint | the output offset, see class notes for values |
Definition at line 126 of file Kinematics.cc. |
|
Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).
- Returns:
- vector of the form
, relative to the base frame
Definition at line 332 of file Kinematics.cc. |
|
Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)).
This function merely calls the other version of calculateGroundPlane with the current gravity vector as the "down" vector. - Returns:
- vector of the form
, relative to the base frame
Definition at line 327 of file Kinematics.cc.
Referenced by GroundPlaneBehavior::processEvent(). |
|
Find the leg which is in least contact with ground (as best we can anyway).
This can be either based on gravity vector from accelerometer readings, or if that may be unreliable due to being in motion, we could do some basic balance modeling. - Returns:
- index of leg which is highest in reference to gravity vector
Definition at line 310 of file Kinematics.cc.
Referenced by calculateGroundPlane(), and GroundPlaneBehavior::processEvent(). |
void Kinematics::getInterestPoint |
( |
const std::string & |
name, |
|
|
unsigned int & |
c, |
|
|
unsigned int & |
l, |
|
|
NEWMAT::Matrix & |
ip |
|
) |
[protected] |
|
|
Returns the location of a named point, relative to the link it is attached to.
- Parameters:
-
[in] | name | the name of the interest point; varies by model, see the diagrams for your model. |
[out] | c | on exit, chain index the IP is on |
[out] | l | on exit, link index the IP is on |
[out] | ip | on exit, a homogeneous column vector of the requested point |
If name is not found, c and l will be -1 and ip will be all 0's. This internal version of the function allows us to use c and l, ourselves, but users will probably want to use the getInterestPoint(name,j,ip) version
Definition at line 257 of file Kinematics.cc. |
void Kinematics::getInterestPoint |
( |
const std::string & |
name, |
|
|
unsigned int & |
link, |
|
|
NEWMAT::Matrix & |
ip |
|
) |
|
|
|
Returns the location of a named point and the link it is attached to.
- Parameters:
-
[in] | name | the name of the interest point; varies by model, see the diagrams for your model. |
[out] | link | on exit, offset of the link, or -1U if not found |
[out] | ip | on exit, a homogeneous column vector of the requested point, relative to the link frame returned in j |
If name is not found, j will be -1 and ip will be all 0's.
Definition at line 250 of file Kinematics.cc.
Referenced by getJointInterestPoint(). |
NEWMAT::ReturnMatrix Kinematics::getJointInterestPoint |
( |
unsigned int |
joint, |
|
|
const std::string & |
name |
|
) |
|
|
NEWMAT::ReturnMatrix Kinematics::getLinkInterestPoint |
( |
unsigned int |
link, |
|
|
const std::string & |
name |
|
) |
[inline] |
|
|
Returns the location of a named point, relative to any desired reference frame.
- Parameters:
-
[in] | link | the desired link reference frame to give results in |
[in] | name | the name of the interest point; varies by model, see the diagrams for your model. |
You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs
Definition at line 185 of file Kinematics.h.
Referenced by KinematicSampleBehavior2::processEvent(). |
void Kinematics::init |
( |
|
) |
[protected] |
|
|
Returns a matrix for transforming from joint ij to joint oj.
- Parameters:
-
[in] | iJ | the output offset to convert from, see class notes for values |
[in] | oJ | the output offset to convert to, see class notes for values |
Definition at line 150 of file Kinematics.cc. |
|
Returns a matrix for transforming from joint frame ij to link frame oj.
- Parameters:
-
[in] | iJ | the output offset to convert from, see class notes for values |
[in] | oL | the output offset to convert to, see class notes for values |
Definition at line 175 of file Kinematics.cc.
Referenced by getLinkInterestPoint(). |
|
Returns a matrix for transforming from link frame ij to joint frame oj.
- Parameters:
-
[in] | iL | the output offset to convert from, see class notes for values |
[in] | oJ | the output offset to convert to, see class notes for values |
Definition at line 200 of file Kinematics.cc. |
|
Returns a matrix for transforming from link ij to link oj.
- Parameters:
-
[in] | iL | the output offset to convert from, see class notes for values |
[in] | oL | the output offset to convert to, see class notes for values |
Definition at line 225 of file Kinematics.cc.
Referenced by projectToPlane(). |
bool Kinematics::lookup |
( |
unsigned int |
tkout, |
|
|
unsigned int & |
c, |
|
|
unsigned int & |
l |
|
) |
[inline, protected] |
|
static NEWMAT::ReturnMatrix Kinematics::pack |
( |
float |
x, |
|
|
float |
y, |
|
|
float |
z, |
|
|
float |
h = 1 |
|
) |
[inline, static] |
|
|
A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector.
- Parameters:
-
[in] | x | the value for the first row |
[in] | y | the value for the second row |
[in] | z | the value for the third row |
[in] | h | the value for the fourth row (defaults to 1 if not specified) |
- Returns:
Definition at line 229 of file Kinematics.h.
Referenced by calculateGroundPlane(), getInterestPoint(), WallTestBehavior::processEvent(), KinematicSampleBehavior::processEvent(), GroundPlaneBehavior::processEvent(), PostureEngine::solveLinkPosition(), and PostureEngine::solveLinkVector(). |
|
Find the point of intersection between a ray and a plane.
- Parameters:
-
| j | is the link number the ray is relative to |
| r_j | is the line through the origin, in homogeneous coordinates |
| b | is the link number the plane is relative to (probably BaseFrameOffset) |
| p_b | represents the plane to be intersected |
| f | is the link number the results should be relative to |
p_b should be of the form - Returns:
- homogeneous coordinate of intersection (may be infinity)
Definition at line 351 of file Kinematics.cc.
Referenced by GroundPlaneBehavior::processEvent(). |
static void Kinematics::unpack |
( |
NEWMAT::Matrix |
m, |
|
|
float & |
ox, |
|
|
float & |
oy, |
|
|
float & |
oz, |
|
|
float & |
oh |
|
) |
[inline, static] |
|
|
A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh.
- Parameters:
-
[in] | m | the matrix to unpack (only uses first column) |
[out] | ox | set to the first row of the first column of m |
[out] | oy | set to the second row of the first column of m |
[out] | oz | set to the third row of the first column of m |
[out] | oh | set to the fourth row of the first column of m |
Definition at line 248 of file Kinematics.h. |
static void Kinematics::unpack |
( |
NEWMAT::Matrix |
m, |
|
|
float & |
ox, |
|
|
float & |
oy, |
|
|
float & |
oz |
|
) |
[inline, static] |
|
|
A simple utility function, pulls the first 3 rows of the first column, divides each by the fourth row, and stores into ox, oy, and oz.
- Parameters:
-
[in] | m | the matrix to unpack (only uses first column) |
[out] | ox | set to the first row of the first column of m, divided by fourth row |
[out] | oy | set to the second row of the first column of m, divided by fourth row |
[out] | oz | set to the third row of the first column of m, divided by fourth row |
Definition at line 239 of file Kinematics.h.
Referenced by KinematicSampleBehavior::processEvent(). |
void Kinematics::update |
( |
unsigned int |
c, |
|
|
unsigned int |
l |
|
) |
[protected, virtual] |
|
Member Data Documentation
|
A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well).
Definition at line 297 of file Kinematics.h.
Referenced by baseToJoint(), baseToLink(), getJointInterestPoint(), init(), jointToBase(), jointToJoint(), jointToLink(), Kinematics(), linkToBase(), linkToJoint(), linkToLink(), lookup(), and update(). |
|
these interest points are shared by all Kinematics classes (i.e. all PostureEngines)
this is to reduce initialization time, but does mean one robot can't do kinematic calculations regarding a different model robot...
Definition at line 12 of file Kinematics.cc.
Referenced by getInterestPoint(), and init(). |
|
holds mapping from tekkotsu output index to chain and link indicies
Definition at line 322 of file Kinematics.h.
Referenced by init(), and lookup(). |
|
cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk)
Definition at line 11 of file Kinematics.cc.
Referenced by getConfig(), and init(). |
The documentation for this class was generated from the following files:
|