diff --git a/robot_infra/README.md b/robot_infra/README.md new file mode 100644 index 0000000..2bc085f --- /dev/null +++ b/robot_infra/README.md @@ -0,0 +1,22 @@ +# Franka Robot Envs + +All robot code is structured as follows: +There is a Flask server which sends commands to the robot via ROS. There is a gym env for the robot which communicates with the Flask server via post requests. + +### Installation + +First, make sure the NUC meets the specifications [here](https://frankaemika.github.io/docs/requirements.html), and install the real time kernel, and `libfranka` and `franka_ros` as described [here](https://frankaemika.github.io/docs/installation_linux.html). + +You'll then want to copy the following files from `launchers` to your catkin workspace: +- copy the two `.launch` files to a `catkin_ws/scripts` folder in your ros workspace +- copy the `.cfg` files to `catkin_ws/src/franka_ros/franka_example_controllers/cfg` +- copy the two `.cpp` files to `catkin_ws/src/franka_ros/franka_example_controllers/src` +- copy the two `.h` files to `catkin_ws/src/franka_ros/franka_example_controllers/include/franka_example_controllers` + +### Usage + +To start using the robot, first power on the robot (small switch on the back of robot control box on the floor). Unlock the robot from the browser interface by going to robot IP address in your browser, then press the black and white button to put the robot in FCI control mode (blue light). + +From there you should be able to navigate to `robot_infra` and then simply run `python franka_server.py`. This should start the impedence controller and the HTTP server. You can test that things are running by trying to move the end effector around, if the impedence controller is running it should be compliant. + +Lastly, any code you write can interact with the robot via the gym interface defined in this repo under `env`. Simply run `pip install -e .` in the `env_franka` directory, and in your code simply initialize the env via `gym.make("Franka-{ENVIRONMENT NAME}-v0)`. \ No newline at end of file diff --git a/robot_infra/__init__.py b/robot_infra/__init__.py new file mode 100644 index 0000000..6f4f0cf --- /dev/null +++ b/robot_infra/__init__.py @@ -0,0 +1,17 @@ +from gym.envs.registration import register +register( + id='Franka-PCB-v0', + entry_point='robot_infra.env:PCBEnv', +) +register( + id='Franka-RouteCable-v0', + entry_point='robot_infra.env:RouteCableEnv', +) +register( + id='Franka-ResetCable-v0', + entry_point='robot_infra.env:ResetCableEnv', +) +register( + id='Franka-BinPick-v0', + entry_point='robot_infra.env:BinPickEnv', +) \ No newline at end of file diff --git a/robot_infra/franka_server.py b/robot_infra/franka_server.py new file mode 100644 index 0000000..6027a76 --- /dev/null +++ b/robot_infra/franka_server.py @@ -0,0 +1,453 @@ +""" +This file starts a control server running on the real time PC connected to the franka robot. +In a screen run `python franka_server.py` +""" +import flask +from flask import Flask, request, jsonify +import rospy +import numpy as np +import json +from franka_gripper.msg import GraspActionGoal, MoveActionGoal, StopActionGoal +from franka_msgs.msg import ErrorRecoveryActionGoal +from franka_msgs.msg import FrankaState +from franka_msgs.msg import ZeroJacobian +from robotiq_2f_gripper_control.msg import _Robotiq2FGripper_robot_output as outputMsg +import geometry_msgs.msg as geom_msg +import time +import select +from multiprocessing import Process +import subprocess +from pyquaternion import Quaternion +from scipy.spatial.transform import Rotation as R +import os +import cv2 +import sys +from dynamic_reconfigure.client import Client + +IMSIZE = 256 + +def get_cameras(): + cams = [] + for i in range(10): + cam = cv2.VideoCapture(i) + if cam.isOpened(): + cams.append(i) + cam.release() + print("cameras", cams) + return cams + + +app = Flask(__name__) +## IF YOU ARE NOT IN IRIS CHANGES THIS FOR YOUR NUC HOME DIR AND ROBOT IP +ROS_WS = "/home/jianlan/code/catkin_ws" +ROBOT_IP = "172.16.0.2" + + +class Launcher: + """Handles the starting and stopping of the impedence controller + (as well as backup) joint recovery policy.""" + + def __init__(self): + self.gripper_command = outputMsg.Robotiq2FGripper_robot_output() + + def start_impedence(self): + ## Launches the impedence controller + self.imp = subprocess.Popen( + [ + "roslaunch", + ROS_WS + "/scripts/impedence.launch", + "robot_ip:=" + ROBOT_IP, + "load_gripper:=false", + ], + stdout=subprocess.PIPE, + ) + time.sleep(5) + + """Update the gripper command according to the character entered by the user.""" + def generate_gripper_command(self, char, command): + if char == "a": + command = outputMsg.Robotiq2FGripper_robot_output() + command.rACT = 1 + command.rGTO = 1 + command.rSP = 255 + command.rFR = 150 + + if char == "r": + command = outputMsg.Robotiq2FGripper_robot_output() + command.rACT = 0 + + if char == "c": + command.rPR = 255 + + if char == "o": + command.rPR = 0 + + # If the command entered is a int, assign this value to rPR + # (i.e., move to this position) + try: + command.rPR = int(char) + if command.rPR > 255: + command.rPR = 255 + if command.rPR < 0: + command.rPR = 0 + except ValueError: + pass + return command + + def stop_impedence(self): + ## Stops the impedence controller + self.imp.terminate() + time.sleep(1) + + def set_currpos(self, msg): + tmatrix = np.array(list(msg.O_T_EE)).reshape(4, 4).T + r = R.from_matrix(tmatrix[:3, :3]) + pose = np.concatenate([tmatrix[:3, -1], r.as_quat()]) + self.pos = pose + self.dq = np.array(list(msg.dq)).reshape((7,)) + self.q = np.array(list(msg.q)).reshape((7,)) + self.force = np.array(list(msg.O_F_ext_hat_K)[:3]) + self.torque = np.array(list(msg.O_F_ext_hat_K)[3:]) + self.vel = self.jacobian @ self.dq + + def set_jacobian(self, msg): + jacobian = np.array(list(msg.zero_jacobian)).reshape((6, 7), order="F") + self.jacobian = jacobian + + def reset_joint(self): + """Resets Joints (needed after running for hours)""" + # import pdb; pdb.set_trace() + # First Stop Impedence + try: + self.stop_impedence() + + except: + print("Impedence Not Running") + + ## Launch joint controller reset + clear() + self.j = subprocess.Popen( + [ + "roslaunch", + ROS_WS + "/scripts/joint.launch", + "robot_ip:=" + ROBOT_IP, + "load_gripper:=false", + ], + stdout=subprocess.PIPE, + ) + time.sleep(1) + print("RUNNING JOINT RESET") + clear() + count = 0 + time.sleep(1) + while not np.allclose(np.array([0, 0, 0, -1.9, -0, 2, 0]) - np.array(self.q), 0, + atol=1e-2, rtol=1e-2): + time.sleep(1) + count += 1 + if count > 100: + print('TIMEOUT') + break + + print("RESET DONE") + self.j.terminate() + time.sleep(1) + clear() + print("KILLED JOINT RESET", self.pos) + self.start_impedence() + print("IMPEDENCE STARTED") + + +"""Starts Impedence controller""" +l = Launcher() +l.start_impedence() + +## Defines the ros topics to publish to +# rospy.init_node("equilibrium_pose_node") +rospy.init_node("franka_control_api") +gripperpub = rospy.Publisher( + "Robotiq2FGripperRobotOutput", outputMsg.Robotiq2FGripper_robot_output, queue_size=1 +) +gripperpub.publish(l.gripper_command) # init reset gripper +time.sleep(1) + +eepub = rospy.Publisher( + "/cartesian_impedance_example_controller/equilibrium_pose", + geom_msg.PoseStamped, + queue_size=10, +) +resetpub = rospy.Publisher( + "/franka_control/error_recovery/goal", ErrorRecoveryActionGoal, queue_size=1 +) +state_sub = rospy.Subscriber( + "franka_state_controller/franka_states", FrankaState, l.set_currpos +) +jacobian_sub = rospy.Subscriber( + "/cartesian_impedance_example_controller/franka_jacobian", + ZeroJacobian, + l.set_jacobian, +) +# l.reset_joint() +client = Client("cartesian_impedance_example_controllerdynamic_reconfigure_compliance_param_node") + +## Route for Starting Impedence +@app.route("/startimp", methods=["POST"]) +def si(): + clear() + l.start_impedence() + return "Started Impedence" + + +## Route for Stopping Impedence +@app.route("/stopimp", methods=["POST"]) +def sti(): + l.stop_impedence() + return "Stopped Impedence" + + +## Route for Getting Pose +@app.route("/getpos", methods=["POST"]) +def gp(): + return jsonify({"pose": np.array(l.pos).tolist()}) + + +@app.route("/getvel", methods=["POST"]) +def gv(): + return jsonify({"vel": np.array(l.vel).tolist()}) + + +@app.route("/getforce", methods=["POST"]) +def gf(): + return jsonify({"force": np.array(l.force).tolist()}) + + +@app.route("/gettorque", methods=["POST"]) +def gt(): + return jsonify({"torque": np.array(l.torque).tolist()}) + + +@app.route("/getq", methods=["POST"]) +def gq(): + return jsonify({"q": np.array(l.q).tolist()}) + + +@app.route("/getdq", methods=["POST"]) +def gdq(): + return jsonify({"dq": np.array(l.dq).tolist()}) + + +@app.route("/getjacobian", methods=["POST"]) +def gj(): + return jsonify({"jacobian": np.array(l.jacobian).tolist()}) + +## Route for Running Joint Reset +@app.route("/jointreset", methods=["POST"]) +def jr(): + msg = ErrorRecoveryActionGoal() + resetpub.publish(msg) + l.reset_joint() + return "Reset Joint" + +##Route for Activating the Gripper +@app.route("/activate_gripper", methods=["POST"]) +def activate_gripper(): + print("activate gripper") + l.gripper_command = l.generate_gripper_command("a", l.gripper_command) + gripperpub.publish(l.gripper_command) + return "Activated" + +## Route for Resetting the Gripper. It will reset and activate the gripper +@app.route("/reset_gripper", methods=["POST"]) +def reset_gripper(): + print("reset gripper") + l.gripper_command = l.generate_gripper_command("r", l.gripper_command) + gripperpub.publish(l.gripper_command) + l.gripper_command = l.generate_gripper_command("a", l.gripper_command) + gripperpub.publish(l.gripper_command) + return "Reset" + +## Route for Opening the Gripper +@app.route("/open", methods=["POST"]) +def open(): + print("open") + l.gripper_command = l.generate_gripper_command("o", l.gripper_command) + gripperpub.publish(l.gripper_command) + return "Opened" + +## Route for Closing the Gripper +@app.route("/close", methods=["POST"]) +def close(): + print("close") + l.gripper_command = l.generate_gripper_command("c", l.gripper_command) + gripperpub.publish(l.gripper_command) + return "Closed" + +## Route for moving the gripper +@app.route("/move", methods=["POST"]) +def move_gripper(): + gripper_pos = request.json + pos = int(gripper_pos["gripper_pos"] * 255) #convert from 0-1 to 0-255 + print(f"move gripper to {pos}") + l.gripper_command = l.generate_gripper_command(pos, l.gripper_command) + gripperpub.publish(l.gripper_command) + return "Moved Gripper" + +## Route for Clearing Errors (Communcation constraints, etc.) +@app.route("/clearerr", methods=["POST"]) +def clear(): + msg = ErrorRecoveryActionGoal() + resetpub.publish(msg) + return "Clear" + +## Route for Sending a pose command +@app.route("/pose", methods=["POST"]) +def pose(): + pos = request.json + pos = np.array(pos["arr"]) + print("Moving to", pos) + msg = geom_msg.PoseStamped() + msg.header.frame_id = "0" + msg.header.stamp = rospy.Time.now() + msg.pose.position = geom_msg.Point(pos[0], pos[1], pos[2]) + msg.pose.orientation = geom_msg.Quaternion(pos[3], pos[4], pos[5], pos[6]) + eepub.publish(msg) + return "Moved" + +@app.route("/getstate", methods=["POST"]) +def gs(): + return jsonify({"pose": np.array(l.pos).tolist(), + "vel": np.array(l.vel).tolist(), + "force": np.array(l.force).tolist(), + "torque": np.array(l.torque).tolist(), + "q": np.array(l.q).tolist(), + "dq": np.array(l.dq).tolist(), + "jacobian": np.array(l.jacobian).tolist()}) +# PCB +# @app.route("/pcb_compliance_mode", methods=["POST"]) +# def pcb_compliance_mode(): +# client.update_configuration({"translational_stiffness": 3000}) +# client.update_configuration({"translational_damping": 180}) +# client.update_configuration({"rotational_stiffness": 150}) +# client.update_configuration({"rotational_damping": 7}) +# client.update_configuration({"translational_clip_neg_x": 0.002}) +# client.update_configuration({"translational_clip_neg_y": 0.001}) +# client.update_configuration({"translational_clip_neg_z": 0.002}) +# client.update_configuration({"translational_clip_x": 0.0015}) +# client.update_configuration({"translational_clip_y": 0.0005}) +# client.update_configuration({"translational_clip_z": 0.0014}) +# client.update_configuration({"rotational_clip_neg_x": 0.015}) +# client.update_configuration({"rotational_clip_neg_y": 0.002}) +# client.update_configuration({"rotational_clip_neg_z": 0.005}) +# client.update_configuration({"rotational_clip_x": 0.016}) +# client.update_configuration({"rotational_clip_y": 0.002}) +# client.update_configuration({"rotational_clip_z": 0.005}) +# client.update_configuration({"translational_Ki": 0}) +# client.update_configuration({"rotational_Ki": 0}) +# return "pcb compliance Mode" + +# Peg +@app.route("/peg_compliance_mode", methods=["POST"]) +def peg_compliance_mode(): + client.update_configuration({"translational_stiffness": 2000}) + client.update_configuration({"translational_damping": 89}) + client.update_configuration({"rotational_stiffness": 150}) + client.update_configuration({"rotational_damping": 7}) + client.update_configuration({"translational_Ki": 30}) + client.update_configuration({"translational_clip_x": 0.005}) + client.update_configuration({"translational_clip_y": 0.005}) + client.update_configuration({"translational_clip_z": 0.005}) + client.update_configuration({"translational_clip_neg_x": 0.005}) + client.update_configuration({"translational_clip_neg_y": 0.005}) + client.update_configuration({"translational_clip_neg_z": 0.005}) + client.update_configuration({"rotational_clip_x": 0.05}) + client.update_configuration({"rotational_clip_y": 0.05}) + client.update_configuration({"rotational_clip_z": 0.05}) + client.update_configuration({"rotational_clip_neg_x": 0.05}) + client.update_configuration({"rotational_clip_neg_y": 0.05}) + client.update_configuration({"rotational_clip_neg_z": 0.05}) + client.update_configuration({"rotational_Ki": 0}) + return "peg compliance Mode" + + +@app.route("/precision_mode", methods=["POST"]) +def precision_mode(): + client.update_configuration({"translational_stiffness": 2000}) + client.update_configuration({"translational_damping": 89}) + client.update_configuration({"rotational_stiffness": 250}) + client.update_configuration({"rotational_damping": 9}) + client.update_configuration({"translational_Ki": 30}) + client.update_configuration({"translational_clip_x": 0.1}) + client.update_configuration({"translational_clip_y": 0.1}) + client.update_configuration({"translational_clip_z": 0.1}) + client.update_configuration({"translational_clip_neg_x": 0.1}) + client.update_configuration({"translational_clip_neg_y": 0.1}) + client.update_configuration({"translational_clip_neg_z": 0.1}) + client.update_configuration({"rotational_clip_x": 0.1}) + client.update_configuration({"rotational_clip_y": 0.1}) + client.update_configuration({"rotational_clip_z": 0.1}) + client.update_configuration({"rotational_clip_neg_x": 0.1}) + client.update_configuration({"rotational_clip_neg_y": 0.1}) + client.update_configuration({"rotational_clip_neg_z": 0.1}) + client.update_configuration({"rotational_Ki": 5}) + return "precision Mode" + +# cameras = get_cameras() #range(10) #[1] +import threading + +class Camera: + def __init__(self): + all_c = get_cameras() + self.cams = [] + for c in all_c: + # newcam = cv2.VideoCapture(c) + # newcam.set(cv2.CAP_PROP_BUFFERSIZE, 1) + self.cams.append(cv2.VideoCapture(c)) + print("CREATED CAM") + self.imsize = 256 + + # self.read_and_save() + self.imthread = threading.Thread( + target=self.read_and_save, name="Image Capture" + ) + self.imthread.start() + time.sleep(10) + + self.get() + + def read_and_save(self): + while True: + frames = [] + for c in self.cams: + r = False + while not r: + r, frame = c.read() + # print("Read") + frame = cv2.resize( + frame, (self.imsize, self.imsize), interpolation=cv2.INTER_AREA + ) + frames.append(frame) + self.lastob = np.concatenate(frames, axis=0) + # print("LAST OB WROTE") + + def get(self): + print(self.imthread.isAlive()) + return self.lastob + +# cam = Camera() + +## Route for Stopping Impedence +@app.route("/image", methods=["POST"]) +def img(): + obs = cam.get() + response = flask.make_response(obs.tobytes()) + response.headers.set("Content-Type", "application/octet-stream") + t6 = time.time() + return response + + +if __name__ == "__main__": + try: + app.run(host="0.0.0.0") + except KeyboardInterrupt: + print("Keyboard Interrupt") + l.stop_impedence() + sys.exit() diff --git a/robot_infra/launchers/cartesian_impedance_example_controller.cpp b/robot_infra/launchers/cartesian_impedance_example_controller.cpp new file mode 100644 index 0000000..89bcaf7 --- /dev/null +++ b/robot_infra/launchers/cartesian_impedance_example_controller.cpp @@ -0,0 +1,325 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace franka_example_controllers { + +bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW* robot_hw, + ros::NodeHandle& node_handle) { + std::vector cartesian_stiffness_vector; + std::vector cartesian_damping_vector; + publisher_franka_jacobian_.init(node_handle, "franka_jacobian", 1); + publisher_franka_debug_.init(node_handle, "franka_debug", 1); + + sub_equilibrium_pose_ = node_handle.subscribe( + "equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this, + ros::TransportHints().reliable().tcpNoDelay()); + + std::string arm_id; + if (!node_handle.getParam("arm_id", arm_id)) { + ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id"); + return false; + } + std::vector joint_names; + if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7) { + ROS_ERROR( + "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, " + "aborting controller init!"); + return false; + } + + auto* model_interface = robot_hw->get(); + if (model_interface == nullptr) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Error getting model interface from hardware"); + return false; + } + try { + model_handle_ = std::make_unique( + model_interface->getHandle(arm_id + "_model")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Exception getting model handle from interface: " + << ex.what()); + return false; + } + + auto* state_interface = robot_hw->get(); + if (state_interface == nullptr) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Error getting state interface from hardware"); + return false; + } + try { + state_handle_ = std::make_unique( + state_interface->getHandle(arm_id + "_robot")); + } catch (hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Exception getting state handle from interface: " + << ex.what()); + return false; + } + + auto* effort_joint_interface = robot_hw->get(); + if (effort_joint_interface == nullptr) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Error getting effort joint interface from hardware"); + return false; + } + for (size_t i = 0; i < 7; ++i) { + try { + joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i])); + } catch (const hardware_interface::HardwareInterfaceException& ex) { + ROS_ERROR_STREAM( + "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what()); + return false; + } + } + + dynamic_reconfigure_compliance_param_node_ = + ros::NodeHandle(node_handle.getNamespace() + "dynamic_reconfigure_compliance_param_node"); + + dynamic_server_compliance_param_ = std::make_unique< + dynamic_reconfigure::Server>( + + dynamic_reconfigure_compliance_param_node_); + dynamic_server_compliance_param_->setCallback( + boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2)); + + position_d_.setZero(); + orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0; + position_d_target_.setZero(); + orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0; + + cartesian_stiffness_.setZero(); + cartesian_damping_.setZero(); + + // data_file_.open("/home/panda/Desktop/data_file.csv", std::ios::out); + // // Add a header to the CSV + // if(data_file_.is_open()) { + // data_file_ << "time_nsec, position_z_target, position_z, position_desired_z, error_before_clipping_z, error_after_clipping_z, force_before_clipping_z, force_after_clipping_z\n"; + // } + + + return true; +} + +void CartesianImpedanceExampleController::starting(const ros::Time& /*time*/) { + // compute initial velocity with jacobian and set x_attractor and q_d_nullspace + // to initial configuration + franka::RobotState initial_state = state_handle_->getRobotState(); + // get jacobian + std::array jacobian_array = + model_handle_->getZeroJacobian(franka::Frame::kEndEffector); + // convert to eigen + Eigen::Map> q_initial(initial_state.q.data()); + Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data())); + + // set equilibrium point to current state + position_d_ = initial_transform.translation(); + orientation_d_ = Eigen::Quaterniond(initial_transform.linear()); + position_d_target_ = initial_transform.translation(); + orientation_d_target_ = Eigen::Quaterniond(initial_transform.linear()); + + // set nullspace equilibrium configuration to initial q + q_d_nullspace_ = q_initial; +} + +void CartesianImpedanceExampleController::update(const ros::Time& time, + const ros::Duration& /*period*/) { + // get state variables + franka::RobotState robot_state = state_handle_->getRobotState(); + std::array coriolis_array = model_handle_->getCoriolis(); + jacobian_array = + model_handle_->getZeroJacobian(franka::Frame::kEndEffector); + publishZeroJacobian(time); + publishDebug(time); + // convert to Eigen + Eigen::Map> coriolis(coriolis_array.data()); + Eigen::Map> jacobian(jacobian_array.data()); + Eigen::Map> q(robot_state.q.data()); + Eigen::Map> dq(robot_state.dq.data()); + Eigen::Map> tau_J_d( // NOLINT (readability-identifier-naming) + robot_state.tau_J_d.data()); + Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); + position << transform.translation(); + Eigen::Quaterniond orientation(transform.linear()); + + Eigen::VectorXd error_before_clipping(6), error_after_clipping(6); + + // compute error to desired pose + // position error + error.head(3) << position - position_d_; + + error_before_clipping.head(3) << error.head(3); + + error.head(3) << error.head(3).cwiseMax(-translational_clip).cwiseMin(translational_clip); + + error_after_clipping.head(3) << error.head(3); + + error_i.head(3) << (error_i.head(3) + error.head(3)).cwiseMax(-0.05).cwiseMin(0.05); + + // orientation error + if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0) { + orientation.coeffs() << -orientation.coeffs(); + } + // "difference" quaternion + Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_); + error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); + // Transform to base frame + error.tail(3) << -transform.linear() * error.tail(3); + + error_before_clipping.tail(3) << error.tail(3); + + error.tail(3) << error.tail(3).cwiseMax(-rotational_clip).cwiseMin(rotational_clip); + + error_after_clipping.tail(3) << error.tail(3); + + error_i.tail(3) << (error_i.tail(3) + error.tail(3)).cwiseMax(-0.1).cwiseMin(0.1); + + // compute control + // allocate variables + Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7); + + // pseudoinverse for nullspace handling + // kinematic pseuoinverse + Eigen::MatrixXd jacobian_transpose_pinv; + pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv); + + Eigen::VectorXd force_before_clipping(6); + force_before_clipping << -cartesian_stiffness_ * error_before_clipping - cartesian_damping_ * (jacobian * dq); + + // PD control with feedforward term in carteasian space + Eigen::VectorXd force_after_clipping(6); + force_after_clipping << -cartesian_stiffness_ * error_after_clipping - cartesian_damping_ * (jacobian * dq); + + + // Cartesian PD control with damping ratio = 1 + tau_task << jacobian.transpose() * + (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq) - Ki * error_i); + // nullspace PD control with damping ratio = 1 + qe << q_d_nullspace_ - q; + qe.head(1) << qe.head(1) * joint1_nullspace_stiffness_; + dqe << dq; + dqe.head(1) << dqe.head(1) * 2.0 * sqrt(joint1_nullspace_stiffness_); + tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) - + jacobian.transpose() * jacobian_transpose_pinv) * + (nullspace_stiffness_ * qe - + (2.0 * sqrt(nullspace_stiffness_)) * dqe); + // Desired torque + tau_d << tau_task + tau_nullspace + coriolis; + // Saturate torque rate to avoid discontinuities + tau_d << saturateTorqueRate(tau_d, tau_J_d); + for (size_t i = 0; i < 7; ++i) { + // joint_handles_[i].setCommand(0.0); + joint_handles_[i].setCommand(tau_d(i)); + } + + // update parameters changed online either through dynamic reconfigure or through the interactive + // target by filtering + cartesian_stiffness_ = + filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_; + cartesian_damping_ = + filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_; + nullspace_stiffness_ = + filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_; + joint1_nullspace_stiffness_ = + filter_params_ * joint1_nullspace_stiffness_target_ + (1.0 - filter_params_) * joint1_nullspace_stiffness_; + position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_; + orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_); + Ki = filter_params_ * Ki_target_ + (1.0 - filter_params_) * Ki; + int64_t total_nsec = time.sec * 1e9 + time.nsec; + if(data_file_.is_open()) { + data_file_ << total_nsec << ", " << position_d_target_(2) << ", " << position(2) << ", " << position_d_(2) << ", " << error_before_clipping(2) << ", " << error_after_clipping(2) << ", " << force_before_clipping(2) << ", " << force_after_clipping(2) << "\n"; + } + +} + +void CartesianImpedanceExampleController::publishZeroJacobian(const ros::Time& time) { + if (publisher_franka_jacobian_.trylock()) { + for (size_t i = 0; i < jacobian_array.size(); i++) { + publisher_franka_jacobian_.msg_.zero_jacobian[i] = jacobian_array[i]; + } + publisher_franka_jacobian_.unlockAndPublish(); + } +} + +void CartesianImpedanceExampleController::publishDebug(const ros::Time& time) { + if (publisher_franka_debug_.trylock()) { + for (size_t i = 0; i < 3; i++) { + publisher_franka_debug_.msg_.position_d[i] = position_d_[i]; + publisher_franka_debug_.msg_.position_d_target[i] = position_d_target_[i]; + publisher_franka_debug_.msg_.position_error[i] = error[i]; + publisher_franka_debug_.msg_.position[i] = position[i]; + } + publisher_franka_debug_.unlockAndPublish(); + } +} + + +Eigen::Matrix CartesianImpedanceExampleController::saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d) { // NOLINT (readability-identifier-naming) + Eigen::Matrix tau_d_saturated{}; + for (size_t i = 0; i < 7; i++) { + double difference = tau_d_calculated[i] - tau_J_d[i]; + tau_d_saturated[i] = + tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_); + } + return tau_d_saturated; +} + +void CartesianImpedanceExampleController::complianceParamCallback( + franka_example_controllers::compliance_paramConfig& config, + uint32_t /*level*/) { + cartesian_stiffness_target_.setIdentity(); + cartesian_stiffness_target_.topLeftCorner(3, 3) + << config.translational_stiffness * Eigen::Matrix3d::Identity(); + cartesian_stiffness_target_.bottomRightCorner(3, 3) + << config.rotational_stiffness * Eigen::Matrix3d::Identity(); + cartesian_damping_target_.setIdentity(); + // Damping ratio = 1 + cartesian_damping_target_.topLeftCorner(3, 3) + << config.translational_damping * Eigen::Matrix3d::Identity(); + cartesian_damping_target_.bottomRightCorner(3, 3) + << config.rotational_damping * Eigen::Matrix3d::Identity(); + + nullspace_stiffness_target_ = config.nullspace_stiffness; + joint1_nullspace_stiffness_target_ = config.joint1_nullspace_stiffness; + + translational_clip = config.translational_clip; + rotational_clip = config.rotational_clip; + Ki_target_.setIdentity(); + Ki_target_.topLeftCorner(3, 3) + << config.translational_Ki * Eigen::Matrix3d::Identity(); + Ki_target_.bottomRightCorner(3, 3) + << config.rotational_Ki * Eigen::Matrix3d::Identity(); +} + +void CartesianImpedanceExampleController::equilibriumPoseCallback( + const geometry_msgs::PoseStampedConstPtr& msg) { + position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + error_i.setZero(); + Eigen::Quaterniond last_orientation_d_target(orientation_d_target_); + orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y, + msg->pose.orientation.z, msg->pose.orientation.w; + if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0) { + orientation_d_target_.coeffs() << -orientation_d_target_.coeffs(); + } +} + +} // namespace franka_example_controllers + +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController, + controller_interface::ControllerBase) diff --git a/robot_infra/launchers/cartesian_impedance_example_controller.h b/robot_infra/launchers/cartesian_impedance_example_controller.h new file mode 100644 index 0000000..1d020e1 --- /dev/null +++ b/robot_infra/launchers/cartesian_impedance_example_controller.h @@ -0,0 +1,89 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace franka_example_controllers { + +class CartesianImpedanceExampleController : public controller_interface::MultiInterfaceController< + franka_hw::FrankaModelInterface, + hardware_interface::EffortJointInterface, + franka_hw::FrankaStateInterface> { + public: + bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; + + private: + // Saturation + Eigen::Matrix saturateTorqueRate( + const Eigen::Matrix& tau_d_calculated, + const Eigen::Matrix& tau_J_d); // NOLINT (readability-identifier-naming) + + std::unique_ptr state_handle_; + std::unique_ptr model_handle_; + std::vector joint_handles_; + std::array jacobian_array; + + double filter_params_{0.005}; + double nullspace_stiffness_{20.0}; + double nullspace_stiffness_target_{20.0}; + double joint1_nullspace_stiffness_{20.0}; + double joint1_nullspace_stiffness_target_{20.0}; + const double delta_tau_max_{1.0}; + Eigen::Matrix cartesian_stiffness_; + Eigen::Matrix cartesian_stiffness_target_; + Eigen::Matrix cartesian_damping_; + Eigen::Matrix cartesian_damping_target_; + Eigen::Matrix Ki; + Eigen::Matrix Ki_target_; + double translational_clip; + double rotational_clip; + Eigen::Matrix q_d_nullspace_; + Eigen::Matrix qe; + Eigen::Matrix dqe; + Eigen::Vector3d position_d_; + Eigen::Matrix error; + Eigen::Matrix error_i; + Eigen::Vector3d position; + Eigen::Quaterniond orientation_d_; + Eigen::Vector3d position_d_target_; + Eigen::Quaterniond orientation_d_target_; + std::ofstream data_file_; + + // Dynamic reconfigure + std::unique_ptr> + dynamic_server_compliance_param_; + ros::NodeHandle dynamic_reconfigure_compliance_param_node_; + void complianceParamCallback(franka_example_controllers::compliance_paramConfig& config, + uint32_t level); + void publishZeroJacobian(const ros::Time& time); + realtime_tools::RealtimePublisher publisher_franka_jacobian_; + void publishDebug(const ros::Time& time); + realtime_tools::RealtimePublisher publisher_franka_debug_; + // Equilibrium pose subscriber + ros::Subscriber sub_equilibrium_pose_; + void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr& msg); +}; + +} // namespace franka_example_controllers diff --git a/robot_infra/launchers/compliance_param.cfg b/robot_infra/launchers/compliance_param.cfg new file mode 100755 index 0000000..4ee9581 --- /dev/null +++ b/robot_infra/launchers/compliance_param.cfg @@ -0,0 +1,19 @@ +#!/usr/bin/env python +PACKAGE = "franka_example_controllers" + +from dynamic_reconfigure.parameter_generator_catkin import * + +gen = ParameterGenerator() + +gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 2000, 0, 4000) +gen.add("translational_damping", double_t, 0, "Cartesian translational damping", 89, 0, 200) +gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 150, 0, 200) +gen.add("rotational_damping", double_t, 0, "Cartesian rotational damping", 7, 0, 30) +gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0.2, 0, 100) +gen.add("joint1_nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 80, 0, 100) +gen.add("translational_clip", double_t, 0, "Value to clip error to in realtime control loop", 0.008, 0, 0.1) +gen.add("rotational_clip", double_t, 0, "Value to clip rotational error to in realtime control loop", 0.05, 0, 0.1) +gen.add("translational_Ki", double_t, 0, "Cartesian translational intergral gain", 0, 0, 200) +gen.add("rotational_Ki", double_t, 0, "Cartesian rotational intergral gain", 0, 0, 200) + +exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param")) diff --git a/robot_infra/launchers/impedence.launch b/robot_infra/launchers/impedence.launch new file mode 100644 index 0000000..93cfa13 --- /dev/null +++ b/robot_infra/launchers/impedence.launch @@ -0,0 +1,12 @@ + + + + + + + + + + + + diff --git a/robot_infra/launchers/joint.launch b/robot_infra/launchers/joint.launch new file mode 100644 index 0000000..80782e6 --- /dev/null +++ b/robot_infra/launchers/joint.launch @@ -0,0 +1,19 @@ + + + + + + + + + + + + + + + + + + + diff --git a/robot_infra/launchers/joint_position_example_controller.cpp b/robot_infra/launchers/joint_position_example_controller.cpp new file mode 100644 index 0000000..40c7985 --- /dev/null +++ b/robot_infra/launchers/joint_position_example_controller.cpp @@ -0,0 +1,88 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#include + +#include + +#include +#include +#include +#include +#include + +namespace franka_example_controllers { + +bool JointPositionExampleController::init(hardware_interface::RobotHW* robot_hardware, + ros::NodeHandle& node_handle) { + position_joint_interface_ = robot_hardware->get(); + if (position_joint_interface_ == nullptr) { + ROS_ERROR( + "JointPositionExampleController: Error getting position joint interface from hardware!"); + return false; + } + std::vector joint_names; + if (!node_handle.getParam("joint_names", joint_names)) { + ROS_ERROR("JointPositionExampleController: Could not parse joint names"); + } + if (joint_names.size() != 7) { + ROS_ERROR_STREAM("JointPositionExampleController: Wrong number of joint names, got " + << joint_names.size() << " instead of 7 names!"); + return false; + } + position_joint_handles_.resize(7); + for (size_t i = 0; i < 7; ++i) { + try { + position_joint_handles_[i] = position_joint_interface_->getHandle(joint_names[i]); + } catch (const hardware_interface::HardwareInterfaceException& e) { + ROS_ERROR_STREAM( + "JointPositionExampleController: Exception getting joint handles: " << e.what()); + return false; + } + } + + //std::array q_start{{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; + //for (size_t i = 0; i < q_start.size(); i++) { + // if (std::abs(position_joint_handles_[i].getPosition() - q_start[i]) > 0.1) { + // ROS_ERROR_STREAM( + // "JointPositionExampleController: Robot is not in the expected starting position for " + // "running this example. Run `roslaunch franka_example_controllers move_to_start.launch " + // "robot_ip:= load_gripper:=` first."); + // return false; + // } + //} + + return true; +} + +void JointPositionExampleController::starting(const ros::Time& /* time */) { + for (size_t i = 0; i < 7; ++i) { + initial_pose_[i] = position_joint_handles_[i].getPosition(); + } + elapsed_time_ = ros::Duration(0.0); +} + +void JointPositionExampleController::update(const ros::Time& /*time*/, + const ros::Duration& period) { + elapsed_time_ += period; + reset_pose_[0] = 0.001550; + reset_pose_[1] = -0.225743; + reset_pose_[2] = -0.045209; + reset_pose_[3] = -2.687688; + reset_pose_[4] = -0.082352; + reset_pose_[5] = 2.533348; + reset_pose_[6] = 1.014246; + double delta_angle = M_PI / 16 * (1 - std::cos(M_PI / 5.0 * elapsed_time_.toSec())) * 0.2; + for (size_t i = 0; i < 7; ++i) { + if (elapsed_time_.toSec() > 30){ + position_joint_handles_[i].setCommand(reset_pose_[i]); // - delta_angle); + } + else { + position_joint_handles_[i].setCommand( ((elapsed_time_.toSec()) / 30.0) * reset_pose_[i] + ((30 - elapsed_time_.toSec()) / 30.0) * initial_pose_[i]); + } + } +} + +} // namespace franka_example_controllers + +PLUGINLIB_EXPORT_CLASS(franka_example_controllers::JointPositionExampleController, + controller_interface::ControllerBase) diff --git a/robot_infra/launchers/joint_position_example_controller.h b/robot_infra/launchers/joint_position_example_controller.h new file mode 100644 index 0000000..ea39a86 --- /dev/null +++ b/robot_infra/launchers/joint_position_example_controller.h @@ -0,0 +1,32 @@ +// Copyright (c) 2017 Franka Emika GmbH +// Use of this source code is governed by the Apache-2.0 license, see LICENSE +#pragma once + +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace franka_example_controllers { + +class JointPositionExampleController : public controller_interface::MultiInterfaceController< + hardware_interface::PositionJointInterface> { + public: + bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override; + void starting(const ros::Time&) override; + void update(const ros::Time&, const ros::Duration& period) override; + + private: + hardware_interface::PositionJointInterface* position_joint_interface_; + std::vector position_joint_handles_; + ros::Duration elapsed_time_; + std::array initial_pose_{}; + std::array reset_pose_{}; +}; + +} // namespace franka_example_controllers diff --git a/robot_infra/setup.py b/robot_infra/setup.py new file mode 100644 index 0000000..60adece --- /dev/null +++ b/robot_infra/setup.py @@ -0,0 +1,17 @@ +from setuptools import setup +setup(name='franka_env', + version='0.0.1', + install_requires=['gym', + 'pypylon', + 'pyrealsense2', + 'opencv-python', + 'pyquaternion', + 'hidapi', + 'pyyaml', + 'rospkg', + 'scipy', + 'requests', + 'Pillow', + 'flask', + 'defusedxml'] +) \ No newline at end of file diff --git a/robot_infra/spacemouse/spacemouse.py b/robot_infra/spacemouse/spacemouse.py new file mode 100644 index 0000000..f88baee --- /dev/null +++ b/robot_infra/spacemouse/spacemouse.py @@ -0,0 +1,345 @@ +"""Driver class for SpaceMouse controller. + +This class provides a driver support to SpaceMouse on Mac OS X. +In particular, we assume you are using a SpaceMouse Wireless by default. + +To set up a new SpaceMouse controller: + 1. Download and install driver from https://www.3dconnexion.com/service/drivers.html + 2. Install hidapi library through pip + (make sure you run uninstall hid first if it is installed). + 3. Make sure SpaceMouse is connected before running the script + 4. (Optional) Based on the model of SpaceMouse, you might need to change the + vendor id and product id that correspond to the device. + +For Linux support, you can find open-source Linux drivers and SDKs online. + See http://spacenav.sourceforge.net/ + +""" + +import time +import threading +from collections import namedtuple +import numpy as np +# try: +import hid +# except ModuleNotFoundError as exc: +# raise ImportError("Unable to load module hid, required to interface with SpaceMouse. " +# "Only Mac OS X is officially supported. Install the additional " +# "requirements with `pip install -r requirements-ik.txt`") from exc +import math + +AxisSpec = namedtuple("AxisSpec", ["channel", "byte1", "byte2", "scale"]) + +SPACE_MOUSE_SPEC = { + "x": AxisSpec(channel=1, byte1=1, byte2=2, scale=1), + "y": AxisSpec(channel=1, byte1=3, byte2=4, scale=-1), + "z": AxisSpec(channel=1, byte1=5, byte2=6, scale=-1), + "roll": AxisSpec(channel=1, byte1=7, byte2=8, scale=-1), + "pitch": AxisSpec(channel=1, byte1=9, byte2=10, scale=-1), + "yaw": AxisSpec(channel=1, byte1=11, byte2=12, scale=1), +} + +def unit_vector(data, axis=None, out=None): + """ + Returns ndarray normalized by length, i.e. eucledian norm, along axis. + + Examples: + + >>> v0 = numpy.random.random(3) + >>> v1 = unit_vector(v0) + >>> numpy.allclose(v1, v0 / numpy.linalg.norm(v0)) + True + >>> v0 = numpy.random.rand(5, 4, 3) + >>> v1 = unit_vector(v0, axis=-1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=2)), 2) + >>> numpy.allclose(v1, v2) + True + >>> v1 = unit_vector(v0, axis=1) + >>> v2 = v0 / numpy.expand_dims(numpy.sqrt(numpy.sum(v0*v0, axis=1)), 1) + >>> numpy.allclose(v1, v2) + True + >>> v1 = numpy.empty((5, 4, 3), dtype=numpy.float32) + >>> unit_vector(v0, axis=1, out=v1) + >>> numpy.allclose(v1, v2) + True + >>> list(unit_vector([])) + [] + >>> list(unit_vector([1.0])) + [1.0] + + """ + if out is None: + data = np.array(data, dtype=np.float32, copy=True) + if data.ndim == 1: + data /= math.sqrt(np.dot(data, data)) + return data + else: + if out is not data: + out[:] = np.array(data, copy=False) + data = out + length = np.atleast_1d(np.sum(data * data, axis)) + np.sqrt(length, length) + if axis is not None: + length = np.expand_dims(length, axis) + data /= length + if out is None: + return data + +def rotation_matrix(angle, direction, point=None): + """ + Returns matrix to rotate about axis defined by point and direction. + + Examples: + + >>> angle = (random.random() - 0.5) * (2*math.pi) + >>> direc = numpy.random.random(3) - 0.5 + >>> point = numpy.random.random(3) - 0.5 + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(angle-2*math.pi, direc, point) + >>> is_same_transform(R0, R1) + True + >>> R0 = rotation_matrix(angle, direc, point) + >>> R1 = rotation_matrix(-angle, -direc, point) + >>> is_same_transform(R0, R1) + True + >>> I = numpy.identity(4, numpy.float32) + >>> numpy.allclose(I, rotation_matrix(math.pi*2, direc)) + True + >>> numpy.allclose(2., numpy.trace(rotation_matrix(math.pi/2, + ... direc, point))) + True + + """ + sina = math.sin(angle) + cosa = math.cos(angle) + direction = unit_vector(direction[:3]) + # rotation matrix around unit vector + R = np.array( + ((cosa, 0.0, 0.0), (0.0, cosa, 0.0), (0.0, 0.0, cosa)), dtype=np.float32 + ) + R += np.outer(direction, direction) * (1.0 - cosa) + direction *= sina + R += np.array( + ( + (0.0, -direction[2], direction[1]), + (direction[2], 0.0, -direction[0]), + (-direction[1], direction[0], 0.0), + ), + dtype=np.float32, + ) + M = np.identity(4) + M[:3, :3] = R + if point is not None: + # rotation not around origin + point = np.array(point[:3], dtype=np.float32, copy=False) + M[:3, 3] = point - np.dot(R, point) + return M + +def to_int16(y1, y2): + """Convert two 8 bit bytes to a signed 16 bit integer.""" + x = (y1) | (y2 << 8) + if x >= 32768: + x = -(65536 - x) + return x + + +def scale_to_control(x, axis_scale=350., min_v=-1.0, max_v=1.0): + """Normalize raw HID readings to target range.""" + x = x / axis_scale + x = min(max(x, min_v), max_v) + return x + + +def convert(b1, b2): + """Converts SpaceMouse message to commands.""" + return scale_to_control(to_int16(b1, b2)) + + +class SpaceMouse(): + """A minimalistic driver class for SpaceMouse with HID library.""" + + def __init__(self, vendor_id=9583, product_id=50741): + """Initialize a SpaceMouse handler. + + Args: + vendor_id: HID device vendor id + product_id: HID device product id + + Note: + Use hid.enumerate() to view all USB human interface devices (HID). + Make sure SpaceMouse is detected before running the script. + You can look up its vendor/product id from this method. + """ + + print("Opening SpaceMouse device") + self.device = hid.device() + # for x in hid.enumerate(): + # print() + # for key in sorted(x.keys()): + # print(key, x[key]) + self.device.open(vendor_id, product_id) # SpaceMouse + # self.device.open(1133, 50732) + # self.device.open(1452, 627) + + print("Manufacturer: %s" % self.device.get_manufacturer_string()) + print("Product: %s" % self.device.get_product_string()) + + self._display_controls() + + # pause = input() + + self.single_click_and_hold = False + + self._control = [0., 0., 0., 0., 0., 0.] + self._right = 0 + self.rotation = np.array([[-1., 0., 0.], [0., 1., 0.], [0., 0., -1.]]) + self._enabled = False + + # launch a new listener thread to listen to SpaceMouse + self.thread = threading.Thread(target=self.run) + self.thread.daemon = True + self.thread.start() + + self.start_control() + + def _device_info(self): + pass + + def _display_controls(self): + """ + Method to pretty print controls. + """ + + def print_command(char, info): + char += " " * (30 - len(char)) + print("{}\t{}".format(char, info)) + + print("") + print_command("Control", "Command") + print_command("Right button", "reset simulation") + print_command("Left button (hold)", "close gripper") + print_command("Move mouse laterally", "move arm horizontally in x-y plane") + print_command("Move mouse vertically", "move arm vertically") + print_command( + "Twist mouse about an axis", "rotate arm about a corresponding axis" + ) + print_command("ESC", "quit") + print("") + + def _reset_internal_state(self): + """ + Resets internal state of controller, except for the reset signal. + """ + self.rotation = np.array([[-1., 0., 0.], [0., 1., 0.], [0., 0., -1.]]) + + def start_control(self): + """ + Method that should be called externally before controller can + start receiving commands. + """ + self._reset_internal_state() + self._reset_state = 0 + self._enabled = True + + def get_controller_state(self): + """Returns the current state of the 3d mouse, a dictionary of pos, orn, grasp, and reset.""" + dpos = self.control[:3] * 0.005 + roll, pitch, yaw = self.control[3:] * 0.005 + + # convert RPY to an absolute orientation + drot1 = rotation_matrix(angle=-pitch, direction=[1., 0, 0], point=None)[:3, :3] + drot2 = rotation_matrix(angle=roll, direction=[0, 1., 0], point=None)[:3, :3] + drot3 = rotation_matrix(angle=yaw, direction=[0, 0, 1.], point=None)[:3, :3] + + self.rotation = self.rotation.dot(drot1.dot(drot2.dot(drot3))) + + return dict( + dpos=dpos, rotation=self.rotation, raw_drotation=np.array([roll, pitch, yaw]), + grasp=self._right, reset=self.single_click_and_hold + ) + + def run(self): + """Listener method that keeps pulling new messages.""" + + t_last_click = -1 + + while True: + d = self.device.read(14) + if d is not None and self._enabled: + if d[0] == 1: ## readings from 6-DoF sensor + self.y = convert(d[1], d[2]) + self.x = convert(d[3], d[4]) + self.z = convert(d[5], d[6]) * -1.0 + + # self.roll = convert(d[7], d[8]) + # self.pitch = convert(d[9], d[10]) + # self.yaw = convert(d[11], d[12]) + + # self._control = [ + # self.x, + # self.y, + # self.z, + # self.roll, + # self.pitch, + # self.yaw, + # ] + + elif d[0] == 2: ## readings from 6-DoF sensor + # self.y = convert(d[1], d[2]) + # self.x = convert(d[3], d[4]) + # self.z = convert(d[5], d[6]) * -1.0 + + self.roll = convert(d[1], d[2]) + self.pitch = convert(d[3], d[4]) + self.yaw = convert(d[5], d[6]) + try: + self._control = [ + self.x, + self.y, + self.z, + self.roll, + self.pitch, + self.yaw, + ] + except: + exit('Cannot launch spacemouse. Try again') + + elif d[0] == 3: ## readings from the side buttons + # print(d) + # press left button + if d[1] == 1: + t_click = time.time() + elapsed_time = t_click - t_last_click + t_last_click = t_click + self.single_click_and_hold = True + + # release left button + if d[1] == 0: + self.single_click_and_hold = False + self._right = 0 + + # right button is for reset + if d[1] == 2: + self._right = 1 + # self._enabled = False + # self._reset_internal_state() + + @property + def control(self): + """Returns 6-DoF control.""" + return np.array(self._control) + + @property + def control_gripper(self): + """Maps internal states into gripper commands.""" + if self.single_click_and_hold: + return 1.0 + return 0 + + +if __name__ == "__main__": + + space_mouse = SpaceMouse() + for i in range(1000): + print(space_mouse.control, space_mouse.control_gripper) + time.sleep(0.02) diff --git a/robot_infra/spacemouse/spacemouse_teleop.py b/robot_infra/spacemouse/spacemouse_teleop.py new file mode 100644 index 0000000..1a0036f --- /dev/null +++ b/robot_infra/spacemouse/spacemouse_teleop.py @@ -0,0 +1,60 @@ +"""Uses a spacemouse as action input into the environment. + +To use this, first clone robosuite (git@github.com:anair13/robosuite.git), +add it to the python path, and ensure you can run the following file (and +see input values from the spacemouse): + +robosuite/devices/spacemouse.py + +You will likely have to `pip install hidapi` and Spacemouse drivers. +""" +from spacemouse import SpaceMouse + +import numpy as np +import os +import argparse +import datetime +from PIL import Image +import pickle + +class SpaceMouseExpert(): + def __init__(self, xyz_dims=3, xyz_remap=[0, 1, 2], xyz_scale=[1, 1, 1], rot_scale=1, all_angles = False): + """TODO: fill in other params""" + + self.xyz_dims = xyz_dims + self.xyz_remap = np.array(xyz_remap) + self.xyz_scale = np.array(xyz_scale) + self.device = SpaceMouse() + self.grasp_input = 0. + self.grasp_output = 1. + self.rot_scale = rot_scale + self.all_angles = all_angles + + def get_action(self): + """Must return (action, valid, reset, accept)""" + state = self.device.get_controller_state() + dpos, rotation, rot, grasp, reset = ( + state["dpos"], + state["rotation"], + state["raw_drotation"], + state["grasp"], + state["reset"], + ) + # detect button press + if grasp and not self.grasp_input: + # open/close gripper + self.grasp_output = 1. if self.grasp_output <= 0. else -1. + self.grasp_input = grasp + # import pdb; pdb.set_trace() + xyz = dpos[self.xyz_remap] * self.xyz_scale + pitch, roll, yaw = tuple(list(rot * self.rot_scale)) + a = xyz[:self.xyz_dims] + if self.all_angles: + # 0 1 2, 3(grasp), 4, 5, 6 + a = np.concatenate([a, [self.grasp_output, roll, pitch, yaw]]) + else: + a = np.concatenate([a, [self.grasp_output, yaw]]) + + valid = not np.all(np.isclose(a, 0)) + + return (a, valid, reset, grasp) \ No newline at end of file diff --git a/serl/wrappers/franka_wrapper.py b/serl/wrappers/franka_wrapper.py index d3d29f6..fe55df5 100644 --- a/serl/wrappers/franka_wrapper.py +++ b/serl/wrappers/franka_wrapper.py @@ -1,5 +1,5 @@ import gym -from franka.env_franka.franka_env.envs.wrappers import GripperCloseEnv, SpacemouseIntervention +from robot_infra.env.wrappers import GripperCloseEnv, SpacemouseIntervention def FrankaWrapper(env: gym.Env,) -> gym.Env: env = GripperCloseEnv(env) env = SpacemouseIntervention(env)