Debian Codegraph @

libs/kinematics/src/CKinematicChain.cpp

raw
1 /* +------------------------------------------------------------------------+
2 | Mobile Robot Programming Toolkit (MRPT) |
3 | https://www.mrpt.org/ |
4 | |
5 | Copyright (c) 2005-2026, Individual contributors, see AUTHORS file |
6 | See: https://www.mrpt.org/Authors - All rights reserved. |
7 | Released under BSD License. See: https://www.mrpt.org/License |
8 +------------------------------------------------------------------------+ */
9
10 #include "kinematics-precomp.h" // Precompiled headers
11 //
12 #include <mrpt/kinematics/CKinematicChain.h>
13 #include <mrpt/opengl/CCylinder.h>
14 #include <mrpt/opengl/CSetOfObjects.h>
15 #include <mrpt/opengl/stock_objects.h>
16 #include <mrpt/serialization/CArchive.h>
17 #include <mrpt/serialization/stl_serialization.h>
18
19 using namespace mrpt;
20 using namespace mrpt::math;
21 using namespace mrpt::poses;
22 using namespace mrpt::kinematics;
23 using namespace std;
24
25 IMPLEMENTS_SERIALIZABLE(CKinematicChain, CSerializable, mrpt::kinematics)
26
27 /** Appends a new link to the robotic arm, with the given Denavit-Hartenberg
28 * parameters (see TKinematicLink for further details) */
29 void CKinematicChain::addLink(double theta, double d, double a, double alpha, bool is_prismatic)
30 {
31 m_links.emplace_back(theta, d, a, alpha, is_prismatic);
32 }
33
34 /** Removes one link from the kinematic chain (0<=idx<N) */
35 void CKinematicChain::removeLink(size_t idx)
36 {
37 ASSERT_LT_(idx, m_links.size());
38 m_links.erase(m_links.begin() + idx);
39 }
40
41 const TKinematicLink& CKinematicChain::getLink(size_t idx) const
42 {
43 ASSERT_LT_(idx, m_links.size());
44 return m_links[idx];
45 }
46
47 TKinematicLink& CKinematicChain::getLinkRef(size_t idx)
48 {
49 ASSERT_LT_(idx, m_links.size());
50 return m_links[idx];
51 }
52
53 void CKinematicChain::setOriginPose(const mrpt::poses::CPose3D& new_pose) { m_origin = new_pose; }
54
55 const mrpt::poses::CPose3D& CKinematicChain::getOriginPose() const { return m_origin; }
56
57 uint8_t CKinematicChain::serializeGetVersion() const { return 1; }
58 void CKinematicChain::serializeTo(mrpt::serialization::CArchive& out) const
59 {
60 out << m_links << m_origin;
61 }
62
63 void CKinematicChain::serializeFrom(mrpt::serialization::CArchive& in, uint8_t version)
64 {
65 switch (version)
66 {
67 case 0:
68 case 1:
69 {
70 in >> m_links;
71 if (version >= 1)
72 {
73 in >> m_origin;
74 }
75 else
76 m_origin = mrpt::poses::CPose3D();
77 }
78 break;
79 default:
80 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
81 };
82 }
83
84 /** Go thru all the links of the chain and compute the global pose of each link.
85 * The "ground" link pose "pose0" defaults to the origin of coordinates,
86 * but anything else can be passed as the optional argument. */
87 void CKinematicChain::recomputeAllPoses(
88 std::vector<mrpt::poses::CPose3D>& poses,
89 [[maybe_unused]] const mrpt::poses::CPose3D& pose0) const
90 {
91 const size_t N = m_links.size();
92
93 poses.resize(N + 1);
94
95 CPose3D p = m_origin; // Cummulative pose
96
97 poses[0] = p;
98
99 for (size_t i = 0; i < N; i++)
100 {
101 // Build the 3D pose change of the i'th link:
102 const double th = m_links[i].theta;
103 const double alpha = m_links[i].alpha;
104 const double d = m_links[i].d;
105 const double a = m_links[i].a;
106
107 const double t_vals[3] = {a * cos(th), a * sin(th), d};
108 const double r_vals[3 * 3] = {
109 cos(th),
110 -sin(th) * cos(alpha),
111 sin(th) * sin(alpha),
112 sin(th),
113 cos(th) * cos(alpha),
114 -cos(th) * sin(alpha),
115 0,
116 sin(alpha),
117 cos(alpha)};
118
119 const CMatrixDouble33 R(r_vals);
120 const CVectorFixedDouble<3> t(t_vals);
121
122 CPose3D link(R, t);
123
124 p.composeFrom(p, link);
125
126 poses[i + 1] = p;
127 }
128 }
129
130 const float R = 0.01f;
131
132 void addBar_D(mrpt::opengl::CSetOfObjects::Ptr& objs, const double d)
133 {
134 mrpt::opengl::CCylinder::Ptr gl_cyl = mrpt::opengl::CCylinder::Create(R, R, d);
135 gl_cyl->setColor_u8(mrpt::img::TColor(0x00, 0x00, 0xff));
136 gl_cyl->setName("cyl.d");
137
138 objs->insert(gl_cyl);
139 }
140
141 void addBar_A(mrpt::opengl::CSetOfObjects::Ptr& objs, const double a)
142 {
143 mrpt::opengl::CCylinder::Ptr gl_cyl2 = mrpt::opengl::CCylinder::Create(R, R, -a);
144 gl_cyl2->setColor_u8(mrpt::img::TColor(0xff, 0x00, 0x00));
145 gl_cyl2->setPose(mrpt::poses::CPose3D(0, 0, 0, 0, 90.0_deg, 0));
146 gl_cyl2->setName("cyl.a");
147
148 objs->insert(gl_cyl2);
149 }
150
151 void CKinematicChain::getAs3DObject(
152 mrpt::opengl::CSetOfObjects::Ptr& obj, std::vector<mrpt::poses::CPose3D>* out_all_poses) const
153 {
154 ASSERT_(obj);
155 const size_t N = m_links.size();
156
157 // Recompute current poses:
158 std::vector<mrpt::poses::CPose3D> all_poses;
159 recomputeAllPoses(all_poses);
160
161 m_last_gl_objects.resize(N + 1);
162
163 // Ground [0] and Links [1-N]:
164 for (size_t i = 0; i <= N; i++)
165 {
166 mrpt::opengl::CSetOfObjects::Ptr gl_corner =
167 mrpt::opengl::stock_objects::CornerXYZSimple(0.1f, 3.0f);
168 gl_corner->setPose(all_poses[i]);
169
170 gl_corner->setName(mrpt::format("%u", static_cast<unsigned int>(i)));
171 gl_corner->enableShowName();
172
173 if (i < N) addBar_D(gl_corner, m_links[i].d);
174 if (i > 0) addBar_A(gl_corner, m_links[i - 1].a);
175
176 obj->insert(gl_corner);
177 m_last_gl_objects[i] = gl_corner;
178 }
179
180 if (out_all_poses) out_all_poses->swap(all_poses);
181 }
182
183 void CKinematicChain::update3DObject(std::vector<mrpt::poses::CPose3D>* out_all_poses) const
184 {
185 ASSERTMSG_(
186 (m_links.size() + 1) == m_last_gl_objects.size(),
187 "The kinematic chain has changed since the last call to "
188 "getAs3DObject()");
189
190 const size_t N = m_links.size();
191
192 // Recompute current poses:
193 std::vector<mrpt::poses::CPose3D> all_poses;
194 recomputeAllPoses(all_poses);
195
196 for (size_t i = 0; i <= N; i++)
197 {
198 mrpt::opengl::CSetOfObjects::Ptr gl_objs =
199 std::dynamic_pointer_cast<mrpt::opengl::CSetOfObjects>(m_last_gl_objects[i]);
200 gl_objs->setPose(all_poses[i]);
201
202 if (i < N)
203 {
204 mrpt::opengl::CCylinder::Ptr glCyl =
205 std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(gl_objs->getByName("cyl.d"));
206 const float d = mrpt::d2f(m_links[i].d);
207 glCyl->setHeight(d);
208 }
209
210 if (i > 0)
211 {
212 mrpt::opengl::CCylinder::Ptr glCyl2 =
213 std::dynamic_pointer_cast<mrpt::opengl::CCylinder>(gl_objs->getByName("cyl.a"));
214 const float a = mrpt::d2f(m_links[i - 1].a);
215 glCyl2->setHeight(-a);
216 }
217 }
218
219 if (out_all_poses) out_all_poses->swap(all_poses);
220 }
221
222 /** Erases all links and leave the robot arm empty. */
223 void CKinematicChain::clear()
224 {
225 m_links.clear();
226 m_last_gl_objects.clear();
227 }
228
229 mrpt::serialization::CArchive& mrpt::kinematics::operator>>(
230 mrpt::serialization::CArchive& in, TKinematicLink& o)
231 {
232 uint32_t version;
233 in >> version;
234 switch (version)
235 {
236 case 0:
237 in >> o.theta >> o.d >> o.a >> o.alpha >> o.is_prismatic;
238 break;
239 default:
240 MRPT_THROW_UNKNOWN_SERIALIZATION_VERSION(version);
241 }
242 return in;
243 }
244 mrpt::serialization::CArchive& mrpt::kinematics::operator<<(
245 mrpt::serialization::CArchive& out, const TKinematicLink& o)
246 {
247 const uint32_t version = 0;
248 out << version;
249 out << o.theta << o.d << o.a << o.alpha << o.is_prismatic;
250 return out;
251 }