Create links programatically and dinamically

edit

I creating a plugin with gazebo7 where I am trying to create links dynamically insde a given model (previously loaded from a sdf file).

I tried to insipire in the maze plugin, but it looks it creates different models for each wall: https://github.com/PeterMitrano/gzmaz...

What I really want is creating links dynamically and latter I will try to create joints dynamically. Then I tried just to update the Sdf with a new link.

  sdf::ElementPtr originalSdf = this->model->UnscaledSDF();
  physics::LinkPtr l = model->GetLink("link");
  sdf::ElementPtr linksdf = l->GetSDF();

  sdf::ElementPtr link2 = linksdf->Clone();
  link2->GetAttribute("name")->Set("link2");
  originalSdf->InsertElement(link2);
  this->model->UpdateParameters(originalSdf);

But I get the following error: Error [Element.hh:336] Unable to find value for key[pose]

The original sdf Tostring is:

    <model name='box'>
  <pose frame='world'>0 0 0.5 0 -0 0</pose>
  <link name='link'>
    <collision name='collision'>
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
      <max_contacts>10</max_contacts>
      <surface>
        <contact>
          <ode/>
        </contact>
        <bounce/>
        <friction>
          <torsional>
            <ode/>
          </torsional>
          <ode/>
        </friction>
      </surface>
    </collision>
    <visual name='visual'>
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
    </visual>
    <self_collide>0</self_collide>
    <inertial>
      <inertia>
        <ixx>1</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>1</iyy>
        <iyz>0</iyz>
        <izz>1</izz>
      </inertia>
      <mass>1</mass>
    </inertial>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <plugin name='cord_plugin' filename='libcord_plugin.so'/>
</model>

After the update the sdf ToString is:

<model name='box'>
  <pose frame='world'>0 0 0.5 0 -0 0</pose>
  <link name='link'>
    <collision name='collision'>
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
      <max_contacts>10</max_contacts>
      <surface>
        <contact>
          <ode/>
        </contact>
        <bounce/>
        <friction>
          <torsional>
            <ode/>
          </torsional>
          <ode/>
        </friction>
      </surface>
    </collision>
    <visual name='visual'>
      <geometry>
        <box>
          <size>1 1 1</size>
        </box>
      </geometry>
    </visual>
    <self_collide>0</self_collide>
    <inertial>
      <inertia>
        <ixx>1</ixx>
        <ixy>0</ixy>
        <ixz>0</ixz>
        <iyy>1</iyy>
        <iyz>0</iyz>
        <izz>1</izz>
      </inertia>
      <mass>1</mass>
    </inertial>
    <kinematic>0</kinematic>
    <gravity>1</gravity>
  </link>
  <plugin name='cord_plugin' filename='libcord_plugin.so'/>
</model>