Controlling Baxter robot using HTC Vive

edit

Hi

I'm very new to ROS & robotics in general so any help/advice/videos are welcome! I am currently trying to accurately map controller movements (from a HTC Vive device) to a Baxter robot. I only have access to a Linux Ubuntu 16.04 machine so any projects requiring Windows I have probably already tried but failed. However, I suspect what I am trying to achieve is not so far off.

Now I have adapted some code I found online & found a way to extract pose data from the controllers, this is my publisher. I will post the code at the end just in case. But basically running rostopic echo /topicname allows me to see the x, y, z points and qx, qy, qz, qw co-ordinates. It seems to be right but how can I double check that these co-ordinates are accurate?

My second question involves the actual mapping to Baxter. I'm struggling to find resources to learn about how mapping actually works. I've gone through the ROS tutorials & read a lot about the IK services that Baxter offers. I assume that is what I need. I know I will need to create a subscriber (possibly 2) for each controller. But I'm still very misty on the process of actually using the Pose data (x ,y, z & quaternions) & how this is used to map the position for the Baxter robot. Can anyone point me in the right direction of what steps I should be taking? or any resources online is appreciated too. Thank you!

Code for controllers: (I've also got a main.py script which imports from this & creates a HTCViveTracking using a vr system object accessed through openvr)

#!/usr/bin/env python

import math

# OpenVR package
import openvr

# ROS packages
import rospy
import tf

# ROS messages
from sensor_msgs.msg import Joy
from geometry_msgs.msg import Point, Pose, Quaternion

# Constants
topic_name = "openvr"

# --- adapted from triad_openvr --- #
#Convert the standard 3x4 position/rotation matrix to a x,y,z location and the appropriate Quaternion
def convert_to_quaternion(pose_mat):
    # Per issue #2, adding a abs() so that sqrt only results in real numbers
    r_w = math.sqrt(abs(1+pose_mat[0][0]+pose_mat[1][1]+pose_mat[2][2]))/2
    r_x = (pose_mat[2][1]-pose_mat[1][2])/(4*r_w)
    r_y = (pose_mat[0][2]-pose_mat[2][0])/(4*r_w)
    r_z = (pose_mat[1][0]-pose_mat[0][1])/(4*r_w)

    x = pose_mat[0][3]
    y = pose_mat[1][3]
    z = pose_mat[2][3]
    return [x,y,z,r_w,r_x,r_y,r_z]

def openvr_state_to_ros_joy(pControllerState, timestamp, device_name):
    """ 
    Converts an OpenVR pControllerState (https://github.com/ValveSoftware/openvr/wiki/IVRSystem::GetControllerState)
    for the HTC Vive Controller (https://docs.unity3d.com/560/Documentation/Manual/OpenVRControllers.html)
    to a 
    ROS Joy MSG (http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html)
    """

    # See http://docs.ros.org/api/sensor_msgs/html/msg/Joy.htm
    joy_msg = Joy()

    # Standard ROS header msg http://docs.ros.org/api/std_msgs/html/msg/Header.html
    joy_msg.header.frame_id = device_name
    joy_msg.header.stamp = timestamp

    # the x axis ...