Add gripper client

README.md

@@ -1 +1,13 @@
-# active_grasp
+# active_grasp
+
+First, run the simulation
+
+```
+roslaunch active_grasp simulation.launch
+```
+
+Then you can run a policy.
+
+```
+python3 run.py ...
+```

launch/panda_visualization.launch

@@ -1,6 +0,0 @@
-<?xml version="1.0" ?>
-<launch>
-  <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 $(find active_grasp)/launch/panda_visualization.rviz" />
-</launch>

launch/simulation.launch

@@ -0,0 +1,15 @@
+<?xml version="1.0" ?>
+<launch>
+  <!-- Arguments -->
+  <arg name="launch_rviz" default="false" />
+
+  <!-- Load parameters. -->
+  <param name="robot_description" command="$(find xacro)/xacro $(find franka_description)/robots/panda_arm_hand.urdf.xacro" />
+
+  <!-- Bringup the robot -->
+  <node pkg="active_grasp" type="simulation.py" name="simulation" args="--gui" output="screen" />
+  <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher" />
+
+  <!-- Visualize the robot. -->
+  <node pkg="rviz" type="rviz" name="rviz" args="-d $(find active_grasp)/launch/panda_visualization.rviz" if="$(arg launch_rviz)" />
+</launch>

nodes/simulation.py

@@ -1,8 +1,12 @@
+#!/usr/bin/env python3
+
 import argparse
 
+import actionlib
 import numpy as np
 import rospy
 
+import franka_gripper.msg
 from geometry_msgs.msg import Pose
 from sensor_msgs.msg import JointState
 
@@ -22,24 +26,31 @@ class BtSimNode:
         self.joint_state_pub = rospy.Publisher(
             "/joint_states", JointState, queue_size=10
         )
-        rospy.Subscriber("/target", Pose, self._target_pose_cb)
+        self.move_server = actionlib.SimpleActionServer(
+            "move",
+            franka_gripper.msg.MoveAction,
+            execute_cb=self.move,
+            auto_start=False,
+        )
+        self.move_server.start()
+        rospy.Subscriber("/target", Pose, self.target_pose_cb)
 
     def run(self):
         rate = rospy.Rate(self.sim.rate)
         self.step_cnt = 0
         while not rospy.is_shutdown():
-            self._handle_updates()
+            self.handle_updates()
             self.sim.step()
             self.step_cnt = (self.step_cnt + 1) % self.sim.rate
             rate.sleep()
 
-    def _handle_updates(self):
+    def handle_updates(self):
         if self.step_cnt % int(self.sim.rate / CONTROLLER_UPDATE_RATE) == 0:
             self.controller.update()
         if self.step_cnt % int(self.sim.rate / JOINT_STATE_PUBLISHER_RATE) == 0:
-            self._publish_joint_state()
+            self.publish_joint_state()
 
-    def _publish_joint_state(self):
+    def publish_joint_state(self):
         q, dq = self.sim.arm.get_state()
         width = self.sim.gripper.read()
         msg = JointState()
@@ -52,7 +63,11 @@ class BtSimNode:
         msg.velocity = dq
         self.joint_state_pub.publish(msg)
 
-    def _target_pose_cb(self, msg):
+    def move(self, goal):
+        self.sim.gripper.move(goal.width)
+        self.move_server.set_succeeded()
+
+    def target_pose_cb(self, msg):
         self.controller.set_target(from_pose_msg(msg))
 
 
@@ -64,6 +79,6 @@ def main(args):
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
-    parser.add_argument("--gui", type=str, default=True)
-    args = parser.parse_args()
+    parser.add_argument("--gui", action="store_true")
+    args, _ = parser.parse_known_args()
     main(args)

policies.py

@@ -3,12 +3,15 @@ import numpy as np
 import rospy
 import scipy.interpolate
 
+from robot_tools.spatial import Rotation, Transform
 from robot_tools.ros import *
 
 
 def get_policy(name):
-    if name == "fixed":
+    if name == "fixed-trajectory":
         return FixedTrajectory()
+    else:
+        raise ValueError("{} policy does not exist.".format(name))
 
 
 class BasePolicy:

run.py

@@ -7,25 +7,52 @@ from std_srvs.srv import Trigger
 
 from policies import get_policy
 
+from robot_tools.ros import *
+
+
+class GraspController:
+    def __init__(self, policy, rate):
+        self.policy = policy
+        self.rate = rate
+
+        self.target_pose_pub = rospy.Publisher("/target", Pose, queue_size=10)
+        self.gripper = PandaGripperRosInterface()
+
+    def explore(self):
+        r = rospy.Rate(self.rate)
+        done = False
+        self.policy.start()
+        while not done:
+            done = self.policy.update()
+            r.sleep()
+
+    def execute_grasp(self):
+        self.gripper.move(0.08)
+        rospy.sleep(1.0)
+        target = self.policy.best_grasp
+        self.target_pose_pub.publish(to_pose_msg(target))
+        rospy.sleep(2.0)
+        self.gripper.move(0.0)
+        rospy.sleep(1.0)
+        target.translation[2] += 0.1
+        self.target_pose_pub.publish(to_pose_msg(target))
+        rospy.sleep(2.0)
+
 
 def main(args):
     rospy.init_node("panda_grasp")
 
     policy = get_policy(args.policy)
-
-    r = rospy.Rate(args.rate)
-    done = False
-    policy.start()
-    while not done:
-        done = policy.update()
-        r.sleep()
-
-    # TODO execute grasp
+    gc = GraspController(policy, args.rate)
+    gc.explore()
+    gc.execute_grasp()
 
 
 if __name__ == "__main__":
     parser = argparse.ArgumentParser()
-    parser.add_argument("--policy", type=str, choices=["fixed"])
+    parser.add_argument(
+        "--policy", type=str, choices=["single-view", "fixed-trajectory"]
+    )
     parser.add_argument("--rate", type=int, default=10)
     args = parser.parse_args()
     main(args)