robowflex::Robot Class Reference

Loads information about a robot and maintains information about a robot's state. More...

#include <robot.h>

Inheritance diagram for robowflex::Robot:

Classes
struct	IKQuery
	Robot IK Query options. IK queries in Robowflex consist of: a) A position specified by some geometric region (a robowflex::Geometry) at a pose. b) An orientation specified by some base orientation with allowable deviations specified by tolerances on the XYZ Euler axes. It is recommended to use the provided constructors to specify a query, or to use the addRequest() function. Multiple target tips can be specified, but note that not all kinematics solvers support multi-tip IK. Additionally, a robowflex::Scene can be specified to do collision-aware IK. More...

Public Types
typedef std::function< bool(YAML::Node &)>	PostProcessYAMLFunction
	A function that runs after loading a YAML file and can modify its contents. Returns true on success, false on failure. More...
typedef std::function< bool(tinyxml2::XMLDocument &)>	PostProcessXMLFunction
	A function that runs after loading a XML file and can modify its contents. Returns true on success, false on failure. More...

Public Member Functions
	Robot (const std::string &name)
	Constructor. More...
	Robot (Robot const &)=delete
void	operator= (Robot const &)=delete
Initialization and Loading
bool	initialize (const std::string &urdf_file)
	Initializes a robot from a kinematic description. A default semantic description is used. More...
bool	initializeKinematics (const std::string &kinematics_file)
	Initialize a robot with a kinematics description. More...
bool	initialize (const std::string &urdf_file, const std::string &srdf_file, const std::string &limits_file="", const std::string &kinematics_file="")
	Initializes a robot from a kinematic and semantic description. All files are loaded under the robot's namespace. More...
bool	initializeFromYAML (const std::string &config_file)
	Initializes a robot from a YAML config which includes URDF (urdf) and optional the SRDF (srdf), joint limits (joint_limits), IK plugins (kinematics) and a default state (robot_state). All files are loaded under the robot's namespace. The names of the YAML keys are in parenthesis. More...
bool	loadYAMLFile (const std::string &name, const std::string &file)
	Loads a YAML file into the robot's namespace under name. More...
bool	loadYAMLFile (const std::string &name, const std::string &file, const PostProcessYAMLFunction &function)
	Loads a YAML file into the robot's namespace under name, with a post-process function. More...
std::string	loadXMLFile (const std::string &file)
	Loads an XML or .xacro file into a string. More...
void	setURDFPostProcessFunction (const PostProcessXMLFunction &function)
	Sets a post processing function for loading the URDF. More...
bool	isLinkURDF (tinyxml2::XMLDocument &doc, const std::string &name)
	Checks if a node link exist with named name_link. More...
void	setSRDFPostProcessFunction (const PostProcessXMLFunction &function)
	Sets a post processing function for loading the SRDF. More...
void	setLimitsPostProcessFunction (const PostProcessYAMLFunction &function)
	Sets a post processing function for loading the joint limits file. More...
void	setKinematicsPostProcessFunction (const PostProcessYAMLFunction &function)
	Sets a post processing function for loading the kinematics plugin file. More...
void	setSRDFPostProcessAddPlanarJoint (const std::string &name)
	Adds a planar virtual joint through the SRDF to the loaded robot with name name. This joint will have three degrees of freedom: <name>/x, <name>/y, and <name>/theta. Will apply this joint between the world and the root frame. More...
void	setSRDFPostProcessAddFloatingJoint (const std::string &name)
	Adds a planar virtual joint through the SRDF to the loaded robot with name name. This joint will have three degrees of freedom: <name>/x, <name>/y, and <name>/theta. Will apply this joint between the world and the root frame. More...
bool	loadKinematics (const std::string &group, bool load_subgroups=true)
	Loads the kinematics plugin for a joint group and its subgroups. No kinematics are loaded by default. More...
Getters and Setters
const std::string &	getModelName () const
	Get the robot's model name. More...
const std::string &	getName () const
	Get the robot's name. More...
const robot_model::RobotModelPtr &	getModelConst () const
	Get a const reference to the loaded robot model. More...
robot_model::RobotModelPtr &	getModel ()
	Get a reference to the loaded robot model. More...
urdf::ModelInterfaceConstSharedPtr	getURDF () const
	Get the raw URDF Model. More...
const std::string &	getURDFString () const
	Get the raw URDF Model as a string. More...
srdf::ModelConstSharedPtr	getSRDF () const
	Get the raw SRDF Model. More...
const std::string &	getSRDFString () const
	Get the raw SRDF Model as a string. More...
const IO::Handler &	getHandlerConst () const
	Get the underlying IO handler used for this robot. More...
IO::Handler &	getHandler ()
	Get the underlying IO handler used for this robot. More...
Robot State Operations
const robot_model::RobotStatePtr &	getScratchStateConst () const
	Get a const reference to the scratch robot state. More...
robot_model::RobotStatePtr &	getScratchState ()
	Get a reference to the scratch robot state. More...
robot_model::RobotStatePtr	cloneScratchState () const
	Allocate a new robot state that is a clone of the current scratch state. More...
robot_model::RobotStatePtr	allocState () const
	Allocate a new robot state. More...
void	setState (const std::vector< double > &positions)
	Sets the scratch state from a vector of joint positions (all must be specified) More...
void	setState (const std::map< std::string, double > &variable_map)
	Sets the scratch state from a map of joint name to position. More...
void	setState (const std::vector< std::string > &variable_names, const std::vector< double > &variable_position)
	Sets the scratch state from a vector of joint names and their positions. More...
void	setState (const sensor_msgs::JointState &state)
	Sets the scratch state from a joint state message. More...
void	setState (const moveit_msgs::RobotState &state)
	Sets the scratch state from a robot state message. More...
void	setStateFromYAMLFile (const std::string &file)
	Sets the scratch state from a robot state message saved to a YAML file. More...
void	setGroupState (const std::string &name, const std::vector< double > &positions)
	Sets the group of the scratch state to a vector of joint positions. More...
std::vector< double >	getState () const
	Gets the current joint positions of the scratch state. More...
moveit_msgs::RobotState	getStateMsg () const
	Get the current scratch state as a message. More...
void	setStateMsgGroupState (moveit_msgs::RobotState &state, const std::string &group, const std::vector< double > &positions) const
	Set the group state of a MoveIt RobotState message. More...
std::vector< std::string >	getJointNames () const
	Gets the names of joints of the robot. More...
bool	hasJoint (const std::string &joint) const
	Checks if a joint exists in the robot. More...
const RobotPose &	getLinkTF (const std::string &name) const
	Get the current pose of a link on the scratch state. More...
const RobotPose	getRelativeLinkTF (const std::string &base, const std::string &target) const
	Get the current pose of a link target in the frame of base. More...
Inverse Kinematics
bool	setFromIK (const IKQuery &query)
	Sets a group of the scratch state from an IK query. If the IK query fails the scratch state retains its initial value. More...
bool	setFromIK (const IKQuery &query, robot_state::RobotState &state) const
	Sets a robot state from an IK query. If the IK query fails the scratch state retains its initial value. More...
bool	validateIKQuery (const IKQuery &query, const robot_state::RobotState &state) const
	Validates that a state satisfies an IK query's request poses. More...
double	distanceToIKQuery (const IKQuery &query, const robot_state::RobotState &state) const
	Returns the distance of the state to satisfying the IK query. More...
std::vector< std::string >	getSolverTipFrames (const std::string &group) const
	Get the tip frames for the IK solver for a given joint model group group. More...
std::string	getSolverBaseFrame (const std::string &group) const
	Get the base frame for the IK solver given a joint model group group. More...
IO
bool	toYAMLFile (const std::string &file) const
	Dumps the current configuration of the robot as a YAML file. More...
bool	dumpGeometry (const std::string &file) const
	Dumps the names of links and absolute paths to their visual mesh files to a YAML file. More...
bool	dumpTransforms (const std::string &filename) const
	Dumps the tranforms of all links of a robot at its current state to a file. More...
bool	dumpPathTransforms (const robot_trajectory::RobotTrajectory &path, const std::string &filename, double fps=30, double threshold=0.0) const
	Dumps the tranforms of all links of a robot through a robot trajectory to a file. More...
bool	dumpToScene (const std::string &filename) const
	Dumps the current scratch configuration of the robot to a YAML file compatible with a scene. More...

Static Public Attributes
static const std::string	ROBOT_DESCRIPTION = "robot_description"
	Default robot description name. More...
static const std::string	ROBOT_SEMANTIC = "_semantic"
	Default robot semantic description suffix. More...
static const std::string	ROBOT_PLANNING = "_planning"
	Default robot planning description suffix. More...
static const std::string	ROBOT_KINEMATICS = "_kinematics"
	Default robot kinematics description suffix. More...

Protected Member Functions
Protected Initialization
bool	loadURDFFile (const std::string &urdf_file)
	Loads the URDF file. More...
bool	loadSRDFFile (const std::string &srdf_file)
	Loads the SRDF file. More...
void	initializeInternal (bool namespaced=true)
	Initializes and loads the robot. Calls post-processing functions and creates scratch state. More...
void	loadRobotModel (const std::string &description)
	Loads a robot model from the loaded information on the parameter server. More...
void	updateXMLString (std::string &string, const PostProcessXMLFunction &function)
	Updates a loaded XML string based on an XML post-process function. Called after initial, unmodified robot is loaded. More...

Protected Attributes
const std::string	name_
	Robot name. More...
IO::Handler	handler_
	IO handler (namespaced with name_) More...
std::string	urdf_
	The URDF as a string. More...
std::string	srdf_
	The SRDF as a string. More...
PostProcessXMLFunction	urdf_function_
	URDF post-processing function. More...
PostProcessXMLFunction	srdf_function_
	SRDF post-processing function. More...
PostProcessYAMLFunction	limits_function_
	Limits YAML post-processing function. More...
PostProcessYAMLFunction	kinematics_function_
	Kinematics plugin YAML post-processing function. More...
std::shared_ptr< robot_model_loader::RobotModelLoader >	loader_
	Robot model loader. More...
robot_model::RobotModelPtr	model_
	Loaded robot model. More...
std::map< std::string, robot_model::SolverAllocatorFn >	imap_
	Kinematic solver allocator map. More...
kinematics_plugin_loader::KinematicsPluginLoaderPtr	kinematics_
	Kinematic plugin loader. More...
robot_state::RobotStatePtr	scratch_
	Scratch robot state. More...

Detailed Description

Loads information about a robot and maintains information about a robot's state.

The Robot class is a wrapper around MoveIt!'s robot_model::RobotModel and a "scratch" robot_state::RobotState. Mostly, this class handles loading the relevant information to the parameter server without the use of a launch file. The necessary files are the URDF and SRDF (both either XML or xacro files), and the joint limits and kinematics plugin (both YAML files). These pieces of information are loaded to the parameter server under the name provided to the constructor so that multiple robots can be used. Note that all information is also loaded under the unique namespace generated by the IO::Handler.

By default, no IK kinematics plugins are loaded due to startup costs (particularly for R2). You must call loadKinematics() with the desired planning groups (as defined by the SRDF) if you want IK.

Additionally, post-processing function hooks are provided if modifications need to be done to any of the necessary files. This enables the addition of extra joints, semantic information, joint limit overloading, etc. as needed. These functions are called after the robot is initially loaded, so it is possible to access robot model information in these functions. For example, see setSRDFPostProcessAddPlanarJoint().

Definition at line 68 of file robowflex_library/include/robowflex_library/robot.h.

Member Typedef Documentation

PostProcessXMLFunction

typedef std::function<bool(tinyxml2::XMLDocument &)> robowflex::Robot::PostProcessXMLFunction

A function that runs after loading a XML file and can modify its contents. Returns true on success, false on failure.

Definition at line 79 of file robowflex_library/include/robowflex_library/robot.h.

PostProcessYAMLFunction

typedef std::function<bool(YAML::Node &)> robowflex::Robot::PostProcessYAMLFunction

A function that runs after loading a YAML file and can modify its contents. Returns true on success, false on failure.

Definition at line 74 of file robowflex_library/include/robowflex_library/robot.h.

Constructor & Destructor Documentation

Robot() [1/2]

Robot::Robot

(

const std::string &

name

)

Constructor.

Parameters

[in]

name

The name of the robot. Used to namespace information under.

Definition at line 26 of file robowflex_library/src/robot.cpp.

                                  : name_(name), handler_(name_)
{
}

Robot() [2/2]

robowflex::Robot::Robot ( Robot const &  )

delete

Member Function Documentation

allocState()

robot_model::RobotStatePtr Robot::allocState

(

)

const

Allocate a new robot state.

Returns

The new robot state.

Definition at line 996 of file robowflex_library/src/robot.cpp.

{
    // No make_shared() for indigo compatibility
    robot_state::RobotStatePtr state;
    state.reset(new robot_state::RobotState(getModelConst()));
    state->setToDefaultValues();
 
    return state;
}

cloneScratchState()

robot_model::RobotStatePtr Robot::cloneScratchState

(

)

const

Allocate a new robot state that is a clone of the current scratch state.

Returns

The new robot state.

Definition at line 502 of file robowflex_library/src/robot.cpp.

{
    auto state = allocState();
    *state = *scratch_;
 
    return state;
}

distanceToIKQuery()

double Robot::distanceToIKQuery	(	const IKQuery &	query,
		const robot_state::RobotState &	state
	)		const

Returns the distance of the state to satisfying the IK query.

Parameters

[in]	query	The query to check.
[in]	state	The state to check against the query.

Returns

The distance of the state to satisfaction of the query.

Definition at line 967 of file robowflex_library/src/robot.cpp.

{
    const auto &constraints = query.getAsConstraints(*this);
    const auto &result = constraints->decide(state, query.verbose);
    return result.distance;
}

dumpGeometry()

bool Robot::dumpGeometry

(

const std::string &

file

)

const

Dumps the names of links and absolute paths to their visual mesh files to a YAML file.

Parameters

[in]

file

File to save to.The name of the link to find the transform of.

Returns

True on success, false on failure.

Definition at line 1163 of file robowflex_library/src/robot.cpp.

{
    YAML::Node link_geometry;
    const auto &urdf = model_->getURDF();
 
    std::vector<urdf::LinkSharedPtr> links;
    urdf->getLinks(links);
 
    for (const auto &link : links)
    {
        YAML::Node node;
 
        YAML::Node visual;
#if ROBOWFLEX_AT_LEAST_KINETIC
        for (const auto &element : link->visual_array)
            if (element)
                visual["elements"].push_back(addLinkVisual(element));
#else
        if (link->visual)
            visual["elements"].push_back(addLinkVisual(link->visual));
#endif
 
        YAML::Node collision;
#if ROBOWFLEX_AT_LEAST_KINETIC
        for (const auto &element : link->collision_array)
            if (element)
                collision["elements"].push_back(addLinkCollision(element));
#else
        if (link->collision)
            collision["elements"].push_back(addLinkCollision(link->collision));
#endif
 
        if (!visual.IsNull() || !collision.IsNull())
        {
            YAML::Node add;
            add["name"] = link->name;
 
            if (!visual.IsNull())
                add["visual"] = visual;
 
            if (!collision.IsNull())
                add["collision"] = collision;
 
            link_geometry.push_back(add);
        }
    }
 
    return IO::YAMLToFile(link_geometry, filename);
}

dumpPathTransforms()

bool Robot::dumpPathTransforms	(	const robot_trajectory::RobotTrajectory &	path,
		const std::string &	filename,
		double	fps = 30,
		double	threshold = 0.0
	)		const

Dumps the tranforms of all links of a robot through a robot trajectory to a file.

Parameters

[in]	path	Path to output.
[in]	filename	Filename to output to.
[in]	fps	The transforms (frames) per second used to interpolate the given path.
[in]	threshold	The minimum distance between states before transforms are output.

Returns

True on success, false on failure.

Definition at line 1221 of file robowflex_library/src/robot.cpp.

{
    YAML::Node node, values;
    const double rate = 1.0 / fps;
 
    // Find the total duration of the path.
    const std::deque<double> &durations = path.getWayPointDurations();
    double total_duration = std::accumulate(durations.begin(), durations.end(), 0.0);
 
    const auto &urdf = model_->getURDF();
 
    robot_state::RobotStatePtr previous, state(new robot_state::RobotState(model_));
 
    for (double duration = 0.0, delay = 0.0; duration <= total_duration; duration += rate, delay += rate)
    {
        YAML::Node point;
 
        path.getStateAtDurationFromStart(duration, state);
        if (previous && state->distance(*previous) < threshold)
            continue;
 
        state->update();
        for (const auto &link_name : model_->getLinkModelNames())
        {
            const auto &urdf_link = urdf->getLink(link_name);
            if (urdf_link->visual)
            {
                const auto &link = model_->getLinkModel(link_name);
                RobotPose tf =
                    state->getGlobalLinkTransform(link);  // * urdfPoseToEigen(urdf_link->visual->origin);
                point[link->getName()] = IO::toNode(TF::poseEigenToMsg(tf));
            }
        }
 
        YAML::Node value;
        value["point"] = point;
        value["duration"] = delay;
        values.push_back(value);
 
        delay = 0;
 
        if (!previous)
            previous.reset(new robot_state::RobotState(model_));
 
        *previous = *state;
    }
 
    node["transforms"] = values;
    node["fps"] = fps;
 
    return IO::YAMLToFile(node, filename);
}

dumpToScene()

bool Robot::dumpToScene

(

const std::string &

filename

)

const

Dumps the current scratch configuration of the robot to a YAML file compatible with a scene.

Parameters

[in]

filename

Filename to output to.

Returns

True on success, false on failure.

Definition at line 1275 of file robowflex_library/src/robot.cpp.

{
    YAML::Node node;
 
    const auto &urdf = model_->getURDF();
    for (const auto &link_name : model_->getLinkModelNames())
    {
        const auto &urdf_link = urdf->getLink(link_name);
        if (urdf_link->visual)
        {
            const auto &link = model_->getLinkModel(link_name);
 
            std::vector<YAML::Node> visuals;
#if ROBOWFLEX_AT_LEAST_KINETIC
            for (const auto &element : urdf_link->visual_array)
                if (element)
                    visuals.emplace_back(addLinkVisual(element, false));
#else
            if (urdf_link->visual)
                visuals.push_back(addLinkVisual(urdf_link->visual, false));
#endif
 
            YAML::Node object;
            object["id"] = link->getName();
 
            YAML::Node meshes;
            YAML::Node mesh_poses;
            YAML::Node primitives;
            YAML::Node primitive_poses;
 
            RobotPose tf = scratch_->getGlobalLinkTransform(link);
            for (const auto &visual : visuals)
            {
                RobotPose pose = tf;
                if (IO::isNode(visual["origin"]))
                {
                    RobotPose offset = TF::poseMsgToEigen(IO::poseFromNode(visual["origin"]));
                    pose = pose * offset;
                }
 
                const auto &posey = IO::toNode(TF::poseEigenToMsg(pose));
 
                const auto &type = visual["type"].as<std::string>();
                if (type == "mesh")
                {
                    YAML::Node mesh;
                    mesh["resource"] = visual["resource"];
                    if (IO::isNode(visual["dimensions"]))
                        mesh["dimensions"] = visual["dimensions"];
                    else
                    {
                        mesh["dimensions"].push_back(1.);
                        mesh["dimensions"].push_back(1.);
                        mesh["dimensions"].push_back(1.);
                    }
 
                    ROBOWFLEX_YAML_FLOW(mesh["dimensions"]);
 
                    meshes.push_back(mesh);
                    mesh_poses.push_back(posey);
                }
                else
                {
                    YAML::Node primitive;
                    primitive["type"] = type;
                    primitive["dimensions"] = visual["dimensions"];
                    primitive_poses.push_back(posey);
                }
            }
 
            if (meshes.size())
            {
                object["meshes"] = meshes;
                object["mesh_poses"] = mesh_poses;
            }
 
            if (primitives.size())
            {
                object["primitives"] = primitives;
                object["primitive_poses"] = primitive_poses;
            }
 
            node["collision_objects"].push_back(object);
        }
    }
 
    YAML::Node scene;
    scene["world"] = node;
 
    return IO::YAMLToFile(scene, filename);
}

dumpTransforms()

bool Robot::dumpTransforms

(

const std::string &

filename

)

const

Dumps the tranforms of all links of a robot at its current state to a file.

Parameters

[in]

filename

Filename to output to.

Returns

True on success, false on failure.

Definition at line 1213 of file robowflex_library/src/robot.cpp.

{
    robot_trajectory::RobotTrajectory trajectory(model_, "");
    trajectory.addSuffixWayPoint(scratch_, 0.0);
 
    return dumpPathTransforms(trajectory, filename, 0., 0.);
}

getHandler()

IO::Handler & Robot::getHandler

(

)

Get the underlying IO handler used for this robot.

Returns

A reference to the IO handler.

Definition at line 515 of file robowflex_library/src/robot.cpp.

{
    return handler_;
}

getHandlerConst()

const IO::Handler & Robot::getHandlerConst

(

)

const

Get the underlying IO handler used for this robot.

Returns

A reference to the IO handler.

Definition at line 510 of file robowflex_library/src/robot.cpp.

{
    return handler_;
}

getJointNames()

std::vector< std::string > Robot::getJointNames

(

)

const

Gets the names of joints of the robot.

Returns

A vector of joint names.

Definition at line 592 of file robowflex_library/src/robot.cpp.

{
    return scratch_->getVariableNames();
}

getLinkTF()

const RobotPose & Robot::getLinkTF

(

const std::string &

name

)

const

Get the current pose of a link on the scratch state.

Parameters

[in]

name

The name of the link to find the transform of.

Returns

The transform of link name.

Definition at line 974 of file robowflex_library/src/robot.cpp.

{
    return scratch_->getGlobalLinkTransform(name);
}

getModel()

robot_model::RobotModelPtr & Robot::getModel

(

)

Get a reference to the loaded robot model.

Returns

The robot model.

Definition at line 467 of file robowflex_library/src/robot.cpp.

{
    return model_;
}

getModelConst()

const robot_model::RobotModelPtr & Robot::getModelConst

(

)

const

Get a const reference to the loaded robot model.

Returns

The robot model.

Definition at line 462 of file robowflex_library/src/robot.cpp.

{
    return model_;
}

getModelName()

const std::string & Robot::getModelName

(

)

const

Get the robot's model name.

Returns

The robot's model name.

Definition at line 452 of file robowflex_library/src/robot.cpp.

{
    return model_->getName();
}

getName()

const std::string & Robot::getName

(

)

const

Get the robot's name.

Returns

The robot's name.

Definition at line 457 of file robowflex_library/src/robot.cpp.

{
    return name_;
}

getRelativeLinkTF()

const RobotPose Robot::getRelativeLinkTF	(	const std::string &	base,
		const std::string &	target
	)		const

Get the current pose of a link target in the frame of base.

Parameters

[in]	base	The link to use as the base frame.
[in]	target	The link to find the transform of.

Returns

The transform of link target in the frame of base.

Definition at line 979 of file robowflex_library/src/robot.cpp.

{
    auto base_tf = scratch_->getGlobalLinkTransform(base);
    auto target_tf = scratch_->getGlobalLinkTransform(target);
 
    return base_tf.inverse() * target_tf;
}

getScratchState()

robot_model::RobotStatePtr & Robot::getScratchState

(

)

Get a reference to the scratch robot state.

Returns

The scratch robot state.

Definition at line 497 of file robowflex_library/src/robot.cpp.

{
    return scratch_;
}

getScratchStateConst()

const robot_model::RobotStatePtr & Robot::getScratchStateConst

(

)

const

Get a const reference to the scratch robot state.

Returns

The scratch robot state.

Definition at line 492 of file robowflex_library/src/robot.cpp.

{
    return scratch_;
}

getSolverBaseFrame()

std::string Robot::getSolverBaseFrame

(

const std::string &

group

)

const

Get the base frame for the IK solver given a joint model group group.

Parameters

[in]

group

The group to get the base frame for.

Returns

The base frame. Will return an empty string on error.

Definition at line 1016 of file robowflex_library/src/robot.cpp.

{
    const auto &jmg = model_->getJointModelGroup(group);
    const auto &solver = jmg->getSolverInstance();
    if (solver)
        return solver->getBaseFrame();
 
    return "";
}

getSolverTipFrames()

std::vector< std::string > Robot::getSolverTipFrames

(

const std::string &

group

)

const

Get the tip frames for the IK solver for a given joint model group group.

Parameters

[in]

group

The group to get the tip frames for.

Returns

The list of tip frames. Will return an empty list on error.

Definition at line 1006 of file robowflex_library/src/robot.cpp.

{
    const auto &jmg = model_->getJointModelGroup(group);
    const auto &solver = jmg->getSolverInstance();
    if (solver)
        return solver->getTipFrames();
 
    return {};
}

getSRDF()

srdf::ModelConstSharedPtr Robot::getSRDF

(

)

const

Get the raw SRDF Model.

Returns

The SRDF model.

Definition at line 482 of file robowflex_library/src/robot.cpp.

{
    return model_->getSRDF();
}

getSRDFString()

const std::string & Robot::getSRDFString

(

)

const

Get the raw SRDF Model as a string.

Returns

The SRDF model as a string.

Definition at line 487 of file robowflex_library/src/robot.cpp.

{
    return srdf_;
}

getState()

std::vector< double > Robot::getState

(

)

const

Gets the current joint positions of the scratch state.

Returns

A vector of joint positions.

Definition at line 565 of file robowflex_library/src/robot.cpp.

{
    const double *positions = scratch_->getVariablePositions();
    std::vector<double> state(positions, positions + scratch_->getVariableCount());
    return state;
}

getStateMsg()

moveit_msgs::RobotState Robot::getStateMsg

(

)

const

Get the current scratch state as a message.

Returns

The scratch state as a MoveIt message.

Definition at line 572 of file robowflex_library/src/robot.cpp.

{
    moveit_msgs::RobotState message;
    moveit::core::robotStateToRobotStateMsg(*scratch_, message);
 
    return message;
}

getURDF()

urdf::ModelInterfaceConstSharedPtr Robot::getURDF

(

)

const

Get the raw URDF Model.

Returns

The URDF Model.

Definition at line 472 of file robowflex_library/src/robot.cpp.

{
    return model_->getURDF();
}

getURDFString()

const std::string & Robot::getURDFString

(

)

const

Get the raw URDF Model as a string.

Returns

The URDF Model as a string.

Definition at line 477 of file robowflex_library/src/robot.cpp.

{
    return urdf_;
}

hasJoint()

bool Robot::hasJoint

(

const std::string &

joint

)

const

Checks if a joint exists in the robot.

Returns

True if the joint exists, false otherwise.

Definition at line 597 of file robowflex_library/src/robot.cpp.

{
    const auto &joint_names = getJointNames();
    return (std::find(joint_names.begin(), joint_names.end(), joint) != joint_names.end());
}

initialize() [1/2]

bool Robot::initialize

(

const std::string &

urdf_file

)

Initializes a robot from a kinematic description. A default semantic description is used.

Parameters

[in]

urdf_file

Location of the robot's URDF (XML or .xacro file).

Returns

True on success, false on failure.

Definition at line 175 of file robowflex_library/src/robot.cpp.

{
    if (loader_)
    {
        RBX_ERROR("Already initialized!");
        return false;
    }
 
    if (not loadURDFFile(urdf_file))
        return false;
 
    // Generate basic SRDF on the fly.
    tinyxml2::XMLDocument doc;
    doc.Parse(urdf_.c_str());
    const auto &name = doc.FirstChildElement("robot")->Attribute("name");
    const std::string &srdf = "<?xml version=\"1.0\" ?><robot name=\"" + std::string(name) + "\"></robot>";
 
    srdf_ = srdf;
 
    initializeInternal();
 
    return true;
}

initialize() [2/2]

bool Robot::initialize	(	const std::string &	urdf_file,
		const std::string &	srdf_file,
		const std::string &	limits_file = "",
		const std::string &	kinematics_file = ""
	)

Initializes a robot from a kinematic and semantic description. All files are loaded under the robot's namespace.

Parameters

[in]	urdf_file	Location of the robot's URDF (XML or .xacro file).
[in]	srdf_file	Location of the robot's SRDF (XML or .xacro file).
[in]	limits_file	Location of the joint limit information (a YAML file). Optional.
[in]	kinematics_file	Location of the kinematics plugin information (a YAML file). Optional.

Returns

True on success, false on failure.

Definition at line 50 of file robowflex_library/src/robot.cpp.

{
    if (loader_)
    {
        RBX_ERROR("Already initialized!");
        return false;
    }
 
    if (not loadURDFFile(urdf_file))
    {
        RBX_ERROR("Failed to load URDF!");
        return false;
    }
 
    if (not loadSRDFFile(srdf_file))
    {
        RBX_ERROR("Failed to load SRDF!");
        return false;
    }
 
    if (not limits_file.empty())
        if (not loadYAMLFile(ROBOT_DESCRIPTION + ROBOT_PLANNING, limits_file, limits_function_))
        {
            RBX_ERROR("Failed to load joint limits!");
            return false;
        }
 
    if (not kinematics_file.empty())
        if (not initializeKinematics(kinematics_file))
        {
            RBX_ERROR("Failed to load kinematics!");
            return false;
        }
 
    initializeInternal();
    return true;
}

initializeFromYAML()

bool Robot::initializeFromYAML

(

const std::string &

config_file

)

Initializes a robot from a YAML config which includes URDF (urdf) and optional the SRDF (srdf), joint limits (joint_limits), IK plugins (kinematics) and a default state (robot_state). All files are loaded under the robot's namespace. The names of the YAML keys are in parenthesis.

Parameters

[in]

config_file

Location of the yaml config file.

Returns

True on success, false on failure.

Definition at line 89 of file robowflex_library/src/robot.cpp.

{
    if (loader_)
    {
        RBX_ERROR("Already initialized!");
        return false;
    }
 
    const auto &yaml = IO::loadFileToYAML(config_file);
    if (not yaml.first)
    {
        RBX_ERROR("Failed to load YAML file `%s`.", config_file);
        return false;
    }
 
    const auto &node = yaml.second;
 
    // URDF
    std::string urdf_file;
    if (IO::isNode(node["urdf"]))
        urdf_file = node["urdf"].as<std::string>();
    else
    {
        RBX_ERROR("No URDF entry in YAML file `%s`!", config_file);
        return false;
    }
 
    // SRDF
    std::string srdf_file;
    if (IO::isNode(node["srdf"]))
        srdf_file = node["srdf"].as<std::string>();
    else
        RBX_WARN("No SRDF entry in YAML!");
 
    // Joint limits
    std::string limits_file;
    if (IO::isNode(node["limits"]))
    {
        if (srdf_file.empty())
        {
            RBX_ERROR("Cannot provide joint limits without SRDF in YAML file `%s`!", config_file);
            return false;
        }
 
        limits_file = node["limits"].as<std::string>();
    }
    else
        RBX_WARN("No joint limits provided!");
 
    // Kinematics plugins
    std::string kinematics_file;
    if (IO::isNode(node["kinematics"]))
    {
        if (srdf_file.empty())
        {
            RBX_ERROR("Cannot provide kinematics without SRDF in YAML file `%s`!", config_file);
            return false;
        }
 
        kinematics_file = node["kinematics"].as<std::string>();
    }
    else
        RBX_WARN("No kinematics plugins provided!");
 
    // Initialize robot
    bool r;
    if (srdf_file.empty())
        r = initialize(urdf_file);
    else
        r = initialize(urdf_file, srdf_file, limits_file, kinematics_file);
 
    // Set default state if provided in file.
    if (r)
    {
        if (IO::isNode(node["robot_state"]))
        {
            const auto &robot_state = IO::robotStateFromNode(node["robot_state"]);
            setState(robot_state);
        }
        else
            RBX_WARN("No default state provided!");
    }
 
    return r;
}

initializeInternal()

void Robot::initializeInternal ( bool  namespaced = true )

protected

Initializes and loads the robot. Calls post-processing functions and creates scratch state.

Parameters

[in]

namespaced

Whether or not the parameter server description is under the handler namespace.

Definition at line 308 of file robowflex_library/src/robot.cpp.

{
    const std::string &description = ((namespaced) ? handler_.getNamespace() : "") + "/" + ROBOT_DESCRIPTION;
 
    loadRobotModel(description);
    if (urdf_function_)
        updateXMLString(urdf_, urdf_function_);
 
    if (srdf_function_)
        updateXMLString(srdf_, srdf_function_);
 
    // If either function was called, reload robot.
    if (urdf_function_ or srdf_function_)
    {
        RBX_INFO("Reloading model after URDF/SRDF post-process function...");
        loadRobotModel(description);
    }
 
    // set strings on parameter server
    handler_.setParam(ROBOT_DESCRIPTION, urdf_);
    handler_.setParam(ROBOT_DESCRIPTION + ROBOT_SEMANTIC, srdf_);
 
    scratch_.reset(new robot_state::RobotState(model_));
    scratch_->setToDefaultValues();
}

initializeKinematics()

bool Robot::initializeKinematics

(

const std::string &

kinematics_file

)

Initialize a robot with a kinematics description.

Parameters

[in]

kinematics_file

Location of the kinematics plugin information (a YAML file).

Returns

True on success, false on failure.

Definition at line 199 of file robowflex_library/src/robot.cpp.

{
    if (kinematics_)
    {
        RBX_ERROR("Already loaded kinematics!");
        return false;
    }
 
    return loadYAMLFile(ROBOT_DESCRIPTION + ROBOT_KINEMATICS, kinematics_file, kinematics_function_);
}

isLinkURDF()

bool Robot::isLinkURDF	(	tinyxml2::XMLDocument &	doc,
		const std::string &	name
	)

Checks if a node link exist with named name_link.

Parameters

[in]	doc	The URDF description.
[in]	name	The name of the link to find.

Returns

True if link exists, false otherwise.

Definition at line 215 of file robowflex_library/src/robot.cpp.

{
    auto *node = doc.FirstChildElement("robot")->FirstChildElement("link");
    while (node != nullptr)
    {
        if (node->Attribute("name", name.c_str()))
            return true;
 
        node = node->NextSiblingElement("link");
    }
    return false;
}

loadKinematics()

bool Robot::loadKinematics	(	const std::string &	group,
		bool	load_subgroups = true
	)

Loads the kinematics plugin for a joint group and its subgroups. No kinematics are loaded by default.

Parameters

[in]	group	Joint group name to load.
[in]	load_subgroups	Load kinematic solvers for subgroups of the requested group.

Returns

True on success, false on failure.

Definition at line 347 of file robowflex_library/src/robot.cpp.

{
    // Needs to be called first to read the groups defined in the SRDF from the ROS params.
    robot_model::SolverAllocatorFn allocator = kinematics_->getLoaderFunction(loader_->getSRDF());
 
    const auto &groups = kinematics_->getKnownGroups();
    if (groups.empty())
    {
        RBX_ERROR("No kinematics plugins defined. Fill and load kinematics.yaml!");
        return false;
    }
 
    if (!model_->hasJointModelGroup(group_name))
    {
        RBX_ERROR("No JMG defined for `%s`!", group_name);
        return false;
    }
 
    std::vector<std::string> load_names;
 
    // If requested, also attempt to load the kinematics solvers for subgroups.
    if (load_subgroups)
    {
        const auto &subgroups = model_->getJointModelGroup(group_name)->getSubgroupNames();
        load_names.insert(load_names.end(), subgroups.begin(), subgroups.end());
    }
 
    // Check if this group also has an associated kinematics solver to load.
    if (std::find(groups.begin(), groups.end(), group_name) != groups.end())
        load_names.emplace_back(group_name);
 
    auto timeout = kinematics_->getIKTimeout();
 
    for (const auto &name : load_names)
    {
        // Check if kinematics have already been loaded for this group.
        if (imap_.find(name) != imap_.end())
            continue;
 
        if (!model_->hasJointModelGroup(name) ||
            std::find(groups.begin(), groups.end(), name) == groups.end())
        {
            RBX_ERROR("No JMG or Kinematics defined for `%s`!", name);
            return false;
        }
 
        robot_model::JointModelGroup *jmg = model_->getJointModelGroup(name);
        kinematics::KinematicsBasePtr solver = allocator(jmg);
 
        if (solver)
        {
            std::string error_msg;
            if (solver->supportsGroup(jmg, &error_msg))
                imap_[name] = allocator;
            else
            {
                RBX_ERROR("Kinematics solver %s does not support joint group %s.  Error: %s",
                          typeid(*solver).name(), name, error_msg);
                return false;
            }
        }
        else
        {
            RBX_ERROR("Kinematics solver could not be instantiated for joint group `%s`.", name);
            return false;
        }
 
        RBX_INFO("Loaded Kinematics Solver for  `%s`", name);
        jmg->setDefaultIKTimeout(timeout[name]);
    }
 
    model_->setKinematicsAllocators(imap_);
    return true;
}

loadRobotModel()

void Robot::loadRobotModel ( const std::string &  description )

protected

Loads a robot model from the loaded information on the parameter server.

Parameters

[in]

description

Robot description on parameter server.

Definition at line 334 of file robowflex_library/src/robot.cpp.

{
    robot_model_loader::RobotModelLoader::Options options(description);
    options.load_kinematics_solvers_ = false;
    options.urdf_string_ = urdf_;
    options.srdf_string_ = srdf_;
 
    loader_.reset(new robot_model_loader::RobotModelLoader(options));
    kinematics_.reset(new kinematics_plugin_loader::KinematicsPluginLoader(description));
 
    model_ = loader_->getModel();
}

loadSRDFFile()

bool Robot::loadSRDFFile ( const std::string &  srdf_file )

protected

Loads the SRDF file.

Parameters

[in]

srdf_file

The SRDF file name.

Returns

True on success, false on failure.

Definition at line 40 of file robowflex_library/src/robot.cpp.

{
    std::string srdf = loadXMLFile(srdf_file);
    if (srdf.empty())
        return false;
 
    srdf_ = srdf;
    return true;
}

loadURDFFile()

bool Robot::loadURDFFile ( const std::string &  urdf_file )

protected

Loads the URDF file.

Parameters

[in]

urdf_file

The URDF file name.

Returns

True on success, false on failure.

Definition at line 30 of file robowflex_library/src/robot.cpp.

{
    std::string urdf = loadXMLFile(urdf_file);
    if (urdf.empty())
        return false;
 
    urdf_ = urdf;
    return true;
}

loadXMLFile()

std::string Robot::loadXMLFile

(

const std::string &

file

)

Loads an XML or .xacro file into a string.

Parameters

[in]

file

File to load.

Returns

XML string upon success, empty string on failure.

Definition at line 276 of file robowflex_library/src/robot.cpp.

{
    std::string string = IO::loadXMLToString(file);
    if (string.empty())
    {
        RBX_ERROR("Failed to load XML file `%s`.", file);
        return "";
    }
 
    return string;
}

loadYAMLFile() [1/2]

bool Robot::loadYAMLFile	(	const std::string &	name,
		const std::string &	file
	)

Loads a YAML file into the robot's namespace under name.

Parameters

[in]	name	Name to load file under.
[in]	file	File to load.

Returns

True on success, false on failure.

Definition at line 243 of file robowflex_library/src/robot.cpp.

{
    PostProcessYAMLFunction function;
    return loadYAMLFile(name, file, function);
}

loadYAMLFile() [2/2]

bool Robot::loadYAMLFile	(	const std::string &	name,
		const std::string &	file,
		const PostProcessYAMLFunction &	function
	)

Loads a YAML file into the robot's namespace under name, with a post-process function.

Parameters

[in]	name	Name to load file under.
[in]	file	File to load.
[in]	function	Optional post processing function.

Returns

True on success, false on failure.

Definition at line 249 of file robowflex_library/src/robot.cpp.

{
    const auto &yaml = IO::loadFileToYAML(file);
    if (!yaml.first)
    {
        RBX_ERROR("Failed to load YAML file `%s`.", file);
        return false;
    }
 
    if (function)
    {
        YAML::Node copy = yaml.second;
        if (!function(copy))
        {
            RBX_ERROR("Failed to process YAML file `%s`.", file);
            return false;
        }
 
        handler_.loadYAMLtoROS(copy, name);
    }
    else
        handler_.loadYAMLtoROS(yaml.second, name);
 
    return true;
}

operator=()

void robowflex::Robot::operator= ( Robot const &  )

delete

setFromIK() [1/2]

bool Robot::setFromIK

(

const IKQuery &

query

)

Sets a group of the scratch state from an IK query. If the IK query fails the scratch state retains its initial value.

Parameters

[in]

query

Query for inverse kinematics. See Robot::IKQuery documentation for more.

Returns

True on success, false on failure.

Definition at line 858 of file robowflex_library/src/robot.cpp.

{
    return setFromIK(query, *scratch_);
}

setFromIK() [2/2]

bool Robot::setFromIK	(	const IKQuery &	query,
		robot_state::RobotState &	state
	)		const

Sets a robot state from an IK query. If the IK query fails the scratch state retains its initial value.

Parameters

[in]	query	Query for inverse kinematics. See Robot::IKQuery documentation for more.
[out]	state	Robot state to set from IK.

Returns

True on success, false on failure.

Definition at line 863 of file robowflex_library/src/robot.cpp.

{
    // copy query for unconstness
    IKQuery query_copy(query);
 
    // If there are no tips in the query,
    // we use the tip frame from the original query group
    if (query_copy.tips[0].empty())
        query_copy.tips = getSolverTipFrames(query.group);
 
    const robot_model::JointModelGroup *jmg = model_->getJointModelGroup(query_copy.group);
    const auto &gsvcf = (query_copy.scene) ? query_copy.scene->getGSVCF(query_copy.verbose) :
                                             moveit::core::GroupStateValidityCallbackFn{};
 
    bool evaluate = not query_copy.metrics.empty() or query_copy.validate;
    kinematic_constraints::ConstraintEvaluationResult result;
 
    const auto &constraints = (evaluate) ? query_copy.getAsConstraints(*this) : nullptr;
 
    // Best state if evaluating metrics.
    auto best = (query_copy.metrics.empty()) ? nullptr : std::make_shared<robot_state::RobotState>(state);
    double best_value = constants::inf;
 
    bool success = false;
    RobotPoseVector targets;
    for (std::size_t i = 0; i < query_copy.attempts and not success; ++i)
    {
        // Sample new target poses from regions.
        query_copy.sampleRegions(targets);
 
#if ROBOWFLEX_AT_LEAST_MELODIC
        // Multi-tip IK: Will delegate automatically to RobotState::setFromIKSubgroups() if the kinematics
        // solver doesn't support multi-tip queries.
        success =
            state.setFromIK(jmg, targets, query_copy.tips, query_copy.timeout, gsvcf, query_copy.options);
#else
        // attempts was a prior field that was deprecated in melodic
        success =
            state.setFromIK(jmg, targets, query_copy.tips, 1, query_copy.timeout, gsvcf, query_copy.options);
#endif
 
        if (evaluate)
        {
            state.update();
            result = constraints->decide(state, query_copy.verbose);
        }
 
        // Externally validate result
        if (query_copy.validate)
        {
            if (query_copy.verbose)
                RBX_INFO("Constraint Distance: %1%", result.distance);
 
            bool no_collision =
                (query_copy.scene) ? not query_copy.scene->checkCollision(state).collision : true;
 
            success =             //
                no_collision and  //
                ((query_copy.valid_distance > 0.) ? result.distance <= query_copy.valid_distance :
                                                    result.satisfied);
        }
 
        // If success, evaluate state for metrics.
        if (success and not query_copy.metrics.empty())
        {
            double v = query_copy.getMetricValue(state, result);
 
            if (query_copy.verbose)
                RBX_INFO("State Metric Value: %1%", v);
 
            if (v < best_value)
            {
                if (query_copy.verbose)
                    RBX_INFO("State is current best!");
 
                best_value = v;
                *best = state;
            }
 
            success = false;
        }
 
        if (query_copy.random_restart and not success)
            state.setToRandomPositions(jmg);
    }
 
    // If evaluating metric, copy best state.
    if (std::isfinite(best_value))
    {
        state = *best;
        success = true;
    }
 
    state.update();
    return success;
}

setGroupState()

void Robot::setGroupState	(	const std::string &	name,
		const std::vector< double > &	positions
	)

Sets the group of the scratch state to a vector of joint positions.

Parameters

[in]	name	Name of group to set.
[in]	positions	Positions to set.

Definition at line 559 of file robowflex_library/src/robot.cpp.

{
    scratch_->setJointGroupPositions(name, positions);
    scratch_->update();
}

setKinematicsPostProcessFunction()

void Robot::setKinematicsPostProcessFunction

(

const PostProcessYAMLFunction &

function

)

Sets a post processing function for loading the kinematics plugin file.

Parameters

[in]

function

The function to use.

Definition at line 238 of file robowflex_library/src/robot.cpp.

{
    kinematics_function_ = function;
}

setLimitsPostProcessFunction()

void Robot::setLimitsPostProcessFunction

(

const PostProcessYAMLFunction &

function

)

Sets a post processing function for loading the joint limits file.

Parameters

[in]

function

The function to use.

Definition at line 233 of file robowflex_library/src/robot.cpp.

{
    limits_function_ = function;
}

setSRDFPostProcessAddFloatingJoint()

void Robot::setSRDFPostProcessAddFloatingJoint

(

const std::string &

name

)

Adds a planar virtual joint through the SRDF to the loaded robot with name name. This joint will have three degrees of freedom: <name>/x, <name>/y, and <name>/theta. Will apply this joint between the world and the root frame.

Parameters

[in]

name

Name for new joint.

Definition at line 437 of file robowflex_library/src/robot.cpp.

{
    setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool {
        tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint");
        virtual_joint->SetAttribute("name", name.c_str());
        virtual_joint->SetAttribute("type", "floating");
        virtual_joint->SetAttribute("parent_frame", "world");
        virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str());
 
        doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint);
 
        return true;
    });
}

setSRDFPostProcessAddPlanarJoint()

void Robot::setSRDFPostProcessAddPlanarJoint

(

const std::string &

name

)

Adds a planar virtual joint through the SRDF to the loaded robot with name name. This joint will have three degrees of freedom: <name>/x, <name>/y, and <name>/theta. Will apply this joint between the world and the root frame.

Parameters

[in]

name

Name for new joint.

Definition at line 422 of file robowflex_library/src/robot.cpp.

{
    setSRDFPostProcessFunction([&, name](tinyxml2::XMLDocument &doc) -> bool {
        tinyxml2::XMLElement *virtual_joint = doc.NewElement("virtual_joint");
        virtual_joint->SetAttribute("name", name.c_str());
        virtual_joint->SetAttribute("type", "planar");
        virtual_joint->SetAttribute("parent_frame", "world");
        virtual_joint->SetAttribute("child_link", model_->getRootLink()->getName().c_str());
 
        doc.FirstChildElement("robot")->InsertFirstChild(virtual_joint);
 
        return true;
    });
}

setSRDFPostProcessFunction()

void Robot::setSRDFPostProcessFunction

(

const PostProcessXMLFunction &

function

)

Sets a post processing function for loading the SRDF.

Parameters

[in]

function

The function to use.

Definition at line 228 of file robowflex_library/src/robot.cpp.

{
    srdf_function_ = function;
}

setState() [1/5]

void Robot::setState

(

const moveit_msgs::RobotState &

state

)

Sets the scratch state from a robot state message.

Parameters

[in]

state

The state to set.

Definition at line 545 of file robowflex_library/src/robot.cpp.

{
    moveit::core::robotStateMsgToRobotState(state, *scratch_);
    scratch_->update();
}

setState() [2/5]

void Robot::setState

(

const sensor_msgs::JointState &

state

)

Sets the scratch state from a joint state message.

Parameters

[in]

state

The state to set.

Definition at line 539 of file robowflex_library/src/robot.cpp.

{
    scratch_->setVariableValues(state);
    scratch_->update();
}

setState() [3/5]

void Robot::setState

(

const std::map< std::string, double > &

variable_map

)

Sets the scratch state from a map of joint name to position.

Parameters

[in]

variable_map

Joint positions to set.

Definition at line 526 of file robowflex_library/src/robot.cpp.

{
    scratch_->setVariablePositions(variable_map);
    scratch_->update();
}

setState() [4/5]

void Robot::setState

(

const std::vector< double > &

positions

)

Sets the scratch state from a vector of joint positions (all must be specified)

Parameters

[in]

positions

Joint positions to set.

Definition at line 520 of file robowflex_library/src/robot.cpp.

{
    scratch_->setVariablePositions(positions);
    scratch_->update();
}

setState() [5/5]

void Robot::setState	(	const std::vector< std::string > &	variable_names,
		const std::vector< double > &	variable_position
	)

Sets the scratch state from a vector of joint names and their positions.

Parameters

[in]	variable_names	Joint names.
[in]	variable_position	Position of joint variable (index matches entry in variable_names)

Definition at line 532 of file robowflex_library/src/robot.cpp.

{
    scratch_->setVariablePositions(variable_names, variable_position);
    scratch_->update();
}

setStateFromYAMLFile()

void Robot::setStateFromYAMLFile

(

const std::string &

file

)

Sets the scratch state from a robot state message saved to a YAML file.

Parameters

[in]

file

The YAML file to load.

Definition at line 551 of file robowflex_library/src/robot.cpp.

{
    moveit_msgs::RobotState state;
    IO::fromYAMLFile(state, file);
 
    setState(state);
}

setStateMsgGroupState()

void Robot::setStateMsgGroupState	(	moveit_msgs::RobotState &	state,
		const std::string &	group,
		const std::vector< double > &	positions
	)		const

Set the group state of a MoveIt RobotState message.

Parameters

[out]	state	The state message to set.
[in]	group	Group in state to set.
[in]	positions	Positions to set group state to.

Definition at line 580 of file robowflex_library/src/robot.cpp.

{
    robot_state::RobotState temp(getModelConst());
    moveit::core::robotStateMsgToRobotState(state, temp);
 
    auto *jmg = getModelConst()->getJointModelGroup(group);
    temp.setJointGroupPositions(jmg, positions);
 
    moveit::core::robotStateToRobotStateMsg(temp, state);
}

setURDFPostProcessFunction()

void Robot::setURDFPostProcessFunction

(

const PostProcessXMLFunction &

function

)

Sets a post processing function for loading the URDF.

Parameters

[in]

function

The function to use.

Definition at line 210 of file robowflex_library/src/robot.cpp.

{
    urdf_function_ = function;
}

toYAMLFile()

bool Robot::toYAMLFile

(

const std::string &

file

)

const

Dumps the current configuration of the robot as a YAML file.

Parameters

[in]

file

File to write to.

Returns

True on success, false on failure.

Definition at line 987 of file robowflex_library/src/robot.cpp.

{
    moveit_msgs::RobotState msg;
    moveit::core::robotStateToRobotStateMsg(*scratch_, msg);
 
    const auto &yaml = IO::toNode(msg);
    return IO::YAMLToFile(yaml, file);
}

updateXMLString()

void Robot::updateXMLString ( std::string &  string, const PostProcessXMLFunction &  function  )

protected

Updates a loaded XML string based on an XML post-process function. Called after initial, unmodified robot is loaded.

Parameters

[in,out]	string	Input XML string.
[in]	function	XML processing function.

Definition at line 288 of file robowflex_library/src/robot.cpp.

{
    if (function)
    {
        tinyxml2::XMLDocument doc;
        doc.Parse(string.c_str());
 
        if (not function(doc))
        {
            RBX_ERROR("Failed to process XML string `%s`.", string);
            return;
        }
 
        tinyxml2::XMLPrinter printer;
        doc.Print(&printer);
 
        string = std::string(printer.CStr());
    }
}

validateIKQuery()

bool Robot::validateIKQuery	(	const IKQuery &	query,
		const robot_state::RobotState &	state
	)		const

Validates that a state satisfies an IK query's request poses.

Parameters

[in]	query	The query to validate.
[in]	state	The state to validate against the query.

Returns

True if the query is satisfied, false otherwise.

Definition at line 960 of file robowflex_library/src/robot.cpp.

{
    const auto &constraints = query.getAsConstraints(*this);
    const auto &result = constraints->decide(state, query.verbose);
    return (query.valid_distance > 0.) ? result.distance <= query.valid_distance : result.satisfied;
}

Member Data Documentation

handler_

IO::Handler robowflex::Robot::handler_

protected

IO handler (namespaced with name_)

Definition at line 793 of file robowflex_library/include/robowflex_library/robot.h.

imap_

std::map<std::string, robot_model::SolverAllocatorFn> robowflex::Robot::imap_

protected

Kinematic solver allocator map.

Definition at line 805 of file robowflex_library/include/robowflex_library/robot.h.

kinematics_

kinematics_plugin_loader::KinematicsPluginLoaderPtr robowflex::Robot::kinematics_

protected

Kinematic plugin loader.

Definition at line 806 of file robowflex_library/include/robowflex_library/robot.h.

kinematics_function_

PostProcessYAMLFunction robowflex::Robot::kinematics_function_

protected

Kinematics plugin YAML post-processing function.

Definition at line 801 of file robowflex_library/include/robowflex_library/robot.h.

limits_function_

PostProcessYAMLFunction robowflex::Robot::limits_function_

protected

Limits YAML post-processing function.

Definition at line 800 of file robowflex_library/include/robowflex_library/robot.h.

loader_

std::shared_ptr<robot_model_loader::RobotModelLoader> robowflex::Robot::loader_

protected

Robot model loader.

Definition at line 803 of file robowflex_library/include/robowflex_library/robot.h.

model_

robot_model::RobotModelPtr robowflex::Robot::model_

protected

Loaded robot model.

Definition at line 804 of file robowflex_library/include/robowflex_library/robot.h.

name_

const std::string robowflex::Robot::name_

protected

Robot name.

Definition at line 792 of file robowflex_library/include/robowflex_library/robot.h.

ROBOT_DESCRIPTION

const std::string Robot::ROBOT_DESCRIPTION = "robot_description"

static

Default robot description name.

Definition at line 81 of file robowflex_library/include/robowflex_library/robot.h.

ROBOT_KINEMATICS

const std::string Robot::ROBOT_KINEMATICS = "_kinematics"

static

Default robot kinematics description suffix.

Definition at line 84 of file robowflex_library/include/robowflex_library/robot.h.

ROBOT_PLANNING

const std::string Robot::ROBOT_PLANNING = "_planning"

static

Default robot planning description suffix.

Definition at line 83 of file robowflex_library/include/robowflex_library/robot.h.

ROBOT_SEMANTIC

const std::string Robot::ROBOT_SEMANTIC = "_semantic"

static

Default robot semantic description suffix.

Definition at line 82 of file robowflex_library/include/robowflex_library/robot.h.

scratch_

robot_state::RobotStatePtr robowflex::Robot::scratch_

protected

Scratch robot state.

Definition at line 808 of file robowflex_library/include/robowflex_library/robot.h.

srdf_

std::string robowflex::Robot::srdf_

protected

The SRDF as a string.

Definition at line 796 of file robowflex_library/include/robowflex_library/robot.h.

srdf_function_

PostProcessXMLFunction robowflex::Robot::srdf_function_

protected

SRDF post-processing function.

Definition at line 799 of file robowflex_library/include/robowflex_library/robot.h.

urdf_

std::string robowflex::Robot::urdf_

protected

The URDF as a string.

Definition at line 795 of file robowflex_library/include/robowflex_library/robot.h.

urdf_function_

PostProcessXMLFunction robowflex::Robot::urdf_function_

protected

URDF post-processing function.

Definition at line 798 of file robowflex_library/include/robowflex_library/robot.h.

---

The documentation for this class was generated from the following files:

• robowflex_library/include/robowflex_library/robot.h
• robowflex_library/src/robot.cpp