Added some docstrings

Author: TimSchneider42

include/franky/robot_pose.hpp

@@ -8,6 +8,12 @@
 
 namespace franky {
 
+/**
+ * @class RobotPose
+ * @brief A class to represent the cartesian pose of a robot.
+ *
+ * This class encapsulates the cartesian pose of a robot, which comprises the end effector pose and the elbow position.
+ */
 class RobotPose {
  public:
   RobotPose();
@@ -17,39 +23,100 @@ class RobotPose {
   // Suppress implicit conversion warning
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wimplicit-conversion"
+  /**
+   * @param end_effector_pose The pose of the end effector.
+   * @param elbow_position The position of the elbow. Optional.
+   */
   RobotPose(Affine end_effector_pose, std::optional<double> elbow_position = std::nullopt);
 #pragma clang diagnostic pop
 
+  /**
+   * @param vector_repr The vector representation of the pose.
+   * @param ignore_elbow Whether to ignore the elbow position. Default is false.
+   */
   explicit RobotPose(const Vector7d &vector_repr, bool ignore_elbow = false);
 
+  /**
+   * @param vector_repr The vector representation of the pose.
+   * @param elbow_position The position of the elbow. Optional.
+   */
   explicit RobotPose(const Vector6d &vector_repr, std::optional<double> elbow_position = std::nullopt);
 
+  /**
+   * @param franka_pose The franka pose.
+   */
   explicit RobotPose(franka::CartesianPose franka_pose);
 
+  /**
+   * @brief Get the vector representation of the pose, which consists of the end effector position and orientation
+   * (as rotation vector) and the elbow position.
+   *
+   * @return The vector representation of the pose.
+   */
   [[nodiscard]] Vector7d vector_repr() const;
 
+  /**
+   * @brief Convert this pose to a franka pose.
+   *
+   * @return The franka pose.
+   */
   [[nodiscard]] franka::CartesianPose as_franka_pose() const;
 
+  /**
+   * @brief Transform this pose with a given affine transformation from the left side.
+   *
+   * @param transform The transform to apply.
+   * @return The transformed robot pose.
+   */
   [[nodiscard]] inline RobotPose leftTransform(const Affine &transform) const {
     return {transform * end_effector_pose_, elbow_position_};
   }
 
+  /**
+   * @brief Transform this pose with a given affine transformation from the right side.
+   *
+   * @param transform The transform to apply.
+   * @return The transformed robot pose.
+   */
   [[nodiscard]] inline RobotPose rightTransform(const Affine &transform) const {
     return {end_effector_pose_ * transform, elbow_position_};
   }
 
+  /**
+   * @brief Change the frame of the end effector by applying a transformation from the right side. This is equivalent to
+   * calling rightTransform(transform).
+   *
+   * @param transform The transform to apply.
+   * @return The robot pose with the new end effector frame.
+   */
   [[nodiscard]] inline RobotPose changeEndEffectorFrame(const Affine &transform) const {
-    return {end_effector_pose_ * transform, elbow_position_};
+    return rightTransform(transform);
   }
 
+  /**
+   * @brief Get the pose with a new elbow position.
+   *
+   * @param elbow_position The new elbow position.
+   * @return The pose with the new elbow position.
+   */
   [[nodiscard]] inline RobotPose withElbowPosition(const std::optional<double> elbow_position) const {
     return {end_effector_pose_, elbow_position};
   }
 
+  /**
+   * @brief Get the end effector pose.
+   *
+   * @return The end effector pose.
+   */
   [[nodiscard]] inline Affine end_effector_pose() const {
     return end_effector_pose_;
   }
 
+  /**
+   * @brief Get the elbow position.
+   *
+   * @return The elbow position.
+   */
   [[nodiscard]] inline std::optional<double> elbow_position() const {
     return elbow_position_;
   }

include/franky/robot_velocity.hpp

@@ -9,6 +9,10 @@
 
 namespace franky {
 
+/**
+ * @brief Class to represent  the cartesian velocity of a robot. It comprists the twist of the end effector and the
+ * joint velocity of the elbow.
+ */
 class RobotVelocity {
  public:
   RobotVelocity();
@@ -18,38 +22,84 @@ class RobotVelocity {
   // Suppress implicit conversion warning
 #pragma clang diagnostic push
 #pragma clang diagnostic ignored "-Wimplicit-conversion"
+  /**
+   * @param end_effector_twist The twist of the end effector.
+   * @param elbow_velocity The velocity of the elbow. Default is 0.0.
+   */
   RobotVelocity(Twist end_effector_twist, double elbow_velocity = 0.0);
 #pragma clang diagnostic pop
 
+  /**
+   * @param vector_repr The vector representation of the velocity.
+   */
   explicit RobotVelocity(const Vector7d &vector_repr);
 
+  /**
+   * @param franka_velocity The franka velocity.
+   */
   explicit RobotVelocity(franka::CartesianVelocities franka_velocity);
 
+  /**
+   * @brief Get the vector representation of the velocity. It consists of the linear and angular velocity of the end
+   * effector and the joint velocity of the elbow.
+   *
+   * @return The vector representation of the velocity.
+   */
   [[nodiscard]] Vector7d vector_repr() const;
 
+  /**
+   * @brief Get the franka velocity.
+   *
+   * @return The franka velocity.
+   */
   [[nodiscard]] franka::CartesianVelocities as_franka_velocity() const;
 
+  /**
+   * @brief Transform the frame of the velocity by applying the given affine transform.
+   *
+   * @param affine The affine to apply.
+   * @return The velocity after the transformation.
+   */
   [[nodiscard]] inline RobotVelocity transform(const Affine &affine) const {
     return transform(affine.rotation());
   }
 
+  /**
+   * @brief Transform the frame of the velocity by applying the given rotation.
+   *
+   * @param rotation The rotation to apply.
+   * @return The velocity after the transformation.
+   */
   template<typename RotationMatrixType>
   [[nodiscard]] inline RobotVelocity transform(const RotationMatrixType &rotation) const {
     return {rotation * end_effector_twist_, elbow_velocity_};
   }
 
-  /// Change the end-effector frame by adding the given offset to the current end-effector frame. Note that the offset
-  /// must be given in world coordinates.
-  /// \param offset_world_frame: The offset to add to the current end-effector frame.
-  /// \return The velocity of the new end-effector frame.
+  /**
+   * @brief Change the end-effector frame by adding the given offset to the current end-effector frame. Note that the
+   * offset must be given in world coordinates.
+   *
+   * @param offset_world_frame The offset to add to the current end-effector frame.
+   * @return The velocity of the new end-effector frame.
+   */
   [[nodiscard]] inline RobotVelocity changeEndEffectorFrame(const Eigen::Vector3d &offset_world_frame) const {
     return {end_effector_twist_.propagateThroughLink(offset_world_frame), elbow_velocity_};
   }
 
+  /**
+   * @brief Get the end effector twist.
+   *
+   * @return The end effector twist.
+   */
   [[nodiscard]] inline Twist end_effector_twist() const {
     return end_effector_twist_;
   }
 
+  /**
+   * @brief Get the elbow velocity.
+   *
+   * @return The elbow velocity.
+   */
   [[nodiscard]] inline std::optional<double> elbow_velocity() const {
     return elbow_velocity_;
   }

include/franky/twist.hpp

@@ -6,46 +6,89 @@
 
 namespace franky {
 
+/**
+ * @brief Class to represent the twist of a robot.
+ */
 class Twist {
  public:
+  /**
+   * @brief Default constructor. Initializes linear and angular velocities to zero.
+   */
   Twist() : linear_velocity_(Eigen::Vector3d::Zero()), angular_velocity_(Eigen::Vector3d::Zero()) {}
 
   Twist(const Twist &twist) = default;
 
+  /**
+   * @param linear_velocity The linear velocity.
+   * @param angular_velocity The angular velocity.
+   */
   Twist(Eigen::Vector3d linear_velocity, Eigen::Vector3d angular_velocity)
       : linear_velocity_(std::move(linear_velocity)), angular_velocity_(std::move(angular_velocity)) {}
 
+  /**
+   * @param vector_repr The vector representation of the twist.
+   */
   explicit Twist(const Vector6d &vector_repr)
       : linear_velocity_(vector_repr.head<3>()), angular_velocity_(vector_repr.tail<3>()) {}
 
+  /**
+   * @brief Get the vector representation of the twist. It consists of the linear and angular velocities.
+   *
+   * @return The vector representation of the twist.
+   */
   [[nodiscard]] inline Vector6d vector_repr() const {
     Vector6d result;
     result << linear_velocity_, angular_velocity_;
     return result;
   }
 
-  [[nodiscard]] inline Twist transformWith(const Affine &affine) const {
-    return transformWith(affine.rotation());
+  /**
+   * @brief Transform the frame of the twist by applying the given affine transform.
+   *
+   * @param transformation The transformation to apply.
+   * @return The twist after the transformation.
+   */
+  [[nodiscard]] inline Twist transformWith(const Affine &transformation) const {
+    return transformWith(transformation.rotation());
   }
 
+  /**
+   * @brief Transform the frame of the twist by applying the given rotation.
+   *
+   * @param rotation The rotation to apply.
+   * @return The twist after the transformation.
+   */
   template<typename RotationMatrixType>
   [[nodiscard]] inline Twist transformWith(const RotationMatrixType &rotation) const {
     return {rotation * linear_velocity_, rotation * angular_velocity_};
   }
 
-  /// Propagate the twist through a link with the given translation. Hence, suppose this twist is the twist of a frame
-  /// A, then this function computes the twist of a frame B that is rigidly attached to frame A by a link with the
-  /// given translation: B = A + T, where T is the translation.
-  /// \param link_translation: The translation of the link. Must be in the same reference frame as this twist.
-  /// \return The twist propagated through the link.
+  /**
+   * @brief Propagate the twist through a link with the given translation. Hence, suppose this twist is the twist of a
+   * frame A, then this function computes the twist of a frame B that is rigidly attached to frame A by a link with the
+   * given translation: B = A + T, where T is the translation.
+   *
+   * @param link_translation: The translation of the link. Must be in the same reference frame as this twist.
+   * @return The twist propagated through the link.
+   */
   [[nodiscard]] inline Twist propagateThroughLink(const Eigen::Vector3d &link_translation) const {
     return {linear_velocity_ + angular_velocity_.cross(link_translation), angular_velocity_};
   }
 
+  /**
+   * @brief Get the linear velocity.
+   *
+   * @return The linear velocity.
+   */
   [[nodiscard]] inline Eigen::Vector3d linear_velocity() const {
     return linear_velocity_;
   }
 
+  /**
+   * @brief Get the angular velocity.
+   *
+   * @return The angular velocity.
+   */
   [[nodiscard]] inline Eigen::Vector3d angular_velocity() const {
     return angular_velocity_;
   }