Irl droid

CMakeLists.txt

@@ -15,7 +15,7 @@ endif()
 # -----------------------------
 # Build options
 # -----------------------------
-option(NO_ROBOT_TESTING "Enable local test mode (skip robot connection etc.)" ON)
+option(NO_ROBOT_TESTING "Enable local test mode (skip robot connection etc.)" OFF)
 
 if(NO_ROBOT_TESTING)
     message(STATUS "NO_ROBOT_TESTING mode is ON")
@@ -53,7 +53,6 @@ find_package(glfw3 REQUIRED)
 find_package(Franka REQUIRED)
 find_package(yaml-cpp REQUIRED)
 find_package(zerolancom REQUIRED)
-
 # -----------------------------
 # Source collection
 # -----------------------------
@@ -64,13 +63,17 @@ file(GLOB_RECURSE SOURCES
 # -----------------------------
 # Executable
 # -----------------------------
-add_executable(proxy src/main.cpp ${SOURCES})
+add_executable(proxy src/main.cpp ${SOURCES}
+    third_party/robotiq-gripper-interface/src/robotiq_gripper_interface.cc
+    third_party/robotiq-gripper-interface/src/helpers.cc
+    third_party/robotiq-gripper-interface/src/timeout_reader.cc)
 
 # -----------------------------
 # Include dirs & libs
 # -----------------------------
 target_include_directories(proxy PRIVATE
     include
+    third_party/robotiq-gripper-interface/include
     ${Franka_INCLUDE_DIRS}
     ${YAML_CPP_INCLUDE_DIR}
     # ${pinocchio_INCLUDE_DIRS}

config/SafetyLimitConfig.cfg

@@ -4,12 +4,12 @@ lpf_cutoff_freq: 100.0
 
 cartesian_pos_upper_limits: [0.5, 0.5, 0.5]
 cartesian_pos_lower_limits: [-0.5, -0.5, -0.5]
-
+#a liitle smaller than the real limits to avoid hitting the mechanical limits
 joint_pos_upper_limits: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]
 joint_pos_lower_limits: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]
 joint_vel_limits: [2.075, 2.075, 2.075, 2.075, 2.51, 2.51, 2.51]
 elbow_vel_limit: 2.1750
-joint_torques_limits: [86.0, 86.0, 86.0, 86.0, 11.5, 11.5, 11.5]
+joint_torques_limits: [40.0, 40.0, 40.0, 40.0, 5.0, 5.0, 5.0]
 
 margin_joint_pos: 0.1
 margin_joint_vel: 0.1

config/controller/hybrid_joint_impedance_controller.cfg

@@ -1,9 +1,20 @@
 name: HybridJointImpedance
 command_topic: FrankaPanda/joint_position_command
+#polymetis default
+kq_gains: [20, 30, 25, 25, 15, 10, 10]
+kqd_gains: [1.0, 1.5, 1.0, 1.0, 0.5, 0.5, 0.5]
+kx_gains: [100, 100, 100, 40, 40, 40]
+kxd_gains: [1, 1, 1, 0.2, 0.2, 0.2]
 
-kq_gains: [600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0]
-kqd_gains: [20.0, 20.0, 20.0, 20.0, 12.0, 8.0, 5.0]
-kx_gains: [150.0, 150.0, 150.0, 20.0, 20.0, 20.0]
-kxd_gains: [20.0, 20.0, 20.0, 2.0, 2.0, 2.0]
 
-ignore_gravity: true
+ignore_gravity: true # use for now
+#real_robot using
+# kq_gains: [16, 24, 20, 20, 12.75, 8.5, 8.5]
+# kqd_gains: [0.5, 0.75, 0.5, 0.5, 0.25, 0.25, 0.25]
+# kx_gains: [35, 35, 35, 14, 14, 14]
+# kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05]
+#polymetis default
+# kq_gains: [20, 30, 25, 25, 15, 10, 10] * 0.85
+# kqd_gains: [1.0, 1.5, 1.0, 1.0, 0.5, 0.5, 0.5] * 0.5
+# kx_gains: [100, 100, 100, 40, 40, 40] *0.35
+# kxd_gains: [1, 1, 1, 0.2, 0.2, 0.2] *0.25

config/gripper/droid_robotiq.yaml

@@ -0,0 +1,20 @@
+# Robotiq gripper settings
+name: FrankaPanda
+
+# Serial communication
+gripper_port: /dev/ttyUSB0
+gripper_baud: 115200
+
+# Scaling (see robotiq interface docs)
+scale_alpha: 1.0
+scale_beta: 0.0
+
+# Receive timeout (ms)
+receive_timeout_ms: 500
+
+# topics
+command_topic: FrankaPanda/robotiq_gripper_command
+state_topic: robotiq_gripper_state
+
+# State publish rate (Hz)
+gripper_pub_rate_hz: 10

config/gripper/robot_lab_gripper_201.yaml

@@ -0,0 +1,10 @@
+# Gripper settings
+name: FrankaFollowerGripper
+gripper_ip: 10.10.10.202
+
+command_topic: FrankaFollowerGripper/franka_gripper_command
+state_topic: franka_gripper_state
+enable_control: true
+
+gripper_pub_rate_hz: 100
+gripper_command_rcvtimeo_ms: 500

config/gripper/robot_lab_gripper_202.yaml

@@ -0,0 +1,10 @@
+# Gripper settings
+name: FrankaLeaderGripper
+gripper_ip: 10.10.10.201
+
+command_topic: FrankaLeaderGripper/franka_gripper_command
+state_topic: franka_gripper_state
+enable_control: false
+
+gripper_pub_rate_hz: 100
+gripper_command_rcvtimeo_ms: 500

config/proxy/droid.yaml

@@ -0,0 +1,14 @@
+# Franka Arm Configuration (follower)
+node_name: FrankaPanda
+robot_ip: 10.10.10.202
+proxy_ip: 192.168.0.232
+group: 224.0.0.1
+group_port: 7730
+group_name: DroidGroup
+
+robot:
+  - type: panda
+    config_path: config/robot/droid_panda.yaml
+grippers:
+  - type: robotiq_gripper
+    config_path: config/gripper/droid_robotiq.yaml

config/proxy/robot_lab_teleop_201_202.yaml

@@ -0,0 +1,17 @@
+# Franka Arm Configuration (leader)
+node_name: FrankaLeader
+proxy_ip: 141.3.53.63
+group: 224.0.0.1
+group_port: 7721
+group_name: hardware_collection
+
+robot:
+  - type: panda
+    config_path: config/robot/robot_lab_201.yaml
+  - type: panda
+    config_path: config/robot/robot_lab_202.yaml
+grippers:
+  - type: franka_gripper
+    config_path: config/gripper/robot_lab_gripper_201.yaml
+  - type: franka_gripper
+    config_path: config/gripper/robot_lab_gripper_202.yaml

config/RobotArmConfig.yaml → config/robot/droid_panda.yaml

@@ -3,7 +3,7 @@ name: FrankaPanda
 robot_ip: 10.10.10.10
 
 # Arm default state
-arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]
+arm_default_state_q: [0.1775, -0.2385, -0.1525, -2.0549, -0.1030, 1.7526, 0]
 arm_default_state_O_T_EE: [1.0, 0.0, 0.0, 0.3, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.5, 0.0, 0.0, 0.0, 1.0]
 
 # Arm settings

config/robot/robot_lab_panda_201.yaml

@@ -0,0 +1,24 @@
+# Franka Arm Configuration (leader)
+name: FrankaLeader
+robot_ip: 10.10.10.201
+
+# Arm default state
+arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]
+arm_default_state_O_T_EE: [1.0, 0.0, 0.0, 0.3, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.5, 0.0, 0.0, 0.0, 1.0]
+
+# Arm settings
+arm_state_pub_rate_hz: 100
+arm_socket_timeout_ms: 100
+arm_max_message_size: 4096
+
+# Collision thresholds (acceleration)
+arm_collision_lower_torque_thresholds_acc: [30, 30, 30, 30, 30, 30, 30]
+arm_collision_upper_torque_thresholds_acc: [45, 45, 45, 45, 45, 45, 45]
+arm_collision_lower_force_thresholds_acc: [8, 8, 8, 8, 8, 8]
+arm_collision_upper_force_thresholds_acc: [15, 15, 15, 15, 15, 15]
+
+# Collision thresholds (nominal)
+arm_collision_lower_torque_thresholds_nom: [35, 35, 35, 35, 35, 35, 35]
+arm_collision_upper_torque_thresholds_nom: [50, 50, 50, 50, 50, 50, 50]
+arm_collision_lower_force_thresholds_nom: [12, 12, 12, 12, 12, 12]
+arm_collision_upper_force_thresholds_nom: [20, 20, 20, 20, 20, 20]

config/robot/robot_lab_panda_202.yaml

@@ -0,0 +1,24 @@
+# Franka Arm Configuration (follower)
+name: FrankaFollower
+robot_ip: 10.10.10.202
+
+# Arm default state
+arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785]
+arm_default_state_O_T_EE: [1.0, 0.0, 0.0, 0.3, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.5, 0.0, 0.0, 0.0, 1.0]
+
+# Arm settings
+arm_state_pub_rate_hz: 100
+arm_socket_timeout_ms: 100
+arm_max_message_size: 4096
+
+# Collision thresholds (acceleration)
+arm_collision_lower_torque_thresholds_acc: [30, 30, 30, 30, 30, 30, 30]
+arm_collision_upper_torque_thresholds_acc: [45, 45, 45, 45, 45, 45, 45]
+arm_collision_lower_force_thresholds_acc: [8, 8, 8, 8, 8, 8]
+arm_collision_upper_force_thresholds_acc: [15, 15, 15, 15, 15, 15]
+
+# Collision thresholds (nominal)
+arm_collision_lower_torque_thresholds_nom: [35, 35, 35, 35, 35, 35, 35]
+arm_collision_upper_torque_thresholds_nom: [50, 50, 50, 50, 50, 50, 50]
+arm_collision_lower_force_thresholds_nom: [12, 12, 12, 12, 12, 12]
+arm_collision_upper_force_thresholds_nom: [20, 20, 20, 20, 20, 20]

include/control_mode/abstract_control_mode.hpp

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <Eigen/Dense>
 #include <memory>
 #include <mutex>
 #include <thread>
@@ -66,8 +67,12 @@ class AbstractControlMode
     void startControl();
     void stopControl();
     const std::string getModeName();
-    void controlTask();
-
+    virtual void controlTask();
+    bool moveToJointPosition(const std::array<double, NUM_DOFS>& target_q,
+                             double max_velocity = 0.1, double tolerance = 1e-2);
+    bool moveToCartesianPose(const Eigen::Vector3d& target_position,
+                             const Eigen::Quaterniond& target_orientation,
+                             double max_velocity = 0.5, double tolerance = 1e-3);
   protected:
     AbstractControlMode(const SafetyLimitConfig& safety_config) : safety_config_(safety_config) {}
     FrankaPanda* robot_;

include/control_mode/control_mode.hpp

@@ -7,6 +7,7 @@
 #include <control_mode/hybrid_joint_impedance_control.hpp>
 #include <control_mode/idle_control_mode.hpp>
 #include <control_mode/joint_impedance_control.hpp>
+#include <control_mode/gravity_comp_control.hpp>
 
 class ControlModeFactory
 {
@@ -32,6 +33,7 @@ class ControlModeFactory
         const SafetyLimitConfig& safety_config)
     {
         registry["Idle"] = std::make_unique<IdleControlMode>(safety_config);
+        registry["GravityComp"] = std::make_unique<GravityCompControl>(safety_config);
         registry["HybridJointImpedance"] =
             std::make_unique<HybridJointImpedanceControl>(safety_config);
         for (const auto& pair : registry)

include/control_mode/gravity_comp_control.hpp

@@ -0,0 +1,30 @@
+#pragma once
+#include "control_mode/abstract_control_mode.hpp"
+#include <array>
+
+// Hand-guiding controller with joint-space spring + damping (no gravity compensation).
+// Intended for the "leader" robot so a human can move it by hand.
+
+class GravityCompControl : public AbstractControlMode
+{
+public:
+    explicit GravityCompControl(const SafetyLimitConfig& safety_config)
+        : AbstractControlMode(safety_config)
+    {
+        controller_name = "GravityComp";
+    }
+    ~GravityCompControl() override = default;
+
+    // Reset internal state. Call this right before starting control.
+    void reset();
+
+private:
+    void initController(FrankaPanda& robot, PandaPinocchioModel& pinocchio_model,
+                        AtomicDoubleBuffer<franka::RobotState>& state_buffer) override;
+
+    franka::Torques controlLoop(const franka::RobotState& robot_state,
+                                franka::Duration duration) override;
+
+    bool hold_initialized_{false};
+    JointPosition hold_q_{JointPosition::Zero()};
+};

include/control_mode/idle_control_mode.hpp

@@ -14,6 +14,7 @@ class IdleControlMode : public AbstractControlMode
   private:
     void initController(FrankaPanda& robot, PandaPinocchioModel& model,
                         AtomicDoubleBuffer<franka::RobotState>& state_buffer) override;
+    void controlTask() override;
     franka::Torques controlLoop(const franka::RobotState& robot_state,
                                 franka::Duration duration) override;
 };