Robotics StackExchange | Archived questions

Velocity Instantaneously x Velocity With Joint Motors and How to Use

Hi everyone,

I'm trying to control the height of a UAV using predictive control. To do so I'm using the "Iris with Standoff" model, but I'm having some trouble to set the velocity of the rotor's joint. I followed the tutorial "Setting Velocity on Links And Joints" (http://gazebosim.org/tutorials?tut=set_velocity&cat=) and tried to use the "Set Velocity Instantaneously", but the UAV doesn't take off, independent of the velocity that I set, even though the rotors rotate. It seems to me, that I should set the velocity with joint motors, but I didn't understand how to use. I can't set the velocity using PID Controllers, because that is already in my controller. I'm setting the velocity with a plugin that is subscribed in a ROS Topic, all the "ROS Thing" is working, I followed the velodyne tutorial (http://gazebosim.org/tutorials?tut=guided_i1). So, in my situation, how should I set the velocity? Instantaneously or with joint motors? And how can I do this?

I'm using Gazebo7, ROS Kinetic and Ubuntu 16.04.

The model.sdf and the plugin that i developed are below. Sorry if the post is too big, it is because of the models and plugins code, first time here.

Mode.sdf

<?xml version='1.0'?>
<sdf version='1.6'>
  <model name='iris'>
    <pose>0 0 0.194923 0 0 0</pose>
    <link name='base_link'>
      <velocity_decay>
        <linear>0.0</linear>
        <angular>0.0</angular>
      </velocity_decay>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.468</mass>
        <inertia>
          <ixx>0.004856</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.004856</iyy>
          <iyz>0</iyz>
          <izz>0.008801</izz>
        </inertia>
      </inertial>
      <collision name='base_link_collision'>
        <pose>0 0 -0.08 0 0 0</pose>
        <geometry>
          <box>
            <size>0.47 0.47 0.23</size>
          </box>
        </geometry>
        <surface>
          <contact>
            <ode>
              <max_vel>100.0</max_vel>
              <min_depth>0.001</min_depth>
            </ode>
          </contact>
          <friction>
            <ode>
              <mu>1.0</mu>
              <mu2>1.0</mu2>
            </ode>
          </friction>
        </surface>
      </collision>
      <visual name='base_link_visual'>
        <geometry>
          <mesh>
            <uri>model://iris_with_standoffs/meshes/iris.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
          </script>
        </material>
      </visual>

      <visual name='front_left_leg_visual'>
        <pose>0.123 0.22 -0.11 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.005</radius>
            <length>0.17</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/FlatBlack</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <visual name='front_right_leg_visual'>
        <pose>0.123 -0.22 -0.11 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.005</radius>
            <length>0.17</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/FlatBlack</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <visual name='rear_left_leg_visual'>
        <pose>-0.140 0.21 -0.11 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.005</radius>
            <length>0.17</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/FlatBlack</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
      <visual name='rear_right_leg_visual'>
        <pose>-0.140 -0.21 -0.11 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.005</radius>
            <length>0.17</length>
          </cylinder>
        </geometry>
        <material>
          <script>
            <name>Gazebo/FlatBlack</name>
            <uri>file://media/materials/scripts/gazebo.material</uri>
          </script>
        </material>
      </visual>
    </link>
    <link name='iris/ground_truth/odometry_sensorgt_link'>
      <pose>0 0 0 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.15</mass>
        <inertia>
          <ixx>0.0001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0002</iyy>
          <iyz>0</iyz>
          <izz>0.0002</izz>
        </inertia>
      </inertial>
    </link>
    <joint name='iris/ground_truth/odometry_sensorgt_joint' type='revolute'>
      <child>iris/ground_truth/odometry_sensorgt_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
          <effort>0</effort>
          <velocity>0</velocity>
        </limit>
        <dynamics>
          <damping>1.0</damping>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <implicit_spring_damper>1</implicit_spring_damper>
        </ode>
      </physics>
    </joint>
    <link name='iris/imu_link'>
      <inertial>
        <pose>0 0 0 0 0 0</pose>
        <mass>0.15</mass>
        <inertia>
          <ixx>0.00001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.00002</iyy>
          <iyz>0</iyz>
          <izz>0.00002</izz>
        </inertia>
      </inertial>
      <sensor name="imu_sensor" type="imu">
        <pose>0 0 0 3.141593 0 0</pose>
        <always_on>1</always_on>
        <update_rate>1000.0</update_rate>
      </sensor>
    </link>
    <joint name='iris/imu_joint' type='revolute'>
      <child>iris/imu_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
          <effort>0</effort>
          <velocity>0</velocity>
        </limit>
        <dynamics>
          <damping>1.0</damping>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <implicit_spring_damper>1</implicit_spring_damper>
        </ode>
      </physics>
    </joint>
<!--
    <link name='iris/imugt_link'>
      <pose>0 0 0 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>1e-02</mass>
        <inertia>
          <ixx>0.0002</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0002</iyy>
          <iyz>0</iyz>
          <izz>0.0002</izz>
        </inertia>
      </inertial>
    </link>
    <joint name='iris/imugt_joint' type='revolute'>
      <child>iris/imugt_link</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
          <effort>0</effort>
          <velocity>0</velocity>
        </limit>
        <dynamics>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
    </joint>
-->
    <link name='rotor_0'>
      <pose>0.13 -0.22 0.023 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.025</mass>
        <inertia>
          <ixx>9.75e-06</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000166704</iyy>
          <iyz>0</iyz>
          <izz>0.000167604</izz>
        </inertia>
      </inertial>
      <collision name='rotor_0_collision'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.1</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_0_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_0_joint' type='revolute'>
      <child>rotor_0</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <damping>0.004</damping>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <implicit_spring_damper>1</implicit_spring_damper>
        </ode>
      </physics>
    </joint>
    <link name='rotor_1'>
      <pose>-0.13 0.2 0.023 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.025</mass>
        <inertia>
          <ixx>9.75e-06</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000166704</iyy>
          <iyz>0</iyz>
          <izz>0.000167604</izz>
        </inertia>
      </inertial>
      <collision name='rotor_1_collision'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.1</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_1_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://iris_with_standoffs/meshes/iris_prop_ccw.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_1_joint' type='revolute'>
      <child>rotor_1</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <damping>0.004</damping>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <implicit_spring_damper>1</implicit_spring_damper>
        </ode>
      </physics>
    </joint>
    <link name='rotor_2'>
      <pose>0.13 0.22 0.023 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.025</mass>
        <inertia>
          <ixx>9.75e-06</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000166704</iyy>
          <iyz>0</iyz>
          <izz>0.000167604</izz>
        </inertia>
      </inertial>
      <collision name='rotor_2_collision'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.1</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_2_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/Blue</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_2_joint' type='revolute'>
      <child>rotor_2</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <damping>0.004</damping>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <implicit_spring_damper>1</implicit_spring_damper>
        </ode>
      </physics>
    </joint>
    <link name='rotor_3'>
      <pose>-0.13 -0.2 0.023 0 -0 0</pose>
      <inertial>
        <pose>0 0 0 0 -0 0</pose>
        <mass>0.025</mass>
        <inertia>
          <ixx>9.75e-06</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.000166704</iyy>
          <iyz>0</iyz>
          <izz>0.000167604</izz>
        </inertia>
      </inertial>
      <collision name='rotor_3_collision'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <cylinder>
            <length>0.005</length>
            <radius>0.1</radius>
          </cylinder>
        </geometry>
        <surface>
          <contact>
            <ode/>
          </contact>
          <friction>
            <ode/>
          </friction>
        </surface>
      </collision>
      <visual name='rotor_3_visual'>
        <pose>0 0 0 0 -0 0</pose>
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>model://iris_with_standoffs/meshes/iris_prop_cw.dae</uri>
          </mesh>
        </geometry>
        <material>
          <script>
            <name>Gazebo/DarkGrey</name>
            <uri>__default__</uri>
          </script>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name='rotor_3_joint' type='revolute'>
      <child>rotor_3</child>
      <parent>base_link</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-1e+16</lower>
          <upper>1e+16</upper>
        </limit>
        <dynamics>
          <damping>0.004</damping>
        </dynamics>
        <use_parent_model_frame>1</use_parent_model_frame>
      </axis>
      <physics>
        <ode>
          <implicit_spring_damper>1</implicit_spring_damper>
        </ode>
      </physics>
    </joint>
    <static>0</static>
  </model>
</sdf>

Plugin Code

#ifndef _IRIS_PLUGIN_HH_
#define _IRIS_PLUGIN_HH_

#include <gazebo/gazebo.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/transport/transport.hh>
#include <gazebo/msgs/msgs.hh>
#include <thread>
#include "ros/ros.h"
#include "ros/callback_queue.h"
#include "ros/subscribe_options.h"
#include "std_msgs/Float32.h"

namespace gazebo
{
  /// \brief A plugin to control a Velodyne sensor.
  class IrisPlugin : public ModelPlugin
  {
    /// \brief Constructor
    public: IrisPlugin() {}

    /// \brief The load function is called by Gazebo when the plugin is
    /// inserted into simulation
    /// \param[in] _model A pointer to the model that this plugin is
    /// attached to.
    /// \param[in] _sdf A pointer to the plugin's SDF element.
    public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
    {
    // Just output a message for now
        std::cerr << "\nThe iris plugin is attach to model[" << _model->GetName() << "\n";

    // Store the model pointer for convenience.
    this->model = _model;   
/*

      // Create the node
      this->node = transport::NodePtr(new transport::Node());
      #if GAZEBO_MAJOR_VERSION < 8
      this->node->Init(this->model->GetWorld()->GetName());
      #else
      this->node->Init(this->model->GetWorld()->Name());
      #endif

      // Create a topic name
      std::string topicName = "~/" + this->model->GetName() + "/vel_cmd";

      // Subscribe to the topic, and register a callback
      this->sub = this->node->Subscribe(topicName,&IrisPlugin::OnMsg, this);
*/
    // Initialize ros, if it has not already bee initialized.
    if (!ros::isInitialized())
    {
      int argc = 0;
      char **argv = NULL;
      ros::init(argc, argv, "gazebo_client", ros::init_options::NoSigintHandler);
    }

    // Create our ROS node. This acts in a similar manner to
    // the Gazebo node
    this->rosNode.reset(new ros::NodeHandle("gazebo_client"));

    // Create a named topic, and subscribe to it.
    ros::SubscribeOptions so = ros::SubscribeOptions::create<std_msgs::Float32>(
    "/" + this->model->GetName() + "/vel_cmd",1,
     boost::bind(&IrisPlugin::OnRosMsg, this, _1), ros::VoidPtr(), &this->rosQueue);

    this->rosSub = this->rosNode->subscribe(so);

    // Spin up the queue helper thread.
    this->rosQueueThread = std::thread(std::bind(&IrisPlugin::QueueThread, this));
    }



    public: void SetVelocity(const double &_vel)
    {
    std::cerr <<_vel<< "\n";
    std::cout << "Funcao" << "\n";  
    this->model->GetJoints()[2]->SetVelocity(0, _vel);
    this->model->GetJoints()[3]->SetVelocity(0, _vel);
    this->model->GetJoints()[4]->SetVelocity(0, -1*_vel);
    this->model->GetJoints()[5]->SetVelocity(0, -1*_vel);
    }

    /// \brief Handle incoming message
    /// \param[in] _msg Repurpose a vector3 message. This function will
    /// only use the x component.
    /*private: void OnMsg(ConstVector3dPtr &_msg)
    {
    std::cerr << "Mensagem" << "\n"; 
      this->SetVelocity(_msg->x());
    }*/

    /// \brief Handle an incoming message from ROS
    /// \param[in] _msg A float value that is used to set the velocity
    /// of the Velodyne.
    public: void OnRosMsg(const std_msgs::Float32ConstPtr &_msg)
    {
      this->SetVelocity(_msg->data);
    }

    /// \brief ROS helper function that processes messages
    private: void QueueThread()
    {
      static const double timeout = 0.01;
      while (this->rosNode->ok())
      {
        this->rosQueue.callAvailable(ros::WallDuration(timeout));
      }
    }


    /// \brief A node used for transport
    private: transport::NodePtr node;

    /// \brief A subscriber to a named topic.
    private: transport::SubscriberPtr sub;

    /// \brief Pointer to the model.
    private: physics::ModelPtr model;

    /// \brief Pointer to the joint.
    private: physics::JointPtr joint;

    /// \brief A node use for ROS transport
    private: std::unique_ptr<ros::NodeHandle> rosNode;

    /// \brief A ROS subscriber
    private: ros::Subscriber rosSub;

    /// \brief A ROS callbackqueue that helps process messages
    private: ros::CallbackQueue rosQueue;

    /// \brief A thread the keeps running the rosQueue
    private: std::thread rosQueueThread;  

    };

  // Tell Gazebo about this plugin, so that Gazebo can call Load on this plugin.
  GZ_REGISTER_MODEL_PLUGIN(IrisPlugin)
}
#endif

Asked by schulze_18 on 2017-12-11 20:04:20 UTC

Comments

Hi boy, have you solved the problem?

Asked by luxiaojun on 2018-05-01 07:09:42 UTC

Answers

Hi, Velocity Instantaneously will only work for one step of the simulation, because of that you need to set this velocity every step of the simulation.

About the UAV not taking off, I can be wrong, but after a lot of reading, I realize that Gazebo does not simulate aerodynamics effects (you need to use a additional plugin for that), and without that the rotor's velocity will not produce any force. The simple solution was developing a plugin that apply on each rotors a force proportional to the velocity. Here is some lines of my plugin:

thrust_force.Set(0,0,k*vel[0]*vel[0]);      
this->model->GetLinks()[3]->AddRelativeForce(thrust_force);

where "k" is the thrust coefficient related to the rotor's blade. Here is the full plugin(link)

Asked by schulze_18 on 2019-11-09 08:12:19 UTC

Comments