From 59d483c3a65b6701659912b819f7bfe4154a64ad Mon Sep 17 00:00:00 2001 From: qiayuan Date: Fri, 29 Jul 2022 11:42:27 +0800 Subject: [PATCH 1/4] Add a new and elegant OmniController --- rm_chassis_controllers/CMakeLists.txt | 9 -- .../config/localization.yaml | 23 ----- .../config/template_mecanum.yaml | 33 ------- .../config/template_omni.yaml | 21 ++--- .../include/rm_chassis_controllers/mecanum.h | 77 ---------------- .../include/rm_chassis_controllers/omni.h | 11 ++- rm_chassis_controllers/package.xml | 7 -- .../rm_chassis_controllers_plugins.xml | 8 -- rm_chassis_controllers/src/mecanum.cpp | 89 ------------------- rm_chassis_controllers/src/omni.cpp | 88 ++++++++++-------- 10 files changed, 67 insertions(+), 299 deletions(-) delete mode 100644 rm_chassis_controllers/config/localization.yaml delete mode 100644 rm_chassis_controllers/config/template_mecanum.yaml delete mode 100644 rm_chassis_controllers/include/rm_chassis_controllers/mecanum.h delete mode 100644 rm_chassis_controllers/src/mecanum.cpp diff --git a/rm_chassis_controllers/CMakeLists.txt b/rm_chassis_controllers/CMakeLists.txt index f1b6d9cf..a89846bb 100644 --- a/rm_chassis_controllers/CMakeLists.txt +++ b/rm_chassis_controllers/CMakeLists.txt @@ -20,13 +20,9 @@ find_package(catkin REQUIRED nav_msgs pluginlib - hardware_interface controller_interface - forward_command_controller realtime_tools - control_toolbox effort_controllers - tf2 tf2_geometry_msgs angles ) @@ -56,13 +52,9 @@ catkin_package( nav_msgs pluginlib - hardware_interface controller_interface - forward_command_controller realtime_tools - control_toolbox effort_controllers - tf2 tf2_geometry_msgs angles LIBRARIES ${PROJECT_NAME} @@ -82,7 +74,6 @@ include_directories( ## Declare cpp executables add_library(${PROJECT_NAME} src/chassis_base.cpp - src/mecanum.cpp src/omni.cpp src/sentry.cpp src/swerve.cpp diff --git a/rm_chassis_controllers/config/localization.yaml b/rm_chassis_controllers/config/localization.yaml deleted file mode 100644 index 26e9d36f..00000000 --- a/rm_chassis_controllers/config/localization.yaml +++ /dev/null @@ -1,23 +0,0 @@ -world_frame: odom -odom_frame: odom -base_link_frame: base_link -frequency: 100 - -odom0: /odom -odom0_config: [ false, false, false, - false, false, false, - true, true, false, - false, false, true, - false, false, false ] -odom0_differential: false -odom0_queue_size: 100 - -imu0: /base_imu -imu0_config: [ false, false, false, - true, true, true, - false, false, false, - true, true, true, - true, true, false ] -imu0_differential: true -imu0_queue_size: 10 -imu0_remove_gravitational_acceleration: true diff --git a/rm_chassis_controllers/config/template_mecanum.yaml b/rm_chassis_controllers/config/template_mecanum.yaml deleted file mode 100644 index 2a5c3ffa..00000000 --- a/rm_chassis_controllers/config/template_mecanum.yaml +++ /dev/null @@ -1,33 +0,0 @@ -controllers: - - chassis_controller: - type: rm_chassis_controllers/MecanumController - # ChassisBase - publish_rate: 100 - enable_odom_tf: true - power: - effort_coeff: 12.0 - vel_coeff: 0.0048 - power_offset: 0 - twist_angular: 0.5233 - timeout: 0.1 - pid_follow: { p: 5, i: 0, d: 0.05, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] - - # MecanumController - wheel_base: 0.395 - wheel_track: 0.415 - wheel_radius: 0.07625 - - left_front: - joint: "left_front_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - right_front: - joint: "right_front_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - left_back: - joint: "left_back_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - right_back: - joint: "right_back_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } diff --git a/rm_chassis_controllers/config/template_omni.yaml b/rm_chassis_controllers/config/template_omni.yaml index b17cf0d2..07647351 100644 --- a/rm_chassis_controllers/config/template_omni.yaml +++ b/rm_chassis_controllers/config/template_omni.yaml @@ -14,17 +14,10 @@ controllers: twist_covariance_diagonal: [ 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 ] # OmniController - chassis_radius: 0.208 - wheel_radius: 0.07625 - left_front: - joint: "left_front_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - right_front: - joint: "right_front_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - left_back: - joint: "left_back_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } - right_back: - joint: "right_back_wheel_joint" - pid: { p: 0.8, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + wheels: + left_front: + pose: [ 0.147, 0.147, 0.785 ] + roller_angle: 2.3561 + radius: 0.07625 + joint: left_front_wheel_joint + pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true } diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/mecanum.h b/rm_chassis_controllers/include/rm_chassis_controllers/mecanum.h deleted file mode 100644 index de0d4267..00000000 --- a/rm_chassis_controllers/include/rm_chassis_controllers/mecanum.h +++ /dev/null @@ -1,77 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by flying on 2021/1/18. -// - -#pragma once - -#include "rm_chassis_controllers/chassis_base.h" - -namespace rm_chassis_controllers -{ -class MecanumController : public ChassisBase -{ -public: - MecanumController() = default; - /** @brief Execute ChassisBase::init. Get necessary handles. - * - * Execute ChassisBase::init. Init lf, lb, rf, rb handles so that we can controls four wheels conveniently. - * - * @param robot_hw The robot hardware abstraction. - * @param root_nh A NodeHandle in the root of the controller manager namespace. This is where the ROS interfaces are - * setup (publishers, subscribers, services). - * @param controller_nh A NodeHandle in the namespace of the controller. This is where the controller-specific - * configuration resides. - * @return True if initialization was successful and the controller - * is ready to be started. - */ - bool init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) override; - -private: - /** @brief Calculate correct command and set it to four wheels. - * - * @param time The current time. - * @param period The time passed since the last call to update. - */ - void moveJoint(const ros::Time& time, const ros::Duration& period) override; - /** @brief Calculate current linear_x, linear_y and angular_z according to current velocity. - * - * @return Calculated vel_data included linear_x, linear_y and angular_z. - */ - geometry_msgs::Twist forwardKinematics() override; - - effort_controllers::JointVelocityController ctrl_lf_, ctrl_rf_, ctrl_lb_, ctrl_rb_; -}; -} // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index 393ef2e7..a708b789 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -1,10 +1,12 @@ // -// Created by yezi on 2021/12/3. +// Created by qiayuan on 2022/7/29. // #pragma once -#include +#include + +#include "rm_chassis_controllers/chassis_base.h" namespace rm_chassis_controllers { @@ -18,7 +20,8 @@ class OmniController : public ChassisBase> joints_; + Eigen::MatrixXd chassis2joints_; }; + } // namespace rm_chassis_controllers diff --git a/rm_chassis_controllers/package.xml b/rm_chassis_controllers/package.xml index 90726c87..710c3a13 100644 --- a/rm_chassis_controllers/package.xml +++ b/rm_chassis_controllers/package.xml @@ -20,19 +20,12 @@ nav_msgs pluginlib - hardware_interface controller_interface - forward_command_controller realtime_tools - control_toolbox effort_controllers - tf2 tf2_geometry_msgs angles - imu_sensor_controller - robot_localization - diff --git a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml index da307050..1244eff8 100644 --- a/rm_chassis_controllers/rm_chassis_controllers_plugins.xml +++ b/rm_chassis_controllers/rm_chassis_controllers_plugins.xml @@ -1,13 +1,5 @@ - - - The StandardController is RoboMaster mecanum wheel Chassis controller. It expects a EffortJointInterface - type of hardware interface. - - diff --git a/rm_chassis_controllers/src/mecanum.cpp b/rm_chassis_controllers/src/mecanum.cpp deleted file mode 100644 index c1fc9659..00000000 --- a/rm_chassis_controllers/src/mecanum.cpp +++ /dev/null @@ -1,89 +0,0 @@ -/******************************************************************************* - * BSD 3-Clause License - * - * Copyright (c) 2021, Qiayuan Liao - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * * Redistributions of source code must retain the above copyright notice, this - * list of conditions and the following disclaimer. - * - * * Redistributions in binary form must reproduce the above copyright notice, - * this list of conditions and the following disclaimer in the documentation - * and/or other materials provided with the distribution. - * - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived from - * this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" - * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE - * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE - * ARE - * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE - * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL - * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR - * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, - * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE - * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. - *******************************************************************************/ - -// -// Created by flying on 2021/1/18. -// -#include "rm_chassis_controllers/mecanum.h" -#include -#include -#include - -namespace rm_chassis_controllers -{ -bool MecanumController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, - ros::NodeHandle& controller_nh) -{ - ChassisBase::init(robot_hw, root_nh, controller_nh); - ros::NodeHandle nh_lf = ros::NodeHandle(controller_nh, "left_front"); - ros::NodeHandle nh_rf = ros::NodeHandle(controller_nh, "right_front"); - ros::NodeHandle nh_lb = ros::NodeHandle(controller_nh, "left_back"); - ros::NodeHandle nh_rb = ros::NodeHandle(controller_nh, "right_back"); - if (!ctrl_lf_.init(effort_joint_interface_, nh_lf) || !ctrl_rf_.init(effort_joint_interface_, nh_rf) || - !ctrl_lb_.init(effort_joint_interface_, nh_lb) || !ctrl_rb_.init(effort_joint_interface_, nh_rb)) - return false; - joint_handles_.push_back(ctrl_lf_.joint_); - joint_handles_.push_back(ctrl_rf_.joint_); - joint_handles_.push_back(ctrl_lb_.joint_); - joint_handles_.push_back(ctrl_rb_.joint_); - return true; -} - -void MecanumController::moveJoint(const ros::Time& time, const ros::Duration& period) -{ - double a = (wheel_base_ + wheel_track_) / 2.0; - ctrl_lf_.setCommand((vel_cmd_.x - vel_cmd_.y - vel_cmd_.z * a) / wheel_radius_); - ctrl_rf_.setCommand((vel_cmd_.x + vel_cmd_.y + vel_cmd_.z * a) / wheel_radius_); - ctrl_lb_.setCommand((vel_cmd_.x + vel_cmd_.y - vel_cmd_.z * a) / wheel_radius_); - ctrl_rb_.setCommand((vel_cmd_.x - vel_cmd_.y + vel_cmd_.z * a) / wheel_radius_); - ctrl_lf_.update(time, period); - ctrl_rf_.update(time, period); - ctrl_lb_.update(time, period); - ctrl_rb_.update(time, period); -} - -geometry_msgs::Twist MecanumController::forwardKinematics() -{ - geometry_msgs::Twist vel_data; - double k = wheel_radius_ / 4.0; - double lf_velocity = ctrl_lf_.joint_.getVelocity(); - double rf_velocity = ctrl_rf_.joint_.getVelocity(); - double lb_velocity = ctrl_lb_.joint_.getVelocity(); - double rb_velocity = ctrl_rb_.joint_.getVelocity(); - vel_data.linear.x = (rf_velocity + lf_velocity + lb_velocity + rb_velocity) * k; - vel_data.linear.y = (rf_velocity - lf_velocity + lb_velocity - rb_velocity) * k; - vel_data.angular.z = 2 * (rf_velocity - lf_velocity - lb_velocity + rb_velocity) * k / (wheel_base_ + wheel_track_); - return vel_data; -} -} // namespace rm_chassis_controllers -PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::MecanumController, controller_interface::ControllerBase) diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index d303b956..b865664b 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -1,61 +1,79 @@ // -// Created by yezi on 2021/12/3. +// Created by qiayuan on 2022/7/29. // -#include -#include #include +#include + +#include #include +#include "rm_chassis_controllers/omni.h" + namespace rm_chassis_controllers { bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh) { ChassisBase::init(robot_hw, root_nh, controller_nh); - if (!controller_nh.getParam("chassis_radius", chassis_radius_)) + + XmlRpc::XmlRpcValue wheels; + controller_nh.getParam("wheels", wheels); + chassis2joints_.resize(wheels.size(), 3); + + size_t i = 0; + for (const auto& wheel : wheels) { - ROS_ERROR("chassis_radius is not set"); - return false; + ROS_ASSERT(wheel.second.hasMember("pose")); + ROS_ASSERT(wheel.second["pose"].getType() == XmlRpc::XmlRpcValue::TypeArray); + ROS_ASSERT(wheel.second["pose"].size() == 2); + ROS_ASSERT(wheel.second.hasMember("roller_angle")); + ROS_ASSERT(wheel.second.hasMember("radius")); + + // Ref: Modern Robotics, Chapter 13.2: Omnidirectional Wheeled Mobile Robots + Eigen::MatrixXd direction(1, 2), in_wheel(2, 2), in_chassis(2, 3); + double beta = (double)wheel.second["pose"][2]; + double roller_angle = (double)wheel.second["roller_angle"]; + direction << 1, tan(roller_angle); + in_wheel << cos(beta), sin(beta), -sin(beta), cos(beta); + in_chassis << -(double)wheel.second["pose"][1], 1., 0., (double)wheel.second["pose"][0], 0., 1.; + Eigen::MatrixXd chassis2joint = 1. / (double)wheel.second["radius"] * direction * in_wheel * in_chassis; + chassis2joints_.block<1, 3>(i, 0) = chassis2joint; + + ros::NodeHandle nh_wheel = ros::NodeHandle(controller_nh, "modules/" + wheel.first); + joints_.push_back(std::make_shared()); + if (!joints_.back()->init(effort_joint_interface_, nh_wheel)) + return false; + + i++; } - ros::NodeHandle nh_lf = ros::NodeHandle(controller_nh, "left_front"); - ros::NodeHandle nh_rf = ros::NodeHandle(controller_nh, "right_front"); - ros::NodeHandle nh_lb = ros::NodeHandle(controller_nh, "left_back"); - ros::NodeHandle nh_rb = ros::NodeHandle(controller_nh, "right_back"); - if (!ctrl_lf_.init(effort_joint_interface_, nh_lf) || !ctrl_rf_.init(effort_joint_interface_, nh_rf) || - !ctrl_lb_.init(effort_joint_interface_, nh_lb) || !ctrl_rb_.init(effort_joint_interface_, nh_rb)) - return false; - joint_handles_.push_back(ctrl_lf_.joint_); - joint_handles_.push_back(ctrl_rf_.joint_); - joint_handles_.push_back(ctrl_lb_.joint_); - joint_handles_.push_back(ctrl_rb_.joint_); return true; } void OmniController::moveJoint(const ros::Time& time, const ros::Duration& period) { - ctrl_rf_.setCommand(((vel_cmd_.x + vel_cmd_.y + sqrt(2) * vel_cmd_.z * chassis_radius_) / sqrt(2)) / wheel_radius_); - ctrl_lf_.setCommand(((-vel_cmd_.x + vel_cmd_.y + sqrt(2) * vel_cmd_.z * chassis_radius_) / sqrt(2)) / wheel_radius_); - ctrl_lb_.setCommand(((-vel_cmd_.x - vel_cmd_.y + sqrt(2) * vel_cmd_.z * chassis_radius_) / sqrt(2)) / wheel_radius_); - ctrl_rb_.setCommand(((vel_cmd_.x - vel_cmd_.y + sqrt(2) * vel_cmd_.z * chassis_radius_) / sqrt(2)) / wheel_radius_); - ctrl_lf_.update(time, period); - ctrl_rf_.update(time, period); - ctrl_lb_.update(time, period); - ctrl_rb_.update(time, period); + Eigen::Vector3d vel_chassis; + vel_chassis << vel_cmd_.z, vel_cmd_.x, vel_cmd_.y; + Eigen::Vector3d vel_joints = chassis2joints_ * vel_chassis; + for (size_t i = 0; i < joints_.size(); i++) + { + joints_[i]->setCommand(vel_joints(i)); + joints_[i]->update(time, period); + } } geometry_msgs::Twist OmniController::forwardKinematics() { - geometry_msgs::Twist vel_data; - double k = wheel_radius_ / 2; - double lf_velocity = ctrl_lf_.joint_.getVelocity(); - double rf_velocity = ctrl_rf_.joint_.getVelocity(); - double lb_velocity = ctrl_lb_.joint_.getVelocity(); - double rb_velocity = ctrl_rb_.joint_.getVelocity(); - vel_data.linear.x = k * (-lf_velocity + rf_velocity - lb_velocity + rb_velocity) / sqrt(2); - vel_data.linear.y = k * (lf_velocity + rf_velocity - lb_velocity - rb_velocity) / sqrt(2); - vel_data.angular.z = k * (lf_velocity + rf_velocity + lb_velocity + rb_velocity) / (2 * chassis_radius_); - return vel_data; + Eigen::VectorXd vel_joints(joints_.size()); + for (size_t i = 0; i < joints_.size(); i++) + vel_joints[i] = joints_[i]->joint_.getVelocity(); + Eigen::Vector3d vel_chassis = chassis2joints_.completeOrthogonalDecomposition().pseudoInverse() * vel_joints; + geometry_msgs::Twist twist; + twist.angular.z = vel_chassis(0); + twist.linear.x = vel_chassis(1); + twist.linear.y = vel_chassis(2); + return twist; } + } // namespace rm_chassis_controllers PLUGINLIB_EXPORT_CLASS(rm_chassis_controllers::OmniController, controller_interface::ControllerBase) From 7f1d5a379203a5834f2b68f9ef28db9b534cab19 Mon Sep 17 00:00:00 2001 From: qiayuan Date: Fri, 29 Jul 2022 22:36:03 +0800 Subject: [PATCH 2/4] Fix bug in the new OmniController --- rm_chassis_controllers/src/omni.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index b865664b..6e4171df 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -26,7 +26,7 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle { ROS_ASSERT(wheel.second.hasMember("pose")); ROS_ASSERT(wheel.second["pose"].getType() == XmlRpc::XmlRpcValue::TypeArray); - ROS_ASSERT(wheel.second["pose"].size() == 2); + ROS_ASSERT(wheel.second["pose"].size() == 3); ROS_ASSERT(wheel.second.hasMember("roller_angle")); ROS_ASSERT(wheel.second.hasMember("radius")); @@ -54,7 +54,7 @@ void OmniController::moveJoint(const ros::Time& time, const ros::Duration& perio { Eigen::Vector3d vel_chassis; vel_chassis << vel_cmd_.z, vel_cmd_.x, vel_cmd_.y; - Eigen::Vector3d vel_joints = chassis2joints_ * vel_chassis; + Eigen::VectorXd vel_joints = chassis2joints_ * vel_chassis; for (size_t i = 0; i < joints_.size(); i++) { joints_[i]->setCommand(vel_joints(i)); From 26e61be249c32ba30e6fd57a26c65e9805fd5f41 Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Sat, 30 Jul 2022 16:30:50 +0800 Subject: [PATCH 3/4] Rename the function forwardKinematics to odometry. --- .../include/rm_chassis_controllers/chassis_base.h | 2 +- rm_chassis_controllers/include/rm_chassis_controllers/omni.h | 2 +- rm_chassis_controllers/include/rm_chassis_controllers/sentry.h | 2 +- rm_chassis_controllers/include/rm_chassis_controllers/swerve.h | 2 +- rm_chassis_controllers/src/chassis_base.cpp | 2 +- rm_chassis_controllers/src/omni.cpp | 2 +- rm_chassis_controllers/src/sentry.cpp | 2 +- rm_chassis_controllers/src/swerve.cpp | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h index 5e116260..629ca5c0 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/chassis_base.h @@ -114,7 +114,7 @@ class ChassisBase : public controller_interface::MultiInterfaceController */ void gyro(); virtual void moveJoint(const ros::Time& time, const ros::Duration& period) = 0; - virtual geometry_msgs::Twist forwardKinematics() = 0; + virtual geometry_msgs::Twist odometry() = 0; /** @brief Init frame on base_link. Integral vel to pos and angle. * * @param time The current time. diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h index a708b789..e02c5d7f 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/omni.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/omni.h @@ -18,7 +18,7 @@ class OmniController : public ChassisBase> joints_; Eigen::MatrixXd chassis2joints_; diff --git a/rm_chassis_controllers/include/rm_chassis_controllers/sentry.h b/rm_chassis_controllers/include/rm_chassis_controllers/sentry.h index ec090c70..f0ee28cc 100644 --- a/rm_chassis_controllers/include/rm_chassis_controllers/sentry.h +++ b/rm_chassis_controllers/include/rm_chassis_controllers/sentry.h @@ -71,7 +71,7 @@ class SentryController : public ChassisBase modules_; }; diff --git a/rm_chassis_controllers/src/chassis_base.cpp b/rm_chassis_controllers/src/chassis_base.cpp index d429c03a..4a20b2dd 100644 --- a/rm_chassis_controllers/src/chassis_base.cpp +++ b/rm_chassis_controllers/src/chassis_base.cpp @@ -274,7 +274,7 @@ void ChassisBase::raw() template void ChassisBase::updateOdom(const ros::Time& time, const ros::Duration& period) { - geometry_msgs::Twist vel_base = forwardKinematics(); // on base_link frame + geometry_msgs::Twist vel_base = odometry(); // on base_link frame if (enable_odom_tf_) { geometry_msgs::Vector3 linear_vel_odom, angular_vel_odom; diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 6e4171df..48b3abba 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -62,7 +62,7 @@ void OmniController::moveJoint(const ros::Time& time, const ros::Duration& perio } } -geometry_msgs::Twist OmniController::forwardKinematics() +geometry_msgs::Twist OmniController::odometry() { Eigen::VectorXd vel_joints(joints_.size()); for (size_t i = 0; i < joints_.size(); i++) diff --git a/rm_chassis_controllers/src/sentry.cpp b/rm_chassis_controllers/src/sentry.cpp index 30f868f2..e26ad8d3 100644 --- a/rm_chassis_controllers/src/sentry.cpp +++ b/rm_chassis_controllers/src/sentry.cpp @@ -105,7 +105,7 @@ void SentryController::catapult(const ros::Time& time, const ros::Duration& peri } } -geometry_msgs::Twist SentryController::forwardKinematics() +geometry_msgs::Twist SentryController::odometry() { geometry_msgs::Twist vel_data; vel_data.linear.x = ctrl_wheel_.joint_.getVelocity() * wheel_radius_; diff --git a/rm_chassis_controllers/src/swerve.cpp b/rm_chassis_controllers/src/swerve.cpp index 38ecafd2..69c21077 100644 --- a/rm_chassis_controllers/src/swerve.cpp +++ b/rm_chassis_controllers/src/swerve.cpp @@ -99,7 +99,7 @@ void SwerveController::moveJoint(const ros::Time& time, const ros::Duration& per } } -geometry_msgs::Twist SwerveController::forwardKinematics() +geometry_msgs::Twist SwerveController::odometry() { geometry_msgs::Twist vel_data{}; geometry_msgs::Twist vel_modules{}; From 83b4c156e7cc5a8c53164aed1c30c8ae3d183d0b Mon Sep 17 00:00:00 2001 From: NaHCO3bc Date: Tue, 16 Aug 2022 15:57:10 +0800 Subject: [PATCH 4/4] Compute the params and fix some bugs. --- .../config/template_omni.yaml | 21 +++++++++++++++---- rm_chassis_controllers/src/omni.cpp | 5 +++-- 2 files changed, 20 insertions(+), 6 deletions(-) diff --git a/rm_chassis_controllers/config/template_omni.yaml b/rm_chassis_controllers/config/template_omni.yaml index 07647351..564ecd64 100644 --- a/rm_chassis_controllers/config/template_omni.yaml +++ b/rm_chassis_controllers/config/template_omni.yaml @@ -16,8 +16,21 @@ controllers: # OmniController wheels: left_front: - pose: [ 0.147, 0.147, 0.785 ] - roller_angle: 2.3561 - radius: 0.07625 + pose: [ 0.147, 0.147, 2.356 ] joint: left_front_wheel_joint - pid: { p: 0.4, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true } + <<: &wheel_setting + roller_angle: 0. + radius: 0.07625 + pid: { p: 0.6, i: 0, d: 0.0, i_max: 0.0, i_min: 0.0, antiwindup: true, publish_state: true } + right_front: + pose: [ 0.147, -0.147, 0.785 ] + joint: right_front_wheel_joint + <<: *wheel_setting + left_back: + pose: [ -0.147, 0.147, -2.356 ] + joint: left_back_wheel_joint + <<: *wheel_setting + right_back: + pose: [ -0.147, -0.147, -0.785 ] + joint: right_back_wheel_joint + <<: *wheel_setting diff --git a/rm_chassis_controllers/src/omni.cpp b/rm_chassis_controllers/src/omni.cpp index 48b3abba..1a76dfe6 100644 --- a/rm_chassis_controllers/src/omni.cpp +++ b/rm_chassis_controllers/src/omni.cpp @@ -40,7 +40,7 @@ bool OmniController::init(hardware_interface::RobotHW* robot_hw, ros::NodeHandle Eigen::MatrixXd chassis2joint = 1. / (double)wheel.second["radius"] * direction * in_wheel * in_chassis; chassis2joints_.block<1, 3>(i, 0) = chassis2joint; - ros::NodeHandle nh_wheel = ros::NodeHandle(controller_nh, "modules/" + wheel.first); + ros::NodeHandle nh_wheel = ros::NodeHandle(controller_nh, "wheels/" + wheel.first); joints_.push_back(std::make_shared()); if (!joints_.back()->init(effort_joint_interface_, nh_wheel)) return false; @@ -67,7 +67,8 @@ geometry_msgs::Twist OmniController::odometry() Eigen::VectorXd vel_joints(joints_.size()); for (size_t i = 0; i < joints_.size(); i++) vel_joints[i] = joints_[i]->joint_.getVelocity(); - Eigen::Vector3d vel_chassis = chassis2joints_.completeOrthogonalDecomposition().pseudoInverse() * vel_joints; + Eigen::Vector3d vel_chassis = + (chassis2joints_.transpose() * chassis2joints_).inverse() * chassis2joints_.transpose() * vel_joints; geometry_msgs::Twist twist; twist.angular.z = vel_chassis(0); twist.linear.x = vel_chassis(1);