<commit_before>#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Wrench.h>
#include <geometry_msgs/WrenchStamped.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 "phantom_omni/PhantomButtonEvent.h"
#include "phantom_omni/OmniFeedback.h"
#include <pthread.h>

float prev_time;

struct OmniState
{
    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]
    float thetas[7];
    int buttons[2];
    int buttons_prev[2];
    bool lock;
    hduVector3Dd lock_pos;
};


class PhantomROS {

	public:
	ros::NodeHandle n;
	ros::Publisher pose_publisher;
	//ros::Publisher omni_pose_publisher;

    ros::Publisher button_publisher;
	ros::Subscriber haptic_sub;
    std::string omni_name;
    std::string sensable_frame_name;
    std::string link_names[7];

    OmniState *state;
    tf::TransformBroadcaster br;

    void init(OmniState *s) 
    {
        ros::param::param(std::string("~omni_name"), omni_name, std::string("omni1"));

        //Publish on NAME_pose
        std::ostringstream stream00;
        stream00 << omni_name << "_pose";
        std::string pose_topic_name = std::string(stream00.str());
        pose_publisher = n.advertise<geometry_msgs::PoseStamped>(pose_topic_name.c_str(), 100);
        //omni_pose_publisher = n.advertise<geometry_msgs::PoseStamped>("omni_pose_internal", 100);

        //Publish button state on NAME_button
        std::ostringstream stream0;
        stream0 << omni_name << "_button";
        std::string button_topic = std::string(stream0.str());
        button_publisher = n.advertise<phantom_omni::PhantomButtonEvent>(button_topic.c_str(), 100);

        //Subscribe to NAME_force_feedback
        std::ostringstream stream01;
        stream01 << omni_name << "_force_feedback";
        std::string force_feedback_topic = std::string(stream01.str());
        haptic_sub = n.subscribe(force_feedback_topic.c_str(), 100, &PhantomROS::force_callback, this);

        //Frame of force feedback (NAME_sensable)
        std::ostringstream stream2;
        stream2 << omni_name << "_sensable";
        sensable_frame_name = std::string(stream2.str());

        for (int i = 0; i < 7; i++)
        {
            std::ostringstream stream1;
            stream1 << omni_name << "_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 = true;
	state->lock_pos = zeros;
    }

    /*******************************************************************************
     ROS node callback.  
    *******************************************************************************/
  //    void force_callback(const geometry_msgs::WrenchConstPtr& wrench)
    void force_callback(const phantom_omni::OmniFeedbackConstPtr& omnifeed)
    {
      ////////////////////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] = omnifeed->force.x - 0.001*state->velocity[0];
	state->force[1] = omnifeed->force.y - 0.001*state->velocity[1];
	state->force[2] = omnifeed->force.z - 0.001*state->velocity[2];

//      state->force[0] = omnifeed->force.x;
//	state->force[1] = omnifeed->force.y;
//	state->force[2] = omnifeed->force.z;

	state->lock_pos[0] = omnifeed->position.x;
	state->lock_pos[1] = omnifeed->position.y;
	state->lock_pos[2] = omnifeed->position.z;
	//        state->force[2] = wrench->force.z;
    } 

    void publish_omni_state()
    {
        //Construct transforms
        tf::Transform l0, sensable, l1, l2, l3, l4, l5, l6, l0_6;
        l0.setOrigin(tf::Vector3(0., 0, 0.15));
        l0.setRotation(tf::Quaternion(0, 0, 0));
        br.sendTransform(tf::StampedTransform(l0, ros::Time::now(), omni_name.c_str(), link_names[0].c_str()));

        sensable.setOrigin(tf::Vector3(0., 0, 0));
        sensable.setRotation(tf::Quaternion(-M_PI/2, 0, M_PI/2));
        br.sendTransform(tf::StampedTransform(sensable, ros::Time::now(), 
			omni_name.c_str(), sensable_frame_name.c_str()));

        l1.setOrigin(tf::Vector3(0., 0, 0.));
        l1.setRotation(tf::Quaternion(-state->thetas[1], 0, 0));
                                                                                                          
        l2.setOrigin(tf::Vector3(0., 0, 0.));                                                           
        l2.setRotation(tf::Quaternion(0, state->thetas[2], 0));                                         
                                                                                                          
        l3.setOrigin(tf::Vector3(-.131, 0, 0.));                                                        
        l3.setRotation(tf::Quaternion(0, state->thetas[3], 0));                            
                                                                                                          
        l4.setOrigin(tf::Vector3(0., 0, -.137));                                                        
        l4.setRotation(tf::Quaternion(state->thetas[4]+M_PI, 0, 0));                       
                                                                                                          
        l5.setOrigin(tf::Vector3(0., 0., 0.));                                                          
        l5.setRotation(tf::Quaternion(0., -state->thetas[5]+M_PI,0));                      

        l6.setOrigin(tf::Vector3(0., 0., 0.));
        l6.setRotation(tf::Quaternion(0.,0, state->thetas[6]+M_PI));
        
        l0_6 = l0 * l1 * l2 * l3 * l4 * l5 * l6;
        //br.sendTransform(tf::StampedTransform(l0_6, ros::Time::now(), link_names[0].c_str(), link_names[6].c_str()));
        //Don't send these as they slow down haptics thread
        //br.sendTransform(tf::StampedTransform(l1, ros::Time::now(), link_names[0].c_str(), link_names[1].c_str()));
        //br.sendTransform(tf::StampedTransform(l2, ros::Time::now(), link_names[1].c_str(), link_names[2].c_str()));
        //br.sendTransform(tf::StampedTransform(l3, ros::Time::now(), link_names[2].c_str(), link_names[3].c_str()));
        //br.sendTransform(tf::StampedTransform(l4, ros::Time::now(), link_names[3].c_str(), link_names[4].c_str()));
        //br.sendTransform(tf::StampedTransform(l5, ros::Time::now(), link_names[4].c_str(), link_names[5].c_str()));
        //br.sendTransform(tf::StampedTransform(link, ros::Time::now(), link_names[5].c_str(), link_names[6].c_str()));
        
        //Sample 'end effector' pose
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.frame_id = link_names[6].c_str();
        pose_stamped.header.stamp = ros::Time::now();
        pose_stamped.pose.position.x = 0.0;   //was 0.03 to end of phantom
        pose_stamped.pose.orientation.w = 1.;
        pose_publisher.publish(pose_stamped);<commit_msg>Fix: link6 transform not published.<commit_after>#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <tf/transform_broadcaster.h>
#include <geometry_msgs/Wrench.h>
#include <geometry_msgs/WrenchStamped.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 "phantom_omni/PhantomButtonEvent.h"
#include "phantom_omni/OmniFeedback.h"
#include <pthread.h>

float prev_time;

struct OmniState
{
    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]
    float thetas[7];
    int buttons[2];
    int buttons_prev[2];
    bool lock;
    hduVector3Dd lock_pos;
};


class PhantomROS {

	public:
	ros::NodeHandle n;
	ros::Publisher pose_publisher;
	//ros::Publisher omni_pose_publisher;

    ros::Publisher button_publisher;
	ros::Subscriber haptic_sub;
    std::string omni_name;
    std::string sensable_frame_name;
    std::string link_names[7];

    OmniState *state;
    tf::TransformBroadcaster br;

    void init(OmniState *s) 
    {
        ros::param::param(std::string("~omni_name"), omni_name, std::string("omni1"));

        //Publish on NAME_pose
        std::ostringstream stream00;
        stream00 << omni_name << "_pose";
        std::string pose_topic_name = std::string(stream00.str());
        pose_publisher = n.advertise<geometry_msgs::PoseStamped>(pose_topic_name.c_str(), 100);
        //omni_pose_publisher = n.advertise<geometry_msgs::PoseStamped>("omni_pose_internal", 100);

        //Publish button state on NAME_button
        std::ostringstream stream0;
        stream0 << omni_name << "_button";
        std::string button_topic = std::string(stream0.str());
        button_publisher = n.advertise<phantom_omni::PhantomButtonEvent>(button_topic.c_str(), 100);

        //Subscribe to NAME_force_feedback
        std::ostringstream stream01;
        stream01 << omni_name << "_force_feedback";
        std::string force_feedback_topic = std::string(stream01.str());
        haptic_sub = n.subscribe(force_feedback_topic.c_str(), 100, &PhantomROS::force_callback, this);

        //Frame of force feedback (NAME_sensable)
        std::ostringstream stream2;
        stream2 << omni_name << "_sensable";
        sensable_frame_name = std::string(stream2.str());

        for (int i = 0; i < 7; i++)
        {
            std::ostringstream stream1;
            stream1 << omni_name << "_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 = true;
	state->lock_pos = zeros;
    }

    /*******************************************************************************
     ROS node callback.  
    *******************************************************************************/
  //    void force_callback(const geometry_msgs::WrenchConstPtr& wrench)
    void force_callback(const phantom_omni::OmniFeedbackConstPtr& omnifeed)
    {
      ////////////////////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] = omnifeed->force.x - 0.001*state->velocity[0];
	state->force[1] = omnifeed->force.y - 0.001*state->velocity[1];
	state->force[2] = omnifeed->force.z - 0.001*state->velocity[2];

//      state->force[0] = omnifeed->force.x;
//	state->force[1] = omnifeed->force.y;
//	state->force[2] = omnifeed->force.z;

	state->lock_pos[0] = omnifeed->position.x;
	state->lock_pos[1] = omnifeed->position.y;
	state->lock_pos[2] = omnifeed->position.z;
	//        state->force[2] = wrench->force.z;
    } 

    void publish_omni_state()
    {
        //Construct transforms
        tf::Transform l0, sensable, l1, l2, l3, l4, l5, l6, l0_6;
        l0.setOrigin(tf::Vector3(0., 0, 0.15));
        l0.setRotation(tf::Quaternion(0, 0, 0));
        br.sendTransform(tf::StampedTransform(l0, ros::Time::now(), omni_name.c_str(), link_names[0].c_str()));

        sensable.setOrigin(tf::Vector3(0., 0, 0));
        sensable.setRotation(tf::Quaternion(-M_PI/2, 0, M_PI/2));
        br.sendTransform(tf::StampedTransform(sensable, ros::Time::now(), 
			omni_name.c_str(), sensable_frame_name.c_str()));

        l1.setOrigin(tf::Vector3(0., 0, 0.));
        l1.setRotation(tf::Quaternion(-state->thetas[1], 0, 0));
                                                                                                          
        l2.setOrigin(tf::Vector3(0., 0, 0.));                                                           
        l2.setRotation(tf::Quaternion(0, state->thetas[2], 0));                                         
                                                                                                          
        l3.setOrigin(tf::Vector3(-.131, 0, 0.));                                                        
        l3.setRotation(tf::Quaternion(0, state->thetas[3], 0));                            
                                                                                                          
        l4.setOrigin(tf::Vector3(0., 0, -.137));                                                        
        l4.setRotation(tf::Quaternion(state->thetas[4]+M_PI, 0, 0));                       
                                                                                                          
        l5.setOrigin(tf::Vector3(0., 0., 0.));                                                          
        l5.setRotation(tf::Quaternion(0., -state->thetas[5]+M_PI,0));                      

        l6.setOrigin(tf::Vector3(0., 0., 0.));
        l6.setRotation(tf::Quaternion(0.,0, state->thetas[6]+M_PI));
        
        l0_6 = l0 * l1 * l2 * l3 * l4 * l5 * l6;
        br.sendTransform(tf::StampedTransform(l0_6, ros::Time::now(), link_names[0].c_str(), link_names[6].c_str()));
        //Don't send these as they slow down haptics thread
        //br.sendTransform(tf::StampedTransform(l1, ros::Time::now(), link_names[0].c_str(), link_names[1].c_str()));
        //br.sendTransform(tf::StampedTransform(l2, ros::Time::now(), link_names[1].c_str(), link_names[2].c_str()));
        //br.sendTransform(tf::StampedTransform(l3, ros::Time::now(), link_names[2].c_str(), link_names[3].c_str()));
        //br.sendTransform(tf::StampedTransform(l4, ros::Time::now(), link_names[3].c_str(), link_names[4].c_str()));
        //br.sendTransform(tf::StampedTransform(l5, ros::Time::now(), link_names[4].c_str(), link_names[5].c_str()));
        //br.sendTransform(tf::StampedTransform(link, ros::Time::now(), link_names[5].c_str(), link_names[6].c_str()));
        
        //Sample 'end effector' pose
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.frame_id = link_names[6].c_str();
        pose_stamped.header.stamp = ros::Time::now();
        pose_stamped.pose.position.x = 0.0;   //was 0.03 to end of phantom
        pose_stamped.pose.orientation.w = 1.;
        pose_publisher.publish(pose_stamped);
{'fraction_non_alphanumeric': 0.1645357868323575, 'fraction_numerical': 0.03112677739275899, 'mean_word_length': 2.273171685569026, 'pattern_counts': {'":': 0, '<': 87, '<?xml version=': 0, '>': 129, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 56, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2676359', 'n_tokens_mistral': 5708, 'n_tokens_neox': 5327, 'n_words': 971}