解决在启动机械臂控制时报的引用错误

src/arbotix_ros/arbotix_controllers/bin/gripper_controller

@@ -3,39 +3,17 @@
 """
   gripper_controller - action based controller for grippers.
   Copyright (c) 2011-2014 Vanadium Labs LLC.  All right reserved.
-
-  Redistribution and use in source and binary forms, with or without
-  modification, are permitted provided that the following conditions are met:
-      * Redistributions of source code must retain the above copyright
-        notice, this list of conditions and the following disclaimer.
-      * Redistributions in binary form must reproduce the above copyright
-        notice, this list of conditions and the following disclaimer in the
-        documentation and/or other materials provided with the distribution.
-      * Neither the name of Vanadium Labs LLC nor the names of its
-        contributors may be used to endorse or promote products derived
-        from this software without specific prior written permission.
-
-  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
-  ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
-  WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
-  DISCLAIMED. IN NO EVENT SHALL VANADIUM LABS OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
-  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
-  LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
-  OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
-  LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
-  OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF
-  ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 """
 
 import rospy, actionlib
 import thread
 
+from parallel_convert import ParallelConvert
 from control_msgs.msg import GripperCommandAction
 from sensor_msgs.msg import JointState
 from std_msgs.msg import Float64
 from math import asin, sin
 from multiprocessing import Lock
-from arbotix_python.parallel_convert import *
 
 
 class GripperModel:
@@ -151,8 +129,6 @@ class TrapezoidGripperModel(GripperModel):
 
 class ParallelGripperModel(GripperModel):
 	# One servo to open/close parallel jaws, typically via linkage.
-	# TODO - currently we only show one finger, and report twice the distance.  Opening
-	# reports correct width but center is not correct
     def __init__(self):
         GripperModel.__init__(self)
         self.convertor = ParallelConvert(self.joint)
@@ -189,9 +165,7 @@ class GripperActionController:
             rospy.logerr('no model specified, exiting')
             exit()
 
-        if model == 'dualservo':
-            self.model = TrapezoidGripperModel()
-        elif model == 'parallel':
+        if model == 'parallel':
             self.model = ParallelGripperModel()
         elif model == 'singlesided':
             self.model = OneSideGripperModel()

src/arbotix_ros/arbotix_controllers/bin/parallel_convert.py

@@ -0,0 +1,45 @@
+#!/usr/bin/env python
+
+"""
+  ParallelConvert:  For Parallel/Prismatic joint, convert Angle to Width
+  Copyright (c) 2014 Vanadium Labs LLC.  All right reserved.
+"""
+import rospy
+from math import asin, sin, cos, sqrt, acos
+
+#  Parallel Gripper gap calculation:
+#
+#        o           (S) is servo axis
+#       /:\    ||      R is radius of servo horn
+#    R / :  \C ||      C is connector to finger
+#     /  :x   \||      Offset is for foam and offset from connection back to finger
+#    /a  :      \
+#  (S). . . . . . o
+#      n    y  ||
+#              || <-- offset
+#              ||finger
+
+PRISM_PARAM_NS = "/arbotix/joints/"
+
+class ParallelConvert:
+    """ For Parallel/Prismatic joint, convert Angle to Width and vice versa"""
+    def __init__(self, joint_name):
+        ns = PRISM_PARAM_NS + joint_name + "/"
+        self.r = rospy.get_param(ns+'radius', .0078)      # Radius of servo horn
+        self.c = rospy.get_param(ns+'connector', 0.024)   # connector from horn to finger
+        # offset back from connection to actual foam pad
+        self.offset = rospy.get_param(ns+'offset', 0.016)
+
+    def widthToAngle(self, width):
+        """ Convert width to servo angle """
+        leg = (width / 2) + self.offset  # Remove double for two fingers and add offset
+        # Law of Cosines
+        return -1 * acos ( (self.r * self.r + leg * leg - self.c * self.c) / (2 * self.r * leg) )
+
+    def angleToWidth(self, ang):
+        """ Convert angle to width for this gripper """
+        n = cos(ang) * self.r               # CAH
+        x = sin(ang) * self.r               # SOH
+        y = sqrt(self.c * self.c - x * x)   # Pythagorean
+        return (n + y - self.offset) * 2  # Remove offset and double to cover two fingers
+

src/arbotix_ros/arbotix_python/bin/arbotix_gui

@@ -40,16 +40,16 @@ class controllerGUI(wx.Frame):
         # Move Servos
         servo = wx.StaticBox(self, -1, 'Move Servos')
         servo.SetFont(wx.Font(10, wx.DEFAULT, wx.NORMAL, wx.BOLD))
-        servoBox = wx.StaticBoxSizer(servo, orient=wx.VERTICAL)
+        servoBox   = wx.StaticBoxSizer(servo, orient=wx.VERTICAL)
         servoSizer = wx.GridBagSizer(5, 5)
 
         joint_defaults = getJointsFromURDF()
 
         i = 0
         dynamixels = rospy.get_param('/arbotix/dynamixels', dict())
-        self.servos = list()
+        self.servos     = list()
         self.publishers = list()
-        self.relaxers = list()
+        self.relaxers   = list()
 
         joints = rospy.get_param('/arbotix/joints', dict())
         # create sliders and publishers
@@ -66,14 +66,14 @@ class controllerGUI(wx.Frame):
 
             # create slider
             s = servoSlider(self, min_angle, max_angle, name, i)
-            servoSizer.Add(s.enabled,(i,0), wx.GBSpan(1,1),  wx.ALIGN_CENTER_VERTICAL)
-            servoSizer.Add(s.position,(i,1), wx.GBSpan(1,1), wx.ALIGN_CENTER_VERTICAL)
+            servoSizer.Add(s.enabled,  (i,0), wx.GBSpan(1,1), wx.ALIGN_CENTER_VERTICAL)
+            servoSizer.Add(s.position, (i,1), wx.GBSpan(1,1), wx.ALIGN_CENTER_VERTICAL)
             self.servos.append(s)
             i += 1
 
         # add everything
         servoBox.Add(servoSizer)
-        sizer.Add(servoBox, (0,1), wx.GBSpan(1,1), wx.EXPAND|wx.TOP|wx.BOTTOM|wx.RIGHT,5)
+        sizer.Add(servoBox, (0,1), wx.GBSpan(1,1), wx.EXPAND|wx.TOP|wx.BOTTOM, 5)
         self.Bind(wx.EVT_CHECKBOX, self.enableSliders)
         # now we can subscribe
         rospy.Subscriber('joint_states', JointState, self.stateCb)
@@ -111,15 +111,16 @@ class controllerGUI(wx.Frame):
 
     def onTimer(self, event=None):
         # send joint updates
-        for s,p in zip(self.servos,self.publishers):
+        for s,p in zip(self.servos, self.publishers):
             if s.enabled.IsChecked():
                 d = Float64()
                 d.data = s.getPosition()
                 p.publish(d)
 
+
 if __name__ == '__main__':
     # initialize GUI
-    rospy.init_node('controllerGUI')
+    rospy.init_node('ARM_Controller_GUI')
     app = wx.App(False)
     frame = controllerGUI(None, True)
     app.MainLoop()

src/turtlebot_arm/turtlebot_arm_moveit_config/config/turtlebot_arm.srdf

@@ -27,6 +27,7 @@
         <joint name="arm_shoulder_pan_joint" value="0" />
         <joint name="arm_wrist_flex_joint" value="0" />
     </group_state>
+
     <group_state name="resting" group="arm">
         <joint name="arm_elbow_flex_joint" value="2.3" />
         <joint name="arm_shoulder_lift_joint" value="-2" />

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/demo.launch

@@ -11,7 +11,7 @@
 
   <!-- Load arm description, state and controllers in simulation mode -->
   <include file="$(find turtlebot_arm_bringup)/launch/arm.launch">
-    <arg name="simulation" value="true"/>
+    <arg name="simulation" value="false"/>
   </include>
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->

src/turtlebot_arm/turtlebot_arm_moveit_config/launch/turtlebot_arm_moveit.launch

@@ -1,7 +1,7 @@
 <launch>
 
   <!-- By default, run in simulation mode -->
-  <arg name="sim" default="true" />
+  <arg name="sim" default="false" />
 
   <!-- By default, we do not start a database (it can be large) -->
   <arg name="db" default="false" />
@@ -10,7 +10,7 @@
   <arg name="debug" default="false" />
   
   <!-- Arm_type from environ.  NOTE: setting from arg not yet supported -->
-  <arg name="arm_type" default="$(optenv TURTLEBOT_ARM1 turtlebot)"/>
+  <arg name="arm_type" default="turtlebot" />
 
   <!-- Load the URDF, SRDF and other .yaml configuration files on the param server -->
   <include file="$(find turtlebot_arm_moveit_config)/launch/planning_context.launch">
@@ -18,9 +18,6 @@
     <arg name="arm_type" value="$(arg arm_type)"/>
   </include>
 
-  <!-- If needed, broadcast static tf for robot root -->
-  
-
   <!-- Publish fake joint states if we do not have a robot connected -->
   <node if="$(arg sim)" name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher">
     <param name="/use_gui" value="false"/>