Possible memory leaks in constraint solver?

[costashatz]

Hello,

After running huge simulations in our cluster, we observed some memory leaks that filled up our RAM and swap (64gb + 64gb!). I was able to create a small program that valgrind finds possible memory leaks:

==7983== Memcheck, a memory error detector
==7983== Copyright (C) 2002-2013, and GNU GPL'd, by Julian Seward et al.
==7983== Using Valgrind-3.10.1 and LibVEX; rerun with -h for copyright info
==7983== Command: ./hexapod
==7983== 
==7983== 
==7983== HEAP SUMMARY:
==7983==     in use at exit: 4,851 bytes in 42 blocks
==7983==   total heap usage: 157,188 allocs, 157,146 frees, 178,053,923 bytes allocated
==7983== 
==7983== 4,720 (1,760 direct, 2,960 indirect) bytes in 10 blocks are definitely lost in loss record 6 of 6
==7983==    at 0x4C2BBA0: malloc (in /usr/lib/valgrind/vgpreload_memcheck-amd64-linux.so)
==7983==    by 0x54206E1: dart::constraint::ConstraintSolver::updateConstraints() (in /usr/local/lib/libdart-core.so.5.1.1)
==7983==    by 0x5420D5F: dart::constraint::ConstraintSolver::solve() (in /usr/local/lib/libdart-core.so.5.1.1)
==7983==    by 0x5426522: dart::simulation::World::step(bool) (in /usr/local/lib/libdart-core.so.5.1.1)
==7983==    by 0x43AB1A: main (in /home/kchatzil/ownCloud/documents/backup-2015-12-10/Workspaces/DART/source/dart_test/build/hexapod)
==7983== 
==7983== LEAK SUMMARY:
==7983==    definitely lost: 1,760 bytes in 10 blocks
==7983==    indirectly lost: 2,960 bytes in 30 blocks
==7983==      possibly lost: 0 bytes in 0 blocks
==7983==    still reachable: 131 bytes in 2 blocks
==7983==         suppressed: 0 bytes in 0 blocks
==7983== Reachable blocks (those to which a pointer was found) are not shown.
==7983== To see them, rerun with: --leak-check=full --show-leak-kinds=all
==7983== 
==7983== For counts of detected and suppressed errors, rerun with: -v
==7983== ERROR SUMMARY: 1 errors from 1 contexts (suppressed: 0 from 0)

Just running a few simulations does not make a problem, but we are using DART in evolutionary algorithms and we are running million of small simulations. So, small memory leaks add up and create a problem.

I am using DARTCollisionDetector, but I also tried with FCL and Bullet and I still get similar memory leak errors.

Any ideas on how to solve this issue?

Sorry for the long question. The small simulation code I am using is the following (basically code from your tutorials):

#include "dart/dart.h"
#include "dart/collision/dart/DARTCollisionDetector.h"
#include "controllerDuty.hpp"

using namespace dart::common;
using namespace dart::dynamics;
using namespace dart::simulation;
using namespace dart::math;

class Controller {
public:
    Controller(const SkeletonPtr& pexod)
        : mPexod(pexod)
    {
        std::vector<float> reference_ = std::vector<float>(36);
        reference_ = {1, 0, 0.5, 0.25, 0.25, 0.5, 1, 0.5, 0.5, 0.25, 0.75, 0.5, 1, 0, 0.5, 0.25, 0.25, 0.5, 1, 0, 0.5, 0.25, 0.75, 0.5, 1, 0.5, 0.5, 0.25, 0.25, 0.5, 1, 0, 0.5, 0.25, 0.75, 0.5};
        controller_ = new ControllerDuty(reference_, std::vector<int>());
        // Grab the current joint angles to use them as desired angles
        mQDesired = mPexod->getPositions();

        size_t nDofs = mPexod->getNumDofs();

        mForces = Eigen::VectorXd::Zero(nDofs);

        // // Set PD control gains
        // mKpPD = 200.0;
        // mKdPD = 20.0;

        mKpPD = Eigen::MatrixXd::Identity(nDofs, nDofs);
        mKdPD = Eigen::MatrixXd::Identity(nDofs, nDofs);
        mP = Eigen::VectorXd::Zero(nDofs);

        for (size_t i = 0; i < 6; ++i) {
            //mKpPD(i, i) = 0.0;
            //mKdPD(i, i) = 0.0;
            mP(i) = 0.0;
        }

        for (size_t i = 6; i < mPexod->getNumDofs(); ++i) {
            // mKpPD(i, i) = 100.0;
            // mKdPD(i, i) = 15.0;
            //mKpPD(i, i) = 10.0;
            //mKdPD(i, i) = 1.2;
            mP(i) = 1.0;
        }
    }

   ~Controller()
    {
        if(controller_)
           delete controller_;
    }

    void update(float t)
    {
        std::vector<float> tmp = controller_->get_pos(t);
        for (int i = 0; i < tmp.size(); i++) {
            mQDesired(i + 6) = tmp[i];
        }
        // std::cout << mPexod->getCOM() << std::endl;
    }

    void setControl(std::vector<float> ctrl)
    {
        if(controller_)
           delete controller_;
        controller_ = new ControllerDuty(ctrl, std::vector<int>());
    }

    void setPDForces()
    {
        if (nullptr == mPexod)
            return;

        // Compute the joint position error
        Eigen::VectorXd q = mPexod->getPositions();

        Eigen::VectorXd q_err = mQDesired - q;

        double gain = 1.0 / (DART_PI * mPexod->getTimeStep());
        Eigen::VectorXd vel = q_err * gain;
        vel = vel.cwiseProduct(mP);

        mPexod->setCommands(vel);
    }

protected:
    /// The manipulator Skeleton that we will be controlling
    SkeletonPtr mPexod;

    Eigen::VectorXd mP;

    /// Desired joint positions when not applying the operational space controller
    Eigen::VectorXd mQDesired;

    /// Control gains for the proportional error terms in the PD controller
    Eigen::MatrixXd mKpPD;

    /// Control gains for the derivative error terms in the PD controller
    Eigen::MatrixXd mKdPD;

    /// Joint forces for the manipulator (output of the Controller)
    Eigen::VectorXd mForces;

    ControllerDuty* controller_;
};

SkeletonPtr createFloor()
{
    SkeletonPtr floor = Skeleton::create("floor");

    // Give the floor a body
    BodyNodePtr body = floor->createJointAndBodyNodePair<WeldJoint>(nullptr).second;

    // Give the body a shape
    double floor_width = 10.0;
    double floor_height = 0.1;
    std::shared_ptr<BoxShape> box(
        new BoxShape(Eigen::Vector3d(floor_width, floor_width, floor_height)));
    box->setColor(dart::Color::Black());

    body->addVisualizationShape(box);
    body->addCollisionShape(box);

    // Put the body into position
    Eigen::Isometry3d tf(Eigen::Isometry3d::Identity());
    tf.translation() = Eigen::Vector3d(0.0, 0.0, -floor_height / 2.0);
    body->getParentJoint()->setTransformFromParentBodyNode(tf);

    return floor;
}

SkeletonPtr createPexod()
{
    // Load the Skeleton from a file
    dart::utils::DartLoader loader;
    SkeletonPtr pexod = loader.parseSkeleton("/home/kchatzil/Workspaces/ResiBots/source/sferes2/pexod.urdf");
    pexod->setName("pexod");

    // Set joint limits
    for (size_t i = 1; i < pexod->getNumJoints(); ++i) {
        pexod->getJoint(i)->setPositionLimitEnforced(true);
        pexod->getJoint(i)->setActuatorType(Joint::VELOCITY);
    }

    pexod->setPosition(5, 0.1);

    return pexod;
}

int main(int argc, char* argv[])
{
    SkeletonPtr floor = createFloor();
    SkeletonPtr pexod = createPexod();

    WorldPtr world = std::make_shared<World>();
    world->getConstraintSolver()->setCollisionDetector(new dart::collision::DARTCollisionDetector());
    world->addSkeleton(floor);
    world->addSkeleton(pexod);
    world->setTimeStep(0.015);

    std::unique_ptr<Controller> controller = std::unique_ptr<Controller>(new Controller(world->getSkeleton("pexod")));
    bool stabilized = false;
    int stab = 0;
    for (int i = 0; i < 1000 && !stabilized; i++) {
        Eigen::VectorXd prev_pos = pexod->getCOM();
        controller->update(world->getTime());
        controller->setPDForces();
        world->step();
        if ((pexod->getCOM() - prev_pos).norm() < 1e-4)
            stab++;
        else
            stab = 0;
        if (stab > 100)
            stabilized = true;
    }

    double t = world->getTime();
    world->setTimeStep(0.015);
    controller->setControl({1, 0, 0.5, 0.25, 0.25, 0.5, 1, 0.5, 0.5, 0.25, 0.75, 0.5, 1, 0, 0.5, 0.25, 0.25, 0.5, 1, 0, 0.5, 0.25, 0.75, 0.5, 1, 0.5, 0.5, 0.25, 0.25, 0.5, 1, 0, 0.5, 0.25, 0.75, 0.5});

    for (int i = 0; i < 5000; i++) {
        if ((world->getTime() - t) >= 5.0)
            break;
        controller->update(world->getTime());
        controller->setPDForces();
        world->step();
    }

    world.reset();
}

To be able to reproduce the error, you will need ControllerDuty.hpp, ControllerDuty.cpp and pexod.urdf:

#ifndef CONTROLLERPHASE_HPP
#define CONTROLLERPHASE_HPP

#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/array.hpp>
#define ARRAY_DIM 100



class ControllerDuty
{
public:
  typedef boost::array<float,ARRAY_DIM> array_t;
protected:

  std::vector<array_t > _legs0commands;
  std::vector<array_t > _legs1commands;
  std::vector<array_t > _legs2commands;
  std::vector<array_t > _legs3commands;
  std::vector<array_t > _legs4commands;
  std::vector<array_t > _legs5commands;


  std::vector<int> _brokenLegs;



public :



  ControllerDuty(const std::vector<float>& ctrl,std::vector<int> brokenLegs):_brokenLegs(brokenLegs)
  {
    assert(ctrl.size()==36);

    _legs0commands.push_back(control_signal(ctrl[0],ctrl[1],ctrl[2]));
    _legs0commands.push_back(control_signal(ctrl[3],ctrl[4],ctrl[5]));
    _legs0commands.push_back(control_signal(ctrl[3],ctrl[4],ctrl[5]));

    _legs1commands.push_back(control_signal(ctrl[6],ctrl[7],ctrl[8]));
    _legs1commands.push_back(control_signal(ctrl[9],ctrl[10],ctrl[11]));
    _legs1commands.push_back(control_signal(ctrl[9],ctrl[10],ctrl[11]));

    _legs2commands.push_back(control_signal(ctrl[12],ctrl[13],ctrl[14]));
    _legs2commands.push_back(control_signal(ctrl[15],ctrl[16],ctrl[17]));
    _legs2commands.push_back(control_signal(ctrl[15],ctrl[16],ctrl[17]));

    _legs3commands.push_back(control_signal(ctrl[18],ctrl[19],ctrl[20]));
    _legs3commands.push_back(control_signal(ctrl[21],ctrl[22],ctrl[23]));
    _legs3commands.push_back(control_signal(ctrl[21],ctrl[22],ctrl[23]));

    _legs4commands.push_back(control_signal(ctrl[24],ctrl[25],ctrl[26]));
    _legs4commands.push_back(control_signal(ctrl[27],ctrl[28],ctrl[29]));
    _legs4commands.push_back(control_signal(ctrl[27],ctrl[28],ctrl[29]));

    _legs5commands.push_back(control_signal(ctrl[30],ctrl[31],ctrl[32]));
    _legs5commands.push_back(control_signal(ctrl[33],ctrl[34],ctrl[35]));
    _legs5commands.push_back(control_signal(ctrl[33],ctrl[34],ctrl[35]));

  }



  array_t control_signal(float amplitude, float phase, float duty_cycle);


  bool isBroken(int leg)
  {
    for (size_t j=0;j<_brokenLegs.size();j++)
        {
            if (leg==_brokenLegs[j])
            {
                return true;
            }
        }
        return false;
  }


    std::vector<float> get_pos(float t);

};

#endif

#include "controllerDuty.hpp"
#define RAD2DYNMX28 651.42
#define RAD2DYN 195.57


typedef boost::array<float,ARRAY_DIM> array_t;

array_t ControllerDuty::control_signal(float amplitude, float phase, float duty_cycle)
{
  array_t temp;
  int up_time=ARRAY_DIM*duty_cycle;
  for(int i=0;i<up_time;i++)
    temp[i]=amplitude;
  for(int i=up_time;i<ARRAY_DIM;i++)
    temp[i]=-amplitude;





  // filtering
  int kernel_size=ARRAY_DIM/10;

  array_t command;

  std::vector<float> kernel(2*kernel_size+1,0);
  float sigma=kernel_size/3;

  float sum=0;
  for(int i=0;i<(int)kernel.size();i++)
    {
      kernel[i]=exp(-(i-kernel_size)*(i-kernel_size)/(2*sigma*sigma))/(sigma*sqrt(M_PI));
      sum+=kernel[i];                           
    }



  for(int i=0;i<ARRAY_DIM;i++)
    {
      command[i]=0;
      for(int d=1;d<=kernel_size;d++)
    {
      if(i-d<0)
        command[i]+=temp[ARRAY_DIM+i-d]*kernel[kernel_size-d];
      else
        command[i]+=temp[i-d]*kernel[kernel_size-d];
    }
      command[i]+=temp[i]*kernel[kernel_size];
      for(int d=1;d<=kernel_size;d++)
    {
      if(i+d>=ARRAY_DIM)
        command[i]+=temp[i+d-ARRAY_DIM]*kernel[kernel_size+d];
      else
        command[i]+=temp[i+d]*kernel[kernel_size+d];
    }

      command[i]/=sum;
    }


  // apply phase
  array_t final_command;
  int current=0;
  int start=floor(ARRAY_DIM*phase);
  for(int i=start;i<ARRAY_DIM;i++)
    {
      final_command[current]=command[i];
      current++;
    }
  for(int i=0;i<start;i++)
    {
      final_command[current]=command[i];
      current++;
    }


  return final_command;


}



std::vector<float> ControllerDuty::get_pos(float t)
{
    std::vector<float> angles;
  ///  t /= 2;// JBM test
    int leg = 0;
    for (size_t i = 0; i < 18; i+=3)
    {
      //   std::cout<<"dans move"<<std::endl;
       for (size_t j=0;j<_brokenLegs.size();j++)
        {
            if (leg==_brokenLegs[j])
            {
                leg++;
                if (_brokenLegs.size()>j+1 && _brokenLegs[j+1]!=leg)
                    break;
            }
        }

         switch(leg)
         {
         case 0:
            angles.push_back(M_PI/8*_legs0commands[0][((int)floor(t*100))%100]);
            angles.push_back(M_PI/4*_legs0commands[1][((int)floor(t*100))%100]);
            angles.push_back(-M_PI/4*_legs0commands[2][((int)floor(t*100))%100]);
           // robot->servos()[i]->set_angle(0,M_PI/8*_legs0commands[0][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+1]->set_angle(0,M_PI/4*_legs0commands[1][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+2]->set_angle(0,-M_PI/4*_legs0commands[2][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           break;

         case 1:
            angles.push_back(M_PI/8*_legs1commands[0][((int)floor(t*100))%100]);
            angles.push_back(M_PI/4*_legs1commands[1][((int)floor(t*100))%100]);
            angles.push_back(-M_PI/4*_legs1commands[2][((int)floor(t*100))%100]);
           // robot->servos()[i]->set_angle(0,M_PI/8*_legs1commands[0][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+1]->set_angle(0,M_PI/4*_legs1commands[1][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+2]->set_angle(0,-M_PI/4*_legs1commands[2][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           break;

         case 2:
            angles.push_back(M_PI/8*_legs2commands[0][((int)floor(t*100))%100]);
            angles.push_back(M_PI/4*_legs2commands[1][((int)floor(t*100))%100]);
            angles.push_back(-M_PI/4*_legs2commands[2][((int)floor(t*100))%100]);
           // robot->servos()[i]->set_angle(0,M_PI/8*_legs2commands[0][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+1]->set_angle(0,M_PI/4*_legs2commands[1][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+2]->set_angle(0,-M_PI/4*_legs2commands[2][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           break;

         case 3:
            angles.push_back(M_PI/8*_legs3commands[0][((int)floor(t*100))%100]);
            angles.push_back(M_PI/4*_legs3commands[1][((int)floor(t*100))%100]);
            angles.push_back(-M_PI/4*_legs3commands[2][((int)floor(t*100))%100]);
           // robot->servos()[i]->set_angle(0,M_PI/8*_legs3commands[0][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+1]->set_angle(0,M_PI/4*_legs3commands[1][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+2]->set_angle(0,-M_PI/4*_legs3commands[2][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           break;

         case 4:
            angles.push_back(M_PI/8*_legs4commands[0][((int)floor(t*100))%100]);
            angles.push_back(M_PI/4*_legs4commands[1][((int)floor(t*100))%100]);
            angles.push_back(-M_PI/4*_legs4commands[2][((int)floor(t*100))%100]);
           // robot->servos()[i]->set_angle(0,M_PI/8*_legs4commands[0][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+1]->set_angle(0,M_PI/4*_legs4commands[1][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+2]->set_angle(0,-M_PI/4*_legs4commands[2][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           break;

         case 5:
            angles.push_back(M_PI/8*_legs5commands[0][((int)floor(t*100))%100]);
            angles.push_back(M_PI/4*_legs5commands[1][((int)floor(t*100))%100]);
            angles.push_back(-M_PI/4*_legs5commands[2][((int)floor(t*100))%100]);
           // robot->servos()[i]->set_angle(0,M_PI/8*_legs5commands[0][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+1]->set_angle(0,M_PI/4*_legs5commands[1][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           // robot->servos()[i+2]->set_angle(0,-M_PI/4*_legs5commands[2][((int)floor(t*100))%100]);  // marche que pour ARRAY_DIM =100
           break;
         }

    ++leg;
    }
    return angles;

}

<?xml version="1.0" ?>
<!-- =================================================================================== -->
<!-- |    This document was autogenerated by xacro from pexod.xacro                    | -->
<!-- |    EDITING THIS FILE BY HAND IS NOT RECOMMENDED                                 | -->
<!-- =================================================================================== -->
<robot name="pexod" xmlns:xacro="http://www.ros.org/wiki/xacro">
  <!-- <xacro:include filename="$(find pexod_description)/urdf/pexod_control.xacro" /> -->
  <!-- MATERIALS -->
  <material name="Blue">
    <color rgba="0 0 1 1"/>
  </material>
  <material name="Red">
    <color rgba="1 0 0 1"/>
  </material>
  <material name="Green">
    <color rgba="0 1 0 1"/>
  </material>
  <material name="Yellow">
    <color rgba="1 1 0 1"/>
  </material>
  <material name="LightGrey">
    <color rgba="0.6 0.6 0.6 1.0"/>
  </material>
  <!-- END OF MATERIALS -->
  <!-- TORSO -->
  <link name="body">
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.24 0.2 0.04"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.24 0.2 0.04"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <mass value="1.031"/>
      <!-- box inertia: 1/12*m(y^2+z^2), ... -->
      <inertia ixx="0.00357413333333" ixy="0" ixz="0" iyy="0.00508626666667" iyz="0" izz="0.00838546666667"/>
    </inertial>
  </link>
  <!-- XACRO MACRO FOR LEGS LINKS/JOINTS -->
  <!-- END OF LEG LINKS/JOINTS -->
  <joint name="body_leg_2" type="revolute">
    <parent link="body"/>
    <child link="leg_2_1"/>
    <limit effort="30.0" lower="-1.57079632679" upper="1.57079632679" velocity="7.0"/>
    <origin rpy="0 0 -0.3" xyz="0.12 0.08 0"/>
    <!-- <xacro:if value="${index == 5 or index == 4 or index == 3}">
                <axis xyz="0 0 1"/>
            </xacro:if>
            <xacro:if value="${index == 2 or index == 1 or index == 0}"> -->
    <axis xyz="0 0 -1"/>
    <!-- </xacro:if> -->
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_2_1">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <mass value="0.02"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8e-06" iyz="0" izz="6.66666666667e-07"/>
    </inertial>
  </link>
  <joint name="leg_2_1_2" type="revolute">
    <parent link="leg_2_1"/>
    <child link="leg_2_2"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="0 0.06 0"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_2_2">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="leg_2_2_3" type="revolute">
    <parent link="leg_2_2"/>
    <child link="leg_2_3"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="-1.57079632679 0 0" xyz="0 0.085 0"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_2_3">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <geometry>
        <cylinder length="0.095" radius="0.025"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="body_leg_3" type="revolute">
    <parent link="body"/>
    <child link="leg_3_1"/>
    <limit effort="30.0" lower="-1.57079632679" upper="1.57079632679" velocity="7.0"/>
    <origin rpy="0 0 0.3" xyz="0.12 -0.08 0"/>
    <!-- <xacro:if value="${index == 5 or index == 4 or index == 3}">
                <axis xyz="0 0 1"/>
            </xacro:if>
            <xacro:if value="${index == 2 or index == 1 or index == 0}"> -->
    <axis xyz="0 0 -1"/>
    <!-- </xacro:if> -->
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_3_1">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <mass value="0.02"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8e-06" iyz="0" izz="6.66666666667e-07"/>
    </inertial>
  </link>
  <joint name="leg_3_1_2" type="revolute">
    <parent link="leg_3_1"/>
    <child link="leg_3_2"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="0 -0.06 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_3_2">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="leg_3_2_3" type="revolute">
    <parent link="leg_3_2"/>
    <child link="leg_3_3"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="1.57079632679 0 0" xyz="0 -0.085 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_3_3">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <geometry>
        <cylinder length="0.095" radius="0.025"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="body_leg_0" type="revolute">
    <parent link="body"/>
    <child link="leg_0_1"/>
    <limit effort="30.0" lower="-1.57079632679" upper="1.57079632679" velocity="7.0"/>
    <origin rpy="0 0 0.3" xyz="-0.14 0.08 0"/>
    <!-- <xacro:if value="${index == 5 or index == 4 or index == 3}">
                <axis xyz="0 0 1"/>
            </xacro:if>
            <xacro:if value="${index == 2 or index == 1 or index == 0}"> -->
    <axis xyz="0 0 -1"/>
    <!-- </xacro:if> -->
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_0_1">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <mass value="0.02"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8e-06" iyz="0" izz="6.66666666667e-07"/>
    </inertial>
  </link>
  <joint name="leg_0_1_2" type="revolute">
    <parent link="leg_0_1"/>
    <child link="leg_0_2"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="0 0.06 0"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_0_2">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="leg_0_2_3" type="revolute">
    <parent link="leg_0_2"/>
    <child link="leg_0_3"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="-1.57079632679 0 0" xyz="0 0.085 0"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_0_3">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <geometry>
        <cylinder length="0.095" radius="0.025"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="body_leg_5" type="revolute">
    <parent link="body"/>
    <child link="leg_5_1"/>
    <limit effort="30.0" lower="-1.57079632679" upper="1.57079632679" velocity="7.0"/>
    <origin rpy="0 0 -0.3" xyz="-0.14 -0.08 0"/>
    <!-- <xacro:if value="${index == 5 or index == 4 or index == 3}">
                <axis xyz="0 0 1"/>
            </xacro:if>
            <xacro:if value="${index == 2 or index == 1 or index == 0}"> -->
    <axis xyz="0 0 -1"/>
    <!-- </xacro:if> -->
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_5_1">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <mass value="0.02"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8e-06" iyz="0" izz="6.66666666667e-07"/>
    </inertial>
  </link>
  <joint name="leg_5_1_2" type="revolute">
    <parent link="leg_5_1"/>
    <child link="leg_5_2"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="0 -0.06 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_5_2">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="leg_5_2_3" type="revolute">
    <parent link="leg_5_2"/>
    <child link="leg_5_3"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="1.57079632679 0 0" xyz="0 -0.085 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_5_3">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <geometry>
        <cylinder length="0.095" radius="0.025"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="body_leg_1" type="revolute">
    <parent link="body"/>
    <child link="leg_1_1"/>
    <limit effort="30.0" lower="-1.57079632679" upper="1.57079632679" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="-0.01 0.08 0"/>
    <!-- <xacro:if value="${index == 5 or index == 4 or index == 3}">
                <axis xyz="0 0 1"/>
            </xacro:if>
            <xacro:if value="${index == 2 or index == 1 or index == 0}"> -->
    <axis xyz="0 0 -1"/>
    <!-- </xacro:if> -->
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_1_1">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.03 0"/>
      <mass value="0.02"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8e-06" iyz="0" izz="6.66666666667e-07"/>
    </inertial>
  </link>
  <joint name="leg_1_1_2" type="revolute">
    <parent link="leg_1_1"/>
    <child link="leg_1_2"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="0 0.06 0"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_1_2">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 0.0425 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="leg_1_2_3" type="revolute">
    <parent link="leg_1_2"/>
    <child link="leg_1_3"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="-1.57079632679 0 0" xyz="0 0.085 0"/>
    <axis xyz="-1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_1_3">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <geometry>
        <cylinder length="0.095" radius="0.025"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.0125 0.0475 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="body_leg_4" type="revolute">
    <parent link="body"/>
    <child link="leg_4_1"/>
    <limit effort="30.0" lower="-1.57079632679" upper="1.57079632679" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="-0.01 -0.08 0"/>
    <!-- <xacro:if value="${index == 5 or index == 4 or index == 3}">
                <axis xyz="0 0 1"/>
            </xacro:if>
            <xacro:if value="${index == 2 or index == 1 or index == 0}"> -->
    <axis xyz="0 0 -1"/>
    <!-- </xacro:if> -->
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_4_1">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <geometry>
        <cylinder length="0.06" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.03 0"/>
      <mass value="0.02"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="8e-06" ixy="0" ixz="0" iyy="8e-06" iyz="0" izz="6.66666666667e-07"/>
    </inertial>
  </link>
  <joint name="leg_4_1_2" type="revolute">
    <parent link="leg_4_1"/>
    <child link="leg_4_2"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="0 0 0" xyz="0 -0.06 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_4_2">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
      <material name="Blue"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <geometry>
        <cylinder length="0.085" radius="0.02"/>
      </geometry>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.01 -0.0425 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <joint name="leg_4_2_3" type="revolute">
    <parent link="leg_4_2"/>
    <child link="leg_4_3"/>
    <limit effort="30.0" lower="-0.78539816339" upper="0.78539816339" velocity="7.0"/>
    <origin rpy="1.57079632679 0 0" xyz="0 -0.085 0"/>
    <axis xyz="1 0 0"/>
    <dynamics damping="0.0"/>
  </joint>
  <link name="leg_4_3">
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <geometry>
        <cylinder length="0.07" radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <visual>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </visual>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <geometry>
        <cylinder length="0.095" radius="0.025"/>
      </geometry>
    </collision>
    <collision>
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0725 0"/>
      <geometry>
        <sphere radius="0.025"/>
      </geometry>
      <material name="Red"/>
    </collision>
    <inertial>
      <!-- CENTER OF MASS -->
      <origin rpy="1.57079632679 0 0" xyz="0.0125 -0.0475 0"/>
      <mass value="0.184"/>
      <!-- Cylinder inertia: 1/12*m*(3*r^2+h^2), 1/12*m*(3*r^2+h^2), 1/12*m*r^2 -->
      <inertia ixx="0.000129183333333" ixy="0" ixz="0" iyy="0.000129183333333" iyz="0" izz="6.13333333333e-06"/>
    </inertial>
  </link>
  <!-- END OF LEG LINKS/JOINTS -->
  <!-- <xacro:include filename="$(find pexod_description)/urdf/modules/imu.xacro" /> -->
  <!-- <xacro:include filename="$(find pexod_description)/urdf/modules/cameras.xacro" /> -->
</robot>

[mxgrey]

Thanks for reporting this issue. I recommend using gist.github.com when you want to include long snippets of text.

Unfortunately, not quite all of DART's codebase has been updated to using C++11 yet, so that leaves us open to some memory leak vulnerabilities. I'm noticing that several vectors of dynamically allocated constraints are not being properly deleted in the destructor of the ConstraintSolver class, and that could certainly explain the leak you're seeing. I'll try switching to shared_ptrs for these constraints, and that should remedy the memory leak.

Just to clarify, are you frequently creating and destroying World objects (and the Skeletons inside of them), or do you recycle World objects so that you don't need to continuously create and destroy them? If you are recycling your World objects, then there must be another cause behind the memory leaks that I haven't identified yet. Switching to shared_ptr should still fix it, but I'd be curious to know where else it might have been leaking.

Also, if you are frequently creating and destroying Skeletons with meshes, then that might result in a known memory leak caused by Assimp. If you are not using any meshes, then you shouldn't need to worry about that one.

[costashatz]

Unfortunately, not quite all of DART's codebase has been updated to using C++11 yet, so that leaves us open to some memory leak vulnerabilities. I'm noticing that several vectors of dynamically allocated constraints are not being properly deleted in the destructor of the ConstraintSolver class, and that could certainly explain the leak you're seeing. I'll try switching to shared_ptrs for these constraints, and that should remedy the memory leak.

I am waiting for this. When can you have this ready? I can help you on that one, if you can point to me where I should look.

Just to clarify, are you frequently creating and destroying World objects (and the Skeletons inside of them), or do you recycle World objects so that you don't need to continuously create and destroy them? If you are recycling your World objects, then there must be another cause behind the memory leaks that I haven't identified yet. Switching to shared_ptr should still fix it, but I'd be curious to know where else it might have been leaking.

I am not recycling World objects. I am creating and destroying them every time.

Also, if you are frequently creating and destroying Skeletons with meshes, then that might result in a known memory leak caused by Assimp. If you are not using any meshes, then you shouldn't need to worry about that one.

I am loading a skeleton once (using URDF file) and then cloning it each time. Inside URDF I only have basic shapes (spheres, cylinders, boxes). So I guess I shouldn't worry about that.

[mxgrey]

I am waiting for this. When can you have this ready?

I'm making the changes right now. It should be done some time this afternoon.

[mxgrey]

Although to clarify, for right now I'm only going to focus on changing the known trouble spots in ConstraintSolver. Attempting to scan through the whole code base for areas that are not yet updated to modern C++ practices would take much longer.

[costashatz]

I'm making the changes right now. It should be done some time this afternoon.

Thanks for prompt answer once again.

[mxgrey]

I've created a pull request #584 with the changes. I have not yet run any tests to make sure that the leaking is gone, but I'd be very surprised if it still persists (unless there's another leak source that wasn't caught in the Valgrind report).

[costashatz]

This was solved by #584 !