robowflex::darts::Robot Class Reference

A sampleable goal region for OMPL for a set of TSRs. Samples goals in a separate thread using a clone of the world. More...

#include <robot.h>

Inheritance diagram for robowflex::darts::Robot:

Public Types
using	GroupsMap = std::map< std::string, std::vector< std::string > >
	A map of group name to member joints. More...
using	NamedStatesMap = std::map< std::string, std::map< std::string, Eigen::VectorXd > >
	A map of group name to a map of state name to configurations. More...

Public Member Functions
Constructors
	Robot (const std::string &name)
	Construct an empty robot. More...
	Robot (robowflex::RobotPtr robot)
	Convert a robowflex::Robot into a Dart robot. More...
	Robot (const std::string &name, const ScenePtr &scene)
	Convert a robowflex::Scene into a Dart robot. More...
RobotPtr	cloneRobot (const std::string &newName) const
	Clone this robot with a new name. More...
File Loading
bool	loadURDF (const std::string &urdf)
	Load a URDF into this robot. More...
bool	loadSRDF (const std::string &srdf)
	Load a SRDF into this robot. More...
State Operations
void	setStateFromMoveItMsg (const moveit_msgs::RobotState &msg)
	Set the current state of this robot from a MoveIt message. More...
void	setMoveItMsgFromState (moveit_msgs::RobotState &msg) const
	Set a MoveIt message from the current state of this robot. More...
void	setStateFromMoveItState (const robot_state::RobotState &state)
	Set the current state of this robot from a MoveIt robot state. More...
void	setMoveItStateFromState (robot_state::RobotState &state) const
	Set a MoveIt robot state from the current state of this robot. More...
void	setStateFromMoveItJMG (const std::string &jmg, const std::vector< double > &joints)
	Set the state using a MoveIt Joint Group. More...
void	setStateFromMoveItJMG (const std::string &jmg, const Eigen::Ref< const Eigen::VectorXd > &vec)
	Set the state using a MoveIt Joint Group. More...
void	setMoveItJMGFromState (const std::string &jmg, std::vector< double > &joints) const
	Set a MoveIt Joint Group state from the current state of the robot. More...
void	setMoveItJMGFromState (const std::string &jmg, Eigen::Ref< Eigen::VectorXd > vec) const
	Set a MoveIt Joint Group state from the current state of the robot. More...
Group Operations
void	setGroups (const GroupsMap &newGroups)
	Get the groups currently in the robot. A map of group name to a list of all joints in the group. More...
const GroupsMap &	getGroups () const
	Get the groups currently in the robot. A map of group name to a list of all joints in the group. More...
std::vector< std::string > &	getGroupJointNames (const std::string &group)
	Get the joint names in a group. More...
const std::vector< std::string > &	getGroupJointNamesConst (const std::string &group) const
	Get the joint names in a group. More...
std::vector< dart::dynamics::Joint * >	getGroupJoints (const std::string &group) const
	Get the joints for a group. More...
std::pair< std::size_t, dart::dynamics::Joint * >	getGroupJoint (const std::string &group, const std::string &joint) const
	Get a joint in a group along with its index in the group. More...
const std::vector< std::size_t > &	getGroupIndices (const std::string &group) const
	Get the indices of joints corresponding to a group. More...
bool	isGroup (const std::string &name) const
	Checks if a group exists. More...
std::size_t	getNumDofsGroup (const std::string &group) const
	Gets the number of DoFs controlled by a group. More...
void	getGroupState (const std::string &group, Eigen::Ref< Eigen::VectorXd > q) const
	Get the current state of the joints of a group. More...
void	setGroupState (const std::string &group, const Eigen::Ref< const Eigen::VectorXd > &q)
	Set the current state of the joints of a group. More...
Constructing Groups
bool	addJointToGroup (const std::string &group, const std::string &joint_name)
	Add a joint to a group. More...
bool	addLinkToGroup (const std::string &group, const std::string &link_name)
	Add a the parent joint of a link to a group. More...
bool	addChainToGroup (const std::string &group, const std::string &tip, const std::string &base)
	Add all the joints between two frames in a chain to a group. More...
bool	addGroupToGroup (const std::string &group, const std::string &other)
	Add all the joints in another group to this group. More...
Named States
void	setNamedGroupStates (const NamedStatesMap &states)
	Set the named states map. More...
const NamedStatesMap &	getNamedGroupStates () const
	Get the named states map. More...
std::vector< std::string >	getNamedGroupStates (const std::string &group) const
	Get all names for named states for a group. More...
bool	getNamedGroupState (const std::string &group, const std::string &name, Eigen::Ref< Eigen::VectorXd > q) const
	Get the configuration of a named group state. More...
void	setNamedGroupState (const std::string &group, const std::string &name, const Eigen::Ref< const Eigen::VectorXd > &q)
	Set or create a named group state. More...
Public Member Functions inherited from robowflex::darts::Structure
	Structure (const std::string &name)
	Create an empty structure. More...
	Structure (const std::string &name, const SceneConstPtr &scene)
	Copy a MoveIt (robowflex::Scene) into a structure. More...
virtual	~Structure ()=default
	Destructor. More...
StructurePtr	cloneStructure (const std::string &newName) const
	Clones this structure with a new name. More...
const std::string &	getName () const
	Get the name of this structure. More...
ACMPtr	getACM ()
	Get the ACM for the structure. More...
const ACMPtr &	getACMConst () const
	Get the ACM for the structure. More...
void	setSkeleton (const dart::dynamics::SkeletonPtr &skeleton)
	Set the skeleton for the structure. More...
dart::dynamics::SkeletonPtr &	getSkeleton ()
	Get the underlying skeleton for the structure. More...
const dart::dynamics::SkeletonPtr &	getSkeletonConst () const
	Get the underlying skeleton for the structure. More...
void	dumpGraphViz (std::ostream &out, bool standalone=true)
	Dumps the structure of the skeleton to a GraphViz file. More...
void	setJoint (const std::string &name, double value)
	Set the value of a 1-DoF joint in the structure. More...
void	setJoint (const std::string &name, const Eigen::Ref< const Eigen::VectorXd > &value)
	Set the value of a n-DoF joint in the structure. More...
bool	solveIK ()
	Solve the current whole-body IK problem imposed on the structure. More...
void	setDof (unsigned int index, double value)
	Set the DoF at index to value. More...
std::vector< std::string >	getJointNames () const
	Get the joint names for this structure. More...
dart::dynamics::Joint *	getJoint (const std::string &joint_name) const
	Get a reference to the joint in the structure. More...
dart::dynamics::BodyNode *	getFrame (const std::string &name="") const
	Get a body node within the structure. More...
dart::dynamics::BodyNode *	getRootFrame () const
	Get the root frame of this structure. More...
void	reparentFreeFrame (dart::dynamics::BodyNode *child, const std::string &parent="")
	Reparents the child node to the parent node. More...
void	setJointParentTransform (const std::string &name, const RobotPose &tf)
	Set the transform from a joint to its parent. More...
void	updateCollisionObject (const std::string &name, const GeometryPtr &geometry, const robowflex::RobotPose &pose)
	Update or add a collision object. More...
std::pair< dart::dynamics::RevoluteJoint *, dart::dynamics::BodyNode * >	addRevoluteFrame (const dart::dynamics::RevoluteJoint::Properties &properties, const dart::dynamics::ShapePtr &shape, dart::dynamics::BodyNode *parent=nullptr)
	Add a new frame attached to a revolute joint to this structure. More...
std::pair< dart::dynamics::PrismaticJoint *, dart::dynamics::BodyNode * >	addPrismaticFrame (const dart::dynamics::PrismaticJoint::Properties &properties, const dart::dynamics::ShapePtr &shape, dart::dynamics::BodyNode *parent=nullptr)
	Add a new frame attached to a prismatic joint to this structure. More...
std::pair< dart::dynamics::FreeJoint *, dart::dynamics::BodyNode * >	addFreeFrame (const dart::dynamics::FreeJoint::Properties &properties, const dart::dynamics::ShapePtr &shape, dart::dynamics::BodyNode *parent=nullptr)
	Add a new frame attached to a free joint to this structure. More...
std::pair< dart::dynamics::WeldJoint *, dart::dynamics::BodyNode * >	addWeldedFrame (const dart::dynamics::WeldJoint::Properties &properties, const dart::dynamics::ShapePtr &shape, dart::dynamics::BodyNode *parent=nullptr)
	Add a new frame attached to a fixed joint to this structure. More...
void	addGround (double z=0., double radius=10.)
	Add a ground plane. More...

Private Member Functions
bool	addNameToGroup (const std::string &group, const std::string &name)
	Adds a name to the group. More...
void	processGroup (const std::string &group)
	Process the members of a group to generate the set of indices. More...

Private Attributes
std::map< std::string, std::vector< std::string > >	groups_
	Map of group names to joint names. More...
std::map< std::string, std::map< std::string, Eigen::VectorXd > >	group_states_
std::map< std::string, std::vector< std::size_t > >	group_indices_
	Indices of group joints. More...
robowflex::RobotPtr	robot_ {nullptr}
	Robowflex robot. More...

Additional Inherited Members
Protected Member Functions inherited from robowflex::darts::Structure
void	createShapeNode (dart::dynamics::BodyNode *body, const dart::dynamics::ShapePtr &shape)
	Create a shape node on a body. More...
Protected Attributes inherited from robowflex::darts::Structure
const std::string	name_ {"robot"}
	Name of the structure. More...
dart::dynamics::SkeletonPtr	skeleton_ {nullptr}
	Underlying skeleton. More...
ACMPtr	acm_
	ACM for structure. More...

Detailed Description

A sampleable goal region for OMPL for a set of TSRs. Samples goals in a separate thread using a clone of the world.

Definition at line 40 of file robowflex_dart/include/robowflex_dart/robot.h.

Member Typedef Documentation

GroupsMap

using robowflex::darts::Robot::GroupsMap = std::map<std::string, std::vector<std::string> >

A map of group name to member joints.

Definition at line 45 of file robowflex_dart/include/robowflex_dart/robot.h.

NamedStatesMap

using robowflex::darts::Robot::NamedStatesMap = std::map<std::string, std::map<std::string, Eigen::VectorXd> >

A map of group name to a map of state name to configurations.

Definition at line 49 of file robowflex_dart/include/robowflex_dart/robot.h.

Constructor & Destructor Documentation

Robot() [1/3]

Robot::Robot

(

const std::string &

name

)

Construct an empty robot.

Parameters

[in]

name

Name of the robot.

Robot

Definition at line 24 of file robowflex_dart/src/robot.cpp.

                                  : Structure(name)
{
}

Robot() [2/3]

Robot::Robot

(

robowflex::RobotPtr

robot

)

Convert a robowflex::Robot into a Dart robot.

Parameters

[in]

robot

Robot to convert.

Definition at line 28 of file robowflex_dart/src/robot.cpp.

                                    : Structure(robot->getName()), robot_(robot)
{
    std::ofstream urdf_file;
    std::string urdf_filename = robowflex::IO::createTempFile(urdf_file);
 
    std::ofstream srdf_file;
    std::string srdf_filename = robowflex::IO::createTempFile(srdf_file);
 
    urdf_file << robot->getURDFString();
    urdf_file.close();
 
    srdf_file << robot->getSRDFString();
    srdf_file.close();
 
    loadURDF(urdf_filename);
    loadSRDF(srdf_filename);
 
    robowflex::IO::deleteFile(urdf_filename);
    robowflex::IO::deleteFile(srdf_filename);
}

Robot() [3/3]

Robot::Robot	(	const std::string &	name,
		const ScenePtr &	scene
	)

Convert a robowflex::Scene into a Dart robot.

Parameters

[in]	name	Name of the robot.
[in]	scene	Scene to convert.

Definition at line 49 of file robowflex_dart/src/robot.cpp.

                                                         : Structure(name, scene)
{
}

Member Function Documentation

addChainToGroup()

bool Robot::addChainToGroup	(	const std::string &	group,
		const std::string &	tip,
		const std::string &	base
	)

Add all the joints between two frames in a chain to a group.

Parameters

[in]	group	Group to add joint to.
[in]	tip	Tip of the chain.
[in]	base	Base of the chain.

Returns

True on success, false on failure.

Definition at line 135 of file robowflex_dart/src/robot.cpp.

{
    auto *node = skeleton_->getBodyNode(tip);
    if (not node)
    {
        RBX_ERROR("Tip link %1% not in skeleton.", tip);
        return false;
    }
 
    std::vector<std::string> names;
    while (node and node->getName() != base)
    {
        const auto &joint = node->getParentJoint();
        if (not joint)
        {
            RBX_ERROR("Link %1% has no parent joint", node->getName());
            return false;
        }
 
        if (joint->getNumDofs() > 0)
            names.emplace_back(joint->getName());
 
        node = joint->getParentBodyNode();
    }
 
    if (not node)
    {
        RBX_ERROR("Base link %1% not parent of tip link %2%", base, tip);
        return false;
    }
 
    for (const auto &name : names)
        addNameToGroup(group, name);
 
    processGroup(group);
    return true;
}

addGroupToGroup()

bool Robot::addGroupToGroup	(	const std::string &	group,
		const std::string &	other
	)

Add all the joints in another group to this group.

Parameters

[in]	group	Group to add other to.
[in]	other	Other group to add.

Returns

True on success, false on failure.

Definition at line 173 of file robowflex_dart/src/robot.cpp.

{
    if (not isGroup(other))
    {
        RBX_ERROR("Group %1% does not exist", other);
        return false;
    }
 
    for (const auto &name : getGroupJointNamesConst(other))
        addNameToGroup(group, name);
 
    processGroup(group);
    return true;
}

addJointToGroup()

bool Robot::addJointToGroup	(	const std::string &	group,
		const std::string &	joint_name
	)

Add a joint to a group.

Parameters

[in]	group	Group to add joint to.
[in]	joint_name	Joint name to add.

Returns

True on success, false on failure.

Definition at line 92 of file robowflex_dart/src/robot.cpp.

{
    const auto &joint = skeleton_->getJoint(joint_name);
    if (not joint)
    {
        RBX_ERROR("Joint %1% not in skeleton.", joint_name);
        return false;
    }
 
    if (joint->getNumDofs() > 0)
        addNameToGroup(group, joint_name);
 
    processGroup(group);
    return true;
}

addLinkToGroup()

bool Robot::addLinkToGroup	(	const std::string &	group,
		const std::string &	link_name
	)

Add a the parent joint of a link to a group.

Parameters

[in]	group	Group to add joint to.
[in]	link_name	Link to add.

Returns

True on success, false on failure.

Definition at line 108 of file robowflex_dart/src/robot.cpp.

{
    const auto &node = skeleton_->getBodyNode(link_name);
    if (not node)
    {
        RBX_ERROR("Link %1% not in skeleton.", link_name);
        return false;
    }
 
    const auto &joint = node->getParentJoint();
    if (not joint)
    {
        RBX_ERROR("Link %1% has no parent joint", link_name);
        return false;
    }
 
    if (joint->getName() == "rootJoint")
        return false;
 
    // Only include mobile joints
    if (joint->getNumDofs() > 0)
        addNameToGroup(group, joint->getName());
 
    processGroup(group);
    return true;
}

addNameToGroup()

bool Robot::addNameToGroup ( const std::string &  group, const std::string &  name  )

private

Adds a name to the group.

Parameters

[in]	group	Group to add name to.
[in]	name	Name to add to group.

Returns

True on success, false on failure.

Definition at line 72 of file robowflex_dart/src/robot.cpp.

{
    if (not isGroup(group))
    {
        RBX_DEBUG("Adding joint %1% to group %2%.", name, group);
        groups_.emplace(group, std::vector<std::string>{name});
        return true;
    }
 
    auto &names = getGroupJointNames(group);
 
    for (const auto &item : names)
        if (item == name)
            return false;
 
    RBX_DEBUG("Adding joint %1% to group %2%.", name, group);
    names.emplace_back(name);
    return true;
}

cloneRobot()

RobotPtr Robot::cloneRobot

(

const std::string &

newName

)

const

Clone this robot with a new name.

Parameters

[in]

newName

Name for cloned robot.

Returns

Cloned robot.

Definition at line 53 of file robowflex_dart/src/robot.cpp.

{
    auto robot = std::make_shared<Robot>(newName);
    robot->setSkeleton(skeleton_->cloneSkeleton(newName));
 
    for (const auto &pair : acm_->getDisabledPairsConst())
        robot->getACM()->disableCollision(pair.first, pair.second);
 
    robot->setGroups(groups_);
    robot->setNamedGroupStates(group_states_);
 
    return robot;
}

getGroupIndices()

const std::vector< std::size_t > & Robot::getGroupIndices

(

const std::string &

group

)

const

Get the indices of joints corresponding to a group.

Parameters

[in]

group

Group to get indices for.

Returns

The indices of the joints in a group.

Definition at line 381 of file robowflex_dart/src/robot.cpp.

{
    return group_indices_.find(group)->second;
}

getGroupJoint()

std::pair< std::size_t, dart::dynamics::Joint * > Robot::getGroupJoint	(	const std::string &	group,
		const std::string &	joint
	)		const

Get a joint in a group along with its index in the group.

Parameters

[in]	group	Group to search.
[in]	joint	Joint to find in group.

Returns

The index of the joint and the joint in the group. {0, nullptr} if it does not exist.

Definition at line 366 of file robowflex_dart/src/robot.cpp.

{
    std::size_t i = 0;
    for (const auto &dof : getGroupJoints(group))
    {
        if (dof->getName() == joint)
            return {i, dof};
 
        i += dof->getNumDofs();
    }
 
    return {0, nullptr};
}

getGroupJointNames()

std::vector< std::string > & Robot::getGroupJointNames

(

const std::string &

group

)

Get the joint names in a group.

Parameters

[in]

group

Group to get joint names for.

Returns

The list of joint names.

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

{
    return groups_.find(group)->second;
}

getGroupJointNamesConst()

const std::vector< std::string > & Robot::getGroupJointNamesConst

(

const std::string &

group

)

const

Get the joint names in a group.

Parameters

[in]

group

Group to get joint names for.

Returns

The list of joint names.

Definition at line 352 of file robowflex_dart/src/robot.cpp.

{
    return groups_.find(group)->second;
}

getGroupJoints()

std::vector< dart::dynamics::Joint * > Robot::getGroupJoints

(

const std::string &

group

)

const

Get the joints for a group.

Parameters

[in]

group

Group to get joint names for.

Returns

The list of joints.

Definition at line 357 of file robowflex_dart/src/robot.cpp.

{
    std::vector<dart::dynamics::Joint *> joints;
    for (const auto &dof : groups_.find(group)->second)
        joints.emplace_back(skeleton_->getJoint(dof));
 
    return joints;
}

getGroups()

const Robot::GroupsMap & Robot::getGroups

(

)

const

Get the groups currently in the robot. A map of group name to a list of all joints in the group.

Returns

The groups in the robot.

Definition at line 342 of file robowflex_dart/src/robot.cpp.

{
    return groups_;
}

getGroupState()

void Robot::getGroupState	(	const std::string &	group,
		Eigen::Ref< Eigen::VectorXd >	q
	)		const

Get the current state of the joints of a group.

Parameters

[in]	group	Group to get state for.
[out]	q	Configuration of the group.

Definition at line 391 of file robowflex_dart/src/robot.cpp.

{
    q = skeleton_->getPositions(getGroupIndices(group));
}

getNamedGroupState()

bool Robot::getNamedGroupState	(	const std::string &	group,
		const std::string &	name,
		Eigen::Ref< Eigen::VectorXd >	q
	)		const

Get the configuration of a named group state.

Parameters

[in]	group	Group of the named state.
[in]	name	Name of the state to get.
[out]	q	Configuration to fill with named state.

Returns

True if state exists, false if not.

Definition at line 423 of file robowflex_dart/src/robot.cpp.

{
    auto gsit = group_states_.find(group);
    if (gsit != group_states_.end())
    {
        auto it = gsit->second.find(name);
        if (it != gsit->second.end())
        {
            q = it->second;
            return true;
        }
 
        return false;
    }
 
    return false;
}

getNamedGroupStates() [1/2]

const Robot::NamedStatesMap & Robot::getNamedGroupStates

(

)

const

Get the named states map.

Returns

The map of named states.

Definition at line 406 of file robowflex_dart/src/robot.cpp.

{
    return group_states_;
}

getNamedGroupStates() [2/2]

std::vector< std::string > Robot::getNamedGroupStates

(

const std::string &

group

)

const

Get all names for named states for a group.

Parameters

[in]

group

Group to get named states for.

Returns

List of all named states.

Definition at line 411 of file robowflex_dart/src/robot.cpp.

{
    std::vector<std::string> names;
    for (const auto &group : group_states_)
    {
        for (const auto &state : group.second)
            names.emplace_back(state.first);
    }
 
    return names;
}

getNumDofsGroup()

std::size_t Robot::getNumDofsGroup

(

const std::string &

group

)

const

Gets the number of DoFs controlled by a group.

Parameters

[in]

group

Group to get DoFs for.

Returns

The number of DoFs in the group.

Definition at line 386 of file robowflex_dart/src/robot.cpp.

{
    return getGroupIndices(group).size();
}

isGroup()

bool Robot::isGroup

(

const std::string &

name

)

const

Checks if a group exists.

Parameters

[in]

name

Name of group to check.

Returns

True if the group exists, false otherwise.

Definition at line 330 of file robowflex_dart/src/robot.cpp.

{
    return groups_.find(name) != groups_.end();
}

loadSRDF()

bool Robot::loadSRDF

(

const std::string &

srdf

)

Load a SRDF into this robot.

Parameters

[in]

srdf

SRDF filename to load. Can be package URI.

Returns

True on success, false on failure.

Definition at line 188 of file robowflex_dart/src/robot.cpp.

{
    const auto &file = IO::getPackageFile(srdf);
    if (file.empty())
    {
        RBX_ERROR("File %1% cannot be found!", srdf);
        return false;
    }
 
    tinyxml2::XMLDocument doc;
    doc.LoadFile(file.c_str());
 
    tinyxml2::XMLElement *root = doc.FirstChildElement();
    if (not root)
    {
        RBX_ERROR("No child element in SRDF");
        return false;
    }
 
    // parse virtual joints
    tinyxml2::XMLElement *vj = root->FirstChildElement("virtual_joint");
    while (vj != nullptr)
    {
        std::string name = vj->Attribute("name");
        std::string type = vj->Attribute("type");
        std::string parent = vj->Attribute("parent_frame");
        std::string child = vj->Attribute("child_link");
 
        dart::dynamics::BodyNode *pnode = nullptr;
        if (parent != "world")
        {
            pnode = skeleton_->getBodyNode(parent);
            if (not pnode)
                RBX_ERROR("Couldn't find %s for virtual joint!", parent);
        }
 
        auto *cnode = skeleton_->getBodyNode(child);
        if (cnode)
        {
            if (type == "floating")
            {
                dart::dynamics::FreeJoint::Properties joint;
                joint.mName = name;
 
                cnode->moveTo<dart::dynamics::FreeJoint>(pnode, joint);
            }
        }
 
        vj = vj->NextSiblingElement("virtual_joint");
    }
 
    // Parse groups
    tinyxml2::XMLElement *group = root->FirstChildElement("group");
    while (group != nullptr)
    {
        const auto &group_name = group->Attribute("name");
 
        tinyxml2::XMLElement *joints = group->FirstChildElement("joint");
        while (joints != nullptr)
        {
            const auto &joint_name = joints->Attribute("name");
            addJointToGroup(group_name, joint_name);
 
            joints = joints->NextSiblingElement("joint");
        }
 
        tinyxml2::XMLElement *links = group->FirstChildElement("link");
        while (links != nullptr)
        {
            const auto &link_name = links->Attribute("name");
            addLinkToGroup(group_name, link_name);
 
            links = links->NextSiblingElement("link");
        }
 
        tinyxml2::XMLElement *chains = group->FirstChildElement("chain");
        while (chains != nullptr)
        {
            const auto &base_link = chains->Attribute("base_link");
            const auto &tip_link = chains->Attribute("tip_link");
            addChainToGroup(group_name, tip_link, base_link);
 
            chains = chains->NextSiblingElement("chain");
        }
 
        tinyxml2::XMLElement *groups = group->FirstChildElement("group");
        while (groups != nullptr)
        {
            const auto &other_name = groups->Attribute("name");
            addGroupToGroup(group_name, other_name);
 
            groups = groups->NextSiblingElement("group");
        }
 
        group = group->NextSiblingElement("group");
    }
 
    // Parse allowed collisions
    tinyxml2::XMLElement *dc = root->FirstChildElement("disable_collisions");
    while (dc != nullptr)
    {
        const auto &l1 = dc->Attribute("link1");
        const auto &l2 = dc->Attribute("link2");
 
        acm_->disableCollision(l1, l2);
 
        dc = dc->NextSiblingElement("disable_collisions");
    }
 
    // Parse named group states
    tinyxml2::XMLElement *gs = root->FirstChildElement("group_state");
    while (gs != nullptr)
    {
        const auto &group = gs->Attribute("group");
        const auto &name = gs->Attribute("name");
 
        Eigen::VectorXd state = Eigen::VectorXd::Zero(getNumDofsGroup(group));
 
        tinyxml2::XMLElement *js = gs->FirstChildElement("joint");
        while (js != nullptr)
        {
            const auto &value = js->Attribute("value");
            const auto &jname = js->Attribute("name");
 
            auto values = robowflex::IO::tokenize<double>(value, " ");
            auto joint = getGroupJoint(group, jname);
            if (joint.second != nullptr)
            {
                for (std::size_t i = 0; i < values.size(); ++i)
                    state[joint.first + i] = values[i];
            }
 
            js = js->NextSiblingElement("joint");
        }
 
        setNamedGroupState(group, name, state);
        gs = gs->NextSiblingElement("group_state");
    }
 
    return true;
}

loadURDF()

bool Robot::loadURDF

(

const std::string &

urdf

)

Load a URDF into this robot.

Parameters

[in]

urdf

URDF filename to load. Can be package URI.

Returns

True on success, false on failure.

Definition at line 67 of file robowflex_dart/src/robot.cpp.

{
    return IO::loadURDF(*this, urdf);
}

processGroup()

void Robot::processGroup ( const std::string &  group )

private

Process the members of a group to generate the set of indices.

Parameters

[in]

group

Group to process.

Definition at line 568 of file robowflex_dart/src/robot.cpp.

{
    std::vector<std::size_t> indices;
 
    auto joints = getGroupJoints(group);
    for (const auto &joint : joints)
    {
        for (std::size_t i = 0; i < joint->getNumDofs(); ++i)
            indices.emplace_back(joint->getDof(i)->getIndexInSkeleton());
    }
 
    auto it = group_indices_.find(group);
    if (it == group_indices_.end())
        group_indices_.emplace(group, indices);
    else
        it->second = indices;
}

setGroups()

void Robot::setGroups

(

const GroupsMap &

newGroups

)

Get the groups currently in the robot. A map of group name to a list of all joints in the group.

Returns

The groups in the robot.

Definition at line 335 of file robowflex_dart/src/robot.cpp.

{
    groups_ = newGroups;
    for (const auto &pair : groups_)
        processGroup(pair.first);
}

setGroupState()

void Robot::setGroupState	(	const std::string &	group,
		const Eigen::Ref< const Eigen::VectorXd > &	q
	)

Set the current state of the joints of a group.

Parameters

[in]	group	Group to set state for.
[out]	q	Configuration to set the group.

Definition at line 396 of file robowflex_dart/src/robot.cpp.

{
    skeleton_->setPositions(getGroupIndices(group), q);
}

setMoveItJMGFromState() [1/2]

void Robot::setMoveItJMGFromState	(	const std::string &	jmg,
		Eigen::Ref< Eigen::VectorXd >	vec
	)		const

Set a MoveIt Joint Group state from the current state of the robot.

Parameters

[in]	jmg	Name of joint group.
[out]	vec	Joint values to set.

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

{
    if (not robot_)
        return;
 
    setMoveItStateFromState(*robot_->getScratchState());                  // copy current state
    robot_->getScratchState()->copyJointGroupPositions(jmg, vec.data());  // copy JMG state
}

setMoveItJMGFromState() [2/2]

void Robot::setMoveItJMGFromState	(	const std::string &	jmg,
		std::vector< double > &	joints
	)		const

Set a MoveIt Joint Group state from the current state of the robot.

Parameters

[in]	jmg	Name of joint group.
[out]	joints	Joint values to set.

Definition at line 550 of file robowflex_dart/src/robot.cpp.

{
    if (not robot_)
        return;
 
    setMoveItStateFromState(*robot_->getScratchState());              // copy current state
    robot_->getScratchState()->copyJointGroupPositions(jmg, joints);  // copy JMG state
}

setMoveItMsgFromState()

void Robot::setMoveItMsgFromState

(

moveit_msgs::RobotState &

msg

)

const

Set a MoveIt message from the current state of this robot.

Parameters

[out]

msg

Message to set.

Definition at line 484 of file robowflex_dart/src/robot.cpp.

{
    msg = moveit_msgs::RobotState();
 
    for (std::size_t i = 0; i < skeleton_->getNumJoints(); ++i)
    {
        auto *joint = skeleton_->getJoint(i);
 
        // ignore fixed joints
        if (joint->getNumDofs() == 0)
            continue;
 
        auto *j = dynamic_cast<dart::dynamics::FreeJoint *>(joint);
        if (j)
        {
            msg.multi_dof_joint_state.joint_names.push_back(joint->getName());
 
            auto tf = joint->getRelativeTransform();
            geometry_msgs::Transform tfmsg;
            tfmsg.translation = TF::vectorEigenToMsg(tf.translation());
            tfmsg.rotation = TF::quaternionEigenToMsg(TF::getPoseRotation(tf));
 
            msg.multi_dof_joint_state.transforms.push_back(tfmsg);
        }
        else
        {
            msg.joint_state.name.push_back(joint->getName());
            msg.joint_state.position.push_back(joint->getPositions()[0]);
        }
    }
}

setMoveItStateFromState()

void Robot::setMoveItStateFromState

(

robot_state::RobotState &

state

)

const

Set a MoveIt robot state from the current state of this robot.

Parameters

[out]

state

MoveIt state to set state to.

Definition at line 523 of file robowflex_dart/src/robot.cpp.

{
    moveit_msgs::RobotState msg;
    setMoveItMsgFromState(msg);
    moveit::core::robotStateMsgToRobotState(msg, state);
}

setNamedGroupState()

void Robot::setNamedGroupState	(	const std::string &	group,
		const std::string &	name,
		const Eigen::Ref< const Eigen::VectorXd > &	q
	)

Set or create a named group state.

Parameters

[in]	group	Group of the named state.
[in]	name	Name of the state to set.
[out]	q	Configuration for the named state.

Definition at line 442 of file robowflex_dart/src/robot.cpp.

{
    auto gsit = group_states_.find(group);
    if (gsit == group_states_.end())
    {
        bool r;
        std::tie(gsit, r) = group_states_.emplace(group, std::map<std::string, Eigen::VectorXd>{});
    }
 
    auto &names = gsit->second;
 
    auto it = names.find(name);
    if (it == names.end())
    {
        bool r;
        std::tie(it, r) = names.emplace(name, q);
    }
    else
        it->second = q;
}

setNamedGroupStates()

void Robot::setNamedGroupStates

(

const NamedStatesMap &

states

)

Set the named states map.

Parameters

[in]

states

New named states map.

Definition at line 401 of file robowflex_dart/src/robot.cpp.

{
    group_states_ = states;
}

setStateFromMoveItJMG() [1/2]

void Robot::setStateFromMoveItJMG	(	const std::string &	jmg,
		const Eigen::Ref< const Eigen::VectorXd > &	vec
	)

Set the state using a MoveIt Joint Group.

Parameters

[in]	jmg	Name of joint group.
[in]	vec	Value of Joint Group to set.

Definition at line 540 of file robowflex_dart/src/robot.cpp.

{
    if (not robot_)
        return;
 
    setMoveItStateFromState(*robot_->getScratchState());                 // copy current state
    robot_->getScratchState()->setJointGroupPositions(jmg, vec.data());  // set only JMG state
    setStateFromMoveItState(*robot_->getScratchState());                 // copy back
}

setStateFromMoveItJMG() [2/2]

void Robot::setStateFromMoveItJMG	(	const std::string &	jmg,
		const std::vector< double > &	joints
	)

Set the state using a MoveIt Joint Group.

Parameters

[in]	jmg	Name of joint group.
[in]	joints	Value of Joint Group to set.

Definition at line 530 of file robowflex_dart/src/robot.cpp.

{
    if (not robot_)
        return;
 
    setMoveItStateFromState(*robot_->getScratchState());             // copy current state
    robot_->getScratchState()->setJointGroupPositions(jmg, joints);  // set only JMG state
    setStateFromMoveItState(*robot_->getScratchState());             // copy back
}

setStateFromMoveItMsg()

void Robot::setStateFromMoveItMsg

(

const moveit_msgs::RobotState &

msg

)

Set the current state of this robot from a MoveIt message.

Parameters

[in]

msg

Message to set state to.

Definition at line 464 of file robowflex_dart/src/robot.cpp.

{
    for (std::size_t i = 0; i < msg.joint_state.name.size(); ++i)
        setJoint(msg.joint_state.name[i], msg.joint_state.position[i]);
 
    for (std::size_t i = 0; i < msg.multi_dof_joint_state.joint_names.size(); ++i)
    {
        auto *joint = skeleton_->getJoint(msg.multi_dof_joint_state.joint_names[i]);
        auto *j = static_cast<dart::dynamics::FreeJoint *>(joint);
 
        auto tfmsg = msg.multi_dof_joint_state.transforms[i];
 
        Eigen::Isometry3d tf;
        tf.translation() = TF::vectorMsgToEigen(tfmsg.translation);
        tf.linear() = TF::quaternionMsgToEigen(tfmsg.rotation).toRotationMatrix();
 
        j->setRelativeTransform(tf);
    }
}

setStateFromMoveItState()

void Robot::setStateFromMoveItState

(

const robot_state::RobotState &

state

)

Set the current state of this robot from a MoveIt robot state.

Parameters

[in]

state

MoveIt state to set state to.

Definition at line 516 of file robowflex_dart/src/robot.cpp.

{
    moveit_msgs::RobotState msg;
    moveit::core::robotStateToRobotStateMsg(state, msg);
    setStateFromMoveItMsg(msg);
}

Member Data Documentation

group_indices_

std::map<std::string, std::vector<std::size_t> > robowflex::darts::Robot::group_indices_

private

Indices of group joints.

Definition at line 304 of file robowflex_dart/include/robowflex_dart/robot.h.

group_states_

std::map<std::string, std::map<std::string, Eigen::VectorXd> > robowflex::darts::Robot::group_states_

private

Map of group names to map of named states.

Definition at line 301 of file robowflex_dart/include/robowflex_dart/robot.h.

groups_

std::map<std::string, std::vector<std::string> > robowflex::darts::Robot::groups_

private

Map of group names to joint names.

Definition at line 300 of file robowflex_dart/include/robowflex_dart/robot.h.

robot_

robowflex::RobotPtr robowflex::darts::Robot::robot_ {nullptr}

private

Robowflex robot.

Definition at line 306 of file robowflex_dart/include/robowflex_dart/robot.h.

---

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

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