|
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