Convenience class that describes the default setup for Fetch. Will first attempt to load configuration and description from the robowflex_resources package. See https://github.com/KavrakiLab/robowflex_resources for this package. If this package is not available, then fetch_description / fetch_moveit_config packages will be used. More...

#include <fetch.h>

Inheritance diagram for robowflex::FetchRobot:

Public Member Functions
	FetchRobot ()
	Constructor. More...

bool	initialize (bool addVirtual=true, bool use_low_limits=false)
	Initialize the robot with arm and arm_with_torso kinematics. More...

bool	addCastersURDF (tinyxml2::XMLDocument &doc)
	Inserts the caster links if they don't exist. More...

void	setBasePose (double x, double y, double theta)
	Sets the base pose of the Fetch robot (a virtual planar joint) More...

void	pointHead (const Eigen::Vector3d &point)
	Points the Fetch's head to a point in the world frame. More...

void	openGripper ()
	Opens the Fetch's gripper. More...

void	closeGripper ()
	Closes the Fetch's gripper. More...

Public Member Functions inherited from robowflex::Robot
	Robot (const std::string &name)
	Constructor. More...

	Robot (Robot const &)=delete

void	operator= (Robot const &)=delete

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...

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...

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...

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...

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 Private Attributes
static const std::string	DEFAULT_URDF {"package://fetch_description/robots/fetch.urdf"}
	Default URDF. More...

static const std::string	DEFAULT_SRDF {"package://fetch_moveit_config/config/fetch.srdf"}
	Default SRDF. More...

static const std::string	DEFAULT_LIMITS {"package://fetch_moveit_config/config/joint_limits.yaml"}
	Default Limits. More...

static const std::string	DEFAULT_KINEMATICS {"package://fetch_moveit_config/config/kinematics.yaml"}
	Default kinematics. More...

static const std::string	RESOURCE_URDF {"package://robowflex_resources/fetch/robots/fetch.urdf"}
	URDF from robowflex_resources. More...

static const std::string	RESOURCE_SRDF {"package://robowflex_resources/fetch/config/fetch.srdf"}
	SRDF from robowflex_resources. More...

static const std::string	RESOURCE_LIMITS {"package://robowflex_resources/fetch/config/joint_limits.yaml"}
	Limits from robowflex_resources. More...

static const std::string	RESOURCE_LIMITS_LOW {"package://robowflex_resources/fetch/config/joint_limits_low.yaml"}
	Lower limits from robowflex_resources. More...

static const std::string	RESOURCE_KINEMATICS {"package://robowflex_resources/fetch/config/kinematics.yaml"}
	kinematics from robowflex_resources More...

Additional Inherited Members
Public Types inherited from robowflex::Robot
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...

Static Public Attributes inherited from robowflex::Robot
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 inherited from robowflex::Robot
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 inherited from robowflex::Robot
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

Convenience class that describes the default setup for Fetch. Will first attempt to load configuration and description from the robowflex_resources package. See https://github.com/KavrakiLab/robowflex_resources for this package. If this package is not available, then fetch_description / fetch_moveit_config packages will be used.

Definition at line 26 of file fetch.h.

Constructor & Destructor Documentation

◆ FetchRobot()

FetchRobot::FetchRobot ( )

Constructor.

Definition at line 29 of file fetch.cpp.

                        : Robot("fetch")
 {
 }

Member Function Documentation

◆ addCastersURDF()

bool FetchRobot::addCastersURDF ( tinyxml2::XMLDocument & doc )

Inserts the caster links if they don't exist.

Parameters

[in] doc urdf description to be processed.

Returns: True on success.

Definition at line 65 of file fetch.cpp.

 {
     for (const auto &name : {"bl_caster", "br_caster", "fl_caster", "fr_caster"})
     {
         auto link_name = std::string(name) + "_link";
         if (not isLinkURDF(doc, link_name))
         {
             tinyxml2::XMLElement *caster_link = doc.NewElement("link");
             caster_link->SetAttribute("name", link_name.c_str());
             doc.FirstChildElement("robot")->InsertFirstChild(caster_link);
  
             auto joint_name = std::string(name) + "_joint";
             tinyxml2::XMLElement *caster_joint = doc.NewElement("joint");
             caster_joint->SetAttribute("name", joint_name.c_str());
             caster_joint->SetAttribute("type", "fixed");
  
             tinyxml2::XMLElement *parent = doc.NewElement("parent");
             parent->SetAttribute("link", "base_link");
             caster_joint->InsertFirstChild(parent);
  
             tinyxml2::XMLElement *child = doc.NewElement("child");
             child->SetAttribute("link", link_name.c_str());
             caster_joint->InsertFirstChild(child);
  
             doc.FirstChildElement("robot")->InsertFirstChild(caster_joint);
         }
     }
  
     return true;
 }

◆ closeGripper()

void FetchRobot::closeGripper ( )

Closes the Fetch's gripper.

Definition at line 119 of file fetch.cpp.

 {
     const std::map<std::string, double> angles = {{"l_gripper_finger_joint", 0.0},
                                                   {"r_gripper_finger_joint", 0.0}};
  
     Robot::setState(angles);
 }

◆ initialize()

bool FetchRobot::initialize	(	bool	addVirtual = `true`,
		bool	use_low_limits = `false`
	)

Initialize the robot with arm and arm_with_torso kinematics.

Parameters

[in]	addVirtual	flag to add virtual joint.
[in]	use_low_limits	flag to use lower joint limits.

Returns: True on success, false on failure.

Definition at line 33 of file fetch.cpp.

 {
     if (addVirtual)
         setSRDFPostProcessAddPlanarJoint("base_joint");
  
     setURDFPostProcessFunction([this](tinyxml2::XMLDocument &doc) { return addCastersURDF(doc); });
  
     bool success = false;
     // First attempt the `robowflex_resources` package, then attempt the "actual" resource files.
     if (IO::resolvePackage(RESOURCE_URDF).empty() or IO::resolvePackage(RESOURCE_SRDF).empty())
     {
         RBX_INFO("Initializing Fetch with `fetch_{description, moveit_config}`");
         success = Robot::initialize(DEFAULT_URDF, DEFAULT_SRDF, DEFAULT_LIMITS, DEFAULT_KINEMATICS);
     }
     else
     {
         RBX_INFO("Initializing Fetch with `robowflex_resources`");
         if (use_low_limits)
             success =
                 Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS_LOW, RESOURCE_KINEMATICS);
         else
             success = Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS, RESOURCE_KINEMATICS);
     }
  
     loadKinematics("arm");
     loadKinematics("arm_with_torso");
  
     FetchRobot::openGripper();
  
     return success;
 }

◆ openGripper()

void FetchRobot::openGripper ( )

Opens the Fetch's gripper.

Definition at line 111 of file fetch.cpp.

 {
     const std::map<std::string, double> angles = {{"l_gripper_finger_joint", 0.04},
                                                   {"r_gripper_finger_joint", 0.04}};
  
     Robot::setState(angles);
 }

◆ pointHead()

void FetchRobot::pointHead ( const Eigen::Vector3d & point )

Points the Fetch's head to a point in the world frame.

Parameters

[in] point The point to look at.

Definition at line 96 of file fetch.cpp.

 {
     const RobotPose point_pose = RobotPose(Eigen::Translation3d(point));
     const RobotPose point_pan = getLinkTF("head_pan_link").inverse() * point_pose;
     const RobotPose point_tilt = getLinkTF("head_tilt_link").inverse() * point_pose;
  
     const double pan = atan2(point_pan.translation().y(), point_pan.translation().x());
     const double tilt = -atan2(point_tilt.translation().z(),
                                hypot(point_tilt.translation().x(), point_tilt.translation().y()));
  
     const std::map<std::string, double> angles = {{"head_pan_joint", pan}, {"head_tilt_joint", tilt}};
  
     Robot::setState(angles);
 }

◆ setBasePose()

void FetchRobot::setBasePose	(	double	x,
		double	y,
		double	theta
	)

Sets the base pose of the Fetch robot (a virtual planar joint)

Parameters

[in]	x	The x position.
[in]	y	The y position.
[in]	theta	The angle.

Definition at line 127 of file fetch.cpp.

 {
     if (hasJoint("base_joint/x") && hasJoint("base_joint/y") && hasJoint("base_joint/theta"))
     {
         const std::map<std::string, double> pose = {
             {"base_joint/x", x}, {"base_joint/y", y}, {"base_joint/theta", theta}};
  
         scratch_->setVariablePositions(pose);
         scratch_->update();
     }
     else
         RBX_WARN("base_joint does not exist, cannot move base! You need to set addVirtual to true");
 }

Member Data Documentation

◆ DEFAULT_KINEMATICS

const std::string FetchRobot::DEFAULT_KINEMATICS {"package://fetch_moveit_config/config/kinematics.yaml"}

staticprivate

Default kinematics.

Definition at line 70 of file fetch.h.

◆ DEFAULT_LIMITS

const std::string FetchRobot::DEFAULT_LIMITS {"package://fetch_moveit_config/config/joint_limits.yaml"}

staticprivate

Default Limits.

Definition at line 69 of file fetch.h.

◆ DEFAULT_SRDF

const std::string FetchRobot::DEFAULT_SRDF {"package://fetch_moveit_config/config/fetch.srdf"}

staticprivate

Default SRDF.

Definition at line 68 of file fetch.h.

◆ DEFAULT_URDF

const std::string FetchRobot::DEFAULT_URDF {"package://fetch_description/robots/fetch.urdf"}

staticprivate

Default URDF.

Definition at line 67 of file fetch.h.

◆ RESOURCE_KINEMATICS

const std::string FetchRobot::RESOURCE_KINEMATICS {"package://robowflex_resources/fetch/config/kinematics.yaml"}

staticprivate

kinematics from robowflex_resources

Definition at line 76 of file fetch.h.

◆ RESOURCE_LIMITS

const std::string FetchRobot::RESOURCE_LIMITS {"package://robowflex_resources/fetch/config/joint_limits.yaml"}

staticprivate

Limits from robowflex_resources.

Definition at line 74 of file fetch.h.

◆ RESOURCE_LIMITS_LOW

const std::string FetchRobot::RESOURCE_LIMITS_LOW {"package://robowflex_resources/fetch/config/joint_limits_low.yaml"}

staticprivate

Lower limits from robowflex_resources.

Definition at line 75 of file fetch.h.

◆ RESOURCE_SRDF

const std::string FetchRobot::RESOURCE_SRDF {"package://robowflex_resources/fetch/config/fetch.srdf"}

staticprivate

SRDF from robowflex_resources.

Definition at line 73 of file fetch.h.

◆ RESOURCE_URDF

const std::string FetchRobot::RESOURCE_URDF {"package://robowflex_resources/fetch/robots/fetch.urdf"}

staticprivate

URDF from robowflex_resources.

Definition at line 72 of file fetch.h.

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

Public Member Functions

Static Private Attributes

Additional Inherited Members

Detailed Description

Constructor & Destructor Documentation

◆ FetchRobot()

Member Function Documentation

◆ addCastersURDF()

◆ closeGripper()

◆ initialize()

◆ openGripper()

◆ pointHead()

◆ setBasePose()

Member Data Documentation

◆ DEFAULT_KINEMATICS

◆ DEFAULT_LIMITS

◆ DEFAULT_SRDF

◆ DEFAULT_URDF

◆ RESOURCE_KINEMATICS

◆ RESOURCE_LIMITS

◆ RESOURCE_LIMITS_LOW

◆ RESOURCE_SRDF

◆ RESOURCE_URDF