shadow_finger.hpp

Go to the documentation of this file.

//
// Created by kganguly on 4/30/19.
//

#pragma once
#include <shadowlibs/shadow_utils.hpp>

namespace shadow_finger {

inline std::string getSavedStateName(std::string finger_name,
                                     std::string saved_state) {
  std::string saved_state_name;
  // "open" or "pack"
  std::size_t thumb_check = finger_name.find("thumb");
  if (thumb_check != std::string::npos)
    saved_state_name = finger_name + "_" + saved_state;
  else
    saved_state_name = finger_name + "_finger_" + saved_state;
  return saved_state_name;
};

inline std::string getMoveGroupName(std::string finger_name) {
  std::string move_group_name;
  std::size_t thumb_check = finger_name.find("thumb");
  if (thumb_check != std::string::npos)
    move_group_name = "rh_" + finger_name;
  else
    move_group_name = "rh_" + finger_name + "_finger";

  return move_group_name;
};

inline std::vector<std::string> getJointNames(
    moveit::planning_interface::MoveGroupInterface &move_group_interface) {
  std::vector<std::string> joint_names;
  joint_names = move_group_interface.getJointNames();
  return joint_names;
};

inline std::vector<std::string>
getJointNames(moveit::planning_interface::MoveGroupInterface::Plan &plan) {
  // This is the trajectory generated by MoveIt plan
  moveit_msgs::RobotTrajectory robotTrajectory = plan.trajectory_;
  // Convert MoveIt to trajectory_msgs joint trajectory
  trajectory_msgs::JointTrajectory planTrajectory =
      robotTrajectory.joint_trajectory;
  // Gives all the joint names in the planning group used when planning
  std::vector<std::string> jointNames = planTrajectory.joint_names;
  return jointNames;
};

inline std::vector<double> getJointValues(
    moveit::planning_interface::MoveGroupInterface &move_group_interface) {
  std::vector<double> joint_vals;
  joint_vals = move_group_interface.getCurrentJointValues();
  return joint_vals;
}

inline std::vector<std::tuple<float, float>>
getFingerJointLimits(std::vector<std::string> &joint_names) {
  urdf::Model model;
  if (!model.initParam("/robot_description")) {
    ROS_ERROR("Failed to parse urdf file");
  }
  ROS_INFO("Successfully parsed urdf file");
  std::vector<std::tuple<float, float>> joint_limits;
  for (auto joint_name : joint_names) {
    urdf::JointConstSharedPtr j = model.getJoint(joint_name);
    joint_limits.push_back(std::make_tuple(j->limits->upper, j->limits->lower));
  }
  return joint_limits;
}

const static std::vector<std::string> biotac_idx = {"first", "middle", "ring",
                                                    "little", "thumb"};

inline int getBiotacIdx(std::string &finger_name) {
  int idx = -1;
  for (auto &each_idx : biotac_idx) {
    idx += 1;
    std::size_t found = finger_name.find(each_idx);
    if (found != std::string::npos) {
      return idx;
    }
  }
  return idx;
}

struct Finger {
  ros::NodeHandle _node_handle;
  std::string _finger_name;
  int _biotac_id;
  std::vector<std::string> _joint_names;
  std::vector<std::tuple<float, float>> _joint_limits;
  moveit::planning_interface::MoveGroupInterface _finger_move_group;
  moveit::planning_interface::MoveGroupInterface::Plan _plan;
  shadow_planning::PlanningOptions _planning_options;
  std::vector<ros::Publisher> _joint_controller_publishers;
  std::string _fsr_topic;

  Finger(std::string &finger_name, ros::NodeHandle &node_handle)
      : _finger_name(finger_name),
        _finger_move_group(shadow_finger::getMoveGroupName(finger_name)),
        _node_handle(node_handle) {
    _joint_names = shadow_finger::getJointNames(_finger_move_group);
    _joint_controller_publishers =
        shadow_utils::createJointControllerPublishers(_joint_names,
                                                      _node_handle);
    _biotac_id = shadow_finger::getBiotacIdx(_finger_name);
    _joint_limits = shadow_finger::getFingerJointLimits(_joint_names);

    // Set FSR topic
    if (_finger_name != "thumb") {
      this->_fsr_topic = std::string(1, this->_finger_name.front()) + "f_fsr";
    } else {
      this->_fsr_topic = "th_fsr";
    }

    ROS_INFO_STREAM("Initialized Finger: " << _finger_name);
  };

  inline std::string getFingerName() { return _finger_name; }

  inline std::tuple<float, float> jointLimitsFromName(std::string &joint_name) {
    std::vector<std::string>::iterator it =
        std::find(_joint_names.begin(), _joint_names.end(), joint_name);
    if (it != _joint_names.end()) {
      int idx = std::distance(_joint_names.begin(), it);
      return _joint_limits[idx];
    } else {
      ROS_ERROR_STREAM("Joint " << joint_name << " not found in "
                                << _finger_name << ".");
      return std::make_tuple(0.0, 0.0);
    }
  }

  struct BioTac {
    int16_t pressure;
    std::vector<int16_t> impedance;
  };

  shadow_finger::Finger::BioTac getBioTacPressure();

  shadow_finger::Finger::BioTac getBioTacImpedance();

  shadow_finger::Finger::BioTac getBioTacImpedancePressure();

  float getFSRValue();
};

inline std::vector<std::string> getJointNames(shadow_finger::Finger &finger) {
  std::vector<std::string> joint_names;
  joint_names = finger._finger_move_group.getJointNames();
  return joint_names;
};

inline std::vector<double> getJointValues(shadow_finger::Finger &finger) {
  std::vector<double> joint_vals;
  joint_vals = finger._finger_move_group.getCurrentJointValues();
  return joint_vals;
}
} // namespace shadow_finger

inline std::ostream &operator<<(std::ostream &os,
                                const shadow_finger::Finger &finger) {
  os << finger._finger_name << '\n';
  return os;
}