gazebo仿真 机械臂抓取和放置 使用ros_control插件

仿真截图

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%20Gazebo


Since 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)  

 http://blog.csdn.net/xu1129005165/article/details/53743026

<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