bool Robot::_createRootLink(const std::string& rootLinkName, Frame& referenceFrame)
{
// TODO: Make Robot a KinObject so that we do not need referenceFrame here
if(!verb.Assert(_links.size()==0, verbosity::ASSERT_CASUAL,
"Cannot create a root link, because one already exists!"))
return false;
_pseudo_link = new Link(this, referenceFrame, "pseudo_link", INVALID_INDEX, false);
_pseudo_link->_isDummy = true;
_pseudo_link->_setParentJoint(_dummyJoint);
Link* rootLink = new Link(this, *_pseudo_link, rootLinkName, 0, true);
_insertLink(rootLink);
_root = rootLink;
_anchor = rootLink;
_pseudo_joint = new Joint(this, BASE_INDEX, _pseudo_link, rootLink,
ProtectedJointProperties("base", Joint::FLOATING),
std::vector<DofProperties>());
_jointNameToIndex[_pseudo_joint->name()] = BASE_INDEX;
_pseudo_link->_addChildJoint(_pseudo_joint);
rootLink->_setParentJoint(_pseudo_joint);
dof(DOF_POS_X).name("DOF_POS_X");
dof(DOF_POS_Y).name("DOF_POS_Y");
dof(DOF_POS_Z).name("DOF_POS_Z");
dof(DOF_ROT_X).name("DOF_ROT_X");
dof(DOF_ROT_Y).name("DOF_ROT_Y");
dof(DOF_ROT_Z).name("DOF_ROT_Z");
return true;
}
int Robot::createJointLinkPair(Link& parentLink, const string& newLinkName,
const ProtectedJointProperties& joint_properties,
const std::vector<DofProperties>& dof_properties)
{
if(parentLink.isDummy())
{
verb.Assert(false, verbosity::ASSERT_CRITICAL,
"Error: You wanted to make '"+newLinkName+
"' a child of a dummy link in robot '"+name()+"'!");
return -1;
}
if(!parentLink.belongsTo(*this))
{
verb.Assert(false, verbosity::ASSERT_CRITICAL,
"Error: You wanted to make '"+newLinkName+"' a child of '"+parentLink.name()+
"', but '"+parentLink.name()+"' does not belong to the robot '"+name()+"'!");
return -2;
}
if(checkForLinkName(newLinkName))
{
verb.Assert(false, verbosity::ASSERT_CRITICAL,
"Error: You wanted to create a new link named '"+newLinkName+
"' but a link ("+to_string(link(newLinkName).id())+
") by that name already exists in the robot named '"+name()+"'!");
return -3;
}
if(checkForJointName(joint_properties._name))
{
verb.Assert(false, verbosity::ASSERT_CRITICAL,
"Error: You wanted to create a new joint named '"+joint_properties._name+
"' but a joint (#"+to_string(joint(joint_properties._name).id())
+") by that name already exists in the robot named '"+name()+"'!");
return -4;
}
Link* newLink = new Link(this, parentLink, newLinkName, _links.size(), false);
_insertLink(newLink);
Joint* newJoint = new Joint(this, _joints.size(), &parentLink, newLink,
joint_properties, dof_properties);
_insertJoint(newJoint);
parentLink._addChildJoint(newJoint);
newLink->_setParentJoint(newJoint);
return _links.size()-1;
}
