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; }