FancySafeBot 0.0.1
A safe robotics library
Loading...
Searching...
No Matches
fsb_body_tree.h
1
2#ifndef FSB_BODY_TREE_H
3#define FSB_BODY_TREE_H
4
5#include <array>
6#include <cstddef>
7#include <cstdint>
8#include "fsb_configuration.h"
9#include "fsb_body.h"
10#include "fsb_joint.h"
11#include "fsb_motion.h"
12#include "fsb_types.h"
13
14namespace fsb
15{
16
83
88{
89 std::array<bool, MaxSize::dofs> set;
90 std::array<real_t, MaxSize::dofs> lower_position;
91 std::array<real_t, MaxSize::dofs> upper_position;
92 std::array<real_t, MaxSize::dofs> max_velocity;
93};
94
100{
101public:
105 BodyTree() = default;
106
108 static constexpr size_t base_index = 0U;
109
120 size_t add_body(
121 size_t parent_body_index, JointType joint_type, const Transform& parent_joint_transform,
122 const Body& body, BodyTreeError& err);
123
134 size_t add_massless_body(
135 size_t parent_body_index, JointType joint_type, const Transform& parent_joint_transform,
136 const MotionVector& origin_offset, BodyTreeError& err);
137
150 set_joint_position_limit(size_t joint_index, real_t lower_position, real_t upper_position);
151
160 BodyTreeError unset_joint_position_limit(size_t joint_index);
161
175 get_joint_position_limit(size_t joint_index, bool& is_set, real_t& lower_position, real_t& upper_position) const;
176
188 set_joint_velocity_limit(size_t joint_index, real_t max_velocity);
189
200 BodyTreeError get_joint_velocity_limit(size_t joint_index, real_t& max_velocity) const;
201
207 void set_gravity(const Vec3& gravity)
208 {
209 m_gravity = gravity;
210 }
211
219 Joint get_joint(size_t joint_index, BodyTreeError& err) const;
220
228 Body get_body(size_t body_index, BodyTreeError& err) const;
229
237 BodyTreeError set_body_mass_props(size_t body_index, const MassProps& mass_props);
238
246 BodyTreeError set_body_origin_offset(size_t body_index, const MotionVector& origin_offset);
247
256 set_parent_joint_transform(size_t body_index, const Transform& parent_joint_transform);
257
265 size_t get_body_dofs(size_t body_index, BodyTreeError& err) const;
266
272 [[nodiscard]] size_t get_num_bodies() const
273 {
274 return m_num_bodies;
275 }
276
282 [[nodiscard]] size_t get_num_dofs() const
283 {
284 return m_num_dofs;
285 }
286
292 [[nodiscard]] size_t get_num_coordinates() const
293 {
294 return m_num_coordinates;
295 }
296
302 [[nodiscard]] Vec3 get_gravity() const
303 {
304 return m_gravity;
305 }
306
313 [[ nodiscard ]] std::array<size_t, MaxSize::bodies> get_leaves(size_t& num_leaves) const
314 {
315 std::array<size_t, MaxSize::bodies> result = {};
316 num_leaves = 0U;
317 for (size_t index = 0; index < m_num_bodies; ++index)
318 {
319 if (m_bodies[index].is_leaf)
320 {
321 result[num_leaves] = index;
322 num_leaves++;
323 }
324 }
325 return result;
326 }
327
328private:
329 [[nodiscard]] BodyTreeError validate_massless_body_input(size_t parent_body_index) const;
330 [[nodiscard]] BodyTreeError
331 validate_add_body_input(size_t parent_body_index, JointType joint_type, const Body& body) const;
332
333 std::array<Body, MaxSize::bodies> m_bodies = {};
334 std::array<Joint, MaxSize::joints> m_joints = {};
335 JointLimits m_limits = {};
336 Vec3 m_gravity = {};
337
338 size_t m_num_coordinates = 0U;
339 size_t m_num_dofs = 0U;
340 size_t m_num_bodies = 1U;
341 size_t m_num_joints = 0U;
342};
343
344// inline methods
345
346inline Joint BodyTree::get_joint(const size_t joint_index, BodyTreeError& err) const
347{
348 Joint joint = {};
349 if (joint_index < m_num_joints)
350 {
351 joint = m_joints[joint_index];
352 err = BodyTreeError::SUCCESS;
353 }
354 else
355 {
357 }
358 return joint;
359}
360
361inline Body BodyTree::get_body(const size_t body_index, BodyTreeError& err) const
362{
363 Body body = {};
364 if (body_index < m_num_bodies)
365 {
366 body = m_bodies[body_index];
367 err = BodyTreeError::SUCCESS;
368 }
369 else
370 {
372 }
373 return body;
374}
375
376inline BodyTreeError
377BodyTree::set_body_mass_props(const size_t body_index, const MassProps& mass_props)
378{
379 auto err = BodyTreeError::SUCCESS;
380 if (body_index < m_num_bodies)
381 {
382 m_bodies[body_index].mass_props = mass_props;
383 err = BodyTreeError::SUCCESS;
384 }
385 else
386 {
388 }
389 return err;
390}
391
392inline BodyTreeError
393BodyTree::set_body_origin_offset(const size_t body_index, const MotionVector& origin_offset)
394{
395 auto err = BodyTreeError::SUCCESS;
396 if (body_index < m_num_bodies)
397 {
398 m_bodies[body_index].origin_offset = origin_offset;
399 err = BodyTreeError::SUCCESS;
400 }
401 else
402 {
404 }
405 return err;
406}
407
409 const size_t body_index, const Transform& parent_joint_transform)
410{
411 auto err = BodyTreeError::SUCCESS;
412 if (body_index < m_num_bodies)
413 {
414 const size_t joint_index = m_bodies[body_index].joint_index;
415 m_joints[joint_index].parent_joint_transform = parent_joint_transform;
416 err = BodyTreeError::SUCCESS;
417 }
418 else
419 {
421 }
422 return err;
423}
424
429} // namespace fsb
430
431#endif
Body tree description of a kinematic chain.
Definition fsb_body_tree.h:100
void set_gravity(const Vec3 &gravity)
Set the gravity vector.
Definition fsb_body_tree.h:207
BodyTreeError unset_joint_position_limit(size_t joint_index)
Unset joint position limits for a joint in the body tree.
Definition fsb_body_tree.cpp:316
std::array< size_t, MaxSize::bodies > get_leaves(size_t &num_leaves) const
Get a list of body indexes that are leaves (bodies without children)
Definition fsb_body_tree.h:313
size_t add_body(size_t parent_body_index, JointType joint_type, const Transform &parent_joint_transform, const Body &body, BodyTreeError &err)
Add a body to the body tree.
Definition fsb_body_tree.cpp:189
size_t get_num_dofs() const
Get the number of body tree degrees of freedom.
Definition fsb_body_tree.h:282
BodyTreeError set_joint_position_limit(size_t joint_index, real_t lower_position, real_t upper_position)
Set joint position limits for a joint in the body tree.
Definition fsb_body_tree.cpp:293
BodyTreeError set_joint_velocity_limit(size_t joint_index, real_t max_velocity)
Set joint velocity limit for a joint in the body tree.
Definition fsb_body_tree.cpp:364
size_t add_massless_body(size_t parent_body_index, JointType joint_type, const Transform &parent_joint_transform, const MotionVector &origin_offset, BodyTreeError &err)
Add a massless body to the body tree.
Definition fsb_body_tree.cpp:143
size_t get_num_coordinates() const
Get the number of generalized position coordinates.
Definition fsb_body_tree.h:292
size_t get_num_bodies() const
Get the number of bodies in tree.
Definition fsb_body_tree.h:272
BodyTree()=default
Construct a new BodyTree object.
BodyTreeError get_joint_velocity_limit(size_t joint_index, real_t &max_velocity) const
Get joint velocity limit for a joint in the body tree.
Definition fsb_body_tree.cpp:385
Vec3 get_gravity() const
Get the gravity vector.
Definition fsb_body_tree.h:302
BodyTreeError get_joint_position_limit(size_t joint_index, bool &is_set, real_t &lower_position, real_t &upper_position) const
Get joint position limits for a joint in the body tree.
Definition fsb_body_tree.cpp:337
size_t get_body_dofs(size_t body_index, BodyTreeError &err) const
Get the number of degrees of freedom for a body.
Definition fsb_body_tree.cpp:237
static constexpr size_t base_index
Index of base body is always 0.
Definition fsb_body_tree.h:108
JointType
Joint type.
Definition fsb_joint.h:28
BodyTreeError set_body_mass_props(size_t body_index, const MassProps &mass_props)
Set the body mass properties object.
Definition fsb_body_tree.h:377
BodyTreeError set_body_origin_offset(size_t body_index, const MotionVector &origin_offset)
Set the body transform offset.
Definition fsb_body_tree.h:393
BodyTreeError set_parent_joint_transform(size_t body_index, const Transform &parent_joint_transform)
Set the parent joint transform object.
Definition fsb_body_tree.h:408
Body get_body(size_t body_index, BodyTreeError &err) const
Get body object from tree.
Definition fsb_body_tree.h:361
Joint get_joint(size_t joint_index, BodyTreeError &err) const
Get a joint object in the body tree.
Definition fsb_body_tree.h:346
BodyTreeError
Error list for body tree operations.
Definition fsb_body_tree.h:28
@ MAX_BODIES_REACHED
No more bodies can be added to the body tree. Maximum is specified by MaxSize::bodies.
@ MAX_JOINT_DOFS_REACHED
Joint requires more DoFs than is available. Maximum is specified by MaxSize::dofs.
@ MASS_ZERO_WITH_INERTIA
Body mass is set to zero but with non-zero inertia for a fixed parent joint.
@ INERTIA_ZERO
At least one of the inertia principal components are zero with a non-fixed parent joint.
@ MAX_JOINT_COORDINATES_REACHED
Joint requires more coordinates than is available. Maximum is specified by MaxSize::coordinates.
@ INERTIA_NOT_POS_DEF
Inertia is not positive definite.
@ UNSUPPORTED_JOINT_TYPE
Joint type is not supported for the operation.
@ MAX_JOINTS_REACHED
No more joints can be added to the body tree. Maximum is specified by MaxSize::joints.
@ BODY_NOT_IN_TREE
Index does not point to an existing body in tree.
@ PARENT_BODY_NONEXISTENT
Parent body index does not point to an existing body in tree.
@ MASS_ZERO
Body mass is zero with a non-fixed parent joint.
@ JOINT_NOT_IN_TREE
Index does not point to an existing joint in tree.
Link body parameters.
Definition fsb_body.h:90
Limits for joint vectors.
Definition fsb_body_tree.h:88
std::array< real_t, MaxSize::dofs > max_velocity
Maximum joint velocity limit.
Definition fsb_body_tree.h:92
std::array< real_t, MaxSize::dofs > lower_position
Minimum joint position limit.
Definition fsb_body_tree.h:90
std::array< bool, MaxSize::dofs > set
Enable limits for each joint DoF.
Definition fsb_body_tree.h:89
std::array< real_t, MaxSize::dofs > upper_position
Maximum joint position limit.
Definition fsb_body_tree.h:91
Joint definition structure.
Definition fsb_joint.h:96
Mass properties of a body.
Definition fsb_body.h:45
Motion vector.
Definition fsb_motion.h:37
Coordinate transform.
Definition fsb_motion.h:22
3D vector
Definition fsb_types.h:27