Convenience class that describes the default setup for Care-O-Bot4. 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 <cob4.h>

Inheritance diagram for robowflex::Cob4Robot:

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

bool	initialize ()
	Initialize the robot with arm_left and arm_right kinematics. More...

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

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

void	openGrippers ()
	Opens the Cob4's grippers. More...

void	openLeftGripper ()
	Opens the Cob4's left gripper. More...

void	openRightGripper ()
	Opens the Cob4's right gripper. More...

void	closeGrippers ()
	Closes the Cob4's grippers. More...

void	closeLeftGripper ()
	Closes the Cob4's left gripper. More...

void	closeRightGripper ()
	Closes the Cob4's right 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://cob_hardware_config/robots/cob4-8/urdf/cob4-8.urdf.xacro"}
	Default URDF. More...

static const std::string	DEFAULT_SRDF {"package://cob_moveit_config/robots/cob4-8/moveit/config/cob4-8.srdf"}
	Default SRDF. More...

static const std::string	DEFAULT_LIMITS {"package://cob_moveit_config/robots/cob4-8/moveit/config/joint_limits.yaml"}
	Default Limits. More...

static const std::string	DEFAULT_KINEMATICS {"package://cob_moveit_config/robots/cob4-8/moveit/config/kinematics.yaml"}
	Default kinematics. More...

static const std::string	RESOURCE_URDF {"package://robowflex_resources/cob/robots/cob4-8.urdf.xacro"}
	URDF from robowflex_resources. More...

static const std::string	RESOURCE_SRDF {"package://robowflex_resources/cob/config/cob4-8.srdf"}
	SRDF from robowflex_resources. More...

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

static const std::string	RESOURCE_KINEMATICS {"package://robowflex_resources/cob/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 Care-O-Bot4. 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 cob4.h.

Constructor & Destructor Documentation

◆ Cob4Robot()

Cob4Robot::Cob4Robot ( )

Constructor.

Definition at line 32 of file cob4.cpp.

                      : Robot("cob4")
 {
 }

Member Function Documentation

◆ closeGrippers()

void Cob4Robot::closeGrippers ( )

Closes the Cob4's grippers.

Definition at line 98 of file cob4.cpp.

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

◆ closeLeftGripper()

void Cob4Robot::closeLeftGripper ( )

Closes the Cob4's left gripper.

Definition at line 106 of file cob4.cpp.

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

◆ closeRightGripper()

void Cob4Robot::closeRightGripper ( )

Closes the Cob4's right gripper.

Definition at line 114 of file cob4.cpp.

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

◆ initialize()

bool Cob4Robot::initialize ( )

Initialize the robot with arm_left and arm_right kinematics.

Returns: True on success, false on failure.

Definition at line 36 of file cob4.cpp.

 {
     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 Cob4 with `cob4_{description, moveit_config}`");
         success = Robot::initialize(DEFAULT_URDF, DEFAULT_SRDF, DEFAULT_LIMITS, DEFAULT_KINEMATICS);
     }
     else
     {
         RBX_INFO("Initializing Cob4 with `robowflex_resources`");
         success = Robot::initialize(RESOURCE_URDF, RESOURCE_SRDF, RESOURCE_LIMITS, RESOURCE_KINEMATICS);
     }
  
     loadKinematics("arm_left");
     loadKinematics("arm_right");
  
     Cob4Robot::openGrippers();
  
     return success;
 }

◆ openGrippers()

void Cob4Robot::openGrippers ( )

Opens the Cob4's grippers.

Definition at line 72 of file cob4.cpp.

 {
     const std::map<std::string, double> angles = {{"gripper_left_finger_1_joint", -1.0},
                                                   {"gripper_left_finger_2_joint", 1.2},
                                                   {"gripper_right_finger_1_joint", -1.0},
                                                   {"gripper_right_finger_2_joint", 1.2}};
  
     Robot::setState(angles);
 }

◆ openLeftGripper()

void Cob4Robot::openLeftGripper ( )

Opens the Cob4's left gripper.

Definition at line 82 of file cob4.cpp.

 {
     const std::map<std::string, double> angles = {{"gripper_left_finger_1_joint", -1.0},
                                                   {"gripper_left_finger_2_joint", 1.2}};
  
     Robot::setState(angles);
 }

◆ openRightGripper()

void Cob4Robot::openRightGripper ( )

Opens the Cob4's right gripper.

Definition at line 90 of file cob4.cpp.

 {
     const std::map<std::string, double> angles = {{"gripper_right_finger_1_joint", -1.0},
                                                   {"gripper_right_finger_2_joint", 1.2}};
  
     Robot::setState(angles);
 }

◆ pointHead()

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

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

Parameters

[in] point The point to look at.

Definition at line 60 of file cob4.cpp.

 {
     const RobotPose point_pose = RobotPose(Eigen::Translation3d(point));
     const RobotPose point_pan = getLinkTF("head_3_link").inverse() * point_pose;
  
     const double pan = atan2(point_pan.translation().y(), point_pan.translation().x());
  
     const std::map<std::string, double> angles = {{"head_1_joint", pan}};
  
     Robot::setState(angles);
 }

◆ setBasePose()

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

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

Parameters

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

Definition at line 122 of file cob4.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 Cob4Robot::DEFAULT_KINEMATICS {"package://cob_moveit_config/robots/cob4-8/moveit/config/kinematics.yaml"}

staticprivate

Default kinematics.

Definition at line 78 of file cob4.h.

◆ DEFAULT_LIMITS

const std::string Cob4Robot::DEFAULT_LIMITS {"package://cob_moveit_config/robots/cob4-8/moveit/config/joint_limits.yaml"}

staticprivate

Default Limits.

Definition at line 77 of file cob4.h.

◆ DEFAULT_SRDF

const std::string Cob4Robot::DEFAULT_SRDF {"package://cob_moveit_config/robots/cob4-8/moveit/config/cob4-8.srdf"}

staticprivate

Default SRDF.

Definition at line 76 of file cob4.h.

◆ DEFAULT_URDF

const std::string Cob4Robot::DEFAULT_URDF {"package://cob_hardware_config/robots/cob4-8/urdf/cob4-8.urdf.xacro"}

staticprivate

Default URDF.

Definition at line 75 of file cob4.h.

◆ RESOURCE_KINEMATICS

const std::string Cob4Robot::RESOURCE_KINEMATICS {"package://robowflex_resources/cob/config/kinematics.yaml"}

staticprivate

kinematics from robowflex_resources

Definition at line 83 of file cob4.h.

◆ RESOURCE_LIMITS

const std::string Cob4Robot::RESOURCE_LIMITS {"package://robowflex_resources/cob/config/joint_limits.yaml"}

staticprivate

Limits from robowflex_resources.

Definition at line 82 of file cob4.h.

◆ RESOURCE_SRDF

const std::string Cob4Robot::RESOURCE_SRDF {"package://robowflex_resources/cob/config/cob4-8.srdf"}

staticprivate

SRDF from robowflex_resources.

Definition at line 81 of file cob4.h.

◆ RESOURCE_URDF

const std::string Cob4Robot::RESOURCE_URDF {"package://robowflex_resources/cob/robots/cob4-8.urdf.xacro"}

staticprivate

URDF from robowflex_resources.

Definition at line 80 of file cob4.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

◆ Cob4Robot()

Member Function Documentation

◆ closeGrippers()

◆ closeLeftGripper()

◆ closeRightGripper()

◆ initialize()

◆ openGrippers()

◆ openLeftGripper()

◆ openRightGripper()

◆ pointHead()

◆ setBasePose()

Member Data Documentation

◆ DEFAULT_KINEMATICS

◆ DEFAULT_LIMITS

◆ DEFAULT_SRDF

◆ DEFAULT_URDF

◆ RESOURCE_KINEMATICS

◆ RESOURCE_LIMITS

◆ RESOURCE_SRDF

◆ RESOURCE_URDF