FancySafeBot 0.0.1
A safe robotics library
Loading...
Searching...
No Matches
fsb_compute_kinematics.h
1
2#ifndef FSB_COMPUTE_KINEMATICS_H
3#define FSB_COMPUTE_KINEMATICS_H
4
5#include <cstdint>
6#include <cstddef>
7
8#include "fsb_body.h"
9#include "fsb_body_tree.h"
10#include "fsb_joint.h"
11#include "fsb_kinematics.h"
12#include "fsb_jacobian.h"
13#include "fsb_motion.h"
14
15namespace fsb
16{
17
29enum class ComputeKinematicsError : uint8_t
30{
34 SUCCESS = 0,
39};
40
45{
46public:
47 ComputeKinematics() = default;
48
56
64 const JointSpacePosition& joint_position, BodyCartesianPva& cartesian) const;
65
74 ForwardKinematicsOption opt, const JointPva& joint, BodyCartesianPva& cartesian) const;
75
85 ForwardKinematicsOption opt, const JointPva& joint, const CartesianPva& base,
86 BodyCartesianPva& cartesian) const;
87
97 size_t body_index, const BodyCartesianPva& cartesian, Jacobian& jacobian) const;
98
104 [[nodiscard]] size_t get_num_bodies() const
105 {
106 return m_body_tree.get_num_bodies();
107 }
108
114 [[nodiscard]] size_t get_num_coordinates() const
115 {
116 return m_body_tree.get_num_coordinates();
117 }
118
119private:
120 BodyTree m_body_tree;
121};
122
124{
125 m_body_tree = tree;
127}
128
130 const JointSpacePosition& joint_position, BodyCartesianPva& cartesian) const
131{
132 const JointPva joint = {joint_position, {}, {}};
134}
135
137 const ForwardKinematicsOption opt, const JointPva& joint, BodyCartesianPva& cartesian) const
138{
139 forward_kinematics(m_body_tree, joint, {}, opt, cartesian);
140}
141
143 const ForwardKinematicsOption opt, const JointPva& joint, const CartesianPva& base,
144 BodyCartesianPva& cartesian) const
145{
146 forward_kinematics(m_body_tree, joint, base, opt, cartesian);
147}
148
150 const size_t body_index, const BodyCartesianPva& cartesian, Jacobian& jacobian) const
151{
152 return calculate_jacobian(body_index, m_body_tree, cartesian, jacobian);
153}
154
159} // namespace fsb
160
161#endif
Body tree description of a kinematic chain.
Definition fsb_body_tree.h:100
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
Compute forward kinematics.
Definition fsb_compute_kinematics.h:45
size_t get_num_bodies() const
Get the number of bodies in the body tree.
Definition fsb_compute_kinematics.h:104
size_t get_num_coordinates() const
Get the number of coordinates (joint position vector size) in the body tree.
Definition fsb_compute_kinematics.h:114
JacobianError calculate_jacobian(size_t body_index, const BodyTree &body_tree, const BodyCartesianPva &cartesian_pva, Jacobian &jacobian)
Determine Jacobian matrix for a single body in tree.
Definition fsb_jacobian.cpp:217
JacobianError
Error values for Jacobian calculations.
Definition fsb_jacobian.h:61
void forward_kinematics(const BodyTree &body_tree, const JointPva &joint_pva, const CartesianPva &base_pva, ForwardKinematicsOption opt, BodyCartesianPva &body_cartesian)
Compute forward kinematics for all bodies in tree.
Definition fsb_kinematics.cpp:38
ForwardKinematicsOption
Forward kinematics options.
Definition fsb_kinematics.h:25
@ POSE
Calculate Cartesian pose from joint position.
ComputeKinematicsError initialize(const BodyTree &tree)
Initialize the computation interface with a body tree.
Definition fsb_compute_kinematics.h:123
ComputeKinematicsError
Interface error codes.
Definition fsb_compute_kinematics.h:30
JacobianError compute_jacobian(size_t body_index, const BodyCartesianPva &cartesian, Jacobian &jacobian) const
Compute Jacobian matrix for a single body in tree.
Definition fsb_compute_kinematics.h:149
void compute_forward_kinematics(ForwardKinematicsOption opt, const JointPva &joint, BodyCartesianPva &cartesian) const
Compute forward kinematics with optional velocity and acceleration.
Definition fsb_compute_kinematics.h:136
void compute_forward_kinematics_pose(const JointSpacePosition &joint_position, BodyCartesianPva &cartesian) const
Compute forward kinematics.
Definition fsb_compute_kinematics.h:129
void compute_forward_kinematics_with_base(ForwardKinematicsOption opt, const JointPva &joint, const CartesianPva &base, BodyCartesianPva &cartesian) const
Compute forward kinematics with optional velocity and acceleration.
Definition fsb_compute_kinematics.h:142
@ INVALID_BODY_TREE
Body tree is invalid.
Container of Cartesian pose, velocity, and acceleration for a list of bodies.
Definition fsb_body.h:79
Cartesian pose, velocity and acceleration.
Definition fsb_motion.h:53
Jacobian matrix.
Definition fsb_jacobian.h:28
Definition fsb_joint.h:77
Joint space position in generalized coordinates.
Definition fsb_joint.h:55