仿真截图
rqtgraph
ROS Control教程官方
http://gazebosim.org/tutorials/?tut=ros_control
下图概述了仿真、硬件、控制器和传输之间的关系:
(图片分 gazebo仿真 和真实硬件两部分.两者实现方式是不同的)
Add transmission elements to a URDF
To use ros_control with your robot, you need to add some additional elements to your URDF. The <transmission> element is used to link actuators to joints, see the <transmission> spec for exact XML format.
在URDF文件里添加transmission元素
使用 ros_control 必须要在urdf里添加一些可选元素<transmission>用于连杆关节的执行器,...
Add the gazebo_ros_control plugin
...
The default plugin XML should be added to your URDF:
默认插件xml:
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/MYROBOT</robotNamespace>
</plugin>
</gazebo>
The gazebo_ros_control <plugin> tag also has the following optional child elements:
<robotNamespace>: The ROS namespace to be used for this instance of the plugin, defaults to robot name in URDF/SDF名称空间
<controlPeriod>: The period of the controller update (in seconds), defaults to Gazebo's period更新周期单位秒
<robotParam>: The location of the robot_description (URDF) on the parameter server, defaults to '/robot_description'
<robotSimType>: The pluginlib name of a custom robot sim interface to be used (see below for more details), defaults to 'DefaultRobotHWSim'默认是DefaultRobotHWSim
Default gazebo_ros_control Behavior
By default, without a <robotSimType> tag, gazebo_ros_control will attempt to get all of the information it needs to interface with a ros_control-based controller out of the URDF. This is sufficient for most cases, and good for at least getting started.
The default behavior provides the following ros_control interfaces:
默认情况下提供了下面的控制接口:
hardware_interface::JointStateInterface
hardware_interface::EffortJointInterface
hardware_interface::VelocityJointInterface - not fully implemented
Advanced: custom gazebo_ros_control Simulation Plugins
高级:自定义gazebo_ros_control仿真插件
The gazebo_ros_control Gazebo plugin also provides a pluginlib-based interface to implement custom interfaces between Gazebo and ros_control for simulating more complex mechanisms (nonlinear springs, linkages, etc).
提供基础库接口实现自定义接口
These plugins must inherit gazebo_ros_control::RobotHWSim which implements a simulated ros_control hardware_interface::RobotHW. RobotHWSim provides API-level access to read and command joint properties in the Gazebo simulator.
自定义插件必须继承自gazebo_ros_control::RobotHWSim实现了模拟ros_control hardware_interface::RobotHW...
The respective RobotHWSim sub-class is specified in a URDF model and is loaded when the robot model is loaded. For example, the following XML will load the default plugin (same behavior as when using no <robotSimType> tag):
自己的RobotHWSim子类在urdf里定义.模型加载完子类也被加载.
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/MYROBOT</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType><!-- gazebo_ros_control/DefaultRobotHWSim 可以改成自定义的子类 -->
</plugin>
</gazebo>
.........
自己的配置gazebo.urdf.xacro
<?xml version="1.0"?>
<robot>
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/zzz_arm</robotNamespace>
<!--<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
使用默认配置手臂轨迹会执行,但抓手抓取会失败
-->
<robotSimType>rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo</robotSimType>
<!--使用一个教程中的自定义插件后抓取放置执行成功,源码见下-->
<controlPeriod>0.001</controlPeriod>
</plugin>
</gazebo>
</robot>
使用默认配置失败信息输出
attaching world object 'coke_can' to link 'grasping_frame'
attached object 'coke_can' to link 'grasping_frame'
Found successful manipution plan!
warn controller handle zzz_arm/fakegazebo_grapper_controller reports status RUNNING
pick up goal failed :Solution found but controller failed during execution.
transmission
<robot xmlns:xacro="http://ros.org/wiki/xacro">
<xacro:macro name="arm_simple_transmission" params="name reduction">
<transmission name="${name}_transmission">
<type>transmission_interface/SimpleTransmission</type>
<actuator name="${name}_motor">
<mechanicalReduction>${reduction}</mechanicalReduction>
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</actuator>
<joint name="${name}_joint">
<hardwareInterface>hardware_interface/PositionJointInterface</hardwareInterface>
</joint>
</transmission>
</xacro:macro>
</robot>
gazebo 使用的参数配置文件:
joint_trajectory_controllers.yaml
zzz_arm:
fakegazebo_arm_controller:
type: position_controllers/JointTrajectoryController
joints:
- shoulder_zhuan_joint
- upper_arm_joint
- fore_arm_joint
- hand_wan_joint
- hand_zhuan_joint
constraints:
goal_time: &goal_time_constraint 10.0
shoulder_zhuan_joint:
goal: &goal_pos_constraint 0.5
upper_arm_joint:
goal: *goal_pos_constraint
fore_arm_joint:
goal: *goal_pos_constraint
hand_wan_joint:
goal: *goal_pos_constraint
hand_zhuan_joint:
goal: *goal_pos_constraint
gains:
shoulder_zhuan_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
upper_arm_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
fore_arm_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
hand_wan_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
hand_zhuan_joint: {p: 1000.0, i: 0.0, d: 0.1, i_clamp: 0.0}
fakegazebo_grapper_controller:
type: position_controllers/JointTrajectoryController
joints:
- finger_1_joint
- finger_2_joint
constraints:
goal_time: *goal_time_constraint
finger_1_joint:
goal: *goal_pos_constraint
finger_2_joint:
goal: *goal_pos_constraint
gains:
finger_1_joint: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}
finger_2_joint: {p: 50.0, d: 1.0, i: 0.01, i_clamp: 1.0}
# Publish all joint states -----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 50
moveit使用的参数配置文件:
fakegazebo_controllers.yaml
controller_manager_ns: controller_manager
controller_list:
- name: zzz_arm/fakegazebo_arm_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- shoulder_zhuan_joint
- upper_arm_joint
- fore_arm_joint
- hand_wan_joint
- hand_zhuan_joint
- name: zzz_arm/fakegazebo_grapper_controller
action_ns: follow_joint_trajectory
type: FollowJointTrajectory
default: true
joints:
- finger_1_joint
- finger_2_joint
rosbook_arm_hardware_gazebo/ROSBookArmHardwareGazebo插件源码
rosbook_arm_hardware_gazebo.cpp
#include <angles/angles.h>
#include <urdf_parser/urdf_parser.h>
#include <joint_limits_interface/joint_limits_urdf.h>
#include <joint_limits_interface/joint_limits_rosparam.h>
#include <pluginlib/class_list_macros.h>
#include <rosbook_arm_hardware_gazebo/rosbook_arm_hardware_gazebo.h>
namespace rosbook_arm_hardware_gazebo
{
using namespace hardware_interface;
ROSBookArmHardwareGazebo::ROSBookArmHardwareGazebo()
: gazebo_ros_control::RobotHWSim()
{}
bool ROSBookArmHardwareGazebo::initSim(const std::string& robot_namespace,
ros::NodeHandle nh,
gazebo::physics::ModelPtr model,
const urdf::Model* const urdf_model,
std::vector<transmission_interface::TransmissionInfo> transmissions)
{
using gazebo::physics::JointPtr;
// Cleanup
sim_joints_.clear();
jnt_pos_.clear();
jnt_vel_.clear();
jnt_eff_.clear();
jnt_pos_cmd_.clear();
// Simulation joints
sim_joints_ = model->GetJoints();
n_dof_ = sim_joints_.size();
std::vector<std::string> jnt_names;
for (size_t i = 0; i < n_dof_; ++i)
{
jnt_names.push_back(sim_joints_[i]->GetName());
}
// Raw data
jnt_pos_.resize(n_dof_);
jnt_vel_.resize(n_dof_);
jnt_eff_.resize(n_dof_);
jnt_pos_cmd_.resize(n_dof_);
// Hardware interfaces
for (size_t i = 0; i < n_dof_; ++i)
{
jnt_state_interface_.registerHandle(
JointStateHandle(jnt_names[i], &jnt_pos_[i], &jnt_vel_[i], &jnt_eff_[i]));
jnt_pos_cmd_interface_.registerHandle(
JointHandle(jnt_state_interface_.getHandle(jnt_names[i]), &jnt_pos_cmd_[i]));
ROS_DEBUG_STREAM("Registered joint '" << jnt_names[i] << "' in the PositionJointInterface.");
}
registerInterface(&jnt_state_interface_);
registerInterface(&jnt_pos_cmd_interface_);
// Position joint limits interface
std::vector<std::string> cmd_handle_names = jnt_pos_cmd_interface_.getNames();
for (size_t i = 0; i < n_dof_; ++i)
{
const std::string name = cmd_handle_names[i];
JointHandle cmd_handle = jnt_pos_cmd_interface_.getHandle(name);
using namespace joint_limits_interface;
boost::shared_ptr<const urdf::Joint> urdf_joint = urdf_model->getJoint(name);
JointLimits limits;
SoftJointLimits soft_limits;
if (!getJointLimits(urdf_joint, limits) || !getSoftJointLimits(urdf_joint, soft_limits))
{
ROS_WARN_STREAM("Joint limits won't be enforced for joint '" << name << "'.");
}
else
{
jnt_limits_interface_.registerHandle(
PositionJointSoftLimitsHandle(cmd_handle, limits, soft_limits));
ROS_DEBUG_STREAM("Joint limits will be enforced for joint '" << name << "'.");
}
}
// PID controllers
pids_.resize(n_dof_);
for (size_t i = 0; i < n_dof_; ++i)
{
ros::NodeHandle joint_nh(nh, "gains/" + jnt_names[i]);
if (!pids_[i].init(joint_nh))
{
return false;
}
}
return true;
}
void ROSBookArmHardwareGazebo::readSim(ros::Time time, ros::Duration period)
{
for (size_t i = 0; i < n_dof_; ++i)
{
jnt_pos_[i] += angles::shortest_angular_distance
(jnt_pos_[i], sim_joints_[i]->GetAngle(0u).Radian());
jnt_vel_[i] = sim_joints_[i]->GetVelocity(0u);
jnt_eff_[i] = sim_joints_[i]->GetForce(0u);
}
}
void ROSBookArmHardwareGazebo::writeSim(ros::Time time, ros::Duration period)
{
// Enforce joint limits
jnt_limits_interface_.enforceLimits(period);
// Compute and send commands
for (size_t i = 0; i < n_dof_; ++i)
{
const double error = jnt_pos_cmd_[i] - jnt_pos_[i];
const double effort = pids_[i].computeCommand(error, period);
sim_joints_[i]->SetForce(0u, effort);
}
}
} // namespace rosbook_hardware_gazebo
PLUGINLIB_EXPORT_CLASS(rosbook_arm_hardware_gazebo::ROSBookArmHardwareGazebo, gazebo_ros_control::RobotHWSim)
Robot Hardware Interface and Resource Manager.
This class provides a standardized interface to a set of robot hardware interfaces to the controller manager. It performs resource conflict checking for a given set of controllers and maintains a map of hardware interfaces. It is meant to be used as a base class for abstracting custom robot hardware.
机器人硬件接口与资源管理器。
这个类为控制器管理器提供了一组机器人硬件接口的标准化接口。它对给定的控制器集执行资源冲突检查,并维护硬件接口映射。它将被用作抽象自定义机器人硬件的基类。
gazebo仿真抓取时,抓不起来或者很容易滑落的问题.
如图夹住的物体不断下滑甚至掉落.
官方 小车轮子对地摩擦的一个例子
http://wiki.ros.org/urdf/Tutorials/Using%20a%20URDF%20in%20GazeboSince the wheels are actually going to touch the ground and thus interact with it physically, we also specify some additional information about the material of the wheels.
当轮子转动,轮子对地就会有一个物理的交互.我们也要指定一些轮子材质的相关信息.
材质信息包含摩擦信息示例如下
<gazebo reference="${prefix}_${suffix}_wheel">
<mu1 value="200.0"/>
<mu2 value="100.0"/>
<kp value="10000000.0" />
<kd value="1.0" />
<material>Gazebo/Grey</material>
</gazebo>
See http://gazebosim.org/tutorials/?tut=ros_urdf for more details.
详细部分:
<gazebo>
Elements For Links
List of elements that are individually parsed:
Name | Type | Description |
---|---|---|
material | value | Material of visual element |
gravity | bool | Use gravity |
dampingFactor | double | Exponential velocity decay of the link velocity - takes the value and multiplies the previous link velocity by (1-dampingFactor). |
maxVel | double | maximum contact correction velocity truncation term. |
minDepth | double | minimum allowable depth before contact correction impulse is applied |
mu1 | double | Friction coefficients μ for the principal contact directions along the contact surface as defined by theOpen Dynamics Engine (ODE) (see parameter descriptions inODE's user guide) |
mu2 | ||
fdir1 | string | 3-tuple specifying direction of mu1 in the collision local reference frame. |
kp | double | Contact stiffness k_p and damping k_d for rigid body contacts as defined by ODE (ODE uses erp and cfm but there is amapping between erp/cfm and stiffness/damping) |
kd | ||
selfCollide | bool | If true, the link can collide with other links in the model. |
maxContacts | int | Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics. |
laserRetro | double | intensity value returned by laser sensor. |
Similar to <gazebo>
elements for <robot>
, any arbitrary blobs that are not parsed according to the table above are inserted into the the corresponding<link>
element in the SDF. This is particularly useful for plugins, as discussed in theROS Motor and Sensor Plugins tutorial.
Friction coefficients μ for the principal contact directions along the contact surface as defined by the Open Dynamics Engine (ODE) http://www.ode.org/ (see parameter descriptions in ODE's user guide) http://www.ode.org/ode-latest-userguide.html#sec_7_3_7
由ODE定义的接触面的主要接触方向上的摩擦系数μ
Contact stiffness k_p and damping k_d for rigid body contacts as defined by ODE (ODE uses erp and cfm but there is a mapping between erp/cfm and stiffness/damping)
由ODE定义的刚体接触的接触强度k_p和阻尼k_d (ODE使用 erp和cfm ,erp/cfm 和 stiffness/damping之间是有映射关系的)
什么是ODE?
Introduction
The Open Dynamics Engine (ODE) is a free, industrial quality library for simulating articulated rigid body dynamics. Proven applications include simulating ground vehicles, legged creatures, and moving objects in VR environments. It is fast, flexible and robust, and has built-in collision detection. ODE is being developed by Russell Smith( http://www.q12.org/) with help from several contributors.(http://ode.org/)
......似乎要跑题
转回来修改设置
urdf 内抓手材质的设置
<gazebo reference="finger_1_link">
<mu1>2000.0</mu1>
<mu2>1000.0</mu2>
<kp>10000000.0</kp>
<kd>1.0</kd>
</gazebo>
<gazebo reference="finger_2_link">
<mu1>2000.0</mu1>
<mu2>1000.0</mu2>
<kp>10000000.0</kp>
<kd>1.0</kd>
</gazebo>
仿真环境中物体以及被抓物体的材质设置
grasp.world
<?xml version='1.0' ?>
<sdf version='1.4'>
<world name='empty'>
<!-- A global light source -->
<include>
<uri>model://sun</uri>
</include>
<!-- A ground plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<!--
<include>
<uri>model://file:///home/myrobot1/src/zzz_arm_2_gazebo/model/beer/model.sdf</uri>
</include>
-->
<!-- Physics settings for simulation -->
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
<gravity>0 0 -9.81</gravity>
</physics>
<model name='coke_can_box_model'>
<pose frame=''>0.175 0.0 0.2165 0 0 0</pose>
<link name='coke_can'>
<inertial>
<mass>0.001</mass>
<inertia>
<ixx>0.00016</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.00016</iyy>
<iyz>0</iyz>
<izz>0.00006</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<!--cylinder>
<radius>0.0075</radius>
<length>.05</length>
</cylinder-->
<!--box>
<size>0.015 0.015 0.05</size>
</box-->
<mesh>
<uri>model://coke_can/meshes/coke_can.dae</uri>
<scale>0.095 0.095 0.18</scale>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<friction>
<ode>
<mu>100.0</mu>
<mu2>100.0</mu2>
</ode>
</friction>
<contact>
<ode>
<kp>10000000.0</kp>
<kd>1.0</kd>
<min_depth>0.001</min_depth>
<max_vel>0.1</max_vel>
</ode>
</contact>
</surface>
</collision>
<visual name='visual'>
<geometry>
<!--cylinder>
<radius>0.0075</radius>
<length>.05</length>
</cylinder-->
<!--box>
<size>0.015 0.015 0.05</size>
</box-->
<mesh>
<uri>model://coke_can/meshes/coke_can.dae</uri>
<scale>0.095 0.095 0.18</scale>
</mesh>
</geometry>
<material>
<script>
<name>Gazebo/Green</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
</link>
</model>
<model name='table_box_model'>
<static>true</static>
<pose frame=''>0.15 0.0 0.1 0 -0 1.5707963265</pose>
<link name='table_box_link'>
<inertial>
<mass>1</mass>
<inertia>
<ixx>0.166667</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>0.166667</iyy>
<iyz>0</iyz>
<izz>0.166667</izz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<box>
<size>0.15 0.08 0.2</size>
</box>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<ode>
<mu>0.6</mu>
<mu2>0.6</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<box>
<size>0.15 0.08 0.2</size>
</box>
</geometry>
<material>
<script>
<name>Gazebo/Grey</name>
<uri>file://media/materials/scripts/gazebo.material</uri>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<kinematic>0</kinematic>
</link>
</model>
</world>
</sdf>
gazebo 仿真尝试抓取圆柱体,立方体,和mesh可乐罐
经测试,在同样的设置下圆柱最容易滑落,立方体次之.可乐罐最不容易掉落.
gazebo与ros_control学习 (1)
<ROS> Gazebo Ros Control 及 Controller运用
http://blog.csdn.net/sunbibei/article/details/53665876
学习机械臂的一些文章
http://www.360doc.com/content/16/0825/16/7821691_585865831.shtml
Gazebo与ros_control(1):让模型动起来
http://blog.csdn.net/yaked/article/details/51412781
Gazebo与ros_control(2):七自由度机械臂和两轮差速运动小车
http://blog.csdn.net/yaked/article/details/51417742
Gazebo与ros_control(3):Moveit输出规划轨迹到Gazebo
http://blog.csdn.net/yaked/article/details/51436262
Gazebo与ros_control(4):举一反三,实战youBot
http://blog.csdn.net/yaked/article/details/51483531