8#include "fsb_configuration.h"
11#include "fsb_motion.h"
89 std::array<bool, MaxSize::dofs>
set;
121 size_t parent_body_index,
JointType joint_type,
const Transform& parent_joint_transform,
135 size_t parent_body_index,
JointType joint_type,
const Transform& parent_joint_transform,
294 return m_num_coordinates;
313 [[ nodiscard ]] std::array<size_t, MaxSize::bodies>
get_leaves(
size_t& num_leaves)
const
315 std::array<size_t, MaxSize::bodies> result = {};
317 for (
size_t index = 0; index < m_num_bodies; ++index)
319 if (m_bodies[index].is_leaf)
321 result[num_leaves] = index;
329 [[nodiscard]]
BodyTreeError validate_massless_body_input(
size_t parent_body_index)
const;
331 validate_add_body_input(
size_t parent_body_index,
JointType joint_type,
const Body& body)
const;
333 std::array<Body, MaxSize::bodies> m_bodies = {};
334 std::array<Joint, MaxSize::joints> m_joints = {};
335 JointLimits m_limits = {};
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;
349 if (joint_index < m_num_joints)
351 joint = m_joints[joint_index];
352 err = BodyTreeError::SUCCESS;
364 if (body_index < m_num_bodies)
366 body = m_bodies[body_index];
367 err = BodyTreeError::SUCCESS;
379 auto err = BodyTreeError::SUCCESS;
380 if (body_index < m_num_bodies)
382 m_bodies[body_index].mass_props = mass_props;
383 err = BodyTreeError::SUCCESS;
395 auto err = BodyTreeError::SUCCESS;
396 if (body_index < m_num_bodies)
398 m_bodies[body_index].origin_offset = origin_offset;
399 err = BodyTreeError::SUCCESS;
409 const size_t body_index,
const Transform& parent_joint_transform)
411 auto err = BodyTreeError::SUCCESS;
412 if (body_index < m_num_bodies)
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;
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
3D vector
Definition fsb_types.h:27