Omega haptic device control the UR robot arm

1. ros program of Omega

HapticDevice.h:

#ifndef HAPTIC_DEVICE_H__
#define HAPTIC_DEVICE_H__

#include "ros/ros.h"
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/TwistStamped.h>
#include <boost/thread/thread.hpp>
#include <mutex>
#include <thread>
#include <vector>

#include "drdc.h"


class HapticDevice
{
	public:
        HapticDevice(ros::NodeHandle& node, float loop_rate, bool set_force);
        virtual ~HapticDevice();

        void PublishHapticData();
        void RegisterCallback();

        void GetHapticDataRun();
		
        void SetForce(double x, double y, double z);

        void SetForceLimit(double x, double y, double z);
        void VerifyForceLimit(double input_force[], std::vector<double> & output);
        void ForceCallback(const geometry_msgs::Vector3::ConstPtr &data);

//        void gravityCompensation();

        void Start();

    protected:
       std::shared_ptr<boost::thread> dev_op_thread_;
       std::shared_ptr<boost::thread> gravityCompensa_thread_;

	private:
       ros::NodeHandle nh_;
       ros::Rate loop_rate_;
       int device_count_;
       int dev_id_;
       bool set_force_;
       bool keep_alive_=false;
       bool device_enabled_ = false;
       std::mutex val_lock_;
       bool force_released_;
       double button0_state_=0.0;
       bool button1_state_=false;

       ros::Publisher position_pub_;
       ros::Publisher velocity_pub_;
       ros::Publisher button_state_pub_;
       ros::Subscriber force_sub_;
       double position_[7];
       double velocity_[7];
       double angle;

       std::string position_topic_;
       std::string velocity_topic_;
       std::string buttons_topic_;
       std::string force_topic_;

       double force_x_limit_;
       double force_y_limit_;
       double force_z_limit_;
       std::vector<double> force_;
       double p[DHD_MAX_DOF];
       double v[DHD_MAX_DOF];
       double r[3][3];
       std::mutex mu;
};

#endif

HapticDevice.cpp:

#include "omega/HapticDevice.h"

// haptic device API
#include "omega/dhdc.h"
#include "std_msgs/Int8MultiArray.h"
#include "std_msgs/Float64MultiArray.h"
#include "eigen3/Eigen/Eigen"
#include <boost/bind.hpp>

#define REFRESH_INTERVAL  0.1   // sec

#define KP    100.0
#define KVP    10.0
#define MAXF    4.0
#define KR      0.3
#define KWR     0.02
#define MAXT    0.1
#define KG    100.0
#define KVG     5.0
#define MAXG    1.0

using namespace Eigen;

HapticDevice::HapticDevice(ros::NodeHandle & node, float loop_rate, bool set_force): loop_rate_(loop_rate)
{
    nh_ = node;

    dev_id_ = -2; // we set a value not in API defined range
    device_enabled_ = -1;

    set_force_ = set_force;

    for (int i = 0; i<7;i++) {
        velocity_[i] = position_[i] = 0.0;
    }
    for (int i = 0; i<DHD_MAX_DOF;i++) {
        v[i] = p[i] = 0.0;
    }

    button0_state_ = false;
    keep_alive_ = false;
    force_released_ = true;

    force_.resize(3);
    force_[0] = 0.0;
    force_[1] = 0.0;
    force_[2] = 0.0;

    SetForceLimit(3.0, 3.0, 3.0);

    // connect to hardware device
    device_count_ = dhdGetDeviceCount();

    // we only accept one haptic device.
    if ( device_count_ >= 1) {
        dev_id_ = dhdOpenID(0); // if open failed, we will get -1, and sucess with 0.
        if ( dev_id_ < 0) {
            ROS_INFO("error: handler device: %s\n", dhdErrorGetLastStr());
            device_enabled_ = false;
            return;
        }
    } else {
        ROS_INFO("No handler device find! %s\n", dhdErrorGetLastStr());
        device_enabled_ = false;
        return;
    }

    device_enabled_ =true;
}


HapticDevice::~HapticDevice()
{
    dev_id_ = -1;
    device_count_ = 0;
    keep_alive_ = false;
    if (dev_op_thread_)
    {
        dev_op_thread_->join();
    }
}

void HapticDevice::PublishHapticData()
{
    // geometry_msgs::Vector3Stamped pos;
    geometry_msgs::TwistStamped pos;
    pos.header.frame_id = ros::this_node::getName();
    pos.header.stamp = ros::Time::now();
    pos.twist.linear.x = position_[0];
    pos.twist.linear.y = position_[1];
    pos.twist.linear.z = position_[2];
    pos.twist.angular.x = position_[3];
    pos.twist.angular.y = position_[4];
    pos.twist.angular.z = position_[5];

    // geometry_msgs::Vector3Stamped vec;
    geometry_msgs::TwistStamped vec;
    vec.header.frame_id = ros::this_node::getName();
    vec.header.stamp = ros::Time::now();
    vec.twist.linear.x = velocity_[0];
    vec.twist.linear.y = velocity_[1];
    vec.twist.linear.z = velocity_[2];
    vec.twist.angular.x = velocity_[3];
    vec.twist.angular.y = velocity_[4];
    vec.twist.angular.z = velocity_[5];

    std_msgs::Float64MultiArray button_stat;
    button_stat.data.push_back(button0_state_);

    position_pub_.publish(pos);
    velocity_pub_.publish(vec);
    button_state_pub_.publish(button_stat);
}

void HapticDevice::RegisterCallback()
{
    position_topic_ = "/haptic/position";
    buttons_topic_ = "/haptic/button_state";
    force_topic_ = "/haptic/force";
    velocity_topic_ = "/haptic/velocity";

    position_pub_ = nh_.advertise<geometry_msgs::TwistStamped>(position_topic_.c_str(),1);
    velocity_pub_ = nh_.advertise<geometry_msgs::TwistStamped>(velocity_topic_.c_str(),1);
    button_state_pub_ = nh_.advertise<std_msgs::Float64MultiArray>(buttons_topic_.c_str(), 1);
    force_sub_ = nh_.subscribe<geometry_msgs::Vector3>(force_topic_.c_str(),1, &HapticDevice::ForceCallback,this);
}

void HapticDevice::ForceCallback(const geometry_msgs::Vector3::ConstPtr &data)
{
    // wrapper force
    SetForce(data->x, data->y, data->z);
}

void HapticDevice::GetHapticDataRun()
{   // get and we will publish immediately

    if (drdOpen () < 0) {
        printf ("error: cannot open device (%s)\n", dhdErrorGetLastStr ());
        dhdSleep (2.0);
      }

      // print out device identifier
      if (!drdIsSupported ()) {
        printf ("unsupported device\n");
        printf ("exiting...\n");
        dhdSleep (2.0);
        drdClose ();
      }
      printf ("%s haptic device detected\n\n", dhdGetSystemName ());

      // perform auto-initialization
      if (!drdIsInitialized () && drdAutoInit () < 0) {
        printf ("error: auto-initialization failed (%s)\n", dhdErrorGetLastStr ());
        dhdSleep (2.0);
      }
      else if (drdStart () < 0) {
        printf ("error: regulation thread failed to start (%s)\n", dhdErrorGetLastStr ());
        dhdSleep (2.0);
      }
      double nullPose[DHD_MAX_DOF] = { 0.0, 0.0, 0.0,  // translation
                                       0.0, 0.0, 0.0,  // rotation (joint angles)
                                       0.0 };          // gripper
      //move to center
      drdMoveTo (nullPose);
      drdSetForceAndTorqueAndGripperForce (0.0, 0.0, 0.0,  // force
                                             0.0, 0.0, 0.0,  // torque
                                             0.0);           // gripper force
      drdRegulatePos  (false);
      drdRegulateRot  (false);
      drdRegulateGrip (false);

      dhdEnableForce (DHD_ON);

    double feed_force[3] = {0.0, 0.0, 0.0};

    ros::spinOnce();

    while (ros::ok() && (keep_alive_ == true)) {

        if (device_count_ >= 1 && dev_id_ >= 0) {
//                dhdGetPosition(&current_position[0], &current_position[1], &current_position[2]);
                drdWaitForTick ();
                drdGetPositionAndOrientation (&(p[0]), &(p[1]), &(p[2]),
                                              &(p[3]), &(p[4]), &(p[5]),
                                              &(p[6]), r);
                // dhdGetGripperAngleDeg(angle);
                // get position/orientation/gripper velocity
                drdGetVelocity (&(v[0]), &(v[1]), &(v[2]),
                                &(v[3]), &(v[4]), &(v[5]),
                                &(v[6]));
                position_[0] = p[0];
                position_[1] = p[1];
                position_[2] = p[2];
                position_[3] = p[3];
                position_[4] = p[4];
                position_[5] = p[5];
                position_[6] = p[6];

                velocity_[0] = v[0];
                velocity_[1] = v[1];
                velocity_[2] = v[2];
                velocity_[3] = v[3];
                velocity_[4] = v[4];
                velocity_[5] = v[5];
                velocity_[6] = v[6];
                button0_state_ = p[6];
                ROS_INFO("position is: %f ,%f ,%f\n %f ,%f ,%f, %f", position_[0], position_[1], position_[2],position_[3],p[4],p[5],p[6]);
        }

        PublishHapticData();

        // apply force
        if (set_force_) {
//            val_lock_.lock();
            feed_force[0] = force_[0];
            feed_force[1] = force_[1];
            feed_force[2] = force_[2];
//            dhdSetForce(feed_force[0], feed_force[1], feed_force[2]);
            drdSetForceAndTorqueAndGripperForce (feed_force[0], feed_force[1], feed_force[2],  // force
                                                   0.0, 0.0, 0.0,  // torque
                                                   0.0);           // gripper force
            ROS_INFO("Force is: %f ,%f ,%f", feed_force[0], feed_force[1], feed_force[2]);
//            val_lock_.unlock();
        }


        ros::spinOnce();
        loop_rate_.sleep();
    }

}

void HapticDevice::SetForce(double x, double y, double z)
{
    double input_force[3] = {0.0, 0.0, 0.0};

    if (set_force_)
    {
        val_lock_.lock();
        input_force[0] = x;
        input_force[1] = y;
        input_force[2] = z;
        VerifyForceLimit(input_force, force_);
        force_released_ = false;
        val_lock_.unlock();
    }
}

void HapticDevice::SetForceLimit(double x, double y, double z)
{
    force_x_limit_ = x;
    force_y_limit_ = y;
    force_z_limit_ = z;
}


void HapticDevice::VerifyForceLimit(double input_force[], std::vector<double> & output)
{

    if (output.size() != 3) {
        output.resize(3);
    }

    output[0] = input_force[0];
    output[1] = input_force[1];
    output[2] = input_force[2];

    if (input_force[0] < -force_x_limit_) output[0] = -force_x_limit_;
    if (input_force[1] < -force_y_limit_) output[1] = -force_y_limit_;
    if (input_force[2] < -force_z_limit_) output[2] = -force_z_limit_;

    if (input_force[0] > force_x_limit_) output[0] = force_x_limit_;
    if (input_force[1] > force_y_limit_) output[1] = force_y_limit_;
    if (input_force[2] > force_z_limit_) output[2] = force_z_limit_;
}


void HapticDevice::Start()
{   
    if (!device_enabled_)
    {
        return; 
    }
    

    RegisterCallback();
    ros::AsyncSpinner spinner(2);
    spinner.start();

    dev_op_thread_ = std::make_shared<boost::thread>(boost::bind(&HapticDevice::GetHapticDataRun, this));

//    gravityCompensa_thread_ = std::make_shared<boost::thread>(boost::bind(&HapticDevice::gravityCompensation, this));

    keep_alive_ = true;

    while (ros::ok() && (keep_alive_ == true)) {
        ros::Duration(0.001).sleep();
        // ROS_INFO("working in main loop");
        //  usleep(1000);
    }

    keep_alive_ = false;
    spinner.stop();
}

2. Control the UR robot

We can subscribe the topic from the omega device, define corresponding callback functions to obtain the real-time values of haptic position, velocity and clipper angle etc.

#include "ros/ros.h"
#include "function/robot.h"
#include <iostream>
#include <fstream>
#include <geometry_msgs/WrenchStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include "std_msgs/Float64MultiArray.h"

using namespace std;

Eigen::Vector3d Pe(0,0,0.180);

geometry_msgs::TwistStamped omeInitPosition;
geometry_msgs::TwistStamped omePosition;
geometry_msgs::TwistStamped velocity_;
geometry_msgs::Vector3 forceOmega;
Eigen::Vector3d previousImpose;

double angle(0);

void OmegaPositionCallback (const geometry_msgs::TwistStamped position){
    omePosition = position;
}

void OmegaVelocityCallback (const geometry_msgs::TwistStamped vel){
    velocity_ = vel;
}

void OmegaAngluarCallback (const std_msgs::Float64MultiArray angle_){
    angle = angle_.data[0];
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "tracking");
  ros::NodeHandle nh;

  ros::Publisher force_pub = nh.advertise<geometry_msgs::Vector3>("/haptic/force", 10);
  ros::Subscriber omegp_sub = nh.subscribe("/haptic/position",1,OmegaPositionCallback);
  ros::Subscriber omegv_sub = nh.subscribe("/haptic/velocity",1,OmegaVelocityCallback); //"/haptic/button_state";
  ros::Subscriber omegAngle_sub = nh.subscribe("/haptic/button_state",1,OmegaAngluarCallback);

  robot ur_(nh,false);
  ur_.setGravity(0.180);
  ur_.setBias(Pe[0],Pe[1],Pe[2]);
  // ur_.setMBK(20,500,89.44);

  ros::Rate loop_rate(1./TIME_STEP);

  while (ros::ok())
  {
    while(!ur_.isOk()){
      ros::spinOnce();
      loop_rate.sleep();
      cout<<"Wait fo initiation..."<<endl;
    }
    //Count the processing time.
    auto start = std::chrono::high_resolution_clock::now();

    printf("-----------------------------step information-----------------------------\n");

    //Obtain the joint states from topic.
    ur_.forward();

    Eigen::Vector3d f_ = ur_.getForce();
    Eigen::Vector3d force = ur_.mapFromHandToWorld(f_) + ur_.getGravity();

    static bool setForce = false;
    static Eigen::Vector3d static_force;
    static Eigen::Matrix4d init_pos;
    if(!setForce){
        static_force=force;
        setForce = true;
        init_pos = ur_.getTransformation();
    }

    Eigen::Vector3d incrementF = (force-static_force);
    Eigen::Vector3d exterForce(0,0,0);

		//impedance control
    ur_.impedenceControlXYZ(incrementF);
    Eigen::Vector3d xpose = ur_.getImpPose();

    forceOmega.x = 120*xpose[0];
    forceOmega.y = 120*xpose[1];
    forceOmega.z = 120*xpose[2];
    force_pub.publish(forceOmega);
    cout<<forceOmega.x<<" "<<forceOmega.y<<" "<<forceOmega.z<<endl;
    cout<<incrementF.transpose()<<endl;
    

    Eigen::VectorXd vel(6,1);
    vel.head(3)<< velocity_.twist.linear.x *0.06,velocity_.twist.linear.y*0.06,velocity_.twist.linear.z *0.06;
    // vel.tail(3)<< 0,0,0;
    // Eigen::VectorXd velocity = ur_.calculateJointSpeed(vel);

    Eigen::Matrix4d Tu = ur_.getTransformation();
    previousImpose = xpose;
    Tu.col(3).head(3) = ur_.getTranslation()+vel.head(3)+xpose;
    double q_sols[8*6];
    double *q;
    q = ur_.getJointsStatus();

    ur_.inverKinematic(Tu,q_sols);

    // tip rotation rx ry rz 
    Eigen::VectorXd twistgy(6,1);
    twistgy.head(3)<<0,0,0;
    twistgy.tail(3)<<velocity_.twist.angular.x*0.05,velocity_.twist.angular.y*0.05,velocity_.twist.angular.z*0.05;
    ur_.jacobian();
    Eigen::VectorXd velIncrement = ur_.calculateJointSpeed(twistgy);

    // printf("%.4f %.4f %.4f\n",velocity[0],velocity[1],velocity[2]);
    ur_.setDestinationObsolute(q_sols);
    ur_.setDestinationDelta(velIncrement);
    printf("%.4f %.4f %.4f %.4f %.4f %.4f\n",q[0]-q_sols[0],q[1]-q_sols[1],q[2]-q_sols[2],q[3]-q_sols[3],q[4]-q_sols[4],q[5]-q_sols[5]);
    cout<<velIncrement.transpose()<<endl;
    // //move robot
    if(angle>0.015) ur_.stepForward();

    ros::spinOnce();
    loop_rate.sleep();

    //Finish Code to be timed.
    auto end = std::chrono::high_resolution_clock::now();
    std::chrono::duration<double, std::milli> elapsed = end - start;
    printf("Elapsed time: %.5f  ms\n", elapsed.count());
  }
  return 0;
}

Reference

https://github.com/zouyuelin/urRemoteControl/tree/omega_