se2ez
|
A representation of a robot (in this case, a kinematic tree in the plane). More...
#include <robot.h>
Classes | |
class | FrameData |
All associated data with a frame in a kinematic tree. More... | |
class | FrameDataConstPtr |
A const shared pointer wrapper for se2ez::Robot::FrameData. More... | |
class | FrameDataPtr |
A shared pointer wrapper for se2ez::Robot::FrameData. More... | |
Public Member Functions | |
Constructors | |
Robot () | |
Constructor. Nothing interesting right now. More... | |
Robot (const Robot &)=delete | |
Robot (Robot &&)=delete | |
Getters and Setters | |
unsigned int | getNumJoints () const |
Gets the number of controllable joints within the tree. More... | |
const std::vector< std::string > & | getEEs () const |
Gets the names of end-effectors of the tree (leaf nodes). More... | |
const std::vector< std::string > & | getFrameNames () const |
Get a vector of all the frame names within this robot. More... | |
FramePtr | getFrame (const std::string &name) |
Get the frame with name name. More... | |
FramePtr | getFrameParent (const std::string &name) |
Get the parent of frame with name name. Throws Error if name is root. More... | |
const FramePtr & | getFrameConst (const std::string &name) const |
Get the frame with name name. More... | |
bool | inTree (const std::string &name) const |
Checks if a frame name is in the tree. More... | |
const FrameDataPtr & | getFrameDataConst (const std::string &frame_name) const |
Retrieves kinematic tree data associated with the frame frame_name. More... | |
const std::string & | getSignature () |
Returns the signature of the robot. More... | |
void | setNamedState (const std::string &name, const StatePtr &state) |
Sets a named state. More... | |
void | setNamedStates (const std::map< std::string, StatePtr > &named) |
Sets a named state. More... | |
void | getNamedState (const std::string &name, StatePtr state) |
Gets a named state. More... | |
std::vector< std::string > | getNamedStates () const |
Gets the names of all named states. More... | |
const std::map< std::string, StatePtr > & | getNamedStatesMap () |
Gets the names of all named states. More... | |
ACMPtr | getACM () |
Gets the robot's default ACM. More... | |
void | setACM (const ACMPtr &acm) |
Sets the robot's default ACM. More... | |
const Eigen::VectorXd | getUpperLimits () const |
Get the upper limits of the joints for this robot. More... | |
const Eigen::VectorXd | getLowerLimits () const |
Get the lower limits of the joints for this robot. More... | |
const std::vector< bool > | getContinuity () const |
Returns a vector of booleans. If an entry is true, that index is a continuous joint. More... | |
const std::vector< std::pair< std::string, FrameDataPtr > > & | getJoints () const |
Get the ordered list of active joints in this robot with their associated frame information. More... | |
const KDL::Tree & | getTree () const |
Get the underlying KDL tree for this robot. More... | |
Private Member Functions | |
Getters / Setters | |
FrameDataPtr | getFrameData (const std::string &frame_name) |
Retrieves kinematic tree data associated with the frame frame_name. More... | |
Private Attributes | |
std::map< std::string, FrameDataPtr > | data_ |
Kinematic tree data for all frames. More... | |
std::vector< std::string > | ees_ |
End-effectors (leaves) in the kinematic tree that are mobile. More... | |
std::vector< std::string > | names_ |
Names of the the kinematic tree frames (ordered). More... | |
std::vector< std::pair< std::string, FrameDataPtr > > | joints_ |
Ordered vector of active joints. More... | |
Eigen::VectorXd | upper_ |
Upper limits of joints. More... | |
Eigen::VectorXd | lower_ |
Lower limits of joints. More... | |
unsigned int | maxDepth_ |
Maximum tree depth. More... | |
KDL::Tree | tree_ |
Underlying tree representation. More... | |
std::shared_ptr< KDL::TreeJntToJacSolver > | jac_ {nullptr} |
Underlying tree representation. More... | |
bool | tree_update_ {true} |
Does the tree need recompiling? More... | |
std::map< std::string, StatePtr > | named_ |
Named states. More... | |
std::string | signature_ |
A string that represents the current kinematic chain. More... | |
ACMPtr | acm_ |
Default robot ACM. More... | |
FrameMapPtr | static_ {nullptr} |
Transforms for all non-movable links. More... | |
Frame / Kinematic Tree Management | |
void | attachFrame (const FramePtr &frame) |
Attach a frame to the root of the kinematic tree. Will detach frame if attached elsewhere already. More... | |
void | attachFrame (const FramePtr &frame, const FramePtr &parent) |
Attach a frame to another frame in the kinematic tree. Will detach frame if attached elsewhere already. More... | |
void | attachFrame (const FramePtr &frame, const std::string &parent_name) |
Attach a frame to another frame in the kinematic tree. Will detach frame if attached elsewhere already. More... | |
void | detachFrame (const FramePtr &frame) |
Detach a frame from a kinematic tree. If frame has children all children will be detached as well. More... | |
void | detachFrame (const std::string &frame_name) |
Detach a frame from a kinematic tree. If frame has children all children will be detached as well. More... | |
void | addFixedRobot (const RobotPtr &frobot, const StatePtr &state, const std::string &scene_name) |
add a fixed Robot(frames with all joints fixed) on the current robot. The frame names are namespaced under the scene_name. More... | |
void | addActiveRobot (const RobotPtr &arobot) |
adds an active Robot with all its frames and corresponding joints. More... | |
void | compileTree () |
Compiles the underlying kinematic tree representation after a call to attachFrame(). More... | |
void | clear () |
Completely clears internal structures, a brand new robot! More... | |
void | removeChild (FrameDataPtr parent, const std::string &child) |
Removes child from the parent in the tree. More... | |
void | updateFrame (const FramePtr &frame, const std::string &parent_name) |
Updates a frame's entry in the kinematic tree with a possibly new parent parent_name. More... | |
void | compileTreeRecursive (const std::string &previous, FrameDataPtr &data, bool movable=false) |
Helper function to compileTree() that recursively build tree representation. More... | |
void | staticFK () |
Compute the transform for all non-movable frames. More... | |
void | generateSignature () |
Creates a unique signature (based on active joints). More... | |
Pose / Configuration Management | |
void | fk (const Eigen::Ref< const Eigen::VectorXd > &q, FrameMapPtr frames) const |
Compute the forward kinematics of the robot at a state q. More... | |
const FrameMap::Value & | getPose (const std::string &frame_name, const Eigen::Ref< const Eigen::VectorXd > &q, FrameMapPtr frames) const |
Gets the pose of a frame frame_name at a configuration q. Also updates all dependent frames in frames. More... | |
const FrameMap::Value & | getJacobian (const std::string &frame_name, const Eigen::Ref< const Eigen::VectorXd > &q, FrameMapPtr frames) const |
Gets the jacobian for frame_name at configuration q. More... | |
Eigen::Ref< Eigen::VectorXd > | getFrameValues (const std::string &frame_name, Eigen::Ref< Eigen::VectorXd > q) const |
Returns a reference to the underlying subvector of the configuration associated with a specific frame. More... | |
const Eigen::Ref< const Eigen::VectorXd > | getFrameValuesConst (const std::string &frame_name, const Eigen::Ref< const Eigen::VectorXd > &q) const |
Returns a reference to the underlying subvector of the configuration associated with a specific frame. More... | |
double | distance (const StatePtr &a, const StatePtr &b, bool weighted=true) const |
Compute the distance between two states. More... | |
void | enforceLimits (StatePtr a) const |
Enforce joint limits on a robot and renormalize angles if necessary. More... | |
bool | inLimits (const StatePtr &a) const |
Check if all configuration parameters are within limits. More... | |
void | interpolate (const StatePtr &from, const StatePtr &to, double t, StatePtr state) const |
Interpolate between two states. More... | |
const FrameMap::Value & | getPose (const FrameDataPtr &data, const Eigen::Ref< const Eigen::VectorXd > &q, FrameMapPtr frames) const |
Gets the pose of a frame data at a configuration q. Also updates all dependent frames in frames. More... | |
Informative | |
using | PrintHelper = std::function< std::string(const FrameDataPtr &)> |
Function used by Robot::printTree() to print additional information about frames. More... | |
std::string | printTree (const PrintHelper &function={}) const |
Returns a string that graphically shows the kinematic structure of the tree. For example: More... | |
std::string | printDebug () const |
Returns a string of complete robot information for debugging purposes. More... | |
void | printTreeRecursive (std::stringstream &ss, const FrameDataPtr &frame, const PrintHelper &function, const std::string &prefix="") const |
Helper function to printTree() that recursively traverses the kinematic tree. More... | |
A representation of a robot (in this case, a kinematic tree in the plane).
using se2ez::Robot::PrintHelper = std::function<std::string(const FrameDataPtr &)> |
Function used by Robot::printTree() to print additional information about frames.
Robot::Robot | ( | ) |
|
delete |
|
delete |
void Robot::addActiveRobot | ( | const RobotPtr & | arobot | ) |
void Robot::addFixedRobot | ( | const RobotPtr & | frobot, |
const StatePtr & | state, | ||
const std::string & | scene_name | ||
) |
add a fixed Robot(frames with all joints fixed) on the current robot. The frame names are namespaced under the scene_name.
[in] | frobot | Robot to add as a fixed scene. |
[in] | state | State of the attached Scene; |
[in] | scene_name | std::string name of the attached scene |
void Robot::attachFrame | ( | const FramePtr & | frame | ) |
void Robot::attachFrame | ( | const FramePtr & | frame, |
const std::string & | parent_name | ||
) |
void Robot::clear | ( | ) |
void Robot::compileTree | ( | ) |
Compiles the underlying kinematic tree representation after a call to attachFrame().
|
private |
Helper function to compileTree() that recursively build tree representation.
[in] | previous | Previous frame in the kinematic tree. |
[in] | data | Frame to add to the tree. |
[in] | movable | Was there a movable joint before this frame? |
void Robot::detachFrame | ( | const FramePtr & | frame | ) |
void Robot::detachFrame | ( | const std::string & | frame_name | ) |
void Robot::enforceLimits | ( | StatePtr | a | ) | const |
void Robot::fk | ( | const Eigen::Ref< const Eigen::VectorXd > & | q, |
FrameMapPtr | frames | ||
) | const |
|
private |
ACMPtr Robot::getACM | ( | ) |
const std::vector< bool > Robot::getContinuity | ( | ) | const |
const std::vector< std::string > & Robot::getEEs | ( | ) | const |
FramePtr Robot::getFrame | ( | const std::string & | name | ) |
const FramePtr & Robot::getFrameConst | ( | const std::string & | name | ) | const |
|
private |
const Robot::FrameDataPtr & Robot::getFrameDataConst | ( | const std::string & | frame_name | ) | const |
const std::vector< std::string > & Robot::getFrameNames | ( | ) | const |
FramePtr Robot::getFrameParent | ( | const std::string & | name | ) |
Eigen::Ref< Eigen::VectorXd > Robot::getFrameValues | ( | const std::string & | frame_name, |
Eigen::Ref< Eigen::VectorXd > | q | ||
) | const |
Returns a reference to the underlying subvector of the configuration associated with a specific frame.
[in] | frame_name | Name of frame to get component of configuration for. |
[in] | q | Configuration to extract component from. |
Pose / Configuration Management
const Eigen::Ref< const Eigen::VectorXd > Robot::getFrameValuesConst | ( | const std::string & | frame_name, |
const Eigen::Ref< const Eigen::VectorXd > & | q | ||
) | const |
Returns a reference to the underlying subvector of the configuration associated with a specific frame.
[in] | frame_name | Name of frame to get component of configuration for. |
[in] | q | Configuration to extract component from. |
const FrameMap::Value & Robot::getJacobian | ( | const std::string & | frame_name, |
const Eigen::Ref< const Eigen::VectorXd > & | q, | ||
FrameMapPtr | frames | ||
) | const |
const std::vector< std::pair< std::string, Robot::FrameDataPtr > > & Robot::getJoints | ( | ) | const |
const Eigen::VectorXd Robot::getLowerLimits | ( | ) | const |
void Robot::getNamedState | ( | const std::string & | name, |
StatePtr | state | ||
) |
std::vector< std::string > Robot::getNamedStates | ( | ) | const |
const std::map< std::string, StatePtr > & Robot::getNamedStatesMap | ( | ) |
unsigned int Robot::getNumJoints | ( | ) | const |
const FrameMap::Value & Robot::getPose | ( | const std::string & | frame_name, |
const Eigen::Ref< const Eigen::VectorXd > & | q, | ||
FrameMapPtr | frames | ||
) | const |
Gets the pose of a frame frame_name at a configuration q. Also updates all dependent frames in frames.
[in] | frame_name | Name of frame to get pose for. |
[in] | q | Configuration of robot to compute pose for. |
[in] | frames | Workspace for computing frames. Respects dirty frame information. |
|
private |
Gets the pose of a frame data at a configuration q. Also updates all dependent frames in frames.
[in] | data | Frame data to get pose for. |
[in] | q | Configuration of robot to compute pose for. |
[in] | frames | Workspace for computing frames. Respects dirty frame information. |
const std::string & Robot::getSignature | ( | ) |
const KDL::Tree & Robot::getTree | ( | ) | const |
const Eigen::VectorXd Robot::getUpperLimits | ( | ) | const |
bool Robot::inLimits | ( | const StatePtr & | a | ) | const |
bool Robot::inTree | ( | const std::string & | name | ) | const |
std::string Robot::printDebug | ( | ) | const |
std::string Robot::printTree | ( | const PrintHelper & | function = {} | ) | const |
Returns a string that graphically shows the kinematic structure of the tree. For example:
* \_ root (output of function goes here) * \_ base * \_ link_1 * \_ link_2 * |\_ link_3 * | \_ e2 * \_ e1 *
[in] | function | A function to be called for each frame, with output attached at end of tree. |
|
private |
Helper function to printTree() that recursively traverses the kinematic tree.
[in] | ss | Stream to output data to. |
[in] | frame | Frame to output data about. |
[in] | function | User provided function to call for each frame. |
[in] | prefix | Prefix to attach to frame output. |
|
private |
void Robot::setACM | ( | const ACMPtr & | acm | ) |
void Robot::setNamedState | ( | const std::string & | name, |
const StatePtr & | state | ||
) |
void Robot::setNamedStates | ( | const std::map< std::string, StatePtr > & | named | ) |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |
|
private |