Model
New package minimal_robot_description, directly create a new model file, minimal_robot_description.urdf (note that the model file here adopts a unified robot description format).
<?xml version="1.0"?>
<robot name="one_DOF_robot">
<!-- Used for fixing robot to Gazebo 'base_link' -->
<link name="world"/>
<joint name="glue_robot_to_world" type="fixed">
<parent link="world"/>
<child link="link1"/>
</joint>
<!-- Base Link -->
<link name="link1">
<collision>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 0.7"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>
<!-- Moveable Link -->
<link name="link2">
<collision>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
<!--box size="0.15 0.15 0.8"-->
</geometry>
</collision>
<visual>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="0.1" ixy="0.0" ixz="0.0"
iyy="0.1" iyz="0.0"
izz="0.005"/>
</inertial>
</link>
<joint name="joint1" type="continuous">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 1" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
</joint>
</robot>
Manually load model
Note that execute the following command in the directory of the urdf model file.
rosrun gazebo_ros spawn_model -urdf -file minimal_robot_description.urdf -model one_DOF
launch file loading model
The first subfield of the startup file is to place the robot model on the parameter server, and the second subfield is to reproduce the model from the parameter server on Gazebo. This can ensure that the ROS node and Gazebo introduce the same details of the robot model, so as to ensure the correct interaction between the ROS node and the sensor.
<launch>
<param name="robot_description"
textfile="$(find minimal_robot_description)/minimal_robot_description.urdf"/>
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description -urdf -model one_DOF_robot" />
</launch>
roslaunch minimal_robot_description minimal_robot_description.launch
Minimum controller for joint servo control
New minimal_ joint_ Cpcontroller package, create a new minimal_joint_cpntroller.cpp file, the program is as follows (note that we generally do not use this method for control, but generally use Gazebo for control).
**#include <ros/ros.h> //ALWAYS need to include this
#include <gazebo_msgs/GetModelState.h>
#include <gazebo_msgs/ApplyJointEffort.h>
#include <gazebo_msgs/GetJointProperties.h>
#include <sensor_msgs/JointState.h>
#include <string.h>
#include <stdio.h>
#include <std_msgs/Float64.h>
#include <math.h>
//some "magic number" global params:
const double Kp = 10.0; //controller gains
const double Kv = 3;
const double dt = 0.01;
//a simple saturation function; provide saturation threshold, sat_val, and arg to be saturated, val
double sat(double val, double sat_val) {
if (val > sat_val)
return (sat_val);
if (val< -sat_val)
return (-sat_val);
return val;
}
double min_periodicity(double theta_val) {
double periodic_val = theta_val;
while (periodic_val > M_PI) {
periodic_val -= 2 * M_PI;
}
while (periodic_val< -M_PI) {
periodic_val += 2 * M_PI;
}
return periodic_val;
}
double g_pos_cmd = 0.0; //position command input-- global var
void posCmdCB(const std_msgs::Float64& pos_cmd_msg) {
ROS_INFO("received value of pos_cmd is: %f", pos_cmd_msg.data);
g_pos_cmd = pos_cmd_msg.data;
}
bool test_services() {
bool service_ready = false;
if (!ros::service::exists("/gazebo/apply_joint_effort", true)) {
ROS_WARN("waiting for apply_joint_effort service");
return false;
}
if (!ros::service::exists("/gazebo/get_joint_properties", true)) {
ROS_WARN("waiting for /gazebo/get_joint_properties service");
return false;
}
ROS_INFO("services are ready");
return true;
}
int main(int argc, char **argv) {
//initializations:
ros::init(argc, argv, "minimal_joint_controller");
ros::NodeHandle nh;
ros::Duration half_sec(0.5);
// make sure services are available before attempting to proceed, else node will crash
while (!test_services()) {
ros::spinOnce();
half_sec.sleep();
}
ros::ServiceClient set_trq_client =
nh.serviceClient<gazebo_msgs::ApplyJointEffort>("/gazebo/apply_joint_effort");
ros::ServiceClient get_jnt_state_client =
nh.serviceClient<gazebo_msgs::GetJointProperties>("/gazebo/get_joint_properties");
gazebo_msgs::ApplyJointEffort effort_cmd_srv_msg;
gazebo_msgs::GetJointProperties get_joint_state_srv_msg;
ros::Publisher trq_publisher = nh.advertise<std_msgs::Float64>("jnt_trq", 1);
ros::Publisher vel_publisher = nh.advertise<std_msgs::Float64>("jnt_vel", 1);
ros::Publisher pos_publisher = nh.advertise<std_msgs::Float64>("jnt_pos", 1);
ros::Publisher joint_state_publisher = nh.advertise<sensor_msgs::JointState>("joint_states", 1);
ros::Subscriber pos_cmd_subscriber = nh.subscribe("pos_cmd", 1, posCmdCB);
std_msgs::Float64 trq_msg, q1_msg, q1dot_msg;
double q1, q1dot, q1_err, trq_cmd;
sensor_msgs::JointState joint_state_msg;
ros::Duration duration(dt);
ros::Rate rate_timer(1 / dt);
effort_cmd_srv_msg.request.joint_name = "joint1";
effort_cmd_srv_msg.request.effort = 0.0;
effort_cmd_srv_msg.request.duration = duration;
get_joint_state_srv_msg.request.joint_name = "joint1";
// set up the joint_state_msg fields to define a single joint,
// called joint1, and initial position and vel values of 0
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.name.push_back("joint1");
joint_state_msg.position.push_back(0.0);
joint_state_msg.velocity.push_back(0.0);
//here is the main controller loop:
while (ros::ok()) {
get_jnt_state_client.call(get_joint_state_srv_msg);
q1 = get_joint_state_srv_msg.response.position[0];
q1_msg.data = q1;
pos_publisher.publish(q1_msg); //republish his val on topic jnt_pos
q1dot = get_joint_state_srv_msg.response.rate[0];
q1dot_msg.data = q1dot;
vel_publisher.publish(q1dot_msg);
joint_state_msg.header.stamp = ros::Time::now();
joint_state_msg.position[0] = q1;
joint_state_msg.velocity[0] = q1dot;
joint_state_publisher.publish(joint_state_msg);
q1_err = min_periodicity(g_pos_cmd - q1); //jnt angle err; watch for periodicity
trq_cmd = Kp * (q1_err) - Kv*q1dot;
trq_msg.data = trq_cmd;
trq_publisher.publish(trq_msg);
effort_cmd_srv_msg.request.effort = trq_cmd; // send torque command to Gazebo
set_trq_client.call(effort_cmd_srv_msg);
//make sure service call was successful
bool result = effort_cmd_srv_msg.response.success;
if (!result)
ROS_WARN("service call to apply_joint_effort failed!");
ros::spinOnce();
rate_timer.sleep();
}
}
The specific control steps of the controller node are as follows: first, obtain the current joint position and speed from Gazebo, and enter it in the topic joint_ Publish on states; Compare the joint sensor value with the command joint angle; Calculate the moment response; By applying_ joint_ The force service sends this force to Gazebo.
rosrun minimal_joint_controller joint_controller rostopic pub pos_cmd std_msgs/Float64 1.0
You can see that the joint rotates according to the expected joint angle.
The Gazebo plug-in performs joint servo control
First, download the plug-in. My version is Ubuntu 16 04, so it's kinetic.
sudo apt-get install ros-kinetic-controller-interface ros-kinetic-gazebo-ros-control ros-kinetic-joint-state-control ros-kinetic-effort-controllers
Or in minimal_ robot_ In the description package, create a new control_config directory, create a new one below_ dof_ ctl_ params. Yaml file
one_DOF_robot:
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
# Position Controllers ---------------------------------------
joint1_position_controller:
type: effort_controllers/JointPositionController
joint: joint1
pid: {p: 10.0, i: 10.0, d: 10.0, i_clamp_min: -10.0, i_clamp_max: 10.0}
New model file_ robot_ description_ w_ jnt_ ctl. urdf.
<?xml version="1.0"?>
<robot name="one_DOF_robot">
<!-- Used for fixing robot to Gazebo 'base_link' -->
<link name="world"/>
<joint name="glue_robot_to_world" type="fixed">
<parent link="world"/>
<child link="link1"/>
</joint>
<!-- Base Link -->
<link name="link1">
<collision>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 0.7"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<box size="0.2 0.2 1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>
<!-- Moveable Link -->
<link name="link2">
<collision>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
<!--box size="0.15 0.15 0.8"-->
</geometry>
</collision>
<visual>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<geometry>
<cylinder length="1" radius="0.1"/>
</geometry>
</visual>
<inertial>
<origin xyz="0 0 0.5" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="0.1" ixy="0.0" ixz="0.0"
iyy="0.1" iyz="0.0"
izz="0.005"/>
</inertial>
</link>
<joint name="joint1" type="revolute">
<parent link="link1"/>
<child link="link2"/>
<origin xyz="0 0 1" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<limit effort="10.0" lower="0.0" upper="2.0" velocity="0.5"/>
<dynamics damping="1.0"/>
</joint>
<transmission name="tran1">
<type>transmission_interface/SimpleTransmission</type>
<joint name="joint1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
</joint>
<actuator name="motor1">
<hardwareInterface>EffortJointInterface</hardwareInterface>
<mechanicalReduction>1</mechanicalReduction>
</actuator>
</transmission>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/one_DOF_robot</robotNamespace>
</plugin>
</gazebo>
</robot>
Use the startup file minimal of the Gazebo plug-in_ robot_ w_ jnt_ ctl. Launch (combined with model loading).
<launch>
<!-- Load joint controller configurations from YAML file to parameter server -->
<rosparam file="$(find minimal_robot_description)/control_config/one_dof_ctl_params.yaml" command="load"/>
<param name="robot_description"
textfile="$(find minimal_robot_description)/minimal_robot_description_w_jnt_ctl.urdf"/>
<!-- Spawn a robot into Gazebo -->
<node name="spawn_urdf" pkg="gazebo_ros" type="spawn_model"
args="-param robot_description -urdf -model one_DOF_robot" />
<!--start up the controller plug-ins via the controller manager -->
<node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
output="screen" ns="/one_DOF_robot" args="joint_state_controller joint1_position_controller"/>
</launch>
The following is the joint servo control.
//Open a blank world roslaunch gazebo_ros empty_world.launch //Load the model and start the plug-in roslaunch minimal_robot_description minimal_robot_w_jnt_ctl.launch //Publish joint angle rostopic pub -r 10 /one_DOF_robot/joint1_position_controller/command std_msgs/Float64 1.0
Run screenshot.
Up to now, I have read the loading methods of the two models and the two joint control methods. Although I can't write the model program and control the joints concretely (it's too difficult and I haven't touched them at all)... I'd better start the later part first. These are not used for the time being