Skip to content

Commit

Permalink
Code cleanup and README edits.
Browse files Browse the repository at this point in the history
  • Loading branch information
@RKJenamani
RKJenamani committed Dec 25, 2023
1 parent 1e2f714 commit 2f4732c
Show file tree
Hide file tree
Showing 9 changed files with 226 additions and 294 deletions.
8 changes: 5 additions & 3 deletions README.md
View file
Original file line number Diff line number Diff line change
@@ -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" />
Expand All @@ -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
Expand All @@ -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
Expand Down
194 changes: 84 additions & 110 deletions include/gen3_compliant_controllers/JointSpaceCompliantController.hpp
View file
Original file line number Diff line number Diff line change
@@ -1,37 +1,33 @@
#ifndef GEN3_COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER_H
#define GEN3_COMPLIANT_CONTROLLERS__JOINT_SPACE_COMPLIANT_CONTROLLER_H

// pinocchio
// pinocchio headers
#include <pinocchio/algorithm/joint-configuration.hpp>
#include <pinocchio/algorithm/rnea.hpp>
#include <pinocchio/parsers/sample-models.hpp>
#include <pinocchio/algorithm/frames.hpp>
#include <pinocchio/algorithm/model.hpp>
#include <pinocchio/multibody/data.hpp>
#include <pinocchio/parsers/urdf.hpp>

// std headers
#include <string>
#include <vector>
#include <chrono>
using namespace std::chrono;

#include <realtime_tools/realtime_buffer.h>
// ros headers
#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>

#include <controller_interface/multi_interface_controller.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 <dynamic_reconfigure/server.h>

// local headers
#include <gen3_compliant_controllers/JointSpaceCompliantControllerConfig.h>
#include <gen3_compliant_controllers/helpers.hpp>

Expand All @@ -42,111 +38,89 @@ class JointSpaceCompliantController
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
// 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
dynamic_reconfigure::Server<
gen3_compliant_controllers::JointSpaceCompliantControllerConfig>
server;
Expand Down
Loading

0 comments on commit 2f4732c

Please sign in to comment.