Code cleanup and README edits.

empriselab · Dec 25, 2023 · 74e0777 · 74e0777
1 parent 1e2f714
commit 74e0777
Show file tree

Hide file tree

Showing 10 changed files with 335 additions and 567 deletions.
diff --git a/.clang-format b/.clang-format
@@ -33,7 +33,7 @@ BreakBeforeBraces: Custom
 BreakBeforeInheritanceComma: true
 BreakBeforeTernaryOperators: true
 BreakConstructorInitializersBeforeComma: true
-ColumnLimit:     80
+ColumnLimit:     200
 CommentPragmas:  ''
 ConstructorInitializerAllOnOneLineOrOnePerLine: true
 ConstructorInitializerIndentWidth: 2
@@ -49,7 +49,7 @@ IncludeBlocks: Regroup
 IncludeCategories:
   - Regex:     '^<([a-z|_]+)>' # standard library headers
     Priority:  1
-  - Regex:     '^(<|")aikido/' # Aikido headers
+  - Regex:     '^(<|")pinocchio/' # Pinocchio headers
     Priority:  3
   - Regex:     '^(<[A-z]+)'    # dependency headers
     Priority:  2

diff --git a/README.md b/README.md
@@ -1,7 +1,7 @@
 # Kinova Gen3 Compliant Controllers
 ![build](https://github.com/empriselab/gen3_compliant_controllers/actions/workflows/build-test.yml/badge.svg)
 
-This repository provides joint-space and task-space compliant ROS1 controllers with [model-free friction observers](https://ieeexplore.ieee.org/document/8781838) for the Kinova Gen3 robot arm.
+This repository provides joint-space and task-space compliant ROS1 controllers with [model-free friction observers](https://ieeexplore.ieee.org/document/8781838) for the Kinova Gen3 robot arm. The controller formulation implemented can be found [here](media/controller_formulation.pdf).
 
 <p align="center">
   <img src="media/demo.gif" alt="Demo GIF" />
@@ -28,6 +28,8 @@ cd .. && catkin build
 ## Usage
 It is preferred to have the robot and the system running ROS on the same network and connected via ethernet (especially important for torque control). For safety purposes, **make sure to have the e-stop next to you when testing.**
 
+To ensure correct biasing of the robot's torque sensors, position the robot to the candlestick configuration with all joint positions set to zero. Then, zero out the actuator torque offset for each joint using the web application. For testing compliant control, return the robot to a general position like HOME; avoid testing at the candlestick position due to potential instability.
+
 Running instructions for testing the controllers:
 ```
 # In terminal 1
@@ -37,9 +39,9 @@ roslaunch gen3_compliant_controllers controller.launch ip_address:=<robot-ip-add
 
 # In terminal 2
 cd ~/hw_test_ws && source devel/setup.bash
-roscd gen3_compliant_controllers/script
+roscd gen3_compliant_controllers/scripts
 python test_controllers.py joint # <task/joint>
-# The robot will drop a bit as it switches to effort mode
+# The robot will jerk a bit as it switches to effort mode
 
 DOF: <specify joint number between 1-NDOF>
 Value: <enter joint value> # in radians

diff --git a/include/gen3_compliant_controllers/JointSpaceCompliantController.hpp b/include/gen3_compliant_controllers/JointSpaceCompliantController.hpp
@@ -1,162 +1,126 @@
 #ifndef GEN3_COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER_H
 #define GEN3_COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER_H
 
-// pinocchio
+// pinocchio headers
+#include <pinocchio/algorithm/frames.hpp>
 #include <pinocchio/algorithm/joint-configuration.hpp>
+#include <pinocchio/algorithm/model.hpp>
 #include <pinocchio/algorithm/rnea.hpp>
+#include <pinocchio/multibody/data.hpp>
 #include <pinocchio/parsers/sample-models.hpp>
+#include <pinocchio/parsers/urdf.hpp>
 
+// std headers
+#include <chrono>
 #include <string>
 #include <vector>
+using namespace std::chrono;
 
-#include <realtime_tools/realtime_buffer.h>
-#include <ros/node_handle.h>
-
-#include "pinocchio/algorithm/frames.hpp"
-#include "pinocchio/algorithm/model.hpp"
-#include "pinocchio/multibody/data.hpp"
-#include "pinocchio/parsers/urdf.hpp"
-
-// ROS messages
-#include <eigen_conversions/eigen_msg.h>
-#include <trajectory_msgs/JointTrajectoryPoint.h>
-
-// ros_controls
-#include <chrono>
-
+// ros headers
 #include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <eigen_conversions/eigen_msg.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <hardware_interface/joint_state_interface.h>
 #include <hardware_interface/robot_hw.h>
 #include <realtime_tools/realtime_buffer.h>
-using namespace std::chrono;
+#include <ros/node_handle.h>
+#include <trajectory_msgs/JointTrajectoryPoint.h>
 
-#include <dynamic_reconfigure/server.h>
+// local headers
 #include <gen3_compliant_controllers/JointSpaceCompliantControllerConfig.h>
 #include <gen3_compliant_controllers/helpers.hpp>
 
 namespace gen3_compliant_controllers {
-class JointSpaceCompliantController
-  : public controller_interface::MultiInterfaceController<
-        hardware_interface::EffortJointInterface,
-        hardware_interface::JointStateInterface>
+class JointSpaceCompliantController : public controller_interface::MultiInterfaceController<hardware_interface::EffortJointInterface, hardware_interface::JointStateInterface>
 {
 public:
+  // Constructor and Destructor
   JointSpaceCompliantController();
   ~JointSpaceCompliantController();
 
-  /** \brief The init function is called to initialize the controller from a
-   * non-realtime thread with a pointer to the hardware interface, itself,
-   * instead of a pointer to a RobotHW.
-   *
-   * \param robot The specific hardware interface used by this controller.
-   *
-   * \param n A NodeHandle in the namespace from which the controller
-   * should read its configuration, and where it should set up its ROS
-   * interface.
-   *
-   * \returns True if initialization was successful and the controller
-   * is ready to be started.
-   */
+  // Initialize the controller
   bool init(hardware_interface::RobotHW* robot, ros::NodeHandle& n) override;
 
-  /** \brief This is called from within the realtime thread just before the
-   * first call to \ref update
-   *
-   * \param time The current time
-   */
-  void starting(const ros::Time& time) override;
-  void stopping(const ros::Time& time) override;
-
-  /*!
-   * \brief Issues commands to the joint. Should be called at regular intervals
-   */
+  // Lifecycle hooks for the controller
+  void starting(const ros::Time& time) override; // Called before the first update
+  void stopping(const ros::Time& time) override; // Called when the controller is stopped
+
+  // Main update loop where control commands are computed and sent to the robot
   void update(const ros::Time& time, const ros::Duration& period) override;
 
 private:
-  std::unique_ptr<JointStateUpdater> mJointStateUpdater;
-  bool shouldAcceptRequests();
-  bool shouldStopExecution(std::string& message);
-
-  realtime_tools::RealtimeBuffer<trajectory_msgs::JointTrajectoryPoint>
-      mCommandsBuffer;
-  std::atomic_bool mExecuteDefaultCommand;
-
-  std::string mName;                    ///< Controller name.
-  std::vector<std::string> mJointNames; ///< Controlled joint names
-
-  // pinocchio objects
-  std::shared_ptr<pinocchio::Model> mModel;
-  std::unique_ptr<pinocchio::Data> mData;
-  pinocchio::Model::Index mEENode;
-  int mNumControlledDofs;
-
-  std::unique_ptr<ros::NodeHandle> mNodeHandle;
-
-  std::vector<hardware_interface::JointHandle> mControlledJointHandles;
-
-  // DYNAMIC PARAMETER OF KINOVA GEN3
-  Eigen::MatrixXd mJointStiffnessMatrix;
-  Eigen::MatrixXd mRotorInertiaMatrix;
-
-  Eigen::MatrixXd mFrictionL;
-  Eigen::MatrixXd mFrictionLp;
-
-  Eigen::MatrixXd mJointKMatrix;
-  Eigen::MatrixXd mJointDMatrix;
-
-  long long int mCount;
-
-  Eigen::VectorXd mActualTheta;
-  Eigen::VectorXd mActualThetaDot;
-
-  Eigen::VectorXd mDesiredPosition;
-  Eigen::VectorXd mDesiredVelocity;
-  Eigen::VectorXd mDesiredTheta;
-  Eigen::VectorXd mDesiredThetaDot;
-
-  Eigen::VectorXd mDesiredEffort;
-  Eigen::VectorXd mJointEffort;
-
-  Eigen::VectorXd mLastDesiredPosition;
-  Eigen::VectorXd mLastDesiredVelocity;
-
-  Eigen::VectorXd mNominalTheta;
-  Eigen::VectorXd mNominalThetaDot;
-  Eigen::VectorXd mNominalThetaDDot;
-
-  Eigen::VectorXd mNominalThetaPrev;
-  Eigen::VectorXd mNominalThetaDotPrev;
-
-  Eigen::VectorXd mZeros;
-  Eigen::VectorXd mGravity;
-  Eigen::VectorXd mQuasiGravity;
-  Eigen::VectorXd mNominalFriction;
-
-  Eigen::VectorXd mActualPosition;
-  Eigen::VectorXd mActualVelocity;
-  Eigen::VectorXd mActualEffort;
-
-  ExtendedJointPosition* mExtendedJoints;
-
-  ros::Subscriber mSubCommand;
-  void commandCallback(
-      const trajectory_msgs::JointTrajectoryPointConstPtr& msg);
-
-  // for debugging
-  std::chrono::time_point<std::chrono::high_resolution_clock> mLastTimePoint;
-
-  // dynamic reconfigure
-  dynamic_reconfigure::Server<
-      gen3_compliant_controllers::JointSpaceCompliantControllerConfig>
-      server;
-  dynamic_reconfigure::Server<
-      gen3_compliant_controllers::JointSpaceCompliantControllerConfig>::
-      CallbackType f;
-
-  void dynamicReconfigureCallback(
-      gen3_compliant_controllers::JointSpaceCompliantControllerConfig& config,
-      uint32_t level);
+  // STATE AND COMMAND HANDLING
+  std::unique_ptr<JointStateUpdater> mJointStateUpdater;                                 // Joint state updater
+  bool shouldAcceptRequests();                                                           // Check if controller should accept requests
+  bool shouldStopExecution(std::string& message);                                        // Check if controller should stop execution
+  realtime_tools::RealtimeBuffer<trajectory_msgs::JointTrajectoryPoint> mCommandsBuffer; // Command buffer
+  std::atomic_bool mExecuteDefaultCommand;                                               // Execute default command flag
+  ros::Subscriber mSubCommand;                                                           // Subscriber for command messages
+  void commandCallback(const trajectory_msgs::JointTrajectoryPointConstPtr& msg);        // Callback for command messages
+
+  // CONTROLLER CONFIGURATION
+  std::string mName;                                                    // Controller name
+  std::vector<std::string> mJointNames;                                 // Controlled joint names
+  std::unique_ptr<ros::NodeHandle> mNodeHandle;                         // Node handle
+  std::vector<hardware_interface::JointHandle> mControlledJointHandles; // Joint handles
+
+  // PINOCCHIO OBJECTS
+  std::shared_ptr<pinocchio::Model> mModel; // Model object
+  std::unique_ptr<pinocchio::Data> mData;   // Data object
+  pinocchio::Model::Index mEENode;          // End effector node index
+  int mNumControlledDofs;                   // Number of controlled joints
+
+  // TUNABLE PARAMETERS
+  Eigen::MatrixXd mJointStiffnessMatrix; // Joint stiffness matrix
+  Eigen::MatrixXd mRotorInertiaMatrix;   // Rotor inertia matrix
+  Eigen::MatrixXd mFrictionL;            // Friction observer matrix 1
+  Eigen::MatrixXd mFrictionLp;           // Friction observer matrix 2
+  Eigen::MatrixXd mJointKMatrix;         // Joint compliance proportional gain matrix
+  Eigen::MatrixXd mJointDMatrix;         // Joint compliance derivative gain matrix
+
+  long long int mCount; // Used during initialization
+
+  // SENSOR READINGS
+  Eigen::VectorXd mCurrentPosition; // Joint position read from sensor
+  Eigen::VectorXd mCurrentVelocity; // Joint velocity read from sensor
+  Eigen::VectorXd mCurrentEffort;   // Joint effort read from sensor
+
+  // COMPUTED VARIABLES
+  Eigen::VectorXd mCurrentTheta; // Normalized joint position
+
+  Eigen::VectorXd mDesiredPosition; // Desired joint position
+  Eigen::VectorXd mDesiredVelocity; // Desired joint velocity
+  Eigen::VectorXd mDesiredTheta;    // Desired motor position
+  Eigen::VectorXd mDesiredThetaDot; // Desired motor velocity
+
+  Eigen::VectorXd mCommandEffort; // Commanded joint effort
+  Eigen::VectorXd mTaskEffort;    // Task effort
+
+  Eigen::VectorXd mLastDesiredPosition; // Last desired joint position (used for checking if desired position has changed)
+  Eigen::VectorXd mLastDesiredVelocity; // Last desired joint velocity
+
+  Eigen::VectorXd mNominalTheta;     // Nominal joint position computed from model
+  Eigen::VectorXd mNominalThetaDot;  // Nominal joint velocity computed from model
+  Eigen::VectorXd mNominalThetaDDot; // Nominal joint acceleration computed from model
+
+  Eigen::VectorXd mNominalThetaPrev;    // Previous nominal joint position
+  Eigen::VectorXd mNominalThetaDotPrev; // Previous nominal joint velocity
+
+  Eigen::VectorXd mZeros;           // Vector of zeros
+  Eigen::VectorXd mGravity;         // Gravity computed from model
+  Eigen::VectorXd mQuasiGravity;    // Quasistatic gravity computed from model
+  Eigen::VectorXd mNominalFriction; // Nominal friction computed from model
+
+  ExtendedJointPosition* mExtendedJoints; // Used for computing normalized joint position
+
+  // DEBUGGING
+  std::chrono::time_point<std::chrono::high_resolution_clock> mLastTimePoint; 
+
+  // DYNAMIC RECONFIGURE
+  dynamic_reconfigure::Server<gen3_compliant_controllers::JointSpaceCompliantControllerConfig> server; // Dynamic reconfigure server
+  dynamic_reconfigure::Server<gen3_compliant_controllers::JointSpaceCompliantControllerConfig>::CallbackType f; // Dynamic reconfigure callback type
+  void dynamicReconfigureCallback(gen3_compliant_controllers::JointSpaceCompliantControllerConfig& config, uint32_t level); // Dynamic reconfigure callback function
 };
 
 } // namespace gen3_compliant_controllers