1. 寫在前面
第一篇 – 機器人描述: http://blog.csdn.net/sunbibei/article/details/52297524
第二篇 – TRANSMISSION
: http://blog.csdn.net/sunbibei/article/details/53287975
在我上一篇博客上, 我們引入了UR5BH機器人, 另外, 對其描述文件中的transmission進行了解析, 其中摘錄瞭如下文件:
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
<!--robotNamespace>/</robotNamespace-->
<!--robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType-->
</plugin>
</gazebo>
... ...
<transmission name="shoulder_pan_trans">
<type>transmission_interface/SimpleTransmission</type>
<joint name="shoulder_pan_joint">
<hardwareInterface>PositionJointInterface</hardwareInterface>
</joint>
<actuator name="shoulder_pan_motor">
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
... ...
<transmission name="bh_j32_transmission">
<type>transmission_interface/SimpleTransmission</type>
<joint name="bh_j32_joint"/>
<actuator name="bh_j32">
<hardwareInterface>PositionJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
<motorTorqueConstant>1</motorTorqueConstant>
</actuator>
</transmission>
在這段描述中, 除了transmission
之外, 另一個引入的, 就是gazebo_ros_control
. 本篇作爲第三篇, 主要和大家分享一些關於gazebo_ros_control
相關的內容。
2. Gazebo ROS Control
在前一篇的基礎上, 實際上我們已經能夠開始配置MoveIt!來進行機器人控制了。 但是, 爲了更進一步的讓事情更加清晰, 我們還是耐下心再探討一下gazebo_ros_control
。 包佈局如下圖所示:
打開package.xml文件, 除了一些依賴包的定義外, 一個比較關鍵的地方是在export
塊中, 摘錄如下:
... ...
<export>
<gazebo_ros_control plugin="${prefix}/robot_hw_sim_plugins.xml"/>
</export>
... ...
瞭解pluginlib
的同學肯定知道, 這是將plugin的定義文件導入的步驟。 不熟悉的同學, 可以參考1, 或者 參考2 進行了解。
導入的文件在文件系統中可以看到, 摘錄如下:
<library path="lib/libdefault_robot_hw_sim">
<class
name="gazebo_ros_control/DefaultRobotHWSim"
type="gazebo_ros_control::DefaultRobotHWSim"
base_class_type="gazebo_ros_control::RobotHWSim">
<description>
A default robot simulation interface which constructs joint handles from an SDF/URDF.
</description>
</class>
</library>
可以看到, 該文件中定義了一個plugin, 就是gazebo_ros_control::DefaultRobotHWSim
, 該plugin可以動態的被加載進來。再查看該包的CMakeLists.txt
, 可以看到下述內容:
## 2.1. Libraries
add_library(${PROJECT_NAME} src/gazebo_ros_control_plugin.cpp)
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
add_library(default_robot_hw_sim src/default_robot_hw_sim.cpp)
target_link_libraries(default_robot_hw_sim ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES})
該包總共會生成兩個庫, 第一個庫是gazebo_ros_control
, 第二個庫是default_robot_hw_sim
, 分別都只有一個文件。 下面我們分別來分析這兩個庫的源碼以及所對應完成的任務。
3. Default Robot HW SIM
在default_robot_hw_sim.cpp
中, 定義了類DefaultRobotHWSim
, 該類繼承於gazebo_ros_control::RobotHWSim
, 父類定義摘錄如下:
namespace gazebo_ros_control {
// Struct for passing loaded joint data
struct JointData
{
std::string name_;
std::string hardware_interface_;
JointData(const std::string& name, const std::string& hardware_interface) :
name_(name),
hardware_interface_(hardware_interface)
{}
};
class RobotHWSim : public hardware_interface::RobotHW
{
public:
virtual ~RobotHWSim() { }
virtual bool initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model, // 留意該指針有兩個const
// TransmissionInfo的內容在前一篇博客中有介紹
std::vector<transmission_interface::TransmissionInfo> transmissions) = 0;
virtual void readSim(ros::Time time, ros::Duration period) = 0;
virtual void writeSim(ros::Time time, ros::Duration period) = 0;
virtual void eStopActive(const bool active) {}
};
}
該類定義了gazebo plugin版本的RobotHW
,RobotHW
定義於ros control
包中, 定義了與實際機器人通訊的基本接口, 在編寫實際機器人的ros control
體系時, 該類是用戶必須實現的類。 主要重載read()
, write()
, preparewitch()
以及doSwitch()
四個接口。 在RobotHW
中, 這四個函數都有默認實現(默認實現是空, 什麼都不做), 都不是純虛函數。 一般而言, 重載讀寫函數即可正常工作了。 這個地方, 只是將接口換了個名字, 並且, 幾個純虛函數都必須在子類中進行實現。
類DefaultRobotHWSim
的定義摘錄如下:
namespace gazebo_ros_control
{
class DefaultRobotHWSim : public gazebo_ros_control::RobotHWSim
{
public:
// 四個純需函數必須在子類中進行實現。
virtual bool initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions);
virtual void readSim(ros::Time time, ros::Duration period);
virtual void writeSim(ros::Time time, ros::Duration period);
virtual void eStopActive(const bool active);
protected:
// Methods used to control a joint. 一個關節!!!
enum ControlMethod {EFFORT, POSITION, POSITION_PID, VELOCITY, VELOCITY_PID};
// 關節限制相關的內容, 不是我們關注的重點, 感興趣的朋友可以參考下述網站:
// http://wiki.ros.org/pr2_controller_manager/safety_limits
void registerJointLimits(const std::string& joint_name,
const hardware_interface::JointHandle& joint_handle,
const ControlMethod ctrl_method,
const ros::NodeHandle& joint_limit_nh,
const urdf::Model *const urdf_model,
int *const joint_type, double *const lower_limit,
double *const upper_limit, double *const effort_limit);
unsigned int n_dof_;
// 關節接口, 這四個類的設計很有意思。
hardware_interface::JointStateInterface js_interface_;
hardware_interface::EffortJointInterface ej_interface_;
hardware_interface::PositionJointInterface pj_interface_;
hardware_interface::VelocityJointInterface vj_interface_;
// 關節限制相關的內容
joint_limits_interface::EffortJointSaturationInterface ej_sat_interface_;
joint_limits_interface::EffortJointSoftLimitsInterface ej_limits_interface_;
joint_limits_interface::PositionJointSaturationInterface pj_sat_interface_;
joint_limits_interface::PositionJointSoftLimitsInterface pj_limits_interface_;
joint_limits_interface::VelocityJointSaturationInterface vj_sat_interface_;
joint_limits_interface::VelocityJointSoftLimitsInterface vj_limits_interface_;
std::vector<double> joint_lower_limits_;
std::vector<double> joint_upper_limits_;
std::vector<double> joint_effort_limits_;
std::vector<std::string> joint_names_;
std::vector<int> joint_types_;
std::vector<ControlMethod> joint_control_methods_;
std::vector<control_toolbox::Pid> pid_controllers_;
std::vector<double> joint_position_;
std::vector<double> joint_velocity_;
std::vector<double> joint_effort_;
std::vector<double> joint_effort_command_;
std::vector<double> joint_position_command_;
std::vector<double> last_joint_position_command_;
std::vector<double> joint_velocity_command_;
std::vector<gazebo::physics::JointPtr> sim_joints_;
// 緊急停止時, e_stop_active = true
bool e_stop_active_, last_e_stop_active_;
};
typedef boost::shared_ptr<DefaultRobotHWSim> DefaultRobotHWSimPtr;
}
#endif // #ifndef __GAZEBO_ROS_CONTROL_PLUGIN_DEFAULT_ROBOT_HW_SIM_H_
由於關於Safty Limits 並不是我們關注的內容。 所以在後面的解析過程中, 我們也會忽略該部分內容, 感興趣的同學, 可以參考上述網站, 內容也不是很多。下面我們主要關注initSim()
的實現, 其他幾個函數無非是調用Gazebo的API完成關節數據的讀寫。
iniSim()
函數的實現摘錄如下:
bool DefaultRobotHWSim::initSim(
const std::string& robot_namespace,
ros::NodeHandle model_nh,
gazebo::physics::ModelPtr parent_model,
const urdf::Model *const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions) {
const ros::NodeHandle joint_limit_nh(model_nh);
// Resize vectors to our DOF
n_dof_ = transmissions.size(); // 也就是說, 該變量保存關節個數
joint_names_.resize(n_dof_);
joint_types_.resize(n_dof_);
joint_lower_limits_.resize(n_dof_);
joint_upper_limits_.resize(n_dof_);
joint_effort_limits_.resize(n_dof_);
joint_control_methods_.resize(n_dof_);
pid_controllers_.resize(n_dof_);
joint_position_.resize(n_dof_);
joint_velocity_.resize(n_dof_);
joint_effort_.resize(n_dof_);
joint_effort_command_.resize(n_dof_);
joint_position_command_.resize(n_dof_);
joint_velocity_command_.resize(n_dof_);
// 初始化所有變量
for(unsigned int j=0; j < n_dof_; j++) {
if(transmissions[j].joints_.size() == 0) {
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has no associated joints.");
continue;
} else if(transmissions[j].joints_.size() > 1) {
ROS_WARN_STREAM_NAMED("default_robot_hw_sim","Transmission " << transmissions[j].name_
<< " has more than one joint. Currently the default robot hardware simulation "
<< " interface only supports one.");
continue;
} // 判定transmission內容是否有錯, DefaultRobotHWSim僅支持一個tramsmission對應一個joint
std::vector<std::string> joint_interfaces = transmissions[j].joints_[0].hardware_interfaces_;
if (joint_interfaces.empty() && !(transmissions[j].actuators_.empty())
&& !(transmissions[j].actuators_[0].hardware_interfaces_.empty())) {
// TODO: Deprecate HW interface specification in actuators in ROS J
joint_interfaces = transmissions[j].actuators_[0].hardware_interfaces_;
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "The <hardware_interface> element of tranmission " <<
transmissions[j].name_ << " should be nested inside the <joint> element, not <actuator>. " <<
"The transmission will be properly loaded, but please update " <<
"your robot model to remain compatible with future versions of the plugin.");
}
if (joint_interfaces.empty()) {
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " does not specify any hardware interface. " <<
"Not adding it to the robot hardware simulation.");
continue;
} else if (joint_interfaces.size() > 1) {
ROS_WARN_STREAM_NAMED("default_robot_hw_sim", "Joint " << transmissions[j].joints_[0].name_ <<
" of transmission " << transmissions[j].name_ << " specifies multiple hardware interfaces. " <<
"Currently the default robot hardware simulation interface only supports one. Using the first entry!");
//continue;
} // 至少包含一個transmission, 多個的話, 將會默認使用第一個, 但並不算錯誤(沒有continue)。
// 初始化各變量(name, pos, vel, eff, 以及cmd
joint_names_[j] = transmissions[j].joints_[0].name_;
joint_position_[j] = 1.0;
joint_velocity_[j] = 0.0;
joint_effort_[j] = 1.0; // N/m for continuous joints
joint_effort_command_[j] = 0.0;
joint_position_command_[j] = 0.0;
joint_velocity_command_[j] = 0.0;
// 常引用
const std::string& hardware_interface = joint_interfaces.front();
// Debug
ROS_DEBUG_STREAM_NAMED("default_robot_hw_sim","Loading joint '" << joint_names_[j]
<< "' of type '" << hardware_interface << "'");
// hardware_interface::JointStateInterface初始化
// 將類中pos, vel, eff三個變量的地址綁定對應於三個變量joint_xxx[j]
js_interface_.registerHandle(hardware_interface::JointStateHandle(
joint_names_[j], &joint_position_[j], &joint_velocity_[j], &joint_effort_[j]));
// 根據申明的transmission, 創建對應的JointHandle.
hardware_interface::JointHandle joint_handle;
if(hardware_interface == "EffortJointInterface") {
// Create effort joint interface
joint_control_methods_[j] = EFFORT;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_effort_command_[j]);
ej_interface_.registerHandle(joint_handle);
} else if(hardware_interface == "PositionJointInterface") {
// Create position joint interface
joint_control_methods_[j] = POSITION;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_position_command_[j]);
pj_interface_.registerHandle(joint_handle);
} else if(hardware_interface == "VelocityJointInterface") {
// Create velocity joint interface
joint_control_methods_[j] = VELOCITY;
joint_handle = hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[j]),
&joint_velocity_command_[j]);
vj_interface_.registerHandle(joint_handle);
} else {
ROS_FATAL_STREAM_NAMED("default_robot_hw_sim","No matching hardware interface found for '"
<< hardware_interface );
return false;
}
// Get the gazebo joint that corresponds to the robot joint.
gazebo::physics::JointPtr joint = parent_model->GetJoint(joint_names_[j]);
if (!joint) {
ROS_ERROR_STREAM("This robot has a joint named \"" << joint_names_[j]
<< "\" which is not in the gazebo model.");
return false;
}
sim_joints_.push_back(joint);
registerJointLimits(joint_names_[j], joint_handle, joint_control_methods_[j],
joint_limit_nh, urdf_model,
&joint_types_[j], &joint_lower_limits_[j], &joint_upper_limits_[j],
&joint_effort_limits_[j]);
// 初始化控制器
if (joint_control_methods_[j] != EFFORT) {
const ros::NodeHandle nh(model_nh, "/gazebo_ros_control/pid_gains/" +
joint_names_[j]);
if (pid_controllers_[j].init(nh, true)) {
switch (joint_control_methods_[j]) {
case POSITION:
joint_control_methods_[j] = POSITION_PID;
break;
case VELOCITY:
joint_control_methods_[j] = VELOCITY_PID;
break;
}
} else {
#if GAZEBO_MAJOR_VERSION > 2
joint->SetParam("fmax", 0, joint_effort_limits_[j]);
#else
joint->SetMaxForce(0, joint_effort_limits_[j]);
#endif
}
}
}
// 註冊接口, 父類RobotHW是繼承於InterfaceManager的
// 實則是調用InterfaceManager的方法。
registerInterface(&js_interface_);
registerInterface(&ej_interface_);
registerInterface(&pj_interface_);
registerInterface(&vj_interface_);
// Initialize the emergency stop code.
e_stop_active_ = false;
last_e_stop_active_ = false;
return true;
}
從上面的實現可以看到, 通過URDF文件中設定transmission
, DefaultRobotHWSim
解析所有的transmission
塊, 保存每個關節的JointHandle
, 並提供讀寫數據的接口。其中JointHandle
的管理方式是調用ROS Control中的hardware_interface
, 而這一塊的內容, 將在後面的博客中專門分析, 這部分的實現非常巧妙, 也很有意思。
4. Gazebo ROS Control Plugin
GazeboRosControlPlugin
繼承於gazebo::ModelPlugin
類, 是一個可以由Gazebo加載的Plugin, 該類中最爲關鍵的兩個函數分別是: Load()
and Update()
. 分別的實現如下:
Load()
的實現如下所示:
void GazeboRosControlPlugin::Load(gazebo::physics::ModelPtr parent, sdf::ElementPtr sdf)
{
ROS_INFO_STREAM_NAMED("gazebo_ros_control","Loading gazebo_ros_control plugin");
// Save pointers to the model
parent_model_ = parent;
sdf_ = sdf;
// 一系列的容錯判斷, 以及獲取sdf中所定義的參數
... ...
// 從sdf_中獲得RobotHWSim的類型
if(sdf_->HasElement("robotSimType")) {
robot_hw_sim_type_str_ = sdf_->Get<std::string>("robotSimType");
} else {
robot_hw_sim_type_str_ = "gazebo_ros_control/DefaultRobotHWSim";
ROS_DEBUG_STREAM_NAMED("loadThread","Using default plugin for RobotHWSim (none specified in URDF/SDF)\""<<robot_hw_sim_type_str_<<"\"");
}
... ...
// Get parameters/settings for controllers from ROS param server
model_nh_ = ros::NodeHandle(robot_namespace_);
... ...
// 從參數服務器中獲得URDF
const std::string urdf_string = getURDF(robot_description_);
if (!parseTransmissionsFromURDF(urdf_string)) {
ROS_ERROR_NAMED("gazebo_ros_control",
"Error parsing URDF in gazebo_ros_control plugin, plugin not active.\n");
return;
}
// 通過ClassLoader加載RobotHWSim(默認使用的是DefaultRobotHWSim)
try {
robot_hw_sim_loader_.reset
(new pluginlib::ClassLoader<gazebo_ros_control::RobotHWSim>
("gazebo_ros_control",
"gazebo_ros_control::RobotHWSim"));
robot_hw_sim_ = robot_hw_sim_loader_->createInstance(robot_hw_sim_type_str_);
urdf::Model urdf_model;
const urdf::Model *const urdf_model_ptr = urdf_model.initString(urdf_string) ? &urdf_model : NULL;
// 調用前面分析過的那個函數, initSim, 在RobotHWSim中初始化所有關節句柄
if(!robot_hw_sim_->initSim(robot_namespace_, model_nh_, parent_model_, urdf_model_ptr, transmissions_)) {
ROS_FATAL_NAMED("gazebo_ros_control","Could not initialize robot simulation interface");
return;
}
// 創建ControllerManager, 可以看到, ControllerManager是在這個地方被初始化。
ROS_DEBUG_STREAM_NAMED("ros_control_plugin","Loading controller_manager");
controller_manager_.reset
(new controller_manager::ControllerManager(robot_hw_sim_.get(), model_nh_));
// 監聽update event, 每個simulation iteration都會廣播該event.
update_connection_ =
gazebo::event::Events::ConnectWorldUpdateBegin
(boost::bind(&GazeboRosControlPlugin::Update, this));
} catch(pluginlib::LibraryLoadException &ex) {
ROS_FATAL_STREAM_NAMED("gazebo_ros_control", "Failed to create robot simulation interface loader: "
<< ex.what());
}
ROS_INFO_NAMED("gazebo_ros_control", "Loaded gazebo_ros_control.");
}
上述函數的實現中, ... ...
代表有省略部分。 僅抽離出來了比較重要的部分, 將一些容錯和參數獲取的代碼給省略了。 可以看到, 在該函數中, 完成了RobotHW
的初始化(在Gazebo中, 是通過RobotHWSim
類呈現, 該類實際上也是RobotHW
的子類), 完成了ControllerManager
的初始化, 並將RosControl循環(update()
函數)與update event進行了綁定, 在每次事件到來的時候, 都將會進行循環。
update()
函數的實現如下所示:
// Called by the world update start event
void GazeboRosControlPlugin::Update() {
// 得到當前時間以及和上一次更新的時間間隔
gazebo::common::Time gz_time_now = parent_model_->GetWorld()->GetSimTime();
ros::Time sim_time_ros(gz_time_now.sec, gz_time_now.nsec);
ros::Duration sim_period = sim_time_ros - last_update_sim_time_ros_;
robot_hw_sim_->eStopActive(e_stop_active_);
// Check if we should update the controllers
if(sim_period >= control_period_) {
// 交替更新前一次時間變量
last_update_sim_time_ros_ = sim_time_ros;
// 調用RobotHW接口, 讀關節數據, 保證當前關節值是最新值
robot_hw_sim_->readSim(sim_time_ros, sim_period);
// Compute the controller commands
bool reset_ctrlrs;
if (e_stop_active_) {
reset_ctrlrs = false;
last_e_stop_active_ = true;
} else {
if (last_e_stop_active_) {
reset_ctrlrs = true;
last_e_stop_active_ = false;
} else {
reset_ctrlrs = false;
}
}
// ControlManager類的update接口調用, 將會順序調用每一個加載上來的控制器的update()
controller_manager_->update(sim_time_ros, sim_period, reset_ctrlrs);
}
// 每一個正在運行的控制器update調用之後, 對應被控制關節的命令就更新完成
// 現在再調用RobotHW的寫操作。 完成對關節命令的寫入。
// 不但在仿真中RosControl循環是這個流程, 在實際RosControl循環中, 也是這個流程。
robot_hw_sim_->writeSim(sim_time_ros, sim_time_ros - last_write_sim_time_ros_);
last_write_sim_time_ros_ = sim_time_ros;// 交替更新前一次時間變量
}
5. ROS Controller運用
在前一篇中, 我們在Gazebo中已經啓動起來了ur2bh機器人。 輸入命令:roslaunch ur5bh_gazebo ur5bh_gazebo.launch
。 在啓動起來Gazebo之後, 命令行中可以看到如下幾行輸出:
而這幾行的輸出, 正是在前面Load()
函數中的提示輸出。 顯然, 我們的GazeboRosControlPlugin
已經在launch文件中, 被啓動加載了。當然, 這個時候ControllerManager
也處於就緒狀態, 可以重新打開命令行, 輸入rosservice call /controller_manager/list_controller_types
進行驗證, 該服務器將會返回一大堆信息。現在, 我們就可以在配置文件裏面指定我們所需要的控制器了。 比如說, 由於我們在transmission
塊中, 指定的是PositionJointInterface
, 所以, 我們可以使用的控制器只能是位置控制器。 下面我們試着加一兩個控制器。
5.1. position_controllers/JointPositionController
該控制器實現於ros_controllers
包中, 這個包提供了很多默認控制器, 可以直接使用, 感興趣的同學, 也可以到Github 上進行下載查看他們的源碼。要配置這些控制器, 只需要簡單的配置文件即可搞定。
打開命令行, 以此輸入:
roscd ur5bh_gazebo && mkdir config
vim config/controllers.yaml
將下述內容複製到該文件中:
shoulder_pan_position_controller:
type: position_controllers/JointPositionController
joint: shoulder_pan_joint
pid:
p: 100.0
d: 10.0
第一行是控制器名稱, 第二行是控制器類型, 第三行是欲控制的關節名稱, 第四行是指定該控制器的PID參數。運行rosed ur5bh_gazebo ur5bh_gazebo.launch
, 加入下述內容在文件中:
<rosparam file="$(find ur5bh_gazebo)/config/controllers.yaml" command="load"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn shoulder_pan_position_controller"
respawn="false" output="screen"/>
上述內容, 第一行是將yaml文件中定義的內容, 加載到參數服務器。 第二行類似於在命令行中運行rosrun controller_manager controller_manager spawn shoulder_pan_position_controller
. 完成的功能, 是創建一個節點, 請求ControllerManager的加載服務器的服務, 請求加載控制器shoulder_pan_position_controller
, 而該控制器的相關參數定義於yaml文件中。
複製保存之後, 打開命令行, 運行roslaunch ur5bh_gazebo ur5bh_gazebo.launch
, 同樣, 可以看到前篇博客所示的狀態, 同時, 在命令行中, 可以看到如下輸出。
再打開一個命令行, 輸入下述命令可以查看我們加載的控制器的效果, rostopic pub /shoulder_pan_position_controller/command std_msgs/Float64 50
在Gazebo中可以看到, 我們的機器人轉動了。 我們在上述yaml中, 只是定義了一個關節的控制器, 當然, 我們可以配置多個控制器, 分別控制多個關節. 方法類似, 就不示例了。
5.2. position_controllers/JointGroupPositionController
另一種方式, 可以加載position_controllers/JointGroupPositionController
, 指定一個控制器控制多個關節。示例如下, 首先在config/controllers.yaml中添加下述內容:
finger1_position_controller:
type: position_controllers/JointGroupPositionController
joints:
- bh_j11_joint
- bh_j12_joint
- bh_j13_joint
在launch文件中, 將發起的節點那一行更換成:
<node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn shoulder_pan_position_controller finger1_position_controller"
respawn="false" output="screen"/>
同樣, 打開命令行, 運行roslaunch ur5bh_gazebo ur5bh_gazebo.launch
, 打開Gazebo之後, 應該能夠在命令行中看到下述輸出內容:
新打開一個命令行, 輸入下述命令:
rostopic pub /finger1_position_controller/command std_msgs/Float64MultiArray '{data: [10.0, 10.0, 10.0]}'
, 在Gazebo中, 可以看到手指動了。
5.3. joint_state_controller
一般情況下, 我們的關節數據都是需要通過/joint_states
話題傳遞出來, 這樣, RobotStatePublisher才能夠解析發佈/tf
數據。 而現在我們是沒有/joint_states
話題的(可以在命令行中通過rostopic list
來查看)。這個話題一個最直接的使用, 就是在rviz中。 在rviz中是可以直接查看我們機器人當前狀態的。比如, 打開rviz(運行rosrun rviz rviz
), 當前我們在rviz中添加RobotModel
插件, 看到的應該是下述狀態:
當然, 也是不能在該插件中看到我們的ur5bh機器人。 解決這個問題, ros control爲我們提供了一個默認控制器, 專門用來發布/joint_states
話題數據。有兩種方式可以添加該控制器, 完成其中一種即可。
5.3.1. 方式一
首先在config/controllers.yaml中添加下述內容:
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
在launch文件中, 將發起的節點那一行更換成:
<node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn shoulder_pan_position_controller finger1_position_controller joint_state_controller"
respawn="false" output="screen"/>
5.3.2. 方式二
除了通過上述方式加載joint_state_controller外, 還可以直接將該控制器寫到URDF文件中去。由於我們在ur5bh_gazebo.launch
文件中, 上傳到參數服務器的文件是ur5bh.urdf
, 所以修改該模型文件, 在ur5bh.urdf
中的gazebo
塊中(在該文件的頭部)。修改後gazebo
塊的內容如下(刪除了其中註釋的部分內容):
<gazebo>
<plugin filename="libgazebo_ros_control.so" name="ros_control">
</plugin>
<plugin name="joint_state_publisher" filename="libgazebo_ros_joint_state_publisher.so">
<jointName>
bh_j11_joint, bh_j12_joint, bh_j13_joint, bh_j21_joint, bh_j22_joint, bh_j23_joint,
bh_j32_joint, bh_j33_joint, elbow_joint, shoulder_lift_joint, shoulder_pan_joint,
wrist_1_joint, wrist_2_joint, wrist_3_joint
</jointName>
<updateRate>50</updateRate>
<alwaysOn>true</alwaysOn>
</plugin>
</gazebo>
5.3.3. 效果驗證
同樣, 打開命令行, 運行roslaunch ur5bh_gazebo ur5bh_gazebo.launch
, 打開Gazebo之後, 再新打開一個命令行, 輸入rostopic echo /joint_states
, 可以看到該話題的數據, 發佈頻率是按照我們指定的50Hz. 這個時候再看rviz, 其中RobotModel
還是一堆錯誤, 這是因爲該插件需要的/tf
數據還是沒有。 解決辦法: 新打開一個命令行, 輸入rosrun robot_state_publisher robot_state_publisher
。 不出意外, 可以看到如下效果:
可以看到, rviz中, 模型的狀態和Gazebo中是保持一直的。所以robot_state_publisher節點的運行是必要的。 可以將其直接添加到ur5bh_gazebo.launch
文件中, 將下述內容添加到launch文件中:
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
</node>
附錄
從rqt的插件中, 可以查看到整體的結構圖, 可以清晰的看到/joint_states起到的作用。
下面把最終的controller.yaml 和 ur5bh_gazebo.launch 兩個文件的全部內容附屬在下面:
ur5bh_gazebo/config/controllers.launch
shoulder_pan_position_controller:
type: position_controllers/JointPositionController
joint: shoulder_pan_joint
pid:
p: 100.0
d: 10.0
finger1_position_controller:
type: position_controllers/JointGroupPositionController
joints:
- bh_j11_joint
- bh_j12_joint
- bh_j13_joint
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
ur5bh_gazebo/launch/ur5bh_gazebo.launch
<?xml version="1.0"?>
<launch>
<!-- startup simulated world -->
<include file="$(find gazebo_ros)/launch/empty_world.launch" />
<!-- send robot urdf to param server -->
<param name="robot_description"
command="$(find xacro)/xacro.py '$(find ur5bh_description)/urdf/ur5bh.urdf'" />
<!-- push robot_description to factory and spawn robot in gazebo -->
<node name="spawn_gazebo_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot -z 0.1"
respawn="false" output="screen" />
<rosparam file="$(find ur5bh_gazebo)/config/controller.yaml" command="load"/>
<node name="joint_controller_spawner" pkg="controller_manager" type="controller_manager"
args="spawn shoulder_pan_position_controller finger1_position_controller joint_state_controller"
respawn="false" output="screen"/>
<!-- Robot state publisher -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
<param name="publish_frequency" type="double" value="50.0" />
<param name="tf_prefix" type="string" value="" />
</node>
</launch>