30 double theta,
double d,
double a,
double alpha,
bool is_prismatic)
32 m_links.emplace_back(theta, d, a, alpha, is_prismatic);
36 void CKinematicChain::removeLink(
const size_t idx)
39 m_links.erase(m_links.begin() + idx);
64 uint8_t CKinematicChain::serializeGetVersion()
const {
return 1; }
67 out << m_links << m_origin;
70 void CKinematicChain::serializeFrom(
95 void CKinematicChain::recomputeAllPoses(
96 std::vector<mrpt::poses::CPose3D>& poses,
99 const size_t N = m_links.size();
107 for (
size_t i = 0; i < N; i++)
110 const double th = m_links[i].theta;
111 const double alpha = m_links[i].alpha;
112 const double d = m_links[i].d;
113 const double a = m_links[i].a;
115 const double t_vals[3] = {a * cos(th), a * sin(th), d};
116 const double r_vals[3 * 3] = {cos(th),
117 -sin(th) * cos(alpha),
118 sin(th) * sin(alpha),
120 cos(th) * cos(alpha),
121 -cos(th) * sin(alpha),
137 const float R = 0.01f;
144 gl_cyl->setName(
"cyl.d");
146 objs->insert(gl_cyl);
155 gl_cyl2->setName(
"cyl.a");
157 objs->insert(gl_cyl2);
160 void CKinematicChain::getAs3DObject(
162 std::vector<mrpt::poses::CPose3D>* out_all_poses)
const
165 const size_t N = m_links.size();
168 std::vector<mrpt::poses::CPose3D> all_poses;
169 recomputeAllPoses(all_poses);
171 m_last_gl_objects.resize(N + 1);
174 for (
size_t i = 0; i <= N; i++)
178 gl_corner->setPose(all_poses[i]);
180 gl_corner->setName(
mrpt::format(
"%u",
static_cast<unsigned int>(i)));
181 gl_corner->enableShowName();
183 if (i < N)
addBar_D(gl_corner, m_links[i].d);
184 if (i > 0)
addBar_A(gl_corner, m_links[i - 1].a);
186 obj->insert(gl_corner);
187 m_last_gl_objects[i] = gl_corner;
190 if (out_all_poses) out_all_poses->swap(all_poses);
193 void CKinematicChain::update3DObject(
194 std::vector<mrpt::poses::CPose3D>* out_all_poses)
const
197 (m_links.size() + 1) == m_last_gl_objects.size(),
198 "The kinematic chain has changed since the last call to "
201 const size_t N = m_links.size();
204 std::vector<mrpt::poses::CPose3D> all_poses;
205 recomputeAllPoses(all_poses);
207 for (
size_t i = 0; i <= N; i++)
210 std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(
211 m_last_gl_objects[i]);
212 gl_objs->setPose(all_poses[i]);
217 std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(
218 gl_objs->getByName(
"cyl.d"));
226 std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(
227 gl_objs->getByName(
"cyl.a"));
228 const float a =
mrpt::d2f(m_links[i - 1].a);
229 glCyl2->setHeight(-a);
233 if (out_all_poses) out_all_poses->swap(all_poses);
240 m_last_gl_objects.clear();
261 const uint32_t version = 0;