Branch data Line data Source code
1 : : // Copyright (c) 2017 Jordan Lack <jlack1987@gmail.com>
2 : : // Copyright (c) 2011-2016 Martin Felis <martin.felis@iwr.uni-heidelberg.de>
3 : : // RDL - Robot Dynamics Library
4 : : // Licensed under the zlib license. See LICENSE for more details.
5 : :
6 : : #ifndef RDL_MODEL_H
7 : : #define RDL_MODEL_H
8 : :
9 : : /**
10 : : * @file Model.hpp
11 : : */
12 : :
13 : : #include <assert.h>
14 : : #include <boost/asio/io_service.hpp>
15 : : #include <boost/function.hpp>
16 : : #include <boost/thread/thread.hpp>
17 : : #include <cstring>
18 : : #include <iostream>
19 : : #include <limits>
20 : : #include <list>
21 : : #include <map>
22 : : #include <rclcpp/rclcpp.hpp>
23 : : #include <type_traits>
24 : :
25 : : #include <rdl_dynamics/Body.hpp>
26 : : #include <rdl_dynamics/Joint.hpp>
27 : : #include <rdl_dynamics/Momentum.hpp>
28 : : #include <rdl_dynamics/RdlExceptions.hpp>
29 : : #include <rdl_dynamics/ReferenceFrame.hpp>
30 : : #include <rdl_dynamics/SpatialAcceleration.hpp>
31 : : #include <rdl_dynamics/SpatialForce.hpp>
32 : : #include <rdl_dynamics/SpatialMotion.hpp>
33 : : #include <rdl_dynamics/SpatialRigidBodyInertia.hpp>
34 : :
35 : : // std::vectors containing any objects that have Eigen matrices or vectors
36 : : // as members need to have a special allocater. This can be achieved with
37 : : // the following macro.
38 : :
39 : : /** @brief Namespace for all structures of the RobotDynamics library
40 : : */
41 : : namespace RobotDynamics
42 : : {
43 : : /** \page modeling_page Model
44 : : *
45 : : * \section model_structure Model Structure
46 : : *
47 : : * RDL stores the model internally in the \link RobotDynamics::Model
48 : : * Model Structure\endlink. For each \link RobotDynamics::Body Body
49 : : * \endlink it contains spatial velocities, accelerations and other
50 : : * variables that describe the state of the rigid body system. Furthermore
51 : : * it contains variables that are used as temporary variables in the
52 : : * algorithms.
53 : : *
54 : : * The construction of \link RobotDynamics::Model Model Structures
55 : : * \endlink makes use of carefully designed constructors of the classes
56 : : * \link RobotDynamics::Body Body \endlink and \link
57 : : * RobotDynamics::Joint Joint \endlink to ease the process of
58 : : * creating articulated models.
59 : : *
60 : : * \link RobotDynamics::Body Bodies \endlink are created by calling one
61 : : * of its constructors. Usually they are created by specifying the mass,
62 : : * center of mass and the inertia at the center of mass.
63 : : * \link RobotDynamics::Joint Joints \endlink are similarly created and is
64 : : * described in detail in \ref joint_description.
65 : : *
66 : : * Adding bodies to the model is done by specifying the
67 : : * parent body by its id, the transformation from the parent origin to the
68 : : * joint origin, the joint specification as an object, and the body itself.
69 : : * These parameters are then fed to the function
70 : : * RobotDynamics::Model::addBody() or
71 : : * RobotDynamics::Model::appendBody().
72 : : *
73 : : * To create a model with a floating base (a.k.a a model with a free-flyer
74 : : * joint) it is recommended to use \link joint_floatingbase \endlink.
75 : : *
76 : : * Once this is done, the model structure can be used with the functions of \ref
77 : : * kinematics_group, \ref dynamics_group, \ref contacts_page, and \ref utils_page to perform
78 : : * computations.
79 : : *
80 : : * After creating a model, access the body frames for moving bodies in the
81 : : * RobotDynamics::Model:bodyFrames vector and for fixed bodies in
82 : : * RobotDynamics::Model::fixedBodyFrames.
83 : : *
84 : : * \section modeling_urdf Using URDF
85 : : *
86 : : * For this see the documentation see \link
87 : : * RobotDynamics::Urdf::urdfReadFromFile \endlink.
88 : : */
89 : :
90 : : /** @brief Contains all information about the rigid body model
91 : : *
92 : : * This class contains all information required to perform the forward
93 : : * dynamics calculation. The variables in this class are also used for
94 : : * storage of temporary values. It is designed for use of the Articulated
95 : : * Rigid Body Algorithm (which is implemented in ForwardDynamics()) and
96 : : * follows the numbering as described in Featherstones book.
97 : : *
98 : : * Please note that body 0 is the root body and the moving bodies start at
99 : : * index 1. This numbering scheme is very beneficial in terms of
100 : : * readability of the code as the resulting code is very similar to the
101 : : * pseudo-code in the RBDA book. The generalized variables q, qdot, qddot
102 : : * and tau however start at 0 such that the first entry (e.g. q[0]) always
103 : : * specifies the value for the first moving body.
104 : : *
105 : : *
106 : : * @todo - Need to implement proper copy constructor, so I can use raw pointers and can clean them up properly
107 : : *
108 : : *
109 : : * \note To query the number of degrees of freedom use Model::dof_count.
110 : : */
111 : : struct Model
112 : : {
113 : : using SharedPtr = std::shared_ptr<Model>;
114 : : /**
115 : : * @brief Constructor
116 : : * @param n_threads The number of threads that the model can create for performing parallel tasks such as parallel kinematics. This
117 : : * defaults to nullptr in which case the number of threads created will be equal to std::thread::hardware_concurrency()
118 : : */
119 : : explicit Model(const rclcpp::Node::SharedPtr& n = nullptr, unsigned int* n_threads = nullptr);
120 : :
121 : 620 : ~Model()
122 : : {
123 : 620 : work.reset();
124 : 620 : thread_pool.join_all();
125 : 620 : }
126 : :
127 : : Model(const Model&) = delete;
128 : : void operator=(const Model&) = delete;
129 : :
130 : : /**
131 : : * @brief Call this after the model is fully populated to declare ros parameters
132 : : * that define the mass parameters for each body. This allows them to be modified at
133 : : * runtime
134 : : *
135 : : * @note If the node provided to the model at runtime is nullptr this function immediately returns
136 : : */
137 : : void setupRosParameters();
138 : :
139 : : // Structural information
140 : :
141 : : ReferenceFramePtr worldFrame; /**< Pointer to world frame */
142 : :
143 : : /* clang-format off */
144 : :
145 : : std::vector<ReferenceFramePtr> bodyFrames; /**< Reference frames for each body. Frame names are
146 : : the same as the body name */
147 : : // clang for some reason continuously reformats the comment on the above line no matter
148 : : // how many times you run it.
149 : : /* clang-format on */
150 : :
151 : : /**< Body centered frames are aligned with the body frame, but located at a body's center of mass */
152 : : std::vector<ReferenceFramePtr> bodyCenteredFrames;
153 : :
154 : : std::vector<ReferenceFramePtr> bodyCenterOfBuoyancyFrames; /**< A vector of body frames that are located
155 : : at the body's center of buoyancy and aligned
156 : : with the body frame */
157 : :
158 : : std::vector<ReferenceFramePtr> fixedBodyFrames;
159 : :
160 : : std::vector<unsigned int> lambda; /**< The id of the parents body */
161 : : std::vector<std::vector<unsigned int>> lambda_chain; /**< Each body's chain of parents */
162 : :
163 : : std::vector<unsigned int> lambda_q; /**< The index of the parent degree of freedom that is directly influencing
164 : : the current one */
165 : :
166 : : std::vector<std::vector<unsigned int>> mu; /**< Contains the ids of all the children of a given body */
167 : :
168 : : /** @brief number of degrees of freedoms of the model
169 : : *
170 : : * This value contains the number of entries in the generalized state (q)
171 : : * velocity (qdot), acceleration (qddot), and force (tau) vector.
172 : : */
173 : : unsigned int dof_count = 0;
174 : :
175 : : /** @brief The size of the \f$\mathbf{q}\f$-vector.
176 : : * For models without spherical joints the value is the same as
177 : : * Model::dof_count, otherwise additional values for the w-component of the
178 : : * Quaternion is stored at the end of \f$\mathbf{q}\f$.
179 : : *
180 : : * \sa \ref joint_description for more details.
181 : : */
182 : : unsigned int q_size = 0;
183 : :
184 : : /** @brief The size of the
185 : : *
186 : : * (\f$\mathbf{\dot{q}}, \mathbf{\ddot{q}}\f$,
187 : : * and \f$\mathbf{\tau}\f$-vector.
188 : : *
189 : : * \sa \ref joint_description for more details.
190 : : */
191 : : unsigned int qdot_size = 0;
192 : :
193 : : /**< total robot mass(kg) */
194 : : double mass = 0.;
195 : : /**< total robot volume(m^3)*/
196 : : double volume = 0.;
197 : :
198 : : /// @brief Id of the previously added body, required for Model::appendBody()
199 : : unsigned int previously_added_body_id = 0;
200 : :
201 : : /// @brief the cartesian vector of the gravity
202 : : Math::MotionVector gravity;
203 : : double fluidDensity = 1000.; /**< The density(\f$ \frac{kg}{m^3} \f$) of the surrounding fluid */
204 : : Math::MotionVector a_f; /**< The acceleration, in base coords, of the fluid the model is immersed in */
205 : : Math::MotionVector v_f; /**< The velocity, in base coords, of the fluid the model is immersed in */
206 : :
207 : : // State information
208 : : /// @brief The spatial velocity of the bodies
209 : : Math::SpatialAccelerationV a; /**< The spatial acceleration of the bodies expressed in body frame */
210 : :
211 : : Math::SpatialMotionV v; /**< The spatial velocity of the bodies expressed in body frame */
212 : :
213 : : ////////////////////////////////////
214 : : // Joints
215 : :
216 : : /// @brief All joints
217 : :
218 : : std::vector<Joint> mJoints;
219 : :
220 : : Math::MotionVectorV S; /**< Motion subspace for 1 dof joints */
221 : : Math::MotionVectorV S_o; /**< The ring derivative of S, a.k.a the rate of growth */
222 : :
223 : : // Joint state variables
224 : : std::vector<Math::SpatialTransform> X_J;
225 : :
226 : : std::vector<Math::SpatialVector> c_J; /**< Apparent derivative of v_J in a coordinate moving with the successor body */
227 : :
228 : : Math::SpatialMotionV v_J; /**< Joints velocity w.r.t its parent joint, expressed in body frame */
229 : :
230 : : std::vector<unsigned int> mJointUpdateOrder;
231 : :
232 : : /// @brief Transformations from the parent body to the frame of the joint.
233 : : // It is expressed in the coordinate frame of the parent.
234 : : std::vector<Math::SpatialTransform> X_T;
235 : :
236 : : /// @brief The number of fixed joints that have been declared before
237 : : /// each joint.
238 : : std::vector<unsigned int> mFixedJointCount;
239 : :
240 : : ////////////////////////////////////
241 : : // Special variables for joints with 3 degrees of freedom
242 : : /// @brief Motion subspace for joints with 3 degrees of freedom
243 : :
244 : : std::vector<Math::Matrix63> multdof3_S;
245 : : std::vector<Math::Matrix63> multdof3_S_o;
246 : : std::vector<Math::Matrix63> multdof3_U;
247 : : std::vector<Math::Matrix3d> multdof3_Dinv;
248 : : std::vector<Math::Vector3d> multdof3_u;
249 : : std::vector<unsigned int> multdof3_w_index;
250 : :
251 : : std::vector<CustomJoint*> mCustomJoints;
252 : :
253 : : ////////////////////////////////////
254 : : // Dynamics variables
255 : :
256 : : /// @brief The velocity dependent spatial acceleration
257 : : std::vector<Math::SpatialVector> c;
258 : :
259 : : /// @brief The spatial inertia of the bodies
260 : : std::vector<Math::SpatialMatrix> IA;
261 : :
262 : : /// @brief The spatial bias force
263 : : Math::SpatialForceV pA;
264 : :
265 : : /// @brief Temporary variable U_i (RBDA p. 130)
266 : : std::vector<Math::SpatialVector> U;
267 : :
268 : : /// @brief Temporary variable D_i (RBDA p. 130)
269 : : Math::VectorNd d;
270 : :
271 : : /// @brief Temporary variable u (RBDA p. 130)
272 : : Math::VectorNd u;
273 : :
274 : : /// @brief Internal forces on the body (used only InverseDynamics())
275 : : Math::SpatialForceV f;
276 : : Math::SpatialForceV f_b;
277 : :
278 : : Math::SpatialInertiaV I; /**< Body inertia expressed in body frame */
279 : : Math::RigidBodyInertiaV Ic; /**< A body inertia. See RobotDynamics::compositeRigidBodyAlgorithm for the uses
280 : : of this */
281 : : std::vector<Math::SpatialMatrix> I_add; /**< Added inertia matrix for each body */
282 : : std::vector<Math::SpatialMatrix> I_H; /**< Combined body and added mass inertia */
283 : : std::vector<Math::SpatialMatrix> Ic_H; /**< Used only in the hydrodynamics implementation of compositeRigidBodyAlgorithm */
284 : :
285 : : Math::SpatialInertiaV Ib_c; /**< Ib_c is the inertia of (non-fixed) bodies expressed in the body's center of
286 : : mass frame. */
287 : : std::vector<Math::Momentum> hc;
288 : :
289 : : /**
290 : : * @note It's CRITICAL that the elements of the zero matrices
291 : : * never be modified! It will break absolutely everything.
292 : : */
293 : : Math::MatrixNd ndof0_mat; /**< ndof x ndof matrix of zeros */
294 : : Math::MatrixNd three_x_qd0; /**< 3 x qdot_size matrix of zeros */
295 : : Math::VectorNd q0_vec; /**< q_size x 1 vector of zeros */
296 : : Math::VectorNd ndof0_vec; /**< ndof x 1 vector of zeros */
297 : : Math::VectorNd nbodies0_vec; /**< mBodies.size() x 1 vector of zeros */
298 : :
299 : : ////////////////////////////////////
300 : : // Bodies
301 : :
302 : : /** @brief Transformation from the parent body to the current body
303 : : * \f[
304 : : * X_{\lambda(i)} = {}^{i} X_{\lambda(i)}
305 : : * \f]
306 : : */
307 : : std::vector<Math::SpatialTransform> X_lambda;
308 : :
309 : : /// @brief All bodies that are attached to a body via a fixed joint.
310 : : std::vector<FixedBody> mFixedBodies;
311 : :
312 : : /** @brief Value that is used to discriminate between fixed and movable
313 : : * bodies.
314 : : *
315 : : * Bodies with id 1 .. (fixed_body_discriminator - 1) are moving bodies
316 : : * while bodies with id fixed_body_discriminator .. max (unsigned int)
317 : : * are fixed to a moving body. The value of max(unsigned int) is
318 : : * determined via std::numeric_limits<unsigned int>::max() and the
319 : : * default value of fixed_body_discriminator is max (unsigned int) / 2.
320 : : *
321 : : * On normal systems max (unsigned int) is 4294967294 which means there
322 : : * could be a total of 2147483646 movable and / or fixed bodies.
323 : : */
324 : : unsigned int fixed_body_discriminator = std::numeric_limits<unsigned int>::max() / 2;
325 : :
326 : : /** @brief All bodies 0 ... N_B, including the base
327 : : *
328 : : * mBodies[0] - base body <br>
329 : : * mBodies[1] - 1st moveable body <br>
330 : : * ... <br>
331 : : * mBodies[N_B] - N_Bth moveable body <br>
332 : : */
333 : : std::vector<Body> mBodies;
334 : :
335 : : /// @brief Human readable names for the bodies
336 : : std::map<std::string, unsigned int> mBodyNameMap;
337 : :
338 : : /**< Map containaing name -> pointer to reference frame for each frame on the robot */
339 : : std::map<std::string, ReferenceFramePtr> referenceFrameMap;
340 : : unsigned int num_branch_ends = 0;
341 : :
342 : : /**< Reference frame who's origin is located at the robots center of mass, aligned with world frame */
343 : : ReferenceFramePtr comFrame;
344 : :
345 : : /**< Reference frame who's origin is located at the robots center of buoyancy */
346 : : ReferenceFramePtr cobFrame;
347 : :
348 : : boost::asio::io_service io_service;
349 : : std::unique_ptr<boost::asio::io_service::work> work;
350 : : std::vector<boost::thread*> threads;
351 : : boost::thread_group thread_pool;
352 : :
353 : : rclcpp::Node::SharedPtr node;
354 : : rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr callback_handle;
355 : :
356 : : rcl_interfaces::msg::SetParametersResult paramCb(const std::vector<rclcpp::Parameter>& parameters);
357 : :
358 : : bool setThreadParameters(int policy, struct sched_param param)
359 : : {
360 : : if (threads.size() > 0)
361 : : {
362 : : for (unsigned int i = 0; i < threads.size(); ++i)
363 : : {
364 : : int retcode;
365 : : if ((retcode = pthread_setschedparam(threads[i]->native_handle(), policy, ¶m)) != 0)
366 : : {
367 : : if (retcode == EPERM)
368 : : {
369 : : std::cerr << "Model.setThreadParameters: Failed setting thread parameters. Invalid permissions" << std::endl;
370 : : }
371 : : else if (retcode == ESRCH)
372 : : {
373 : : std::cerr << "Model.setThreadParameters: Failed setting thread parameters. No thread with the ID could be found" << std::endl;
374 : : }
375 : : else if (retcode == EINVAL)
376 : : {
377 : : std::cerr << "Model.setThreadParameters: Failed setting thread parameters. policy is not a recognized policy, or param does not make sense for "
378 : : "the policy."
379 : : << std::endl;
380 : : }
381 : : else
382 : : {
383 : : std::cerr << "Model.setThreadParameters: Failed setting thread parameters. pthread_setschedparam returned with unknown error code " << retcode
384 : : << std::endl;
385 : : }
386 : :
387 : : return false;
388 : : }
389 : : }
390 : : }
391 : :
392 : : return true;
393 : : }
394 : :
395 : : /** @brief Connects a given body to the model
396 : : *
397 : : * When adding a body there are basically informations required:
398 : : * - what kind of body will be added?
399 : : * - where is the new body to be added?
400 : : * - by what kind of joint should the body be added?
401 : : *
402 : : * The first information "what kind of body will be added" is contained
403 : : * in the Body class that is given as a parameter.
404 : : *
405 : : * The question "where is the new body to be added?" is split up in two
406 : : * parts: first the parent (or successor) body to which it is added and
407 : : * second the transformation to the origin of the joint that connects the
408 : : * two bodies. With these two informations one specifies the relative
409 : : * positions of the bodies when the joint is in neutral position.gk
410 : : *
411 : : * The last question "by what kind of joint should the body be added?" is
412 : : * again simply contained in the Joint class.
413 : : *
414 : : * \param parent_id id of the parent body
415 : : * \param joint_frame the transformation from the parent frame to the origin
416 : : * of the joint frame (represents X_T in RBDA)
417 : : * \param joint specification for the joint that describes the
418 : : * connection
419 : : * \param body specification of the body itself
420 : : *
421 : : * \param body_name human readable name for the body (can be used to
422 : : * retrieve its id with GetBodyId())
423 : : *
424 : : * \returns id of the added body
425 : : */
426 : : unsigned int addBody(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body, std::string body_name = "");
427 : :
428 : : unsigned int addBodySphericalJoint(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body,
429 : : std::string body_name = "");
430 : :
431 : : /** @brief Adds a Body to the model such that the previously added Body
432 : : * is the Parent.
433 : : *
434 : : * This function is basically the same as Model::addBody() however the
435 : : * most recently added body (or body 0) is taken as parent.
436 : : */
437 : : unsigned int appendBody(const Math::SpatialTransform& joint_frame, const Joint& joint, const Body& body, std::string body_name = "");
438 : :
439 : : unsigned int addBodyCustomJoint(const unsigned int parent_id, const Math::SpatialTransform& joint_frame, CustomJoint* custom_joint, const Body& body,
440 : : std::string body_name = "");
441 : :
442 : : /** @brief Returns the id of a body that was passed to addBody()
443 : : *
444 : : * Bodies can be given a human readable name. This function allows to
445 : : * resolve its name to the numeric id.
446 : : *
447 : : * \note Instead of querying this function repeatedly, it might be
448 : : * advisable to query it once and reuse the returned id.
449 : : *
450 : : * \returns the id of the body or @code{.cpp} std::numeric_limits<unsigned int>::max() @endcode if the id was not found.
451 : : */
452 : 540 : unsigned int GetBodyId(const char* body_name) const
453 : : {
454 : 540 : if (mBodyNameMap.count(body_name) == 0)
455 : : {
456 : 1 : return std::numeric_limits<unsigned int>::max();
457 : : }
458 : :
459 : 539 : return mBodyNameMap.find(body_name)->second;
460 : : }
461 : :
462 : : /**
463 : : * @brief Get a fixed or moveable body's reference frame.
464 : : * @return Pointer to the frame, or nullptr if it doesn't exist
465 : : *
466 : : */
467 : 64 : ReferenceFramePtr getReferenceFrame(const std::string& frameName) const
468 : : {
469 : 64 : ReferenceFramePtr frame;
470 : : try
471 : : {
472 : 64 : frame = referenceFrameMap.at(frameName);
473 : : }
474 : 1 : catch (const std::out_of_range& e)
475 : : {
476 : 1 : frame = nullptr;
477 : 1 : }
478 : :
479 : 64 : return frame;
480 : 0 : }
481 : :
482 : : /** @brief Returns the name of a body for a given body id */
483 : 4 : std::string GetBodyName(unsigned int body_id) const
484 : : {
485 : 4 : std::map<std::string, unsigned int>::const_iterator iter = mBodyNameMap.begin();
486 : :
487 : 12 : while (iter != mBodyNameMap.end())
488 : : {
489 : 11 : if (iter->second == body_id)
490 : : {
491 : 3 : return iter->first;
492 : : }
493 : :
494 : 8 : ++iter;
495 : : }
496 : :
497 : 1 : return "";
498 : : }
499 : :
500 : : /** @brief Checks whether the body is rigidly attached to another body.
501 : : */
502 : 10791 : bool IsFixedBodyId(unsigned int body_id) const
503 : : {
504 : 10791 : if ((body_id >= fixed_body_discriminator) && (body_id < std::numeric_limits<unsigned int>::max()) && (body_id - fixed_body_discriminator < mFixedBodies.size()))
505 : : {
506 : 685 : return true;
507 : : }
508 : 10106 : return false;
509 : : }
510 : :
511 : 120 : bool IsBodyId(unsigned int id) const
512 : : {
513 : 120 : if ((id > 0) && (id < mBodies.size()))
514 : : {
515 : 101 : return true;
516 : : }
517 : :
518 : 19 : if ((id >= fixed_body_discriminator) && (id < std::numeric_limits<unsigned int>::max()))
519 : : {
520 : 18 : if (id - fixed_body_discriminator < mFixedBodies.size())
521 : : {
522 : 17 : return true;
523 : : }
524 : : }
525 : 2 : return false;
526 : : }
527 : :
528 : : /** @brief Determines id the actual parent body.
529 : : *
530 : : * @note When adding bodies using joints with multiple degrees of
531 : : * freedom, additional virtual bodies are added for each degree of
532 : : * freedom. This function returns the id of the actual
533 : : * non-virtual parent body.
534 : : */
535 : 4 : unsigned int GetParentBodyId(unsigned int id)
536 : : {
537 : 4 : if (id >= fixed_body_discriminator)
538 : : {
539 : 1 : return mFixedBodies[id - fixed_body_discriminator].mMovableParent;
540 : : }
541 : :
542 : 3 : unsigned int parent_id = lambda[id];
543 : :
544 : 5 : while (mBodies[parent_id].mIsVirtual)
545 : : {
546 : 2 : parent_id = lambda[parent_id];
547 : : }
548 : :
549 : 3 : return parent_id;
550 : : }
551 : :
552 : : /**
553 : : * @brief Returns the joint frame transformtion, i.e. the second argument to
554 : : Model::addBody().
555 : : */
556 : 7 : Math::SpatialTransform GetJointFrame(unsigned int id)
557 : : {
558 : 7 : if (id >= fixed_body_discriminator)
559 : : {
560 : 1 : return mFixedBodies[id - fixed_body_discriminator].mParentTransform;
561 : : }
562 : :
563 : 6 : unsigned int child_id = id;
564 : 6 : unsigned int parent_id = lambda[id];
565 : :
566 : 6 : if (mBodies[parent_id].mIsVirtual)
567 : : {
568 : 6 : while (mBodies[parent_id].mIsVirtual)
569 : : {
570 : 4 : child_id = parent_id;
571 : 4 : parent_id = lambda[child_id];
572 : : }
573 : 2 : return X_T[child_id];
574 : : }
575 : : else
576 : : {
577 : 4 : return X_T[id];
578 : : }
579 : : }
580 : :
581 : : /**
582 : : * @brief Sets the joint frame transformtion, i.e. the second argument to Model::addBody().
583 : : */
584 : 4 : void SetJointFrame(unsigned int id, const Math::SpatialTransform& transform)
585 : : {
586 : 4 : if (id >= fixed_body_discriminator)
587 : : {
588 : 1 : throw RdlException("Error: setting of parent transform not supported for fixed bodies!");
589 : : }
590 : :
591 : 3 : unsigned int child_id = id;
592 : 3 : unsigned int parent_id = lambda[id];
593 : :
594 : 3 : if (mBodies[parent_id].mIsVirtual)
595 : : {
596 : 3 : while (mBodies[parent_id].mIsVirtual)
597 : : {
598 : 2 : child_id = parent_id;
599 : 2 : parent_id = lambda[child_id];
600 : : }
601 : 1 : X_T[child_id] = transform;
602 : : }
603 : 2 : else if (id > 0)
604 : : {
605 : 1 : X_T[id] = transform;
606 : : }
607 : 3 : }
608 : :
609 : : /**
610 : : * @brief Gets the quaternion for body i (only valid if body i is connected by
611 : : * a JointTypeSpherical joint)
612 : : *
613 : : * See \ref joint_singularities for details.
614 : : */
615 : 66 : Math::Quaternion GetQuaternion(unsigned int i, const Math::VectorNd& Q) const
616 : : {
617 : 66 : assert(mJoints[i].mJointType == JointTypeSpherical);
618 : 66 : unsigned int q_index = mJoints[i].q_index;
619 : 66 : return Math::Quaternion(Q[q_index], Q[q_index + 1], Q[q_index + 2], Q[multdof3_w_index[i]]);
620 : : }
621 : :
622 : : /**
623 : : * @brief Sets the quaternion for body i (only valid if body i is connected by
624 : : * a JointTypeSpherical joint)
625 : : *
626 : : * See \ref joint_singularities for details.
627 : : */
628 : 17 : void SetQuaternion(unsigned int i, const Math::Quaternion& quat, Math::VectorNd& Q) const
629 : : {
630 : 17 : assert(mJoints[i].mJointType == JointTypeSpherical);
631 : 17 : unsigned int q_index = mJoints[i].q_index;
632 : :
633 : 17 : Q[q_index] = quat[0];
634 : 17 : Q[q_index + 1] = quat[1];
635 : 17 : Q[q_index + 2] = quat[2];
636 : 17 : Q[multdof3_w_index[i]] = quat[3];
637 : 17 : }
638 : :
639 : 121 : unsigned int getCommonMovableParentId(unsigned int id_1, unsigned int id_2) const
640 : : {
641 : 121 : if (IsFixedBodyId(id_1))
642 : : {
643 : 6 : id_1 = mFixedBodies[id_1 - fixed_body_discriminator].mMovableParent;
644 : : }
645 : :
646 : 121 : if (IsFixedBodyId(id_2))
647 : : {
648 : 5 : id_2 = mFixedBodies[id_2 - fixed_body_discriminator].mMovableParent;
649 : : }
650 : :
651 : 121 : if (id_1 == id_2)
652 : : {
653 : 9 : return id_1;
654 : : }
655 : :
656 : 112 : if (id_1 == 0 || id_2 == 0)
657 : : {
658 : 1 : return 0;
659 : : }
660 : :
661 : 111 : unsigned int chain_1_size = lambda_chain[id_1].size();
662 : 111 : unsigned int chain_2_size = lambda_chain[id_2].size();
663 : :
664 : 111 : if (chain_1_size <= chain_2_size)
665 : : {
666 : : // Can start at 1 bc every list should start with world body id which is zero
667 : 239 : for (unsigned int i = 1; i < chain_1_size; i++)
668 : : {
669 : 191 : if (lambda_chain[id_1][i] != lambda_chain[id_2][i])
670 : : {
671 : 16 : return lambda_chain[id_1][i - 1];
672 : : }
673 : : }
674 : :
675 : 48 : return lambda_chain[id_1][chain_1_size - 1];
676 : : }
677 : : else
678 : : {
679 : : // Can start at 1 bc every list should start with world body id which is zero
680 : 171 : for (unsigned int i = 1; i < chain_2_size; i++)
681 : : {
682 : 130 : if (lambda_chain[id_1][i] != lambda_chain[id_2][i])
683 : : {
684 : 6 : return lambda_chain[id_2][i - 1];
685 : : }
686 : : }
687 : :
688 : 41 : return lambda_chain[id_2][chain_2_size - 1];
689 : : }
690 : : }
691 : :
692 : : void crawlChainKinematics(unsigned int b_id, std::atomic<unsigned int>* branch_ends, const Math::VectorNd* Q, const Math::VectorNd* QDot,
693 : : const Math::VectorNd* QDDot);
694 : :
695 : : private:
696 : 5347 : bool validateRigidBodyInertia(const Body& body)
697 : : {
698 : 5347 : if (body.mIsVirtual)
699 : : {
700 : : // don't care about virtual bodies really
701 : 2044 : return true;
702 : : }
703 : :
704 : 3303 : RobotDynamics::Math::Matrix3d rbi = body.mInertia;
705 : 3303 : if (rbi(0, 0) <= 0 || rbi(1, 1) <= 0 || rbi(2, 2) <= 0)
706 : : {
707 : 2 : std::cerr << "\033[1;31m Invalid inertia: Each element of the trace must be > 0 \033[0m" << std::endl;
708 : 2 : return false;
709 : : }
710 : :
711 : 3301 : if (rbi(0, 0) >= (rbi(1, 1) + rbi(2, 2)))
712 : : {
713 : 0 : std::cerr << "\033[1;31m Invalid inertia: Triangle inequality not satisfied \033[0m\n" << std::endl;
714 : 0 : return false;
715 : : }
716 : :
717 : 3301 : if (rbi(1, 1) >= (rbi(2, 2) + rbi(0, 0)))
718 : : {
719 : 0 : std::cerr << "\033[1;31m Invalid inertia: Triangle inequality not satisfied \033[0m\n" << std::endl;
720 : 0 : return false;
721 : : }
722 : :
723 : 3301 : if (rbi(2, 2) >= (rbi(1, 1) + rbi(0, 0)))
724 : : {
725 : 10 : std::cerr << "\033[1;31m Invalid inertia: Triangle inequality not satisfied \033[0m\n" << std::endl;
726 : 10 : return false;
727 : : }
728 : :
729 : 3291 : if (!(rbi - rbi.transpose()).isZero(1.e-8))
730 : : {
731 : 0 : std::cerr << "\033[1;31m Invalid inertia: Inertia matrix is not symmetric \033[0m\n" << std::endl;
732 : 0 : return false;
733 : : }
734 : :
735 : 3291 : Eigen::EigenSolver<Eigen::Matrix3d> solver(rbi);
736 : 3291 : Eigen::EigenSolver<Eigen::Matrix3d>::EigenvalueType eivals = solver.eigenvalues();
737 : :
738 : 13164 : for (unsigned int i = 0; i < eivals.rows(); i++)
739 : : {
740 : 9873 : if (eivals[i].real() <= 0)
741 : : {
742 : 0 : std::cerr << "\033[1;31m Invalid inertia: Inertia matrix is not positive definite \033[0m\n" << std::endl;
743 : 0 : return false;
744 : : }
745 : : }
746 : :
747 : 3291 : return true;
748 : : }
749 : : };
750 : : typedef std::shared_ptr<Model> ModelPtr;
751 : : } // namespace RobotDynamics
752 : :
753 : : #endif
|
