Merge pull request #41 from rail-berkeley/server-cleanup

print jacobian warning instead of error; add method for getpos_euler

docs/real_franka.md

@@ -36,9 +36,7 @@ The peg insertion task is best for getting started with running SERL on a real r
     python serl_robo_infra/robot_servers/franka_server.py --gripper_type=<Robotiq|Franka|None> --robot_ip=<robot_IP> --gripper_ip=<[Optional] Robotiq_gripper_IP>
     ```
     This should start the impedance controller and a Flask server ready to recieve requests.
-5. The reward in this task is given by checking whether the end-effector pose matches a fixed target pose. Grasp the desired peg with  `curl -X POST http://127.0.0.1:5000/close_gripper` and manually move the arm into a pose where the peg is inserted into the board. Print the current pose with `curl -X POST http://127.0.0.1:5000/getpos` and update the `TARGET_POSE` in [peg_env/config.py](../serl_robot_infra/franka_env/envs/peg_env/config.py) with the measured end-effector pose.
-
-    **Note: the `getpos` command prints the pose in xyz+quaternion format, but `TARGET_POSE` expects xyz+euler(rpy) format. Please use your favourite quat to euler calculator to do the conversion.
+5. The reward in this task is given by checking whether the end-effector pose matches a fixed target pose. Grasp the desired peg with  `curl -X POST http://127.0.0.1:5000/close_gripper` and manually move the arm into a pose where the peg is inserted into the board. Print the current pose with `curl -X POST http://127.0.0.1:5000/getpos_euler` and update the `TARGET_POSE` in [peg_env/config.py](../serl_robot_infra/franka_env/envs/peg_env/config.py) with the measured end-effector pose.
 
     **Note: make sure the wrist joint is centered (away from joint limits) and z-axis euler angle is positive at the target pose to avoid discontinuities.
 

serl_robot_infra/README.md

@@ -48,7 +48,8 @@ The HTTP server is used to communicate between the ROS controller and gym enviro
 | startimp | Stop the impedance controller |
 | stopimp | Start the impedance controller |
 | pose | Command robot to go to desired end-effector pose given in base frame (xyz+quaternion) |
-| getpos | Return current end-effector pose in robot base frame (xyz+rpy)|
+| getpos | Return current end-effector pose in robot base frame (xyz+quaternion)|
+| getpos_euler | Return current end-effector pose in robot base frame (xyz+rpy)|
 | getvel | Return current end-effector velocity in robot base frame |
 | getforce | Return estimated force on end-effector in stiffness frame |
 | gettorque | Return estimated torque on end-effector in stiffness frame |
@@ -71,7 +72,7 @@ These commands can also be called in terminal. Useful ones include:
 curl -X POST http://127.0.0.1:5000/activate_gripper # Activate gripper
 curl -X POST http://127.0.0.1:5000/close_gripper # Close gripper
 curl -X POST http://127.0.0.1:5000/open_gripper # Open gripper
-curl -X POST http://127.0.0.1:5000/getpos # Print current end-effector pose
+curl -X POST http://127.0.0.1:5000/getpos_euler # Print current end-effector pose
 curl -X POST http://127.0.0.1:5000/jointreset # Perform joint reset
 curl -X POST http://127.0.0.1:5000/stopimp # Stop the impedance controller
 curl -X POST http://127.0.0.1:5000/startimp # Start the impedance controller (**Only run this after stopimp**)

serl_robot_infra/robot_servers/franka_server.py

@@ -158,7 +158,13 @@ def _set_currpos(self, msg):
         self.q = np.array(list(msg.q)).reshape((7,))
         self.force = np.array(list(msg.K_F_ext_hat_K)[:3])
         self.torque = np.array(list(msg.K_F_ext_hat_K)[3:])
-        self.vel = self.jacobian @ self.dq
+        try:
+            self.vel = self.jacobian @ self.dq
+        except:
+            self.vel = np.zeros(6)
+            rospy.logwarn(
+                "Jacobian not set, end-effector velocity temporarily not available"
+            )
 
     def _set_jacobian(self, msg):
         jacobian = np.array(list(msg.zero_jacobian)).reshape((6, 7), order="F")
@@ -231,6 +237,12 @@ def stop_impedance():
     def get_pos():
         return jsonify({"pose": np.array(robot_server.pos).tolist()})
 
+    @webapp.route("/getpos_euler", methods=["POST"])
+    def get_pos_euler():
+        r = R.from_quat(robot_server.pos[3:])
+        euler = r.as_euler("xyz")
+        return jsonify({"pose": np.concatenate([robot_server.pos[:3], euler]).tolist()})
+
     @webapp.route("/getvel", methods=["POST"])
     def get_vel():
         return jsonify({"vel": np.array(robot_server.vel).tolist()})