|
20 | 20 |
|
21 | 21 | #include <rclcpp_lifecycle/lifecycle_node.hpp> |
22 | 22 |
|
23 | | -namespace kinematics_interface { |
24 | | - class KinematicsBaseClass { |
25 | | - public: |
26 | | - KinematicsBaseClass() = default; |
| 23 | +namespace kinematics_interface |
| 24 | +{ |
| 25 | +class KinematicsBaseClass |
| 26 | +{ |
| 27 | +public: |
| 28 | + KinematicsBaseClass() = default; |
27 | 29 |
|
28 | | - virtual ~KinematicsBaseClass() = default; |
| 30 | + virtual ~KinematicsBaseClass() = default; |
29 | 31 |
|
30 | | - /** |
| 32 | + /** |
31 | 33 | * \brief Create an interface object which takes calculate forward and inverse kinematics |
32 | 34 | */ |
33 | | - virtual bool |
34 | | - initialize(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, const std::string &end_effector_name) = 0; |
| 35 | + virtual bool initialize( |
| 36 | + std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node, |
| 37 | + const std::string & end_effector_name) = 0; |
35 | 38 |
|
36 | | - /** |
| 39 | + /** |
37 | 40 | * \brief Convert Cartesian delta-x to joint delta-theta, using the Jacobian. |
38 | 41 | * \param[in] joint_pos joint positions of the robot in radians |
39 | 42 | * \param[in] delta_x_vec input Cartesian deltas (x, y, z, wx, wy, wz) |
40 | 43 | * \param[out] delta_theta_vec output vector with joint states |
41 | 44 | * \return true if successful |
42 | 45 | */ |
43 | | - virtual bool |
44 | | - convert_cartesian_deltas_to_joint_deltas(const std::vector<double> &joint_pos, |
45 | | - const std::vector<double> &delta_x_vec, |
46 | | - const std::string &link_name, |
47 | | - std::vector<double> &delta_theta_vec) = 0; |
| 46 | + virtual bool convert_cartesian_deltas_to_joint_deltas( |
| 47 | + const std::vector<double> & joint_pos, const std::vector<double> & delta_x_vec, |
| 48 | + const std::string & link_name, std::vector<double> & delta_theta_vec) = 0; |
48 | 49 |
|
49 | | - /** |
| 50 | + /** |
50 | 51 | * \brief Convert joint delta-theta to Cartesian delta-x. |
51 | | - * \param joint_pos joint positions of the robot in radians |
| 52 | + * \param joint_pos joint positions of the robot in radiansgit checkout |
52 | 53 | * \param[in] delta_theta_vec vector with joint states |
53 | 54 | * \param[out] delta_x_vec Cartesian deltas (x, y, z, wx, wy, wz) |
54 | 55 | * \return true if successful |
55 | 56 | */ |
56 | | - virtual bool |
57 | | - convert_joint_deltas_to_cartesian_deltas(const std::vector<double> &joint_pos, |
58 | | - const std::vector<double> &delta_theta_vec, |
59 | | - const std::string &link_name, |
60 | | - std::vector<double> &delta_x_vec) = 0; |
| 57 | + virtual bool convert_joint_deltas_to_cartesian_deltas( |
| 58 | + const std::vector<double> & joint_pos, const std::vector<double> & delta_theta_vec, |
| 59 | + const std::string & link_name, std::vector<double> & delta_x_vec) = 0; |
61 | 60 |
|
62 | | - /** |
| 61 | + /** |
63 | 62 | * \brief Calculates the joint transform for a specified link using provided joint positions. |
64 | 63 | * \param[in] joint_pos joint positions of the robot in radians |
65 | 64 | * \param[in] link_name the name of the link to find the transform for |
66 | 65 | * \param[out] transform_vec transformation matrix of the specified link in column major format. |
67 | 66 | * \return true if successful |
68 | 67 | */ |
69 | | - virtual bool |
70 | | - calculate_link_transform(const std::vector<double> &joint_pos, const std::string &link_name, |
71 | | - std::vector<double> &transform_vec) = 0; |
| 68 | + virtual bool calculate_link_transform( |
| 69 | + const std::vector<double> & joint_pos, const std::string & link_name, |
| 70 | + std::vector<double> & transform_vec) = 0; |
72 | 71 |
|
73 | | - /** |
| 72 | + /** |
74 | 73 | * \brief Calculates the joint transform for a specified link using provided joint positions. |
75 | 74 | * \param[in] joint_pos joint positions of the robot in radians |
76 | 75 | * \param[in] link_name the name of the link to find the transform for |
77 | 76 | * \param[out] jacobian Jacobian matrix of the specified link in column major format. |
78 | 77 | * \return true if successful |
79 | 78 | */ |
80 | | - virtual bool |
81 | | - calculate_jacobian(const std::vector<double> &joint_pos, const std::string &link_name, |
82 | | - std::vector<double> &jacobian) = 0; |
83 | | - |
84 | | - }; |
| 79 | + virtual bool calculate_jacobian( |
| 80 | + const std::vector<double> & joint_pos, const std::string & link_name, |
| 81 | + std::vector<double> & jacobian) = 0; |
| 82 | +}; |
85 | 83 |
|
86 | 84 | } // namespace kinematics_interface |
87 | 85 |
|
|
0 commit comments