From cd2a0f4b3eb0d4ad6b4dcf6d4e839cfb6eac363b Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Fri, 24 Nov 2023 17:00:16 +0100 Subject: [PATCH 1/8] adding ros2 building version of torque control feature --- CMakeLists.txt | 6 + README.md | 31 +-- config/robile1.yaml | 5 + config/robile2.yaml | 5 + config/robile3.yaml | 5 + config/robile4.yaml | 5 + include/kelo_tulip/KELORobotKinematics.h | 55 +++++ include/kelo_tulip/PlatformDriver.h | 13 +- .../PlatformToWheelInverseKinematicsSolver.h | 64 +++++ include/kelo_tulip/SmartWheelKinematics.h | 43 ++++ .../kelo_tulip/VelocityPlatformController.h | 28 +++ src/KELORobotKinematics.c | 96 ++++++++ src/PlatformDriver.cpp | 223 +++++++++++++++--- src/PlatformDriverROS.cpp | 30 ++- src/PlatformToWheelInverseKinematicsSolver.c | 149 ++++++++++++ src/SmartWheelKinematics.c | 41 ++++ src/VelocityPlatformController.cpp | 125 ++++++++++ 17 files changed, 869 insertions(+), 55 deletions(-) create mode 100644 include/kelo_tulip/KELORobotKinematics.h create mode 100644 include/kelo_tulip/PlatformToWheelInverseKinematicsSolver.h create mode 100644 include/kelo_tulip/SmartWheelKinematics.h create mode 100644 src/KELORobotKinematics.c create mode 100644 src/PlatformToWheelInverseKinematicsSolver.c create mode 100644 src/SmartWheelKinematics.c diff --git a/CMakeLists.txt b/CMakeLists.txt index 75dd599..6252ef6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -78,6 +78,9 @@ add_executable(platform_driver src/PlatformDriver.cpp src/PlatformDriverROS.cpp src/modules/RobileMasterBattery.cpp + src/PlatformToWheelInverseKinematicsSolver.c + src/SmartWheelKinematics.c + src/KELORobotKinematics.c ) ament_target_dependencies(platform_driver rclcpp std_msgs sensor_msgs geometry_msgs nav_msgs tf2 tf2_ros tf2_geometry_msgs) @@ -89,6 +92,9 @@ target_link_libraries(platform_driver ${Boost_LIBRARIES} soem pthread + gsl + gslcblas + m ) IF(USE_SETCAP) diff --git a/README.md b/README.md index b4f1248..86dff9a 100644 --- a/README.md +++ b/README.md @@ -11,19 +11,20 @@ You can move your mobile platform via a joypad for test purposes or use any soft ## System requirements -This software was tested on Ubuntu 16 with ROS Kinetic, Ubuntu 18 with ROS Melodic and Ubuntu 20 with ROS Noetic. Other Linux flavors should work as well. +This software was tested on Ubuntu 22.04 with ROS Humble and ROS Rolling. Please switch to `ros1` branch to use other tested versions: Ubuntu 16 with ROS Kinetic, Ubuntu 18 with ROS Melodic and Ubuntu 20 with ROS Noetic. Other Linux flavors should work as well. -For ROS it is enough to install the base system (for Noetic ros-noetic-ros-base). In addition it requires +For ROS it is enough to install the base system (for Humble ros-humble-ros-base). Additionally, GSL library should be installed, as it is used for the feature of compliant control of the wheels. ``` -sudo apt install ros-noetic-tf +sudo apt install ros-humble-ros-base +sudo apt install libgsl-dev ``` If another ROS version is used, replace the term noetic accordingly in these commands. ## Installation -The package can be compiled like any ROS package. Clone or copy it into a ROS workspace source folder and run `catkin_make` or `catkin build`, depending on your preferences. +The package can be compiled like any ROS package. Clone or copy it into a ROS workspace source folder and run `colcon build`, depending on your preferences. ### Permissions @@ -34,17 +35,9 @@ Special permissions need to be granted to the generated executable, since it nee sudo setcap cap_net_raw+ep ``` -Optionally, the command can be applied during the build process by passing the `-DUSE_SETCAP=ON` option to catkin. Default is `OFF`. +The executable can be found in `install/kelo_tulip/lib` directory. -``` -catkin_make -DUSE_SETCAP=ON -``` -OR -``` -catkin build kelo_tulip -DUSE_SETCAP=ON -``` - -**Note**: If you use this flag, you will be asked to input the sudo password during the build process. It might look like the build is going on but you need to lookout for `[sudo] password for ` in the output and enter the password after this prompt had appeared. If not, the build process will continue forever. +**Note**: This comand is included in the CMakeLists.txt, so it will ask for user password while building kelo_tulip package. It is sometimes not visible in the terminal. It might look like the build is going on but you need to lookout for `[sudo] password for ` in the output and enter the password after this prompt had appeared. If not, the build process will continue forever. ### Finding dynamic libraries @@ -57,13 +50,13 @@ devel/lib/kelo_tulip/platform_driver: error while loading shared libraries: libt In that case it is recommended to set the path to those libraries on system level. The following method should work on default installations, but please adapt accordingly if you already made other changes in your system. -For ROS Noetic, the following command will add an entry to point to the dynamic libraries of ROS: +For ROS2 Humble, the following command will add an entry to point to the dynamic libraries of ROS: ``` -sudo sh -c 'echo "/opt/ros/noetic/lib/" > /etc/ld.so.conf.d/ros.conf' +sudo sh -c 'echo "/opt/ros/humble/lib/" > /etc/ld.so.conf.d/ros.conf' ``` -If not using ROS Noetic, the path there should be changed accordingly. +If not using ROS2 Humble, the path there should be changed accordingly. Afterwards you need to run @@ -79,12 +72,12 @@ to make the changes take effect. The program can be started by running ``` -roslaunch kelo_tulip example.launch +ros2 launch kelo_tulip example.launch ``` ### Parameters -The default launch file in `kelo_tulip/launch/example.launch` loads the YAML configuration from `config/example.yaml`. Feel free to change parameters directly in this config file, or to make a copy and adjust the launch file to load the new file. +The default launch file in `kelo_tulip/launch/example.launch.py` loads the YAML configuration from `config/example.yaml`. Feel free to change parameters directly in this config file, or to make a copy and adjust the launch file to load the new file. #### Network interface diff --git a/config/robile1.yaml b/config/robile1.yaml index cedf28f..071b40c 100644 --- a/config/robile1.yaml +++ b/config/robile1.yaml @@ -3,6 +3,11 @@ platform_driver: num_wheels: 1 num_masters: 1 device: eno1 + /base_control_mode: VELOCITY_CONTROL + + platform_Kd_x: 1200.0 + platform_Kd_y: 1200.0 + platform_Kd_a: 400.0 wheel0: ethercat_number: 3 diff --git a/config/robile2.yaml b/config/robile2.yaml index 207e897..bcf6e0b 100644 --- a/config/robile2.yaml +++ b/config/robile2.yaml @@ -3,6 +3,11 @@ platform_driver: num_wheels: 2 num_masters: 1 device: eno1 + /base_control_mode: VELOCITY_CONTROL + + platform_Kd_x: 1200.0 + platform_Kd_y: 1200.0 + platform_Kd_a: 400.0 wheel0: ethercat_number: 5 diff --git a/config/robile3.yaml b/config/robile3.yaml index 8cdd525..4a19547 100644 --- a/config/robile3.yaml +++ b/config/robile3.yaml @@ -3,7 +3,12 @@ platform_driver: num_wheels: 4 num_masters: 1 device: eno1 + /base_control_mode: VELOCITY_CONTROL + platform_Kd_x: 1200.0 + platform_Kd_y: 1200.0 + platform_Kd_a: 400.0 + vlin_max: 1.2 va_max: 1.0 diff --git a/config/robile4.yaml b/config/robile4.yaml index f0c3262..e6915c6 100644 --- a/config/robile4.yaml +++ b/config/robile4.yaml @@ -3,6 +3,11 @@ platform_driver: num_wheels: 4 num_masters: 1 device: eno1 + /base_control_mode: VELOCITY_CONTROL + + platform_Kd_x: 1200.0 + platform_Kd_y: 1200.0 + platform_Kd_a: 400.0 vlin_max: 1.2 va_max: 1.0 diff --git a/include/kelo_tulip/KELORobotKinematics.h b/include/kelo_tulip/KELORobotKinematics.h new file mode 100644 index 0000000..2e91f54 --- /dev/null +++ b/include/kelo_tulip/KELORobotKinematics.h @@ -0,0 +1,55 @@ +/** + * @file KELORobotKinematics.h + * @author Kishan Sawant (kishan.sawant@smail.inf.h-brs.de) + * @brief function to calculate the 3x8 Jacobian matrix(A) and to calculate forces at individual wheel units + * @date 2022-02-06 + * + */ +#ifndef KELO_ROBOT_KINEMATICS_H +#define KELO_ROBOT_KINEMATICS_H + +#include + +/** + * @brief function to calculate the 3x8 matrix(A) to map force from individual wheels to platform + * + * @param A 3x8 matrix(Jacobian) which maps form individual wheels to platform + * @param pivot_angles 1D array of shape (4,) that stores current pivot angles of individual pair of wheels + * @param wheel_coordinates 1-D array of shape (8,) which holds the co-ordinates of individual wheels (x1,y1,x2,y2,..,y4) + */ +void jacobian_matrix_calculator(gsl_matrix *A, + const double *pivot_angles, + const double *wheel_coordinates); + +/** + * @brief to solve the inverse of the Jacobian matrix(A) and calculate the pivot forces using it + * + * @param pivot_forces forces at individual wheel units + * @param A Jacobian matrix of shape 3x8, which is further used to store W.dot(A).dot(K) + * @param A_tmp a temporary matrix used to store the matrix multiplication of W.dot(A) + * @param A_inv_T_tmp used as temporary memory to calculate A_inv_T + * @param A_inv_T initially store left singular matrix of SVD decomposition of A and later it stores the inverse of W.dot(A).dot(K) + * @param u a vector consisting of singular values of Sigma matrix in SVD decomposition of A(Jacobian matrix) + * @param u_inv inverse of the diagonal matrix consisting of eigenvalues of Sigma matrix + * @param V right singular matrix of SVD decomposition + * @param W weight matrix of shape 3x3 + * @param K weight matrix of shape 8x8 + * @param work temporary memory to facilitate SVD of Jacobian matrix(A) + * @param b platform force matrix of shape 3x1 + * @param M number of wheel units (8) + */ +void force_vector_finder(double *pivot_forces, + gsl_matrix *A, + gsl_matrix *A_tmp, + gsl_matrix *A_inv_T_tmp, + gsl_matrix *A_inv_T, + gsl_vector *u, + gsl_matrix *u_inv, + gsl_matrix *V, + const gsl_matrix *W, + const gsl_matrix *K, + gsl_vector *work, + const gsl_matrix *b, + const unsigned int M); + +#endif diff --git a/include/kelo_tulip/PlatformDriver.h b/include/kelo_tulip/PlatformDriver.h index 3e9c7f7..77c4d30 100644 --- a/include/kelo_tulip/PlatformDriver.h +++ b/include/kelo_tulip/PlatformDriver.h @@ -109,7 +109,9 @@ class PlatformDriver { void setProcessData(int slave, rxpdo1_t* data); void setTargetVelocity(double vx, double vy, double va); - + void setMeasuredVelocity(double &vx, double &vy, double &va); + void setPlatformDampingParameters(double *damping_parameters); + void setControlMode(std::string &base_control_mode); void setWheelDistance(double x); void setWheelDiameter(double x); void setCurrentStop(double x); @@ -174,10 +176,10 @@ class PlatformDriver { ecx_redportt ecx_redport; ecx_contextt ecx_context; - std::string device; std::vector modules; char IOmap[4096]; + std::string device; bool ethercatInitialized; boost::thread* ethercatThread; volatile bool stopThread; @@ -204,12 +206,17 @@ class PlatformDriver { double wheelDistance; double wheelDiameter; + double motor_constant; double maxCalibrationTime; double vCalibration; double currentCalibration; double currentStop; double currentDrive; + double measured_platform_velocity[3]; + double platform_damping_parameters[3]; + std::string platform_control_mode; + double maxvlin; double maxva; double maxvlinacc; @@ -218,6 +225,8 @@ class PlatformDriver { double wheelsetpointmin; double wheelsetpointmax; + double torque_wheelsetpointmin; + double torque_wheelsetpointmax; volatile bool statusError; volatile bool slipError; diff --git a/include/kelo_tulip/PlatformToWheelInverseKinematicsSolver.h b/include/kelo_tulip/PlatformToWheelInverseKinematicsSolver.h new file mode 100644 index 0000000..8881a3a --- /dev/null +++ b/include/kelo_tulip/PlatformToWheelInverseKinematicsSolver.h @@ -0,0 +1,64 @@ +/** + * @file PlatformToWheelInverseKinematicsSolver.h + * @author Sivva Rahul Sai (rahul.sivva@smail.inf.h-brs.de) + * @brief this file consists of a functionality to print a matrix and to integrate all functions related to calculating torques at individual wheel units + * @date 2022-03-12 + * + */ +#ifndef __cplusplus +#include +#endif + +#ifndef PLATFORM_WHEEL_INVERSE_KINEMATICS_SOLVER_H +#define PLATFORM_WHEEL_INVERSE_KINEMATICS_SOLVER_H + +#include + +/** + * @brief function which takes pivot angles and platform forces as input and calculates the required wheel torques by suitably calling individual functinalities + * + * @param wheel_torques 1-D array of shape (8,) which is a placeholder for calculated wheel torques for each wheel unit + * @param pivot_angles 1D array of shape (4,) that stores current pivot angles of individual pair of wheels + * @param b platform force matrix of shape 3x1 + * @param b_verify platform force matrix of shape 3x1, which is calculated from pivot forces to verify results + * @param A Jacobian matrix of shape 3x8, which is further used to store W.dot(A).dot(K) + * @param A_inv_T initially store left singular matrix of SVD decomposition of A and later it stores the inverse of W.dot(A).dot(K) + * @param A_tmp a temporary matrix used to store the matrix multiplication of W.dot(A) + * @param A_inv_T_tmp used as temporary memory to calculate A_inv_T + * @param work temporary memory to facilitate SVD of Jacobian matrix(A) + * @param W weight matrix of shape 3x3 + * @param K weight matrix of shape 8x8 + * @param u a vector consisting of singular values of Sigma matrix in SVD decomposition of A(Jacobian matrix) + * @param V right singular matrix of SVD decomposition + * @param u_inv inverse of the diagonal matrix consisting of eigenvalues of Sigma matrix + * @param M number of wheel units (8) + * @param N platform force dimension (3) + * @param debug boolean used to run the function to display necessary print values + */ +void functions_main(double *wheel_torques, + double *pivot_angles, + double *wheel_coordinates, + double *pivot_angles_deviation, + const gsl_matrix *b, + gsl_matrix *b_verify, + gsl_matrix *A, + gsl_matrix *A_inv_T, + gsl_matrix *A_tmp, + gsl_matrix *A_inv_T_tmp, + gsl_vector *work, + const gsl_matrix *W, + const gsl_matrix *K, + gsl_vector *u, + gsl_matrix *V, + gsl_matrix *u_inv, + const unsigned int M, + const unsigned int N, + const bool debug); + +/** + * @brief function to print a matrix + * + * @param m matrix to be printed + */ +void print_matrix(const gsl_matrix *m); +#endif \ No newline at end of file diff --git a/include/kelo_tulip/SmartWheelKinematics.h b/include/kelo_tulip/SmartWheelKinematics.h new file mode 100644 index 0000000..438276e --- /dev/null +++ b/include/kelo_tulip/SmartWheelKinematics.h @@ -0,0 +1,43 @@ +/** + * @file SmartWheelKinematics.h + * @author Kavya Shankar (kavya.shankar@smail.inf.h-brs.de) + * @brief the mapping from pivot forces to wheel torques is formulated + * @date 2022-03-12 + * + */ + +#ifndef TORQUE_TRANSMISSION_H +#define TORQUE_TRANSMISSION_H + +/** + * @brief for a given pair of wheels, torques are calculated to generate corresponding force + * + * @param pivot_forces forces at individual wheel units + * @param wheel_torques 1-D array of shape (8,) which is a placeholder for calculated wheel torques for each wheel unit + * @param radius radius of all wheel units + * @param castor_offset castor-offset of a pair of wheels from pivot + * @param half_wheel_distance half of the distance between two wheel units in a pair of wheels + */ + +void map_pivot_force_to_wheel_torque(const double *pivot_forces, + double *wheel_torques, + const double radius, + const double castor_offset, + const double half_wheel_distance); +/** + * @brief pivot forces for every pair of wheels are provide as input to "map_pivot_force_to_wheel_torque" to calculate corresponding torque values + * + * @param pivot_forces forces at individual wheel units + * @param wheel_torques 1-D array of shape (8,) which is a placeholder for calculated wheel torques for each wheel unit + * @param radius radius of wheel units + * @param castor_offset castor-offset of a pair of wheels from pivot + * @param half_wheel_distance half of the distance between two wheel units in a pair of wheels + */ + +void map_pivot_forces_to_wheel_torques(const double *pivot_forces, + double *wheel_torques, + const double radius, + const double castor_offset, + const double half_wheel_distance); + +#endif \ No newline at end of file diff --git a/include/kelo_tulip/VelocityPlatformController.h b/include/kelo_tulip/VelocityPlatformController.h index ec63eab..b79246c 100644 --- a/include/kelo_tulip/VelocityPlatformController.h +++ b/include/kelo_tulip/VelocityPlatformController.h @@ -52,6 +52,19 @@ #include #include +extern "C" { +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +} + namespace kelo { @@ -71,6 +84,21 @@ namespace kelo void calculatePlatformRampedVelocities(); + void calculateWheelTargetTorques(double *wheel_torques, + double *pivot_angles, + double *wheel_coordinates, + double *pivot_angles_deviation, + double *measured_platform_velocity, + double *platform_damping_parameters, + const gsl_matrix *K, + const gsl_matrix *W, + const unsigned int M, + const unsigned int N); + + void getPivotError(const size_t &wheel_index, + const float &raw_pivot_angle, + float &pivot_error); + void calculateWheelTargetVelocity(const size_t &wheel_index, const float &pivot_angle, float &target_ang_vel_l, diff --git a/src/KELORobotKinematics.c b/src/KELORobotKinematics.c new file mode 100644 index 0000000..c8a4b7d --- /dev/null +++ b/src/KELORobotKinematics.c @@ -0,0 +1,96 @@ +/** + * @file KELORobotKinematics.c + * @author Kishan Sawant (kishan.sawant@smail.inf.h-brs.de) + * @brief function to calculate the 3x8 Jacobian matrix(A) and to calculate forces at individual wheel units + * @date 2022-02-06 + * + */ +#include +#include +#include +#include +#include + +void jacobian_matrix_calculator(gsl_matrix *A, + const double *pivot_angles, + const double *wheel_coordinates) +{ + int i; + for (i = 0; i < 4; ++i) + { + gsl_matrix_set(A, 0, 2 * i, gsl_sf_cos(pivot_angles[i])); + gsl_matrix_set(A, 0, 2 * i + 1, -gsl_sf_sin(pivot_angles[i])); + gsl_matrix_set(A, 1, 2 * i, gsl_sf_sin(pivot_angles[i])); + gsl_matrix_set(A, 1, 2 * i + 1, gsl_sf_cos(pivot_angles[i])); + gsl_matrix_set(A, 2, 2 * i, wheel_coordinates[2 * i] * gsl_sf_sin(pivot_angles[i]) - wheel_coordinates[2 * i + 1] * gsl_sf_cos(pivot_angles[i])); + gsl_matrix_set(A, 2, 2 * i + 1, wheel_coordinates[2 * i] * gsl_sf_cos(pivot_angles[i]) + wheel_coordinates[2 * i + 1] * gsl_sf_sin(pivot_angles[i])); + } +} + +void force_vector_finder(double *pivot_forces, + gsl_matrix *A, + gsl_matrix *A_tmp, + gsl_matrix *A_inv_T_tmp, + gsl_matrix *A_inv_T, + gsl_vector *u, + gsl_matrix *u_inv, + gsl_matrix *V, + const gsl_matrix *W, + const gsl_matrix *K, + gsl_vector *work, + const gsl_matrix *b, + const unsigned int M) +{ + /** + * @brief Equations: + J# = Mq * V * pinv_dls(S) * U.T * Mx + B = Mx * J * Mq (Note: B = U * S * V.T) + J# * b = pivot_forces + * + */ + + /** + * @brief B = Mx * J * Mq + * + */ + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, W, A, 0.0, A_tmp); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A_tmp, K, 0.0, A); + gsl_matrix_transpose_memcpy(A_inv_T, A); // we need M>N to solve SVD, but as M < N, transpose of A has been considered + /** + * @brief Note: singular values are invariant to matrix transpose, as S = S.T + * + */ + + /** + * @brief B = U * S * V.T + * + */ + gsl_linalg_SV_decomp(A_inv_T, V, u, work); // SVD decomposition (A gets replaced by values of U matrix) + gsl_matrix_transpose(V); // here V is indeed U, as transpose of A is being considered + + /** + * @brief finding S_inv + * + */ + size_t i; + for (i = 0; i < u->size; i++) + { + gsl_matrix_set(u_inv, i, i, 1/gsl_vector_get(u,i)); + } + + /** + * @brief J# = M_q * V * pinv_dls(S) * U.T * M_x + * + */ + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, K, A_inv_T, 0.0, A_inv_T_tmp); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A_inv_T_tmp, u_inv, 0.0, A_inv_T); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A_inv_T, V, 0.0, A_inv_T_tmp); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A_inv_T_tmp, W, 0.0, A_inv_T); + gsl_matrix_view _X = gsl_matrix_view_array(pivot_forces,M,1); + + /** + * @brief J# * b = pivot_forces + * + */ + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A_inv_T, b, 0.0, &_X.matrix); +} diff --git a/src/PlatformDriver.cpp b/src/PlatformDriver.cpp index 1835212..ebcde6e 100755 --- a/src/PlatformDriver.cpp +++ b/src/PlatformDriver.cpp @@ -67,6 +67,7 @@ PlatformDriver::PlatformDriver(std::string device, std::vector , nWheels(nWheels) { this->device = device; + platform_control_mode = "VELOCITY_CONTROL"; if (wheelConfigs->size() != wheelData->size() || nWheels > wheelConfigs->size()) { // should not happen std::cout << "SmartWheel error: wheel data structs have inconsistent size\n"; @@ -76,6 +77,7 @@ PlatformDriver::PlatformDriver(std::string device, std::vector wheelDistance = 0.0775; wheelDiameter = 0.104; + motor_constant = 0.29; //units: (Ampere/Newton-meter) currentStop = 1; currentDrive = 20; @@ -97,6 +99,9 @@ PlatformDriver::PlatformDriver(std::string device, std::vector wheelsetpointmin = 0.01; wheelsetpointmax = 35.0; + torque_wheelsetpointmin = 0.01; + torque_wheelsetpointmax = 3.0; + EcatError = FALSE; current_ts = 0; swErrorCount = 0; @@ -150,6 +155,23 @@ void PlatformDriver::setTargetVelocity(double vx, double vy, double va) { velocityPlatformController.setPlatformTargetVelocity(vx, vy, va); } +void PlatformDriver::setMeasuredVelocity(double &vx, double &vy, double &va) { + measured_platform_velocity[0] = vx; + measured_platform_velocity[1] = vy; + measured_platform_velocity[2] = va; +} + +void PlatformDriver::setPlatformDampingParameters(double *damping_parameters) { + + for (size_t i = 0; i < 3; ++i) { + platform_damping_parameters[i] = damping_parameters[i]; + } +} + +void PlatformDriver::setControlMode(std::string &base_control_mode) { + platform_control_mode = base_control_mode; +} + void PlatformDriver::setWheelDistance(double x) { wheelDistance = x; } @@ -434,8 +456,8 @@ void PlatformDriver::ethercatHandler() { wkc = ecx_receive_processdata(&ecx_context, ethercatTimeout); // TODO fix -static int expectedWkc = 0; -static int lastWkc = 0; + static int expectedWkc = 0; + static int lastWkc = 0; //if (step < 10) // std::cout << "wkc=" << wkc << std::endl; @@ -708,11 +730,22 @@ void PlatformDriver::updateEncoders() { void PlatformDriver::doStop() { rxpdo1_t rxdata; rxdata.timestamp = current_ts + 100 * 1000; // TODO - rxdata.command1 = COM1_ENABLE1 | COM1_ENABLE2 | COM1_MODE_VELOCITY; - rxdata.limit1_p = currentStop; - rxdata.limit1_n = -currentStop; - rxdata.limit2_p = currentStop; - rxdata.limit2_n = -currentStop; + if (platform_control_mode == "TORQUE_CONTROL") + { + rxdata.command1 = COM1_ENABLE1 | COM1_ENABLE2 | COM1_MODE_TORQUE; + rxdata.limit1_p = 0.0; + rxdata.limit1_n = 0.0; + rxdata.limit2_p = 0.0; + rxdata.limit2_n = 0.0; + } + else + { + rxdata.command1 = COM1_ENABLE1 | COM1_ENABLE2 | COM1_MODE_VELOCITY; + rxdata.limit1_p = currentStop; + rxdata.limit1_n = -currentStop; + rxdata.limit2_p = currentStop; + rxdata.limit2_n = -currentStop; + } rxdata.setpoint1 = 0; rxdata.setpoint2 = 0; @@ -737,36 +770,162 @@ void PlatformDriver::doControl() { rxdata.setpoint1 = 0; rxdata.setpoint2 = 0; + if (platform_control_mode == "TORQUE_CONTROL") + { + /* initialise struct to be sent to wheels */ + rxdata.timestamp = current_ts + 100 * 1000; // TODO + rxdata.command1 = COM1_ENABLE1 | COM1_ENABLE2 | COM1_MODE_TORQUE; + rxdata.limit1_p = 3; + rxdata.limit1_n = -3; + rxdata.limit2_p = 3; + rxdata.limit2_n = -3; + } + else + { + rxdata.timestamp = current_ts + 100 * 1000; // TODO + rxdata.command1 = COM1_ENABLE1 | COM1_ENABLE2 | COM1_MODE_VELOCITY; + rxdata.limit1_p = currentDrive; + rxdata.limit1_n = -currentDrive; + rxdata.limit2_p = currentDrive; + rxdata.limit2_n = -currentDrive; + } + // update desired velocity of platform, based on target velocity and veloity ramps velocityPlatformController.calculatePlatformRampedVelocities(); - for (unsigned int i = firstWheel; i < firstWheel + nWheels; i++) { + const unsigned int N = 3; + const unsigned int M = nWheels * 2; + double wheel_torques[M]; + double pivot_angles[nWheels]; + double wheel_coordinates[M]; + double pivot_angles_deviation[nWheels]; + + for (unsigned int i = 0; i < nWheels; i++) { + txpdo1_t* ecData = (txpdo1_t*) ecx_slave[(*wheelConfigs)[i].ethercatNumber].inputs; + pivot_angles[i] = ecData->encoder_pivot; + wheel_coordinates[2*i] = (*wheelConfigs)[i].x; + wheel_coordinates[2*i + 1] = (*wheelConfigs)[i].y; + pivot_angles_deviation[i] = (*wheelConfigs)[i].a; + } + + if (platform_control_mode == "TORQUE_CONTROL") + { + + gsl_matrix *W = gsl_matrix_alloc(N, N); + gsl_matrix *K = gsl_matrix_alloc(M, M); + + // setting the weights + size_t i; + for (i = 0; i < M; i++) + { + gsl_matrix_set(K, i, i, 1.0); + /* To deactivate some of the wheels, setting weights to zero */ + // if (i == 0 || i == 1 || i == 4 || i == 5) + // { + // gsl_matrix_set(K, i, i, 1.0); + // } + // else + // { + // gsl_matrix_set(K, i, i, 0.0); + // } + if (i < N) + { + gsl_matrix_set(W, i, i, 1.0); + } + } + + /* calculate wheel target torques based on the desired velocity*/ + velocityPlatformController.calculateWheelTargetTorques(wheel_torques, + pivot_angles, + wheel_coordinates, + pivot_angles_deviation, + measured_platform_velocity, + platform_damping_parameters, + K, + W, + M, + N); + + // releasing memory + gsl_matrix_free(W); + gsl_matrix_free(K); + } + + for (size_t i = firstWheel; i < firstWheel + nWheels; i++) { txpdo1_t* wheel_data = getProcessData((*wheelConfigs)[i].ethercatNumber); - float setpoint1, setpoint2; - - /* calculate wheel target velocity */ - velocityPlatformController.calculateWheelTargetVelocity(i, wheel_data->encoder_pivot, - setpoint2, setpoint1); - setpoint1 *= -1; // because of inverted frame - - /* avoid sending close to zero values */ - if ( fabs(setpoint1) < wheelsetpointmin ) - { - setpoint1 = 0; - } - if ( fabs(setpoint2) < wheelsetpointmin ) - { - setpoint2 = 0; - } - - /* avoid sending very large values */ - setpoint1 = Utils::clip(setpoint1, wheelsetpointmax, -wheelsetpointmax); - setpoint2 = Utils::clip(setpoint2, wheelsetpointmax, -wheelsetpointmax); - - /* send calculated target velocity values to EtherCAT */ - rxdata.setpoint1 = setpoint1; - rxdata.setpoint2 = setpoint2; + float setpoint1 = 0.0; + float setpoint2 = 0.0; + if (platform_control_mode == "TORQUE_CONTROL") + { + // std::cout << "Entered TCM" << std::endl; + float pivot_error = 0.0; + double motor_torque_to_align_wheels = 1.3; // Nm/A + double error_margin = 0.25; // allowed pivot_error + setpoint1 = - wheel_torques[2 * i]; // because of inverted frame + setpoint2 = wheel_torques[2 * i + 1]; + + velocityPlatformController.getPivotError(i, wheel_data->encoder_pivot, pivot_error); + + /* avoid sending close to zero values */ + if ( fabs(setpoint1) < torque_wheelsetpointmin ) + { + setpoint1 = 0; + } + if ( fabs(setpoint2) < torque_wheelsetpointmin ) + { + setpoint2 = 0; + } + + /* avoid sending very large values */ + setpoint1 = Utils::clip(setpoint1, torque_wheelsetpointmax, -torque_wheelsetpointmax); + setpoint2 = Utils::clip(setpoint2, torque_wheelsetpointmax, -torque_wheelsetpointmax); + + if (pivot_error < -error_margin) + { + // rotate anti-clockwise + setpoint1 += motor_torque_to_align_wheels; + setpoint2 += motor_torque_to_align_wheels; + + } + else if (pivot_error > error_margin) + { + // rotate clockwise + setpoint1 -= motor_torque_to_align_wheels; + setpoint2 -= motor_torque_to_align_wheels; + } + + /* send calculated current values to EtherCAT */ + rxdata.setpoint1 = setpoint1 / motor_constant; + rxdata.setpoint2 = setpoint2 / motor_constant; + } + else + { + + /* calculate wheel target velocity */ + velocityPlatformController.calculateWheelTargetVelocity(i, wheel_data->encoder_pivot, + setpoint2, setpoint1); + setpoint1 *= -1; // because of inverted frame + + /* avoid sending close to zero values */ + if ( fabs(setpoint1) < wheelsetpointmin ) + { + setpoint1 = 0; + } + if ( fabs(setpoint2) < wheelsetpointmin ) + { + setpoint2 = 0; + } + + /* avoid sending very large values */ + setpoint1 = Utils::clip(setpoint1, wheelsetpointmax, -wheelsetpointmax); + setpoint2 = Utils::clip(setpoint2, wheelsetpointmax, -wheelsetpointmax); + + /* send calculated target velocity values to EtherCAT */ + rxdata.setpoint1 = setpoint1; + rxdata.setpoint2 = setpoint2; + } + rxpdo1_t* ecData = (rxpdo1_t*) ecx_slave[(*wheelConfigs)[i].ethercatNumber].outputs; *ecData = rxdata; } diff --git a/src/PlatformDriverROS.cpp b/src/PlatformDriverROS.cpp index d44d0c6..fb3a0cc 100755 --- a/src/PlatformDriverROS.cpp +++ b/src/PlatformDriverROS.cpp @@ -86,9 +86,21 @@ class PlatormDriver : public rclcpp::Node this->declare_parameter("start_delay", rclcpp::ParameterType::PARAMETER_DOUBLE); this->declare_parameter("current_max", rclcpp::ParameterType::PARAMETER_DOUBLE); this->declare_parameter("start_retry_delay", rclcpp::ParameterType::PARAMETER_DOUBLE); + this->declare_parameter("device", rclcpp::ParameterType::PARAMETER_STRING); + this->declare_parameter("num_wheels", rclcpp::ParameterType::PARAMETER_INTEGER); + this->declare_parameter("/base_control_mode", rclcpp::ParameterType::PARAMETER_STRING); + this->declare_parameter("platform_Kd_x", rclcpp::ParameterType::PARAMETER_DOUBLE); + this->declare_parameter("platform_Kd_y", rclcpp::ParameterType::PARAMETER_DOUBLE); + this->declare_parameter("platform_Kd_a", rclcpp::ParameterType::PARAMETER_DOUBLE); + + // get and set damping parameters + double damping_parameters[3]; + this->get_parameter("platform_Kd_x", damping_parameters[0]); + this->get_parameter("platform_Kd_y", damping_parameters[1]); + this->get_parameter("platform_Kd_a", damping_parameters[2]); + driver->setPlatformDampingParameters(damping_parameters); // declare wheel parameters - this->declare_parameter("num_wheels", rclcpp::ParameterType::PARAMETER_INTEGER); this->get_parameter("num_wheels", nWheels); std::cout << "Number of wheels: " << nWheels << std::endl; @@ -124,7 +136,7 @@ class PlatormDriver : public rclcpp::Node std::string device; unsigned int nWheelsMaster; - this->declare_parameter("device", rclcpp::ParameterType::PARAMETER_STRING); + if (!this->get_parameter("device", device)) { RCLCPP_ERROR_STREAM(this->get_logger(), "Missing 'device' parameter"); @@ -238,6 +250,7 @@ class PlatormDriver : public rclcpp::Node double r_w = 0.0524; // the radius of the wheel unsigned int nWheels = 0; + std::string base_control_mode; bool useJoy = false; bool debugMode = false; @@ -551,6 +564,13 @@ class PlatormDriver : public rclcpp::Node { useJoy = true; + // set base_control_mode + + if (this->get_parameter("/base_control_mode", base_control_mode)) + { + driver->setControlMode(base_control_mode); + } + if (joy->axes[5] > 0.5 && joyScale < 1.0) { joyScale = joyScale * 2.0; @@ -583,6 +603,11 @@ class PlatormDriver : public rclcpp::Node void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg) { + // set base_control_mode + if (this->get_parameter("/base_control_mode", base_control_mode)) + { + driver->setControlMode(base_control_mode); + } // if (!useJoy && !debugMode) if (!useJoy) driver->setTargetVelocity(msg->linear.x, msg->linear.y, msg->angular.z); @@ -609,6 +634,7 @@ class PlatormDriver : public rclcpp::Node // calculate robot velocity double vx, vy, va, encDisplacement; calculateRobotVelocity(vx, vy, va, encDisplacement); + driver->setMeasuredVelocity(vx, vy, va); // calculate robot displacement and current pose calculateRobotPose(vx, vy, va); diff --git a/src/PlatformToWheelInverseKinematicsSolver.c b/src/PlatformToWheelInverseKinematicsSolver.c new file mode 100644 index 0000000..f6b6f1c --- /dev/null +++ b/src/PlatformToWheelInverseKinematicsSolver.c @@ -0,0 +1,149 @@ +/** + * @file PlatformToWheelInverseKinematicsSolver.c + * @author Sivva Rahul Sai (rahul.sivva@smail.inf.h-brs.de) + * @brief this file consists of a functionality to print a matrix and to integrate all functions related to calculating torques at individual wheel units + * @date 2022-03-12 + * + */ +#include +#include +#include +#include +#include +#include +#include + +void print_matrix(const gsl_matrix *m) +{ + size_t i, j; + + for (i = 0; i < m->size1; i++) + { + for (j = 0; j < m->size2; j++) + { + printf("%f\t", gsl_matrix_get(m, i, j)); + } + } +} + +void functions_main(double *wheel_torques, + double *pivot_angles, + double *wheel_coordinates, + double *pivot_angles_deviation, + const gsl_matrix *b, + gsl_matrix *b_verify, + gsl_matrix *A, + gsl_matrix *A_inv_T, + gsl_matrix *A_tmp, + gsl_matrix *A_inv_T_tmp, + gsl_vector *work, + const gsl_matrix *W, + const gsl_matrix *K, + gsl_vector *u, + gsl_matrix *V, + gsl_matrix *u_inv, + const unsigned int M, + const unsigned int N, + const bool debug) +{ + /** + * @brief 1. initialise robot geometrical parameters + * https://github.com/kelo-robotics/kelo_tulip/blob/73e6d134bd31da6c580846dc907ff1ce2565b406/src/VelocityPlatformController.cpp + * https://github.com/kelo-robotics/kelo_tulip/blob/master/src/PlatformDriver.cpp + * + */ + + double radius = 0.052; // (0.105/2) [m] + double castor_offset = 0.01; // 0.01 [m] + double half_wheel_distance = 0.0275; // (0.055/2) [m] + + /** + * @brief updating pivot angles + * + */ + size_t i; + for (i = 0; i < 4; i++) + { + pivot_angles[i] = pivot_angles[i] - pivot_angles_deviation[i]; + if (pivot_angles[i] > 2 * M_PI) pivot_angles[i] -= 2 * M_PI; + else if (pivot_angles[i] < 0.0) pivot_angles[i] += 2 * M_PI; + } + + double pivot_forces[8]; + + /** + * @brief 2. get jacobian (A) -> KELORobotKinematics.c + * + */ + jacobian_matrix_calculator(A, + pivot_angles, + wheel_coordinates); + + /** + * @brief 3. find force array (pivot_forces) -> KELORobotKinematics.c + * + */ + force_vector_finder(pivot_forces, + A, + A_tmp, + A_inv_T_tmp, + A_inv_T, + u, + u_inv, + V, + W, + K, + work, + b, + M); + + /** + * @brief 4. find torques at individual wheels (wheel_torques) -> SmartWheelKinematics.c + * + */ + map_pivot_forces_to_wheel_torques(pivot_forces, + wheel_torques, + radius, + castor_offset, + half_wheel_distance); + + /** + * @brief 5. to print results for debugging + * + */ + if (debug) + { + /** + * @brief printing angles after offsetting the pivots + * + */ + printf("\n\n\n\nPivot angles:\n"); + for (int i = 0; i < 4; i++) + { + printf("%f\t", pivot_angles[i]); + } + + printf("\nPivot forces:\n"); + for (int i = 0; i < 8; i++) + { + printf("%f\t", pivot_forces[i]); + } + + // printf("\nPivot force magnitudes:\n"); + // for (int i = 0; i < 8; i += 2) + // { + // printf("%f\t", sqrt(pivot_forces[i] * pivot_forces[i] + pivot_forces[i + 1] * pivot_forces[i + 1])); + // } + + printf("\nWheel torques:\n"); + for (int i = 0; i < 8; i++) + { + printf("%f\t", wheel_torques[i]); + } + gsl_matrix_view _Y = gsl_matrix_view_array(pivot_forces, M, 1); + gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0, A, &_Y.matrix, 0.0, b_verify); + printf("\nReverse calculation: Platform force: \n"); + print_matrix(b_verify); + print_matrix(b); + } +} \ No newline at end of file diff --git a/src/SmartWheelKinematics.c b/src/SmartWheelKinematics.c new file mode 100644 index 0000000..bfd6fad --- /dev/null +++ b/src/SmartWheelKinematics.c @@ -0,0 +1,41 @@ +/** + * @file SmartWheelKinematics.h + * @author Kavya Shankar (kavya.shankar@smail.inf.h-brs.de) + * @brief the mapping from pivot forces to wheel torques is formulated + * @date 2022-03-12 + * + */ + +#include +#include + +#define LEFT_WHEEL 0 +#define RIGHT_WHEEL 1 +#define X_COMPONENT 0 +#define Y_COMPONENT 1 + +void map_pivot_force_to_wheel_torque(const double *pivot_forces, + double *wheel_torques, + const double radius, + const double castor_offset, + const double half_wheel_distance) +{ + wheel_torques[LEFT_WHEEL] = (radius / 2) * (pivot_forces[Y_COMPONENT] * (castor_offset / half_wheel_distance) + pivot_forces[X_COMPONENT]); + wheel_torques[RIGHT_WHEEL] = (radius / 2) * (-pivot_forces[Y_COMPONENT] * (castor_offset / half_wheel_distance) + pivot_forces[X_COMPONENT]); +} + +void map_pivot_forces_to_wheel_torques(const double *pivot_forces, + double *wheel_torques, + const double radius, + const double castor_offset, + const double half_wheel_distance) +{ + for (int wheel_pair_index = 0; wheel_pair_index < 4; wheel_pair_index++) + { + map_pivot_force_to_wheel_torque(&pivot_forces[2 * wheel_pair_index], + &wheel_torques[2 * wheel_pair_index], + radius, + castor_offset, + half_wheel_distance); + } +} diff --git a/src/VelocityPlatformController.cpp b/src/VelocityPlatformController.cpp index 9f1a39b..718998f 100644 --- a/src/VelocityPlatformController.cpp +++ b/src/VelocityPlatformController.cpp @@ -193,6 +193,131 @@ namespace kelo time_last_ramping = now; } + void VelocityPlatformController::calculateWheelTargetTorques( + double *wheel_torques, + double *pivot_angles, + double *wheel_coordinates, + double *pivot_angles_deviation, + double *measured_platform_velocity, + double *platform_damping_parameters, + const gsl_matrix *K, + const gsl_matrix *W, + const unsigned int M, + const unsigned int N) + { + // TODO: pass wheel coordinates; pivot_deviations; + bool debug = false; + + gsl_matrix *A = gsl_matrix_alloc(N, M); + gsl_matrix *A_inv_T = gsl_matrix_alloc(M, N); + gsl_matrix *A_tmp = gsl_matrix_alloc(N, M); + gsl_matrix *A_inv_T_tmp = gsl_matrix_alloc(M, N); + gsl_vector *work = gsl_vector_alloc(N); + gsl_vector *u = gsl_vector_alloc(N); + gsl_matrix *V = gsl_matrix_alloc(N, N); + gsl_matrix *u_inv = gsl_matrix_alloc(N, N); + gsl_matrix *b_verify = gsl_matrix_alloc(N, 1); + gsl_matrix *b = gsl_matrix_alloc(N, 1); + + double target_platform_torque[N]; + target_platform_torque[0] = platform_damping_parameters[0] * (platform_ramped_vel_.x - measured_platform_velocity[0]); + target_platform_torque[1] = platform_damping_parameters[1] * (platform_ramped_vel_.y - measured_platform_velocity[1]); + target_platform_torque[2] = platform_damping_parameters[2] * (platform_ramped_vel_.a - measured_platform_velocity[2]); + + // setting desired force to be experienced by the platform + gsl_matrix_set(b, 0, 0, target_platform_torque[0]); // force is set in X-direction + gsl_matrix_set(b, 1, 0, target_platform_torque[1]); // force is set in Y-direction + gsl_matrix_set(b, 2, 0, target_platform_torque[2]); // moment is set in anti-clockwise direction + + // TODO: Calculate wheel target torques + functions_main(wheel_torques, + pivot_angles, + wheel_coordinates, + pivot_angles_deviation, + b, + b_verify, + A, + A_inv_T, + A_tmp, + A_inv_T_tmp, + work, + W, + K, + u, + V, + u_inv, + M, + N, + debug); + + gsl_matrix_free(b_verify); + gsl_matrix_free(b); + gsl_matrix_free(A); + gsl_matrix_free(A_inv_T); + gsl_matrix_free(A_tmp); + gsl_matrix_free(A_inv_T_tmp); + gsl_vector_free(u); + gsl_matrix_free(u_inv); + gsl_matrix_free(V); + gsl_vector_free(work); + } + + void VelocityPlatformController::getPivotError( + const size_t &wheel_index, + const float &raw_pivot_angle, + float &pivot_error) + { + /* command 0 angular vel when platform has been commanded 0 vel + * If this is not done, then the wheels pivot to face front of platform + * even when the platform is commanded zero velocity. + */ + if ( platform_ramped_vel_.x == 0 && platform_ramped_vel_.y == 0 && platform_ramped_vel_.a == 0 ) + { + pivot_error = 0.0f; + return; + } + + const WheelParamVelocity &wheel_param = wheel_params_[wheel_index]; + + /* pivot angle w.r.t. front of platform (between -pi and pi) */ + float pivot_angle = Utils::clipAngle(raw_pivot_angle + - wheel_param.pivot_offset); + + /* pivot angle to unity vector */ + Point2D unit_pivot_vector; + unit_pivot_vector.x = cos(pivot_angle); + unit_pivot_vector.y = sin(pivot_angle); + + /* position of wheels relative to platform centre */ + Point2D position_l, position_r; + position_l.x = (wheel_param.relative_position_l.x * unit_pivot_vector.x + - wheel_param.relative_position_l.y * unit_pivot_vector.y) + + wheel_param.pivot_position.x; + position_l.y = (wheel_param.relative_position_l.x * unit_pivot_vector.y + + wheel_param.relative_position_l.y * unit_pivot_vector.x) + + wheel_param.pivot_position.y; + position_r.x = (wheel_param.relative_position_r.x * unit_pivot_vector.x + - wheel_param.relative_position_r.y * unit_pivot_vector.y) + + wheel_param.pivot_position.x; + position_r.y = (wheel_param.relative_position_r.x * unit_pivot_vector.y + + wheel_param.relative_position_r.y * unit_pivot_vector.x) + + wheel_param.pivot_position.y; + + /* velocity target vector at pivot position */ + Point2D target_vel_at_pivot; + target_vel_at_pivot.x = platform_ramped_vel_.x + - (platform_ramped_vel_.a * wheel_param.pivot_position.y); + target_vel_at_pivot.y = platform_ramped_vel_.y + + (platform_ramped_vel_.a * wheel_param.pivot_position.x); + + /* target pivot vector to angle */ + float target_pivot_angle = atan2(target_vel_at_pivot.y, target_vel_at_pivot.x); + + /* calculate error pivot angle as shortest route */ + pivot_error = Utils::getShortestAngle(target_pivot_angle, + pivot_angle); + } + void VelocityPlatformController::calculateWheelTargetVelocity( const size_t &wheel_index, const float &raw_pivot_angle, From 93645f3da54feae219e09f74f86f491de2ad171d Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Mon, 19 Feb 2024 10:54:46 +0100 Subject: [PATCH 2/8] bug fix: damping params set after driver initialisation --- src/PlatformDriverROS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/PlatformDriverROS.cpp b/src/PlatformDriverROS.cpp index fb3a0cc..1bf7b2b 100755 --- a/src/PlatformDriverROS.cpp +++ b/src/PlatformDriverROS.cpp @@ -98,7 +98,6 @@ class PlatormDriver : public rclcpp::Node this->get_parameter("platform_Kd_x", damping_parameters[0]); this->get_parameter("platform_Kd_y", damping_parameters[1]); this->get_parameter("platform_Kd_a", damping_parameters[2]); - driver->setPlatformDampingParameters(damping_parameters); // declare wheel parameters this->get_parameter("num_wheels", nWheels); @@ -149,6 +148,7 @@ class PlatormDriver : public rclcpp::Node unsigned int firstWheel = 0; driver = new kelo::PlatformDriver(device, modules, &wheelConfigs, &wheelData, firstWheel, nWheelsMaster); + driver->setPlatformDampingParameters(damping_parameters); wheelIndex += nWheelsMaster; From 67a6d5acff2a801d7d794a4931b8e4714f82fc47 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Mon, 19 Feb 2024 13:45:51 +0100 Subject: [PATCH 3/8] reducing gains for robiles --- config/example.yaml | 51 ++++++++++++++++++++++++++------------------- config/robile1.yaml | 6 +++--- config/robile2.yaml | 6 +++--- config/robile3.yaml | 6 +++--- config/robile4.yaml | 6 +++--- 5 files changed, 41 insertions(+), 34 deletions(-) diff --git a/config/example.yaml b/config/example.yaml index dc1389a..8056578 100644 --- a/config/example.yaml +++ b/config/example.yaml @@ -1,28 +1,35 @@ -num_wheels: 4 +platform_driver: + ros__parameters: + + num_wheels: 4 + device: eno1 + /base_control_mode: VELOCITY_CONTROL -device: enp2s0 + platform_Kd_x: 300.0 + platform_Kd_y: 300.0 + platform_Kd_a: 100.0 -wheel0: - ethercat_number: 2 - x: 0.175 - y: 0.1605 - a: -2.50 + wheel0: + ethercat_number: 2 + x: 0.175 + y: 0.1605 + a: -2.50 -wheel1: - ethercat_number: 3 - x: -0.175 - y: 0.1605 - a: -1.25 + wheel1: + ethercat_number: 3 + x: -0.175 + y: 0.1605 + a: -1.25 -wheel2: - ethercat_number: 5 - x: -0.175 - y: -0.1607 - a: -2.14 + wheel2: + ethercat_number: 5 + x: -0.175 + y: -0.1607 + a: -2.14 -wheel3: - ethercat_number: 6 - x: 0.175 - y: -0.1605 - a: 1.49 + wheel3: + ethercat_number: 6 + x: 0.175 + y: -0.1605 + a: 1.49 diff --git a/config/robile1.yaml b/config/robile1.yaml index 071b40c..cb7d511 100644 --- a/config/robile1.yaml +++ b/config/robile1.yaml @@ -5,9 +5,9 @@ platform_driver: device: eno1 /base_control_mode: VELOCITY_CONTROL - platform_Kd_x: 1200.0 - platform_Kd_y: 1200.0 - platform_Kd_a: 400.0 + platform_Kd_x: 300.0 + platform_Kd_y: 300.0 + platform_Kd_a: 100.0 wheel0: ethercat_number: 3 diff --git a/config/robile2.yaml b/config/robile2.yaml index bcf6e0b..d9c0766 100644 --- a/config/robile2.yaml +++ b/config/robile2.yaml @@ -5,9 +5,9 @@ platform_driver: device: eno1 /base_control_mode: VELOCITY_CONTROL - platform_Kd_x: 1200.0 - platform_Kd_y: 1200.0 - platform_Kd_a: 400.0 + platform_Kd_x: 300.0 + platform_Kd_y: 300.0 + platform_Kd_a: 100.0 wheel0: ethercat_number: 5 diff --git a/config/robile3.yaml b/config/robile3.yaml index 4a19547..0c5ca68 100644 --- a/config/robile3.yaml +++ b/config/robile3.yaml @@ -5,9 +5,9 @@ platform_driver: device: eno1 /base_control_mode: VELOCITY_CONTROL - platform_Kd_x: 1200.0 - platform_Kd_y: 1200.0 - platform_Kd_a: 400.0 + platform_Kd_x: 300.0 + platform_Kd_y: 300.0 + platform_Kd_a: 100.0 vlin_max: 1.2 va_max: 1.0 diff --git a/config/robile4.yaml b/config/robile4.yaml index e6915c6..324f0eb 100644 --- a/config/robile4.yaml +++ b/config/robile4.yaml @@ -5,9 +5,9 @@ platform_driver: device: eno1 /base_control_mode: VELOCITY_CONTROL - platform_Kd_x: 1200.0 - platform_Kd_y: 1200.0 - platform_Kd_a: 400.0 + platform_Kd_x: 300.0 + platform_Kd_y: 300.0 + platform_Kd_a: 100.0 vlin_max: 1.2 va_max: 1.0 From b7a4dbabef3eb2e81e8d5f1f3ebe44d1b1e0ab14 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Mon, 19 Feb 2024 13:48:57 +0100 Subject: [PATCH 4/8] adding config file for freddy --- config/freddy.yaml | 39 +++++++++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) create mode 100644 config/freddy.yaml diff --git a/config/freddy.yaml b/config/freddy.yaml new file mode 100644 index 0000000..d7b0f06 --- /dev/null +++ b/config/freddy.yaml @@ -0,0 +1,39 @@ +num_wheels: 4 +vlin_max: 1.2 + +va_max: 1.0 + +device: eno1 +/base_control_mode: VELOCITY_CONTROL + +platform_Kd_x: 1200.0 +platform_Kd_y: 1200.0 +platform_Kd_a: 400.0 + +wheel0: + ethercat_number: 2 + x: 0.188 + y: 0.2075 + a: 5.25 + reverse_velocity: 1 + +wheel1: + ethercat_number: 10 + x: -0.188 + y: 0.2075 + a: -1.55 + reverse_velocity: 1 + +wheel2: + ethercat_number: 6 + x: -0.188 + y: -0.2075 + a: -1.37 + reverse_velocity: 1 + +wheel3: + ethercat_number: 11 + x: 0.188 + y: -0.2075 + a: -1.23 + reverse_velocity: 1 \ No newline at end of file From 56a93fdb0f909f427ed488d6e97f958a170190a3 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Mon, 19 Feb 2024 14:50:20 +0100 Subject: [PATCH 5/8] updating readme --- README.md | 44 +++++++++++++++++++++++--------------------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 86dff9a..a596558 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,12 @@ +## WARNING + +``` +The current implementation of *torque control* is not stable, so for stable use it is recommended to use the robot in *velocity control* mode, which is enabled by default.The instability of the torque control may be due to occasional loss of contact with the ground, causing the wheels to spin at high speed. +``` + # KELO Tulip -This package contains the *KELO Tulip* software. This software takes a velocity vector for the overall platform and converts it to commands for the individual KELO Drives of the platform. It implements an EtherCAT master to communicate with the KELO Drives and provides a simple velocity controller that can be used on real robots as well as for simulation. +This package contains the *KELO Tulip* software. This software takes a velocity vector for the overall platform and converts it to commands for the individual KELO Drives of the platform with an option to control either in VELOCITY_CONTROL or in TORQUE_CONTROL modes. It implements an EtherCAT master to communicate with the KELO Drives and provides a simple velocity controller that can be used on real robots as well as for simulation. ## KELO Drives and how to build a platform The [KELO Drive](https://www.kelo-robotics.com/technologies/#kelo-drives) is the patent pending novel drive concept for mobile robots developed by KELO robotics. These wheels can be attached to any rigid platform in order to transform it into a mobile robot. It can even be used to [robotize a container](https://www.kelo-robotics.com/customized-designs/#robotized-material-container). A few screws are enough. @@ -11,7 +17,7 @@ You can move your mobile platform via a joypad for test purposes or use any soft ## System requirements -This software was tested on Ubuntu 22.04 with ROS Humble and ROS Rolling. Please switch to `ros1` branch to use other tested versions: Ubuntu 16 with ROS Kinetic, Ubuntu 18 with ROS Melodic and Ubuntu 20 with ROS Noetic. Other Linux flavors should work as well. +This software has been tested on Ubuntu 22.04 with ROS Humble and ROS Rolling. Please switch to the `ros1` branch to use other tested versions, including Ubuntu 16 with ROS Kinetic, Ubuntu 18 with ROS Melodic and Ubuntu 20 with ROS Noetic. For ROS it is enough to install the base system (for Humble ros-humble-ros-base). Additionally, GSL library should be installed, as it is used for the feature of compliant control of the wheels. @@ -20,7 +26,7 @@ sudo apt install ros-humble-ros-base sudo apt install libgsl-dev ``` -If another ROS version is used, replace the term noetic accordingly in these commands. +If another ROS version is used, replace the term *humble* accordingly in these commands. ## Installation @@ -41,7 +47,7 @@ The executable can be found in `install/kelo_tulip/lib` directory. ### Finding dynamic libraries -If using Ubuntu 18 or newer (with ROS Melodic or Noetic), running the setcap command as described above can cause the executable to not find all dynamic libraries anymore. This is because the dynamic linker works in a "secure-execution mode" when the capabilities of a program changed, in which it ignores most environment variables such as LD_LIBRARY_PATH. A typical error after starting reads like this: +In newer ROS verxions running the setcap command as described above can cause the executable to not find all dynamic libraries anymore. This is because the dynamic linker works in a "secure-execution mode" when the capabilities of a program changed, in which it ignores most environment variables such as LD_LIBRARY_PATH. A typical error after starting reads like this: ``` devel/lib/kelo_tulip/platform_driver: error while loading shared libraries: libtf2_ros.so: cannot open shared object file: No such file or directory @@ -77,14 +83,14 @@ ros2 launch kelo_tulip example.launch ### Parameters -The default launch file in `kelo_tulip/launch/example.launch.py` loads the YAML configuration from `config/example.yaml`. Feel free to change parameters directly in this config file, or to make a copy and adjust the launch file to load the new file. +The default launch file in `kelo_tulip/launch/example.launch.py` loads the YAML configuration from `config/example.yaml`. Feel free to change parameters directly in this config file, or update the environment variable `ROBOT_NAME`. Currently parameters for some of the configurations (*robile1, robile2, robile3, robile4, freddy*) are avaialble in the config folder. #### Network interface The following setting defines the network interface used by the driver: ``` -device: enp2s0 +device: eno1 ``` This setting must be adjusted to the network interface by which the KELO drives are connected via EtherCAT. @@ -112,7 +118,7 @@ The values `x` and `y` are the coordinates of the wheels center according to the ### ROS Interfaces -Currently the kelo_tulip software uses ROS as a middleware, subscribing resp. publishing to the following topics. +Currently the kelo_tulip software uses ROS as a middleware, and publishing data to the following topics. #### /cmd_vel @@ -143,6 +149,13 @@ On this topic an integer representing status information about the controller is | 0x0400 | Timestamp of one KELO Drive did not increase as expected, probably it stopped communicating. | 0x0800 | One KELO Drive did not have the expected status bits set, probably it got deactivated for some reasons. +## Controllers + +The robot can be controlled in both velocity and torque control modes. To switch between them, the ROS parameter `/base_control_mode` can be set to either *VELOCITY_CONTROL* or *TORQUE_CONTROL*. By default it is set to *VELOCITY_CONTROL* in the configuration file. To switch between them from the command line, e.g. to torque control, the following command can be used, + +``` +ros2 param set /platform_driver /base_control_mode TORQUE_CONTROL +``` ### Velocity controller @@ -187,19 +200,8 @@ This is the main function that computes the setpoint for the left and right hub The result is the setpoints for the left and right hubwheel, which can then be sent to the real KELO drive. +### Torque controller +kelo_tulip also includes an implementation of a simple torque controller, which allows for compliant motion of the robot. The controller itself is developed in C language and it is ROS independent. - - - - - - - - - - - - - - +The description of how the torque controller works is as follows.The joypad command is taken as input and mapped to the platform force, which is a scaled quantity of the platform velocity difference, scaled by a factor of the *platform_damping* parameters instantiated in the configuration file. An inverse dynamics solver is used to determine the wheel torques to achieve the desired platform force. It also allows to enable/disable/provide weights both in the joint space of wheels and the Cartesian task space. For this purpose, the diagnal elements of the matrices `W` and `K` in the *doControl()* function of *PlatformDriver.cpp* should be updated. Assigning it to zero deactivates the corresponding dimension of control. \ No newline at end of file From 5f144e02007a3bce744a16ee1840e3d27078ee2f Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Mon, 19 Feb 2024 15:40:02 +0100 Subject: [PATCH 6/8] restructuring readme --- README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/README.md b/README.md index a596558..eda02c6 100644 --- a/README.md +++ b/README.md @@ -1,8 +1,8 @@ ## WARNING -``` -The current implementation of *torque control* is not stable, so for stable use it is recommended to use the robot in *velocity control* mode, which is enabled by default.The instability of the torque control may be due to occasional loss of contact with the ground, causing the wheels to spin at high speed. -``` + + The current implementation of *torque control* is not stable, so for stable use it is recommended to use the robot in *velocity control* mode, which is enabled by default.The instability of the torque control may be due to occasional loss of contact with the ground, causing the wheels to spin at high speed. + # KELO Tulip From 4d8bec7b9ec17460e4273f79ab8026d4eb18a880 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Mon, 19 Feb 2024 15:44:16 +0100 Subject: [PATCH 7/8] restructuring readme --- README.md | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/README.md b/README.md index eda02c6..78f78f5 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,5 @@ -## WARNING - - - The current implementation of *torque control* is not stable, so for stable use it is recommended to use the robot in *velocity control* mode, which is enabled by default.The instability of the torque control may be due to occasional loss of contact with the ground, causing the wheels to spin at high speed. +> [!WARNING] +> The current implementation of *torque control* is not stable, so for stable use it is recommended to use the robot in *velocity control* mode, which is enabled by default.The instability of the torque control may be due to occasional loss of contact with the ground, causing the wheels to spin at high speed. # KELO Tulip From 4a718237eba019b092f63d0718dd907c39196bb6 Mon Sep 17 00:00:00 2001 From: KishanSawant Date: Wed, 6 Mar 2024 13:14:05 +0100 Subject: [PATCH 8/8] updating robile4 config --- config/robile4.yaml | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/config/robile4.yaml b/config/robile4.yaml index 324f0eb..5adb3cf 100644 --- a/config/robile4.yaml +++ b/config/robile4.yaml @@ -16,26 +16,26 @@ platform_driver: ethercat_number: 5 x: 0.233 y: 0.1165 - a: -4.77 + a: 1.57 reverse_velocity: 1 wheel1: ethercat_number: 7 x: -0.233 y: 0.1165 - a: 1.49 + a: 1.57 reverse_velocity: 1 wheel2: ethercat_number: 9 x: -0.233 y: -0.1165 - a: 1.51 + a: 1.57 reverse_velocity: 1 wheel3: ethercat_number: 3 x: 0.233 y: -0.1165 - a: 1.71 + a: 1.57 reverse_velocity: 1