Cleanup

launch/panda_visualization.launch

@@ -1,7 +1,7 @@
 <?xml version="1.0" ?>
 <launch>
-  <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm.urdf.xacro" />
+  <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
 
   <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
-  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d /home/michel/catkin_ws/src/active_grasp/launch/panda_visualization.rviz" />
+  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" />
 </launch>

launch/panda_visualization.rviz

@@ -5,6 +5,8 @@ Panels:
     Property Tree Widget:
       Expanded:
         - /Global Options1
+        - /RobotModel1/Status1
+        - /TF1/Frames1
       Splitter Ratio: 0.5
     Tree Height: 549
   - Class: rviz/Selection
@@ -61,6 +63,16 @@ Visualization Manager:
         Expand Link Details: false
         Expand Tree: false
         Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
         panda_link0:
           Alpha: 1
           Show Axes: false
@@ -106,12 +118,32 @@ Visualization Manager:
           Show Axes: false
           Show Trail: false
           Value: true
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
       Name: RobotModel
       Robot Description: robot_description
       TF Prefix: ""
       Update Interval: 0
       Value: true
       Visual Enabled: true
+    - Class: rviz/TF
+      Enabled: false
+      Frame Timeout: 15
+      Frames:
+        All Enabled: false
+      Marker Alpha: 1
+      Marker Scale: 0.5
+      Name: TF
+      Show Arrows: false
+      Show Axes: true
+      Show Names: true
+      Tree:
+        {}
+      Update Interval: 0
+      Value: false
     - Class: rviz/InteractiveMarkers
       Enable Transparency: true
       Enabled: true
@@ -149,7 +181,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 1.841537594795227
+      Distance: 1.2702323198318481
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
@@ -165,9 +197,9 @@ Visualization Manager:
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: 0.29539811611175537
+      Pitch: 0.38539746403694153
       Target Frame: <Fixed Frame>
-      Yaw: 5.128592014312744
+      Yaw: 5.683584213256836
     Saved: ~
 Window Geometry:
   Displays:
@@ -185,5 +217,5 @@ Window Geometry:
   Views:
     collapsed: true
   Width: 1200
-  X: 646
+  X: 1045
-  Y: 193
+  Y: 249

robot_sim.py

@@ -105,9 +105,13 @@ class SimPandaEnv(object):
 
     def _publish_state(self, event):
         positions, velocities = self.arm.get_state()
+        width = self.gripper.read()
         msg = JointState()
         msg.header.stamp = rospy.Time.now()
-        msg.name = ["panda_joint{}".format(i) for i in range(1, 8)]
+        msg.name = ["panda_joint{}".format(i) for i in range(1, 8)] + [
-        msg.position = positions
+            "panda_finger_joint1",
+            "panda_finger_joint2",
+        ]
+        msg.position = np.r_[positions, 0.5 * width, 0.5 * width]
         msg.velocity = velocities
         self.state_pub.publish(msg)

run_demo.py

@@ -13,7 +13,7 @@ dt = 1.0 / 60.0
 rospy.init_node("demo")
 
 env = SimPandaEnv(gui)
-model = Model("panda_link0", "panda_link8")
+model = Model("panda_link0", "panda_ee")
 
 q, dq = env.arm.get_state()
 x0 = model.pose(q)
@@ -24,7 +24,7 @@ marker = InteractiveMarkerWrapper("target", "panda_link0", x0)
 
 # run the control loop
 while True:
-    controller.set_target(marker.get_pose())
+    controller.set_target(marker.pose)
     q, dq = env.arm.get_state()
     cmd = controller.update(q, dq)
     env.arm.set_desired_joint_velocities(cmd)

utils.py

@@ -5,6 +5,7 @@ import PyKDL as kdl
 import geometry_msgs.msg
 from interactive_markers.interactive_marker_server import *
 from interactive_markers.menu_handler import *
+import std_msgs.msg
 from visualization_msgs.msg import *
 
 
@@ -20,66 +21,52 @@ class InteractiveMarkerWrapper(object):
         int_marker.scale = 0.2
         int_marker.pose = to_pose_msg(x0)
 
+        # Attach visible sphere
         marker = Marker()
         marker.type = Marker.SPHERE
-        marker.scale.x = 0.05
+        marker.scale = to_vector3_msg([0.05, 0.05, 0.05])
-        marker.scale.y = 0.05
+        marker.color = to_color_msg([0.0, 0.5, 0.5, 0.6])
-        marker.scale.z = 0.05
-        marker.color.r = 0.0
-        marker.color.g = 0.5
-        marker.color.b = 0.5
-        marker.color.a = 0.6
 
         ctrl = InteractiveMarkerControl()
         ctrl.always_visible = True
         ctrl.markers.append(marker)
         int_marker.controls.append(ctrl)
 
+        # Attach rotation controls
         ctrl = InteractiveMarkerControl()
-        ctrl.orientation.w = 1
-        ctrl.orientation.x = 1
-        ctrl.orientation.y = 0
-        ctrl.orientation.z = 0
         ctrl.name = "rotate_x"
+        ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1]))
         ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
-        ctrl.orientation.w = 1
-        ctrl.orientation.x = 0
-        ctrl.orientation.y = 0
-        ctrl.orientation.z = 1
         ctrl.name = "rotate_y"
+        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1]))
         ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
-        ctrl.orientation.w = 1
-        ctrl.orientation.x = 0
-        ctrl.orientation.y = 1
-        ctrl.orientation.z = 0
         ctrl.name = "rotate_z"
+        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1]))
         ctrl.interaction_mode = InteractiveMarkerControl.ROTATE_AXIS
         int_marker.controls.append(ctrl)
 
+        # Attach translation controls
         ctrl = InteractiveMarkerControl()
         ctrl.name = "move_x"
-        ctrl.orientation.w = 1.0
+        ctrl.orientation = to_quat_msg(Rotation.from_quat([1, 0, 0, 1]))
-        ctrl.orientation.x = 1.0
         ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
         int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
         ctrl.name = "move_y"
-        ctrl.orientation.w = 1.0
+        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 1, 0, 1]))
-        ctrl.orientation.y = 1.0
         ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
         int_marker.controls.append(ctrl)
 
         ctrl = InteractiveMarkerControl()
         ctrl.name = "move_z"
-        ctrl.orientation.w = 1.0
+        ctrl.orientation = to_quat_msg(Rotation.from_quat([0, 0, 1, 1]))
-        ctrl.orientation.z = 1.0
         ctrl.interaction_mode = InteractiveMarkerControl.MOVE_AXIS
         int_marker.controls.append(ctrl)
 
@@ -89,9 +76,6 @@ class InteractiveMarkerWrapper(object):
     def cb(self, feedback):
         self.pose = from_pose_msg(feedback.pose)
 
-    def get_pose(self):
-        return self.pose
-
 
 class Transform(object):
     def __init__(self, rotation, translation):
@@ -173,6 +157,15 @@ def kdl_to_mat(m):
 # ROS Conversions
 
 
+def to_color_msg(color):
+    msg = std_msgs.msg.ColorRGBA()
+    msg.r = color[0]
+    msg.g = color[1]
+    msg.b = color[2]
+    msg.a = color[3]
+    return msg
+
+
 def to_point_msg(point):
     msg = geometry_msgs.msg.Point()
     msg.x = point[0]