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