robowflex::darts::Structure Class Reference

Wrapper class for a dart::dynamics::Skeleton. More...

#include <structure.h>

Inheritance diagram for robowflex::darts::Structure:

Public Member Functions
Constructors
	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...
Getters and Setters
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...
Getting and Setting Configurations
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...
Modifying Frames
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...
Constructing Frames
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...

Protected Member Functions
void	createShapeNode (dart::dynamics::BodyNode *body, const dart::dynamics::ShapePtr &shape)
	Create a shape node on a body. More...

Protected Attributes
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

Wrapper class for a dart::dynamics::Skeleton.

Definition at line 52 of file structure.h.

Constructor & Destructor Documentation

Structure() [1/2]

Structure::Structure

(

const std::string &

name

)

Create an empty structure.

Parameters

[in]

name

Name of the structure.

Structure

Definition at line 26 of file structure.cpp.

  : name_(name), skeleton_(dart::dynamics::Skeleton::create(name_)), acm_(std::make_shared<ACM>(this))
{
    skeleton_->setSelfCollisionCheck(true);
}

Structure() [2/2]

Structure::Structure	(	const std::string &	name,
		const SceneConstPtr &	scene
	)

Copy a MoveIt (robowflex::Scene) into a structure.

Parameters

[in]	name	Name of the structure.
[in]	scene	Scene to copy.

Definition at line 32 of file structure.cpp.

                                                                      : Structure(name)
{
    dart::dynamics::WeldJoint::Properties properties;
    dart::dynamics::BodyNode::Properties node;
 
    node.mName = properties.mName = "root";
    properties.mT_ParentBodyToJoint = robowflex::TF::identity();
    const auto &pair =
        skeleton_->createJointAndBodyNodePair<dart::dynamics::WeldJoint>(nullptr, properties, node);
    const auto &root = pair.second;
 
    const auto &objects = scene->getCollisionObjects();
    for (const auto &object : objects)
    {
        const auto &geometry = scene->getObjectGeometry(object);
        const auto &pose = scene->getObjectPose(object);
 
        dart::dynamics::FreeJoint::Properties joint;
        joint.mName = object;
        joint.mT_ParentBodyToJoint = robowflex::TF::identity();
 
        auto shape = makeGeometry(geometry);
 
        auto pair = addFreeFrame(joint, shape, root);
        setJointParentTransform(object, pose);
        setColor(pair.second, dart::Color::Blue(0.2));
    }
 
    const auto &acm = scene->getACMConst();
    std::vector<std::string> names;
    acm.getAllEntryNames(names);
 
    collision_detection::AllowedCollision::Type type;
    for (const auto &name1 : names)
        for (const auto &name2 : names)
            if (acm.getEntry(name1, name2, type))
            {
                if (type == collision_detection::AllowedCollision::NEVER)
                    acm_->enableCollision(name1, name2);
                else if (type == collision_detection::AllowedCollision::ALWAYS)
                    acm_->disableCollision(name1, name2);
            }
}

~Structure()

virtual robowflex::darts::Structure::~Structure ( )

virtualdefault

Destructor.

Member Function Documentation

addFreeFrame()

std::pair< dart::dynamics::FreeJoint *, dart::dynamics::BodyNode * > Structure::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.

Parameters

[in]	properties	Joint properties to use for joint.
[in]	shape	Shape to attach to frame.
[in]	parent	Parent frame to attach new joint to.

Returns

The created joint and body node.

Definition at line 212 of file structure.cpp.

{
    dart::dynamics::BodyNode::Properties node;
    node.mName = properties.mName;
 
    auto pair = skeleton_->createJointAndBodyNodePair<dart::dynamics::FreeJoint>(parent, properties, node);
    createShapeNode(pair.second, shape);
 
    pair.first->setPositionLimitEnforced(true);
    return pair;
}

addGround()

void Structure::addGround	(	double	z = 0.,
		double	radius = 10.
	)

Add a ground plane.

Parameters

[in]	z	Z- height of the plane.
[in]	radius	X- and Y- width of the plane.

Definition at line 240 of file structure.cpp.

{
    const double thickness = 0.01;
 
    dart::dynamics::WeldJoint::Properties joint;
    joint.mName = "ground";
    joint.mT_ParentBodyToJoint.translation() = Eigen::Vector3d(0, 0, z - thickness);
 
    auto ground = makeBox(radius, radius, thickness);
    auto pair = addWeldedFrame(joint, ground);
 
    setColor(pair.second, dart::Color::Blue(0.2));
}

addPrismaticFrame()

std::pair< dart::dynamics::PrismaticJoint *, dart::dynamics::BodyNode * > Structure::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.

Parameters

[in]	properties	Joint properties to use for joint.
[in]	shape	Shape to attach to frame.
[in]	parent	Parent frame to attach new joint to.

Returns

The created joint and body node.

Definition at line 196 of file structure.cpp.

{
    dart::dynamics::BodyNode::Properties node;
    node.mName = properties.mName;
 
    auto pair =
        skeleton_->createJointAndBodyNodePair<dart::dynamics::PrismaticJoint>(parent, properties, node);
    createShapeNode(pair.second, shape);
 
    pair.first->setPositionLimitEnforced(true);
    return pair;
}

addRevoluteFrame()

std::pair< dart::dynamics::RevoluteJoint *, dart::dynamics::BodyNode * > Structure::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.

Parameters

[in]	properties	Joint properties to use for joint.
[in]	shape	Shape to attach to frame.
[in]	parent	Parent frame to attach new joint to.

Returns

The created joint and body node.

Definition at line 180 of file structure.cpp.

{
    dart::dynamics::BodyNode::Properties node;
    node.mName = properties.mName;
 
    auto pair =
        skeleton_->createJointAndBodyNodePair<dart::dynamics::RevoluteJoint>(parent, properties, node);
    createShapeNode(pair.second, shape);
 
    pair.first->setPositionLimitEnforced(true);
    return pair;
}

addWeldedFrame()

std::pair< dart::dynamics::WeldJoint *, dart::dynamics::BodyNode * > Structure::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.

Parameters

[in]	properties	Joint properties to use for joint.
[in]	shape	Shape to attach to frame.
[in]	parent	Parent frame to attach new joint to.

Returns

The created joint and body node.

Definition at line 227 of file structure.cpp.

{
    dart::dynamics::BodyNode::Properties node;
    node.mName = properties.mName;
 
    auto pair = skeleton_->createJointAndBodyNodePair<dart::dynamics::WeldJoint>(parent, properties, node);
    createShapeNode(pair.second, shape);
 
    return pair;
}

cloneStructure()

StructurePtr Structure::cloneStructure

(

const std::string &

newName

)

const

Clones this structure with a new name.

Parameters

[in]

newName

Name for clone of the structure.

Returns

The cloned structure.

Definition at line 76 of file structure.cpp.

{
    auto structure = std::make_shared<Structure>(newName);
    structure->setSkeleton(skeleton_->cloneSkeleton(newName));
 
    for (const auto &pair : acm_->getDisabledPairsConst())
        structure->getACM()->disableCollision(pair.first, pair.second);
 
    return structure;
}

createShapeNode()

void Structure::createShapeNode ( dart::dynamics::BodyNode *  body, const dart::dynamics::ShapePtr &  shape  )

protected

Create a shape node on a body.

Parameters

[in,out]	body	Body to add shape node to.
[in]	shape	Shape to add to body.

Definition at line 150 of file structure.cpp.

{
    if (not shape)
        return;
 
    body->createShapeNodeWith<dart::dynamics::VisualAspect,     //
                              dart::dynamics::CollisionAspect,  //
                              dart::dynamics::DynamicsAspect>(shape);
 
    dart::dynamics::Inertia inertia;
    double mass = magic::DEFAULT_DENSITY * shape->getVolume();
    if (mass <= 0)
        mass = 1.;
 
    inertia.setMass(mass);
    inertia.setMoment(shape->computeInertia(mass));
 
    if (not inertia.verify(false))
        inertia = dart::dynamics::Inertia(mass);
 
    body->setInertia(inertia);
    body->setRestitutionCoeff(magic::DEFAULT_RESTITUTION);
 
    auto *joint = body->getParentJoint();
    if (joint)
        for (std::size_t i = 0; i < joint->getNumDofs(); ++i)
            joint->getDof(i)->setDampingCoefficient(magic::DEFAULT_DAMPING);
}

dumpGraphViz()

void Structure::dumpGraphViz	(	std::ostream &	out,
		bool	standalone = true
	)

Dumps the structure of the skeleton to a GraphViz file.

Parameters

[in]	out	Stream to output to.
[in]	standalone	If false, does not include graph header.

Definition at line 118 of file structure.cpp.

{
    if (standalone)
        out << "digraph {" << std::endl;
 
    const std::string &sname = skeleton_->getName();
    for (const auto &node : skeleton_->getBodyNodes())
    {
        const std::string &name = node->getName();
        const std::size_t index = node->getIndexInSkeleton();
 
        out << sname << "_" << index << "[label=\"" << name << "\"]" << std::endl;
 
        const auto &joint = node->getParentJoint();
        const std::string &jname = joint->getName();
        const std::string &type = joint->getType();
 
        const auto &parent = joint->getParentBodyNode();
        if (parent)
        {
            const auto &pindex = parent->getIndexInSkeleton();
            out << sname << "_" << pindex << "->"  //
                << sname << "_" << index           //
                << "[label=\"" << jname << std::endl
                << type << "\"]" << std::endl;
        }
    }
 
    if (standalone)
        out << "}" << std::endl;
}

getACM()

ACMPtr Structure::getACM

(

)

Get the ACM for the structure.

Returns

The ACM.

Definition at line 92 of file structure.cpp.

{
    return acm_;
}

getACMConst()

const ACMPtr & Structure::getACMConst

(

)

const

Get the ACM for the structure.

Returns

The ACM.

Definition at line 97 of file structure.cpp.

{
    return acm_;
}

getFrame()

dart::dynamics::BodyNode * Structure::getFrame

(

const std::string &

name = ""

)

const

Get a body node within the structure.

Parameters

[in]

name

Name of the node to retrieve.

Returns

The body node if it exists, nullptr otherwise. If name is empty, returns root.

Definition at line 292 of file structure.cpp.

{
    return skeleton_->getBodyNode(name);
}

getJoint()

dart::dynamics::Joint * Structure::getJoint

(

const std::string &

joint_name

)

const

Get a reference to the joint in the structure.

Parameters

[in]

joint_name

Name of the joint to retrieve.

Returns

The joint if it exists, nullptr otherwise.

Definition at line 287 of file structure.cpp.

{
    return skeleton_->getJoint(joint_name);
}

getJointNames()

std::vector< std::string > Structure::getJointNames

(

)

const

Get the joint names for this structure.

Returns

The names of all the joints in this structure.

Definition at line 278 of file structure.cpp.

{
    std::vector<std::string> names;
    for (const auto *joint : skeleton_->getJoints())
        names.emplace_back(joint->getName());
 
    return names;
}

getName()

const std::string & Structure::getName

(

)

const

Get the name of this structure.

Returns

Name of the structure.

Definition at line 87 of file structure.cpp.

{
    return name_;
}

getRootFrame()

dart::dynamics::BodyNode * Structure::getRootFrame

(

)

const

Get the root frame of this structure.

Returns

The root node.

Definition at line 343 of file structure.cpp.

{
    return skeleton_->getRootBodyNode();
}

getSkeleton()

dart::dynamics::SkeletonPtr & Structure::getSkeleton

(

)

Get the underlying skeleton for the structure.

Returns

The skeleton.

Definition at line 108 of file structure.cpp.

{
    return skeleton_;
}

getSkeletonConst()

const dart::dynamics::SkeletonPtr & Structure::getSkeletonConst

(

)

const

Get the underlying skeleton for the structure.

Returns

The skeleton.

Definition at line 113 of file structure.cpp.

{
    return skeleton_;
}

reparentFreeFrame()

void Structure::reparentFreeFrame	(	dart::dynamics::BodyNode *	child,
		const std::string &	parent = ""
	)

Reparents the child node to the parent node.

Parameters

[in]	child	Child to reparent.
[in]	parent	Name of new parent for the child frame.

Definition at line 297 of file structure.cpp.

{
    auto *frame = getFrame(parent);
 
    RobotPose tf;
    if (frame)
        tf = child->getTransform(frame);
    else
        tf = child->getWorldTransform();
 
    dart::dynamics::FreeJoint::Properties joint;
    joint.mName = child->getName();
    auto *jt = child->moveTo<dart::dynamics::FreeJoint>(skeleton_, frame, joint);
 
    setJointParentTransform(joint.mName, tf);
}

setDof()

void Structure::setDof	(	unsigned int	index,
		double	value
	)

Set the DoF at index to value.

Parameters

[in]	index	Index of DoF to set.
[in]	value	Value to set DoF.

Definition at line 273 of file structure.cpp.

{
    skeleton_->getDof(index)->setPosition(value);
}

setJoint() [1/2]

void Structure::setJoint	(	const std::string &	name,
		const Eigen::Ref< const Eigen::VectorXd > &	value
	)

Set the value of a n-DoF joint in the structure.

Parameters

[in]	name	Name of joint.
[in]	value	Value to set joint.

Definition at line 259 of file structure.cpp.

{
    auto *joint = skeleton_->getJoint(name);
    joint->setPositions(value);
}

setJoint() [2/2]

void Structure::setJoint	(	const std::string &	name,
		double	value
	)

Set the value of a 1-DoF joint in the structure.

Parameters

[in]	name	Name of joint.
[in]	value	Value to set joint.

Definition at line 254 of file structure.cpp.

{
    setJoint(name, Eigen::VectorXd::Constant(1, value));
}

setJointParentTransform()

void Structure::setJointParentTransform	(	const std::string &	name,
		const RobotPose &	tf
	)

Set the transform from a joint to its parent.

Parameters

[in]	name	Name of joint to set transform of.
[in]	tf	Transform to set.

Definition at line 314 of file structure.cpp.

{
    auto *joint = skeleton_->getJoint(name);
    if (joint == nullptr)
        RBX_ERROR("Cannot find joint named %s to set TF!", name);
 
    joint->setTransformFromParentBodyNode(tf);
}

setSkeleton()

void Structure::setSkeleton

(

const dart::dynamics::SkeletonPtr &

skeleton

)

Set the skeleton for the structure.

Parameters

[in]

skeleton

The new skeleton.

Definition at line 102 of file structure.cpp.

{
    skeleton_ = skeleton;
    skeleton_->setName(name_);
}

solveIK()

bool Structure::solveIK

(

)

Solve the current whole-body IK problem imposed on the structure.

Returns

Returns true on success false on failure.

Definition at line 265 of file structure.cpp.

{
    auto ik = skeleton_->getIK(true);
    // ik->getSolver()->setTolerance(1e-9);
    // ik->getSolver()->setNumMaxIterations(100);
    return ik->solveAndApply(true);
}

updateCollisionObject()

void Structure::updateCollisionObject	(	const std::string &	name,
		const GeometryPtr &	geometry,
		const robowflex::RobotPose &	pose
	)

Update or add a collision object.

Parameters

[in]	name	Name of object to add.
[in]	geometry	Geometry of object.
[in]	pose	Pose of object to set.

Definition at line 323 of file structure.cpp.

{
    auto nodes = skeleton_->getBodyNodes(name);  // Get all nodes with this name
    if (!nodes.empty())
        setJointParentTransform(name, pose);
    else
    {
        dart::dynamics::FreeJoint::Properties joint;
        joint.mName = name;
        joint.mT_ParentBodyToJoint = robowflex::TF::identity();
 
        auto shape = makeGeometry(geometry);
 
        auto pair = addFreeFrame(joint, shape);
        setJointParentTransform(name, pose);
        setColor(pair.second, dart::Color::Blue(0.2));
    }
}

Member Data Documentation

acm_

ACMPtr robowflex::darts::Structure::acm_

protected

ACM for structure.

Definition at line 262 of file structure.h.

name_

const std::string robowflex::darts::Structure::name_ {"robot"}

protected

Name of the structure.

Definition at line 260 of file structure.h.

skeleton_

dart::dynamics::SkeletonPtr robowflex::darts::Structure::skeleton_ {nullptr}

protected

Underlying skeleton.

Definition at line 261 of file structure.h.

---

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

• structure.h
• structure.cpp