<commit_before>/*
 * See ../LICENSE.md for original copyright and license.
 */

/*
 * Copyright (c) 2013, <NAME>, BioRobotics Lab at KoreaTech
 * All right reserved.
 *
 * Based on original code from Healthcare Robotics Lab at Georgia Tech
 *
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <string.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <sstream>

#include <HL/hl.h>
#include <HD/hd.h>
#include <HDU/hduError.h>
#include <HDU/hduVector.h>
#include <HDU/hduMatrix.h>
#include <HDU/hduQuaternion.h>

#include "sensable_phantom/PhantomButtonEvent.h"
#include <pthread.h>

float prev_time;

struct PhantomState
{
  hduVector3Dd position; //3x1 vector of position
  hduVector3Dd velocity; //3x1 vector of velocity
  hduVector3Dd inp_vel1; //3x1 history of velocity used for filtering velocity estimate
  hduVector3Dd inp_vel2;
  hduVector3Dd inp_vel3;
  hduVector3Dd out_vel1;
  hduVector3Dd out_vel2;
  hduVector3Dd out_vel3;
  hduVector3Dd pos_hist1; //3x1 history of position used for 2nd order backward difference estimate of velocity
  hduVector3Dd pos_hist2;
  hduVector3Dd rot;
  hduVector3Dd joints;
  hduVector3Dd force; //3 element double vector force[0], force[1], force[2]
  hduVector3Dd torque; //3 element double vector torque[0], torque[1], torque[2]

  hduMatrix hd_cur_transform;

  float thetas[7];
  int buttons[2];
  int buttons_prev[2];
  bool lock;
  hduVector3Dd lock_pos;
};

class PhantomROS
{

public:
  ros::NodeHandlePtr node_;
  ros::NodeHandlePtr pnode_;

  ros::Publisher pose_publisher_;

  ros::Publisher button_publisher_;
  ros::Subscriber wrench_sub_;
  std::string base_link_name_;
  std::string sensable_frame_name_;
  std::string link_names_[7];

  std::string tf_prefix_;
  double table_offset_;
  bool locked_;

  PhantomState *state_;
  tf::TransformBroadcaster br_;
  tf::TransformListener ls_;

  PhantomROS() : table_offset_(0.0), locked_(false), state_(NULL)
  {
  }

  int init(PhantomState *s)
  {
    if(!s)
    {
      ROS_FATAL("Internal error. PhantomState is NULL.");
      return -1;
    }

    node_ = ros::NodeHandlePtr(new ros::NodeHandle);
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));

    // Vertical displacement from base_link to link_0. Defaults to Omni offset.
    pnode_->param(std::string("table_offset"), table_offset_, .135);

    // On startup device will generate forces to hold end-effector at origin.
    pnode_->param(std::string("locked"), locked_, false);

    //Frame attached to the base of the phantom (NAME/base_link)
    base_link_name_ = "base_link";

    //Publish on NAME/pose
    std::string pose_topic_name = "pose";
    pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);

    //Publish button state on NAME/button
    std::string button_topic = "button";
    button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);

    //Subscribe to NAME/force_feedback
    std::string force_feedback_topic = "force_feedback";
    wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);

    //Frame of force feedback (NAME/sensable_origin)
    sensable_frame_name_ = "sensable_origin";

    for (int i = 0; i < 7; i++)
    {
      std::ostringstream stream1;
      stream1 << "link_" << i;
      link_names_[i] = std::string(stream1.str());
    }

    state_ = s;
    state_->buttons[0] = 0;
    state_->buttons[1] = 0;
    state_->buttons_prev[0] = 0;
    state_->buttons_prev[1] = 0;
    hduVector3Dd zeros(0, 0, 0);
    state_->velocity = zeros;
    state_->inp_vel1 = zeros; //3x1 history of velocity
    state_->inp_vel2 = zeros; //3x1 history of velocity
    state_->inp_vel3 = zeros; //3x1 history of velocity
    state_->out_vel1 = zeros; //3x1 history of velocity
    state_->out_vel2 = zeros; //3x1 history of velocity
    state_->out_vel3 = zeros; //3x1 history of velocity
    state_->pos_hist1 = zeros; //3x1 history of position
    state_->pos_hist2 = zeros; //3x1 history of position
    state_->lock = locked_;
    state_->lock_pos = zeros;
    state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);

    return 0;
  }

  /*******************************************************************************
   ROS node callback.
   *******************************************************************************/
  void wrench_callback(const geometry_msgs::WrenchStampedConstPtr& wrench)
  {
    // Both force and torque supplied in the same coordinate frame
    geometry_msgs::Vector3Stamped f_in, f_out;
    geometry_msgs::Vector3Stamped t_in, t_out;

    t_in.header = f_in.header = wrench->header;

    f_in.vector = wrench->wrench.force;
    t_in.vector = wrench->wrench.torque;

    ls_.transformVector(sensable_frame_name_, f_in, f_out);
    ls_.transformVector(sensable_frame_name_, t_in, t_out);

    ////////////////////Some people might not like this extra damping, but it
    ////////////////////helps to stabilize the overall force feedback. It isn't
    ////////////////////like we are getting direct impedance matching from the
    ////////////////////omni anyway
    state_->force[0] = f_out.vector.x - 0.001 * state_->velocity[0];
    state_->force[1] = f_out.vector.y - 0.001 * state_->velocity[1];
    state_->force[2] = f_out.vector.z - 0.001 * state_->velocity[2];

    // TODO torque should be split back to gimbal axes
    state_->torque[0] = t_out.vector.x;
    state_->torque[1] = t_out.vector.y;
    state_->torque[2] = t_out.vector.z;
  }

  void publish_phantom_state()
  {
    // Construct transforms
    tf::Transform l0, sensable;
    // Distance from table top to first intersection of the axes<commit_msg>Catch tf exceptions<commit_after>/*
 * See ../LICENSE.md for original copyright and license.
 */

/*
 * Copyright (c) 2013, <NAME>, BioRobotics Lab at KoreaTech
 * All right reserved.
 *
 * Based on original code from Healthcare Robotics Lab at Georgia Tech
 *
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/WrenchStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf/transform_listener.h>

#include <string.h>
#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <sstream>

#include <HL/hl.h>
#include <HD/hd.h>
#include <HDU/hduError.h>
#include <HDU/hduVector.h>
#include <HDU/hduMatrix.h>
#include <HDU/hduQuaternion.h>

#include "sensable_phantom/PhantomButtonEvent.h"
#include <pthread.h>

float prev_time;

struct PhantomState
{
  hduVector3Dd position; //3x1 vector of position
  hduVector3Dd velocity; //3x1 vector of velocity
  hduVector3Dd inp_vel1; //3x1 history of velocity used for filtering velocity estimate
  hduVector3Dd inp_vel2;
  hduVector3Dd inp_vel3;
  hduVector3Dd out_vel1;
  hduVector3Dd out_vel2;
  hduVector3Dd out_vel3;
  hduVector3Dd pos_hist1; //3x1 history of position used for 2nd order backward difference estimate of velocity
  hduVector3Dd pos_hist2;
  hduVector3Dd rot;
  hduVector3Dd joints;
  hduVector3Dd force; //3 element double vector force[0], force[1], force[2]
  hduVector3Dd torque; //3 element double vector torque[0], torque[1], torque[2]

  hduMatrix hd_cur_transform;

  float thetas[7];
  int buttons[2];
  int buttons_prev[2];
  bool lock;
  hduVector3Dd lock_pos;
};

class PhantomROS
{

public:
  ros::NodeHandlePtr node_;
  ros::NodeHandlePtr pnode_;

  ros::Publisher pose_publisher_;

  ros::Publisher button_publisher_;
  ros::Subscriber wrench_sub_;
  std::string base_link_name_;
  std::string sensable_frame_name_;
  std::string link_names_[7];

  std::string tf_prefix_;
  double table_offset_;
  bool locked_;

  PhantomState *state_;
  tf::TransformBroadcaster br_;
  tf::TransformListener ls_;

  PhantomROS() : table_offset_(0.0), locked_(false), state_(NULL)
  {
  }

  int init(PhantomState *s)
  {
    if(!s)
    {
      ROS_FATAL("Internal error. PhantomState is NULL.");
      return -1;
    }

    node_ = ros::NodeHandlePtr(new ros::NodeHandle);
    pnode_ = ros::NodeHandlePtr(new ros::NodeHandle("~"));
    pnode_->param(std::string("tf_prefix"), tf_prefix_, std::string(""));

    // Vertical displacement from base_link to link_0. Defaults to Omni offset.
    pnode_->param(std::string("table_offset"), table_offset_, .135);

    // On startup device will generate forces to hold end-effector at origin.
    pnode_->param(std::string("locked"), locked_, false);

    //Frame attached to the base of the phantom (NAME/base_link)
    base_link_name_ = "base_link";

    //Publish on NAME/pose
    std::string pose_topic_name = "pose";
    pose_publisher_ = node_->advertise<geometry_msgs::PoseStamped>(pose_topic_name, 100);

    //Publish button state on NAME/button
    std::string button_topic = "button";
    button_publisher_ = node_->advertise<sensable_phantom::PhantomButtonEvent>(button_topic, 100);

    //Subscribe to NAME/force_feedback
    std::string force_feedback_topic = "force_feedback";
    wrench_sub_ = node_->subscribe(force_feedback_topic, 100, &PhantomROS::wrench_callback, this);

    //Frame of force feedback (NAME/sensable_origin)
    sensable_frame_name_ = "sensable_origin";

    for (int i = 0; i < 7; i++)
    {
      std::ostringstream stream1;
      stream1 << "link_" << i;
      link_names_[i] = std::string(stream1.str());
    }

    state_ = s;
    state_->buttons[0] = 0;
    state_->buttons[1] = 0;
    state_->buttons_prev[0] = 0;
    state_->buttons_prev[1] = 0;
    hduVector3Dd zeros(0, 0, 0);
    state_->velocity = zeros;
    state_->inp_vel1 = zeros; //3x1 history of velocity
    state_->inp_vel2 = zeros; //3x1 history of velocity
    state_->inp_vel3 = zeros; //3x1 history of velocity
    state_->out_vel1 = zeros; //3x1 history of velocity
    state_->out_vel2 = zeros; //3x1 history of velocity
    state_->out_vel3 = zeros; //3x1 history of velocity
    state_->pos_hist1 = zeros; //3x1 history of position
    state_->pos_hist2 = zeros; //3x1 history of position
    state_->lock = locked_;
    state_->lock_pos = zeros;
    state_->hd_cur_transform = hduMatrix::createTranslation(0, 0, 0);

    return 0;
  }

  /*******************************************************************************
   ROS node callback.
   *******************************************************************************/
  void wrench_callback(const geometry_msgs::WrenchStampedConstPtr& wrench)
  {
    // Both force and torque supplied in the same coordinate frame
    geometry_msgs::Vector3Stamped f_in, f_out;
    geometry_msgs::Vector3Stamped t_in, t_out;

    t_in.header = f_in.header = wrench->header;

    f_in.vector = wrench->wrench.force;
    t_in.vector = wrench->wrench.torque;

    try
    {
      ls_.transformVector(sensable_frame_name_, f_in, f_out);
      ls_.transformVector(sensable_frame_name_, t_in, t_out);
    }
    catch(tf::TransformException& ex)
    {
      ROS_ERROR("%s", ex.what());
    }

    ////////////////////Some people might not like this extra damping, but it
    ////////////////////helps to stabilize the overall force feedback. It isn't
    ////////////////////like we are getting direct impedance matching from the
    ////////////////////omni anyway
    state_->force[0] = f_out.vector.x - 0.001 * state_->velocity[0];
    state_->force[1] = f_out.vector.y - 0.001 * state_->velocity[1];
    state_->force[2] = f_out.vector.z - 0.001 * state_->velocity[2];

    // TODO torque should be split back to gimbal axes
    state_->torque[0] = t_out.vector.x;
    state_->torque[1] = t_out.vector.y;
    state_->torque[2] = t_out.vector.z;
  }

  void publish_phantom_state()
  {
    // Construct transforms
    tf::Transform l0, sensable;
    // Distance from table top to first intersection of the axes
{'fraction_non_alphanumeric': 0.15351687090817526, 'fraction_numerical': 0.022326674500587545, 'mean_word_length': 3.681728880157171, 'pattern_counts': {'":': 0, '<': 53, '<?xml version=': 0, '>': 111, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2057242', 'n_tokens_mistral': 4259, 'n_tokens_neox': 3945, 'n_words': 972}