Robowflex  v0.1
Making MoveIt Easy
robowflex::Cob4Robot Class Reference

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::stringgetModelName () const
 Get the robot's model name. More...
 
const std::stringgetName () 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::stringgetURDFString () const
 Get the raw URDF Model as a string. More...
 
srdf::ModelConstSharedPtr getSRDF () const
 Get the raw SRDF Model. More...
 
const std::stringgetSRDFString () const
 Get the raw SRDF Model as a string. More...
 
const IO::HandlergetHandlerConst () const
 Get the underlying IO handler used for this robot. More...
 
IO::HandlergetHandler ()
 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::stringgetJointNames () 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 RobotPosegetLinkTF (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::stringgetSolverTipFrames (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::SolverAllocatorFnimap_
 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.

32  : Robot("cob4")
33 {
34 }
Robot(const std::string &name)
Constructor.

Member Function Documentation

◆ closeGrippers()

void Cob4Robot::closeGrippers ( )

Closes the Cob4's grippers.

Definition at line 98 of file cob4.cpp.

99 {
100  const std::map<std::string, double> angles = {{"l_gripper_finger_joint", 0.0},
101  {"r_gripper_finger_joint", 0.0}};
102 
103  Robot::setState(angles);
104 }
void setState(const std::vector< double > &positions)
Sets the scratch state from a vector of joint positions (all must be specified)

◆ closeLeftGripper()

void Cob4Robot::closeLeftGripper ( )

Closes the Cob4's left gripper.

Definition at line 106 of file cob4.cpp.

107 {
108  const std::map<std::string, double> angles = {{"gripper_left_finger_1_joint", 0.0},
109  {"gripper_left_finger_2_joint", 0.0}};
110 
111  Robot::setState(angles);
112 }

◆ closeRightGripper()

void Cob4Robot::closeRightGripper ( )

Closes the Cob4's right gripper.

Definition at line 114 of file cob4.cpp.

115 {
116  const std::map<std::string, double> angles = {{"gripper_right_finger_1_joint", 0.0},
117  {"gripper_right_finger_2_joint", 0.0}};
118 
119  Robot::setState(angles);
120 }

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

37 {
38  bool success = false;
39 
40  // First attempt the `robowflex_resources` package, then attempt the "actual" resource files.
42  {
43  RBX_INFO("Initializing Cob4 with `cob4_{description, moveit_config}`");
45  }
46  else
47  {
48  RBX_INFO("Initializing Cob4 with `robowflex_resources`");
50  }
51 
52  loadKinematics("arm_left");
53  loadKinematics("arm_right");
54 
56 
57  return success;
58 }
static const std::string DEFAULT_LIMITS
Default Limits.
Definition: cob4.h:77
static const std::string DEFAULT_SRDF
Default SRDF.
Definition: cob4.h:76
static const std::string RESOURCE_URDF
URDF from robowflex_resources.
Definition: cob4.h:80
void openGrippers()
Opens the Cob4's grippers.
Definition: cob4.cpp:72
static const std::string RESOURCE_KINEMATICS
kinematics from robowflex_resources
Definition: cob4.h:83
static const std::string DEFAULT_KINEMATICS
Default kinematics.
Definition: cob4.h:78
static const std::string RESOURCE_LIMITS
Limits from robowflex_resources.
Definition: cob4.h:82
static const std::string DEFAULT_URDF
Default URDF.
Definition: cob4.h:75
static const std::string RESOURCE_SRDF
SRDF from robowflex_resources.
Definition: cob4.h:81
bool initialize(const std::string &urdf_file)
Initializes a robot from a kinematic description. A default semantic description is used.
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.
#define RBX_INFO(fmt,...)
Output a info logging message.
Definition: log.h:118
std::string resolvePackage(const std::string &path)
Resolves package:// URLs to their canonical form. The path does not need to exist,...

◆ openGrippers()

void Cob4Robot::openGrippers ( )

Opens the Cob4's grippers.

Definition at line 72 of file cob4.cpp.

73 {
74  const std::map<std::string, double> angles = {{"gripper_left_finger_1_joint", -1.0},
75  {"gripper_left_finger_2_joint", 1.2},
76  {"gripper_right_finger_1_joint", -1.0},
77  {"gripper_right_finger_2_joint", 1.2}};
78 
79  Robot::setState(angles);
80 }

◆ openLeftGripper()

void Cob4Robot::openLeftGripper ( )

Opens the Cob4's left gripper.

Definition at line 82 of file cob4.cpp.

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

◆ openRightGripper()

void Cob4Robot::openRightGripper ( )

Opens the Cob4's right gripper.

Definition at line 90 of file cob4.cpp.

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

◆ pointHead()

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

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

Parameters
[in]pointThe point to look at.

Definition at line 60 of file cob4.cpp.

61 {
62  const RobotPose point_pose = RobotPose(Eigen::Translation3d(point));
63  const RobotPose point_pan = getLinkTF("head_3_link").inverse() * point_pose;
64 
65  const double pan = atan2(point_pan.translation().y(), point_pan.translation().x());
66 
67  const std::map<std::string, double> angles = {{"head_1_joint", pan}};
68 
69  Robot::setState(angles);
70 }
T atan2(T... args)
const RobotPose & getLinkTF(const std::string &name) const
Get the current pose of a link on the scratch state.
std::decay< decltype(std::declval< moveit::core::Transforms >().getTransform("")) >::type RobotPose
A pose (point in SE(3)) used in various functions. Defined from what MoveIt! uses.
Definition: adapter.h:24

◆ setBasePose()

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

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

Parameters
[in]xThe x position.
[in]yThe y position.
[in]thetaThe angle.

Definition at line 122 of file cob4.cpp.

123 {
124  if (hasJoint("base_joint/x") && hasJoint("base_joint/y") && hasJoint("base_joint/theta"))
125  {
126  const std::map<std::string, double> pose = {
127  {"base_joint/x", x}, {"base_joint/y", y}, {"base_joint/theta", theta}};
128 
129  scratch_->setVariablePositions(pose);
130  scratch_->update();
131  }
132  else
133  RBX_WARN("base_joint does not exist, cannot move base! You need to set addVirtual to true");
134 }
robot_state::RobotStatePtr scratch_
Scratch robot state.
bool hasJoint(const std::string &joint) const
Checks if a joint exists in the robot.
#define RBX_WARN(fmt,...)
Output a warning logging message.
Definition: log.h:110

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: