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
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
Hi boy, have you solved the problem?
Asked by luxiaojun on 2018-05-01 07:09:42 UTC