30 #ifndef EXOTICA_CORE_KINEMATIC_ELEMENT_H_
31 #define EXOTICA_CORE_KINEMATIC_ELEMENT_H_
35 #include <kdl/frames.hpp>
36 #include <kdl/segment.hpp>
44 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 KDL::Frame
frame = KDL::Frame::Identity();
50 Eigen::Vector3d
scale = Eigen::Vector3d::Ones();
51 Eigen::Vector4d
color = Eigen::Vector4d(1.0, 1.0, 1.0, 1.0);
57 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
66 std::shared_ptr<KinematicElement> my_parent =
parent.lock();
69 my_parent->RemoveExpiredChildren();
75 std::shared_ptr<KinematicElement> element =
parent.lock();
77 while (element && element->id > 0)
79 if (element->is_robot_link)
84 element = element->parent.lock();
89 inline KDL::Frame
GetPose(
const double& x = 0.0)
103 for (
size_t i = 0; i <
children.size(); ++i)
117 std::vector<std::weak_ptr<KinematicElement>>
children;
120 KDL::Frame
frame = KDL::Frame::Identity();
128 Eigen::Vector3d
scale = Eigen::Vector3d::Ones();
130 Eigen::Vector4d
color = Eigen::Vector4d(0.5, 0.5, 0.5, 1.0);
136 std::stack<std::shared_ptr<KinematicElement>> elements;
137 for (
auto child :
children) elements.push(child.lock());
138 while (!elements.empty())
140 auto parent = elements.top();
143 for (
auto child :
parent->children) elements.push(child.lock());
149 #endif // EXOTICA_CORE_KINEMATIC_ELEMENT_H_