Some question about the code to calculate the velocity

CMakeLists.txt → geomagic_control/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 2.8.3)
-project(phantom_omni)
+project(geomagic_control)
 
 find_package(catkin REQUIRED COMPONENTS roscpp std_msgs geometry_msgs genmsg rosconsole tf urdf)
 
@@ -13,6 +13,6 @@ include_directories(${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
 
 add_executable(omni src/omni.cpp)
 
-target_link_libraries(omni HD HDU rt ncurses ${catkin_LIBRARIES})
+target_link_libraries(omni HD HL HDU rt ncurses ${catkin_LIBRARIES})
 
 

geomagic_control/README.md

@@ -0,0 +1,27 @@
+phantom_omni
+============
+
+ROS Node for Geomagic Touch ethernet devices.
+
+Device name and publish rate can be adjusted in the geomagic_headless.launch file.
+
+Publishes:
+- OMNI_NAME_joint_states (sensor_msgs/JointState): The state of each of the omni's joints.
+- OMNI_NAME_button (phantom_omni/PhantomButtonEvent): Events for the grey and white buttons.
+
+Subscribes:
+- OMNI_NAME_force_feedback (phantom_omni/OmniFeedback): Force feedback to be displayed on the omni. Takes a force and a position. If you simultaneously click the grey and white buttons, the omni will 'lock' to the position.
+
+This is based on the [package of Dane Powell](https://github.com/danepowell/phantom_omni). However, it has several advantages:
+- Compatible with Ubuntu 14.04LTS and ROS Indigo
+- Uses the more beautiful URDF model from [Francisco](https://github.com/fsuarez6/phantom_omni/tree/hydro-devel/omni_description)
+
+To see it in action, simply:
+```
+roslaunch geomagic_control geomagic.launch
+```
+
+In case you have some troubles and your first 3 values are 0, you probably don't have English set as main language. Thats a problem coming from Open Haptics. Simply add this to your ~/.bashrc
+```
+export LC_ALL=en_US.UTF-8
+```

cfg/omni.rviz → geomagic_control/cfg/omni.rviz

@@ -95,7 +95,7 @@ Visualization Manager:
           Show Trail: false
           Value: true
       Name: RobotModel
-      Robot Description: omni_robot_description
+      Robot Description: Geomagic_robot_description
       TF Prefix: ""
       Update Interval: 0
       Value: true

geomagic_control/launch/geomagic.launch

@@ -0,0 +1,6 @@
+<launch>
+  <include file="$(find geomagic_control)/launch/geomagic_headless.launch" />
+  <!-- rviz just lets you see the end result :) -->
+  <param name="Geomagic_robot_description" command="cat $(find geomagic_description)/urdf/omni.urdf" />
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find geomagic_control)/cfg/omni.rviz" required="true" />
+</launch>

geomagic_control/launch/geomagic_headless.launch

@@ -0,0 +1,24 @@
+<launch>
+
+   <arg name="device_name" default="Geomagic"/>
+   <param name="publish_rate" type="int" value="100" />
+
+
+   <group ns="$(arg device_name)">
+	<!-- omni node publishes joint states on omni1_joint_states -->
+        <node name="$(arg device_name)" pkg="geomagic_control" type="omni" output="screen">
+            <param name="device_name" type="str" value="$(arg device_name)"/>
+
+        </node>
+
+	<!-- robot_description tells robot_state_publisher where to find the omni's urdf description -->
+        <param name="$(arg device_name)_robot_description" command="cat $(find geomagic_description)/urdf/omni.urdf" />
+
+	<!-- robot_state_publisher reads Geomagic_joint_states and publishes corresponding tfs -->
+        <node name="$(arg device_name)_robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher">
+	  <!--<remap from="joint_states" to="$(arg device_name)/joint_states" />-->
+	  <remap from="robot_description" to="$(arg device_name)_robot_description" />
+	</node>
+	</group>
+</launch>
+

msg/OmniFeedback.msg → geomagic_control/msg/OmniFeedback.msg

@@ -1,4 +1,5 @@
 # This is the force as estimated from the applied torques as well as the current
 # end effector position of the robot arm                           
 geometry_msgs/Vector3  force                                                                  
-geometry_msgs/Vector3  position  
+geometry_msgs/Vector3  position 
+bool[] lock 

package.xml → geomagic_control/package.xml

@@ -1,10 +1,10 @@
 <package>
-  <name>phantom_omni</name>
+  <name>geomagic_control</name>
   <version>1.0.0</version>
   <description>
-     ROS Node for Sensable Phantom Omni devices.
+     ROS Node for Geomagic devices.
   </description>
-  <maintainer email="git@danepowell.com">Dane Powell</maintainer>
+  <maintainer email="sb@generationrobots.com">Sven Bock</maintainer>
   <author>Hai Nguyen, Marc Killpack, Chi-Hung King, Advisor: Prof. Charlie Kemp, Lab: Healthcare Robotics Lab at Georgia Tech</author>
   <license>new BSD</license>
 
@@ -30,7 +30,7 @@
   <run_depend>rosconsole</run_depend>
   <run_depend>urdf</run_depend>
   <run_depend>sensor_msgs</run_depend>
-  <run_depend>omni_description</run_depend>
+  <run_depend>geomagic_description</run_depend>
 </package>
 
 

rosdep.yaml → geomagic_control/rosdep.yaml

@@ -24,5 +24,4 @@ libgl1-mesa-dev:
     ubuntu: libgl1-mesa-dev
 libglw1-mesa-dev:
     ubuntu: libglw1-mesa-dev
-libraw1394-dev:
-    ubuntu: libraw1394-dev
+

geomagic_control/scripts/lockDoF.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python
+# THIS SCRIPT WAS JUST TO TEST STUFF AND SHOULD NOT  BE USED ANY MORE
+import rospy
+import math
+from sensor_msgs.msg import JointState
+from geomagic_control.msg import OmniFeedback
+import tf
+from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
+from std_msgs.msg import Header
+def PS(origin,position=[0,0,0],orientation=[0,0,0,1]):
+    """
+        Creates a PoseStamped()
+
+        :param origin: frame id
+        :type: origin: str
+        :param position: position (default [0,0,0])
+        :type: position: list
+        :param oriention: orientation (default [0,0,0,1])
+        :type: orientation: list
+        :return: the created PoseStamped()
+        :rtype: PoseStamped
+    """
+    h=Header()
+    h.frame_id=origin
+    h.stamp=rospy.Time().now()
+
+    p=Pose()
+    if type(position) == Point:
+        p.position = position
+    else:
+        p.position=Point(*position)
+    if type(orientation) == Quaternion:
+        p.orientation = orientation 
+    elif len(orientation) == 4:
+        p.orientation=Quaternion(*orientation)
+    elif len(orientation) == 3:
+        p.orientation = Quaternion(*tf.transformations.quaternion_from_euler(*orientation))
+    else:
+        p.orientation = Quaternion(0,0,0,1)
+    return PoseStamped(h,p)
+class GeomagicInterface:
+    def __init__(self):
+#         self.listener = tf.TransformListener()
+        self.of = OmniFeedback()
+        self.of.lock = [False for i in xrange(3)]
+        self.pub = rospy.Publisher("Geomagic/force_feedback",OmniFeedback,queue_size=1)
+        self.sub = rospy.Subscriber("Geomagic/end_effector_pose",JointState,self.callback, queue_size=1)
+        self.lock = 0.0
+
+    def callback(self,js):
+#         x = js.position[0]
+#         self.of.force.x = 0.04 *(self.lock - x) - 0.001 * js.velocity[0];
+        self.of.position.x = js.position[0]
+        self.of.position.y = js.position[1]
+        self.of.position.z = js.position[2]
+        self.of.lock = [False,True,True]
+        self.pub.publish(self.of) 
+#     def start(self):
+#         while not rospy.is_shutdown():
+#             x = self.getPose("tip", "base").pose.position.x
+#             print self.of.force.x
+#             self.pub.publish(self.of)
+#             rospy.sleep(0.1)
+
+#     def getPose(self,target,source,timeout=1):
+#         now = rospy.Time.now()
+#         end = rospy.Time.now()+rospy.Duration(timeout)
+#         while not rospy.is_shutdown() and now < end:
+#             now = rospy.Time.now()
+#             try:
+#                 (trans,rot) = self.listener.lookupTransform(source,target, rospy.Time(0))
+#                 return PS(source,trans,rot)
+#             except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
+#                 rospy.sleep(0.01)
+#                 continue
+#         raise Exception,"Transform %s -> %s never appeared"%(target,source)
+#         return None
+
+
+if __name__ == '__main__':
+
+    rospy.init_node("geomagic_touch_dof_locker")
+    gi = GeomagicInterface()
+
+
+
+
+
+    rospy.spin()