joint.h

Go to the documentation of this file.

#pragma once
 
#include <angles/angles.h>
#include <control_toolbox/pid.h>
#include <franka/robot_state.h>
#include <joint_limits_interface/joint_limits.h>
#include <ros/ros.h>
#include <Eigen/Dense>
#include <boost/optional.hpp>
#include <gazebo/physics/Joint.hh>
 
namespace franka_gazebo {
 
enum ControlMethod { EFFORT, POSITION, VELOCITY };
 
struct Joint {
 public:
  Joint() = default;
  Joint(Joint&&) = default;
  Joint(const Joint&) = delete;
 
  void update(const ros::Duration& dt);
 
  std::string name;
 
  gazebo::physics::JointPtr handle;
 
  int type;
 
  joint_limits_interface::JointLimits limits;
 
  Eigen::Vector3d axis;
 
  double command = 0;
 
  double desired_position = 0;
 
  double desired_velocity = 0;
 
  boost::optional<ControlMethod> control_method = boost::none;
 
  double gravity = 0;
 
  double position = 0;
 
  double velocity = 0;
 
  double effort = 0;
 
  double jerk = 0;
 
  double acceleration = 0;
 
  double contact_threshold = std::numeric_limits<double>::infinity();
 
  double collision_threshold = std::numeric_limits<double>::infinity();
 
  double stop_position = 0;
 
  double getDesiredPosition(const franka::RobotMode& mode) const;
 
  double getDesiredVelocity(const franka::RobotMode& mode) const;
 
  double getDesiredAcceleration(const franka::RobotMode& mode) const;
 
  double getDesiredTorque(const franka::RobotMode& mode) const;
 
  double getLinkMass() const;
 
  bool isInContact() const;
 
  bool isInCollision() const;
 
  control_toolbox::Pid position_controller;
 
  control_toolbox::Pid velocity_controller;
 
 private:
  double lastVelocity = std::numeric_limits<double>::quiet_NaN();
  double lastAcceleration = std::numeric_limits<double>::quiet_NaN();
};
 
}  // namespace franka_gazebo