Robotics StackExchange | Archived questions

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

Answers