revolute joint dos not work when adding an angular velocity to the model
Hi,
I am new to the Gazebo. I have made a model plugin (based on the Animated Box example. My model has two revolute
joints (each are limited to the 15°). When I add an angular velocity to the model by
this->model->SetAngularVel(math::Vector3(0, 0, LinearSpeed));
The revolute
joints stop moving. Can any one help me how I can make the joints moving in the moment of the rotation of the model?
Thanks ,
You can find below my source files.
World's SDF File
Blockquote
<?xml version="1.0"?>
<!-- Ground Plane -->
<include>
<uri>model://ground_plane</uri>
</include>
<include>
<uri>model://sun</uri>
</include>
<model name="demo_joint_types">
<!-- A Heavy base keeps the joints from falling over -->
<link name="heavy_base01">
<!-- position the base such that the bottom touches the floor -->
<pose>1 0.01 0.05 0 0 0</pose>
<inertial>
<mass>100</mass> <!-- kg -->
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>8,3341</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>8,3341</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>16,666</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="heavy_base_collision01">
<geometry>
<box>
<size>1 1 0.01</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<visual name="heavy_base_visual01">
<geometry>
<box>
<size>1 1 0.01</size>
</box>
</geometry>
</visual>
</link>
<link name="revolute_base01">
<!-- Put the bottom of the child on top of the parent
z = 0.5 * revolute_base_sz_z + heavy_base_sz_z
x = put it near revolute label on heavy base
-->
<pose >1 0.01 0.8 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>0.1868</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.1867</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.0000415</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="revolute_base_collision01">
<geometry>
<box>
<size>0.01 0.02 1.5</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<!--<visual name="revolute_base_visual01">
<geometry>
<box>
<size>0.01 0.02 1.5</size>
</box>
</geometry>
</visual> -->
</link>
<joint name="revolute_base_to_heavy_base01" type="fixed">
<parent>heavy_base01</parent>
<child>revolute_base01</child>
</joint>
<link name="revolute_base02">
<!-- Put the bottom of the child on top of the parent
z = 0.5 * revolute_base_sz_z + heavy_base_sz_z
x = put it near revolute label on heavy base
-->
<pose >1 -0.01 0.8 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>0.1868</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.1867</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.0000415</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="revolute_base_collision02">
<geometry>
<box>
<size>0.01 0.02 1.5</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<!--<visual name="revolute_base_visual02">
<geometry>
<box>
<size>0.01 0.02 1.5</size>
</box>
</geometry>
</visual> -->
</link>
<joint name="revolute_base_to_heavy_base02" type="fixed">
<parent>heavy_base01</parent>
<child>revolute_base02</child>
</joint>
<link name="revolute_arm01">
<!-- Put the arm near the top of the revolute_base
z = revolute_base_sz_z + heavy_base_sz_z - 0.5 * revolute_arm_sz_z
y = -0.5 * revolute_arm_sz_y + fudge(0.05)
x = revolute_base_x - 0.5 * revolute_base_sz_x + 0.5 * revolute_arm_sz_x + fudge(0.01)
-->
<pose >1 0.1 0.85 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z)0,75 -->
<ixx>0.16351</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.16351</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.00166</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="revolute_arm_collision01">
<geometry>
<box>
<size>0.1 0.1 1.4</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<visual name="revolute_arm_visual01">
<geometry>
<box>
<size>0.1 0.1 1.4</size>
</box>
</geometry>
</visual>
</link>
<joint name="revolute_demo01" type="revolute">
<parent>revolute_base01</parent>
<child>revolute_arm01</child>
<axis>
<!-- <limit>
<lower>-0.698132</lower>
<upper>0.698132</upper>
</limit> -->
<xyz>0 1 0</xyz>
</axis>
<!-- Move the joint to the size a bit to be more like an arm
y = 0.5 * relative_arm_sz_y - fudge(0.05) -->
<pose>0 0.06 0 0 0 0</pose>
</joint>
<link name="revolute_arm02">
<!-- Put the arm near the top of the revolute_base
z = revolute_base_sz_z + heavy_base_sz_z - 0.5 * revolute_arm_sz_z
y = -0.5 * revolute_arm_sz_y + fudge(0.05)
x = revolute_base_x - 0.5 * revolute_base_sz_x + 0.5 * revolute_arm_sz_x + fudge(0.01)
-->
<pose >1 -0.1 0.85 0 0 0</pose>
<inertial>
<inertia>
<!-- (mass / 12) * (y*y + z*z) -->
<ixx>0.16351</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<!-- (mass / 12) * (x*x + z*z) -->
<iyy>0.16351</iyy>
<iyz>0</iyz>
<!-- (mass / 12) * (y*y + x*x) -->
<izz>0.00166</izz>
</inertia>
</inertial>
<!-- Same geometry as visual -->
<collision name="revolute_arm_collision02">
<geometry>
<box>
<size>0.1 0.1 1.4</size>
</box>
</geometry>
</collision>
<!-- Same geometry as collision -->
<visual name="revolute_arm_visual02">
<geometry>
<box>
<size>0.1 0.1 1.4</size>
</box>
</geometry>
</visual>
</link>
<joint name="revolute_demo02" type="revolute">
<parent>revolute_base01</parent>
<child>revolute_arm02</child>
<axis>
<!-- <limit>
<lower>-1.57</lower>
<upper>0.0</upper>
<velocity>2.0</velocity>
</limit> -->
<xyz>0 1 0</xyz>
</axis>
<!-- Move the joint to the size a bit to be more like an arm
y = 0.5 * relative_arm_sz_y - fudge(0.05) -->
<pose>0 -0.1 0.0 0 0 0</pose>
</joint>
<plugin name="push_animate" filename="libanimated_box.so"/>
Plugin Source File
#include <boost/bind.hpp>
#include <gazebo/gazebo.hh>
#include <ignition/math.hh>
#include <gazebo/physics/physics.hh>
#include <gazebo/common/common.hh>
#include <stdio.h>
static const double AngularLegSpeed = 1; // rad/s
static const double LinearSpeed = 0.1; // m/s
static const double LegAngle = 15; // degre
namespace gazebo
{
class AnimatedBox : public ModelPlugin
{
bool m_isInitialised;
public: AnimatedBox():m_isInitialised(false){}
public: void Load(physics::ModelPtr _parent, sdf::ElementPtr /*_sdf*/)
{
// Store the pointer to the model
this->model = _parent;
// Listen to the update
this->updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&AnimatedBox::OnUpdate, this));
}
public: void OnUpdate()
{
// Apply a small linear velocity to the model.
this->model->SetAngularVel(math::Vector3(0, 0, LinearSpeed));
//this->model->SetLinearVel(math::Vector3(LinearSpeed, 0, 0));
if(!m_isInitialised)
{
this->model->GetJoint("revolute_demo01")->SetVelocity(0, AngularLegSpeed);
this->model->GetJoint("revolute_demo02")->SetVelocity(0, -AngularLegSpeed);
m_isInitialised = true;
}
else
{
angle01 = this->model->GetJoint("revolute_demo01")->GetAngle(0);
angle02 = this->model->GetJoint("revolute_demo02")->GetAngle(0);
if (angle01.Degree() > LegAngle)
{
this->model->GetJoint("revolute_demo01")->SetVelocity(0, -AngularLegSpeed);
}
else if (angle01.Degree() < -LegAngle)
{
this->model->GetJoint("revolute_demo01")->SetVelocity(0, AngularLegSpeed);
}
if (angle02.Degree() < -LegAngle)
{
this->model->GetJoint("revolute_demo02")->SetVelocity(0, AngularLegSpeed);
}
else if (angle02.Degree() > LegAngle)
{
this->model->GetJoint("revolute_demo02")->SetVelocity(0, -AngularLegSpeed);
}
}
}
// Pointer to the model
private: physics::ModelPtr model;
physics::JointPtr joint;
math::Angle angle01;
math::Angle angle02;
// Pointer to the update event connection
private: event::ConnectionPtr updateConnection;
}; // Register this plugin with the simulator GZREGISTERMODEL_PLUGIN(AnimatedBox) }
Asked by Mobile_robot on 2018-01-12 09:51:13 UTC
Comments