robot_velocity.hpp

Go to the documentation of this file.

#pragma once
 
#include <franka/control_types.h>
 
#include <Eigen/Core>
#include <optional>
 
#include "franky/elbow_state.hpp"
#include "franky/twist.hpp"
#include "franky/types.hpp"
 
namespace franky {
 
class RobotVelocity {
 public:
  RobotVelocity();
 
  RobotVelocity(const RobotVelocity &robot_velocity);
 
  // Suppress implicit conversion warning
#pragma clang diagnostic push
#pragma clang diagnostic ignored "-Wimplicit-conversion"
  RobotVelocity(const Twist &end_effector_twist, std::optional<double> elbow_velocity = std::nullopt);
#pragma clang diagnostic pop
 
  explicit RobotVelocity(const Vector7d &vector_repr, bool ignore_elbow = false);
 
  explicit RobotVelocity(const Vector6d &vector_repr, std::optional<double> elbow_velocity = std::nullopt);
 
  explicit RobotVelocity(franka::CartesianVelocities franka_velocity);
 
  [[nodiscard]] Vector7d vector_repr() const;
 
  [[nodiscard]] franka::CartesianVelocities as_franka_velocity(
      const std::optional<ElbowState> &elbow_state = std::nullopt,
      FlipDirection default_elbow_flip_direction = FlipDirection::kNegative) const;
 
  [[nodiscard]] inline RobotVelocity transform(const Affine &affine) const { return transform(affine.rotation()); }
 
  template <typename RotationMatrixType>
  [[nodiscard]] inline RobotVelocity transform(const RotationMatrixType &rotation) const {
    return {rotation * end_effector_twist_, elbow_velocity_};
  }
 
  [[nodiscard]] inline RobotVelocity changeEndEffectorFrame(const Eigen::Vector3d &offset_world_frame) const {
    return {end_effector_twist_.propagateThroughLink(offset_world_frame), elbow_velocity_};
  }
 
  [[nodiscard]] inline RobotVelocity withElbowVelocity(const std::optional<double> elbow_velocity) const {
    return {end_effector_twist_, elbow_velocity};
  }
 
  [[nodiscard]] inline Twist end_effector_twist() const { return end_effector_twist_; }
 
  [[nodiscard]] inline std::optional<double> elbow_velocity() const { return elbow_velocity_; }
 
  friend std::ostream &operator<<(std::ostream &os, const RobotVelocity &robot_velocity);
 
 private:
  Twist end_effector_twist_;
  std::optional<double> elbow_velocity_ = std::nullopt;
};
 
inline RobotVelocity operator*(const Affine &affine, const RobotVelocity &robot_velocity) {
  return robot_velocity.transform(affine);
}
 
template <typename RotationMatrixType>
inline RobotVelocity operator*(const RotationMatrixType &rotation, const RobotVelocity &robot_velocity) {
  return robot_velocity.transform(rotation);
}
 
}  // namespace franky