Gazebo | Ignition | Community
Ask Your Question

Revision history [back]

Hi,

You could create a world plugin which spawns the spheres. Here is a similar example of a plugin: (don't forget to set them as static if you don't want them to move, that way it doesn't use as much CPU as well)

#include "gazebo/gazebo.hh"
#include "physics/physics.hh"
#include "common/Plugin.hh"
#include "transport/transport.hh"
#include <math.h>

namespace gazebo
{
  class FactoryLiquid : public WorldPlugin
  {
        public: virtual ~FactoryLiquid()
        {

        }
    public: void Load(physics::WorldPtr _parent, sdf::ElementPtr _sdf)
    {
        math::Vector3 p3, init_pos;

        ////////////////////////////////////////////////////////////////
        /////// SDF PARAMETERS

        ////////////// Get nr of spheres
        if (!_sdf->HasElement("nr_spheres"))
        {
          std::cout << "Missing parameter <nr_spheres> in FactoryLiquid, default to 0" << std::endl;
          nr_spheres = 0;
        }
        else nr_spheres = _sdf->GetElement("nr_spheres")->GetValueUInt();

        ////////////// Set up the initial position parameter
        if (!_sdf->HasElement("init_pos"))
        {
          std::cout << "Missing parameter <init_pos> in FactoryLiquid, default to 0 0 0" << std::endl;
          init_pos.x = 0.0;
          init_pos.y = 0.0;
          init_pos.z = 0.0;
        }
        else init_pos = _sdf->GetElement("init_pos")->GetValueVector3();

        // etc. other parameters from the world file

        /////// END SDF PARAMETERS
        //////////////////////////////////////////////////////////////////


        //////////////////////////////////////////////////////////////////
        //////////////////////////////////////////////////////////////////
        //////////////////////////// START XML LIQUID
        xml << "<?xml version='1.0'?>\n";
        xml << "<sdf version='1.4'>\n";
        xml << "<model name='liquid_spheres'>\n";
        xml << "\t<static>false</static>\n";
        xml << "\t<pose>" << init_pos.x << " " << init_pos.y << " " << init_pos.z << " 0 0 0 </pose>\n";

        for (unsigned int i=0; i<nr_spheres; i++)
        {
                p3 = FactoryLiquid::part_position(i, radius, spawned, level);
                xml << "\t\t<link name='sphere_link_" << i << "'>\n";
                xml << "\t\t\t<self_collide>true</self_collide>\n";
                xml << "\t\t\t<pose>" << p3.x << " " << p3.y << " " << p3.z << " 0 0 0</pose>\n";

                xml << "\t\t\t<inertial>\n";
                xml << "\t\t\t\t<pose> 0 0 0 0 0 0 </pose>\n";
                xml << "\t\t\t\t<inertia>\n";
                xml << "\t\t\t\t\t<ixx>" << inertia << "</ixx>\n";
                xml << "\t\t\t\t\t<ixy>0</ixy>\n";
                xml << "\t\t\t\t\t<ixz>0</ixz>\n";
                xml << "\t\t\t\t\t<iyy>" << inertia << "</iyy>\n";
                xml << "\t\t\t\t\t<iyz>0</iyz>\n";
                xml << "\t\t\t\t\t<izz>" << inertia << "</izz>\n";
                xml << "\t\t\t\t</inertia>\n";
                xml << "\t\t\t\t<mass>" << mass << "</mass>\n";
                xml << "\t\t\t</inertial>\n";

                xml << "\t\t\t<collision name='collision_" << i << "'>\n";
                xml << "\t\t\t\t<geometry>\n";
                xml << "\t\t\t\t\t<sphere>\n";
                xml << "\t\t\t\t\t\t<radius>" << radius << "</radius>\n";
                xml << "\t\t\t\t\t</sphere>\n";
                xml << "\t\t\t\t</geometry>\n";
                xml << "\t\t\t\t<surface>\n";
                xml << "\t\t\t\t\t<friction>\n";
                xml << "\t\t\t\t\t\t<ode>\n";
                xml << "\t\t\t\t\t\t\t<mu>" << mu << "</mu>\n";
                xml << "\t\t\t\t\t\t\t<mu2>" << mu2 << "</mu2>\n";
                xml << "\t\t\t\t\t\t\t<fdir1>0.0 0.0 0.0</fdir1>\n";
                xml << "\t\t\t\t\t\t\t<slip1>" << slip1 << "</slip1>\n";
                xml << "\t\t\t\t\t\t\t<slip2>" << slip2 << "</slip2>\n";
                xml << "\t\t\t\t\t\t</ode>\n";
                xml << "\t\t\t\t\t</friction>\n";
                xml << "\t\t\t\t\t<bounce>\n";
                xml << "\t\t\t\t\t\t<restitution_coefficient>" << bounce << "</restitution_coefficient>\n";
                xml << "\t\t\t\t\t\t<threshold>10000.0</threshold>\n";
                xml << "\t\t\t\t\t</bounce>\n";
                xml << "\t\t\t\t\t<contact>\n";
                xml << "\t\t\t\t\t\t<ode>\n";
                xml << "\t\t\t\t\t\t\t<soft_cfm>" << cfm << "</soft_cfm>\n";
                xml << "\t\t\t\t\t\t\t<soft_erp>" << erp << "</soft_erp>\n";
                xml << "\t\t\t\t\t\t\t<kp>" << kp << "</kp>\n";
                xml << "\t\t\t\t\t\t\t<kd>" << kd << "</kd>\n";
                xml << "\t\t\t\t\t\t\t<max_vel>100.0</max_vel>\n";
                xml << "\t\t\t\t\t\t\t<min_depth>0.001</min_depth>\n";
                xml << "\t\t\t\t\t\t</ode>\n";
                xml << "\t\t\t\t\t</contact>\n";
                xml << "\t\t\t\t</surface>\n";
                xml << "\t\t\t</collision>\n";

                xml << "\t\t\t<visual name='sphere_visual_" << i << "'>\n";
                xml << "\t\t\t\t<geometry>\n";
                xml << "\t\t\t\t\t<sphere>\n";
                xml << "\t\t\t\t\t\t<radius>" << radius << "</radius>\n";
                xml << "\t\t\t\t\t</sphere>\n";
                xml << "\t\t\t\t</geometry>\n";
                xml << "\t\t\t\t<material>\n";
                xml << "\t\t\t\t\t<script>\n";
                xml << "\t\t\t\t\t\t<uri>file://media/materials/scripts/gazebo.material</uri>\n";
                xml << "\t\t\t\t\t\t<name>Gazebo/Red</name>\n";
                xml << "\t\t\t\t\t</script>\n";
                xml << "\t\t\t\t</material>\n";
                xml << "\t\t\t</visual>\n";
                xml << "\t\t</link>\n";
        }

                xml << "</model>\n";
                xml << "</gazebo>\n";

                /////////////////////////////////////////
        //std::cout << xml.str() << "\n";

        sdf::SDF sphereSDF;
        sphereSDF.SetFromString(xml.str());


        _parent->InsertModelSDF(sphereSDF);

    }

    // function to position the particles
    public: math::Vector3 part_position(int i, double radius, int& spawned, int& level)
    {

                return v3;
    }
};

  // Register this plugin with the simulator
  GZ_REGISTER_WORLD_PLUGIN(FactoryLiquid)
}

This is how you add it to the world:

    ...
    <plugin name="factory_liquid" filename="libfactory_liquid.so">
            <init_pos>0.2 -0.0 1.0</init_pos>
            <nr_spheres>40</nr_spheres>
            <!-- other parameters -->
    </plugin>
    ...

Cheers, Andrei