initial import from ISIR's svn

eigen2.py

@@ -0,0 +1,39 @@
+#! /usr/bin/env python
+# encoding: utf-8
+# JB Mouret - 2009
+
+"""
+Quick n dirty eigen2 detection
+"""
+
+import os, glob, types
+import Options, Configure
+
+def detect_eigen2(conf):
+	env = conf.env
+	opt = Options.options
+
+	conf.env['LIB_EIGEN2'] = ''
+	conf.env['EIGEN2_FOUND'] = False
+	if Options.options.no_eigen2:
+		return 0
+	if Options.options.eigen2:
+		conf.env['CPPPATH_EIGEN2'] = Options.options.eigen2
+		conf.env['LIBPATH_EIGEN2'] = Options.options.eigen2
+	else:
+		conf.env['CPPPATH_EIGEN2'] = ['/usr/include/eigen2', '/usr/local/include/eigen2', '/usr/include', '/usr/local/include']
+		#conf.env['LIBPATH_EIGEN2'] = ['/usr/lib', '/usr/local/lib']
+
+	res = Configure.find_file('Eigen/Core', conf.env['CPPPATH_EIGEN2'])
+	conf.check_message('header','Eigen/Core', (res != '') , res)
+	if (res == '') :
+		return 0
+	conf.env['EIGEN2_FOUND'] = True
+	return 1
+
+def detect(conf):
+	return detect_eigen2(conf)
+
+def set_options(opt):
+	opt.add_option('--eigen2', type='string', help='path to eigen2', dest='eigen2')
+	opt.add_option('--no-eigen2', type='string', help='disable eigen2', dest='no_eigen2')

ode.py

@@ -0,0 +1,32 @@
+
+#! /usr/bin/env python
+# encoding: utf-8
+# JB Mouret - 2009
+
+"""
+Quick n dirty ODE detection
+"""
+
+import os, glob, types
+import Options, Configure, config_c
+
+import commands
+
+def detect_ode(conf):
+    env = conf.env
+    opt = Options.options
+    ret = conf.find_program('ode-config')
+    conf.check_message_1('Checking for ODE')
+    if not ret:
+        conf.check_message_2('not found', 'RED')
+        return 0
+    conf.check_message_2('ok')
+    res = commands.getoutput('ode-config --cflags --libs')
+    config_c.parse_flags(res, 'ODE', env)
+    return 1
+
+def detect(conf):
+    return detect_ode(conf)
+
+def set_options(opt):
+    pass

src/demos/basic.cc

@@ -0,0 +1,46 @@
+#include <iostream>
+
+#include <boost/foreach.hpp>
+#include "ode/environment.hh"
+#include "ode/capped_cyl.hh"
+#include "renderer/osg_visitor.hh"
+
+int main()
+{
+  dInitODE();
+
+  renderer::OsgVisitor v;
+  ode::Environment env;
+
+  std::vector<ode::Object::ptr_t> robot;
+  std::vector<ode::Servo::ptr_t> servos;
+
+  ode::Object::ptr_t p1
+    (new ode::CappedCyl(env,Eigen::Vector3d(0, 0.0, 1), 
+			0.1, 0.1, 0.5));
+  robot.push_back(p1);
+  ode::Object::ptr_t p2
+    (new ode::CappedCyl(env, Eigen::Vector3d(0, 0.25, 1 + 0.25), 
+			0.1, 0.1, 0.5));
+  p2->set_rotation(M_PI / 2.0, 0, 0);
+  robot.push_back(p2);
+
+  ode::Servo::ptr_t s1
+    (new ode::Servo(env, Eigen::Vector3d(0, 0, 1 + 0.25), *p1, *p2));
+  servos.push_back(s1);
+
+  float x = 0;
+  while(!v.done())
+    {
+      v.visit(robot);
+      v.update();
+      env.next_step(0.001);
+      BOOST_FOREACH(ode::Servo::ptr_t s, servos) 
+	s->next_step(0.001);
+      s1->set_angle(ode::Servo::DIHEDRAL, cos(x));
+      x += 0.01;
+    }
+
+  return 0;
+}
+

src/demos/controllerPhase.cpp

@@ -0,0 +1,71 @@
+#include "controllerPhase.hpp"
+
+#define RAD2DYN 195.57
+void ControllerPhase::moveRobot(robot_t& robot, float t)
+{
+
+    //std::cout<<"debut move"<<std::endl;
+    size_t leg = 0;
+    for (size_t i = 0; i < robot->servos().size(); i+=3)
+    {
+        //std::cout<<"dans move"<<std::endl;
+        for (int j=0;j<_brokenLegs.size();j++)
+        {
+            if (leg==_brokenLegs[j])
+            {
+                leg++;
+                if (_brokenLegs.size()>j+1 && _brokenLegs[j+1]!=leg)
+                    break;
+            }
+        }
+
+        robot->servos()[i]->set_angle(0,_legsParams[leg][0]*M_PI/3+ _legsParams[leg][1]*M_PI/3*delayedPhase(t,_legsParams[leg][2]));
+        robot->servos()[i + 1]->set_angle(0,_legsParams[leg][3]*M_PI/3+_legsParams[leg][4]*delayedPhase(t,_legsParams[leg][5]));
+        robot->servos()[i + 2]->set_angle(0,_legsParams[leg][3]*M_PI/3-_legsParams[leg][4]*delayedPhase(t,_legsParams[leg][6]));
+
+	// robot->servos()[i + 3]->set_angle(0,0);
+        ++leg;
+    }
+
+}
+
+
+
+std::vector<int> ControllerPhase::get_pos_dyna(float t)
+{
+    std::vector<int> pos;
+    //std::cout<<"debut move"<<std::endl;
+    size_t leg = 0;
+    for (size_t i = 0; i < 24; i+=4)
+    {
+        //std::cout<<"dans move"<<std::endl;
+        for (int j=0;j<_brokenLegs.size();j++)
+        {
+            if (leg==_brokenLegs[j])
+            {
+                leg++;
+                if (_brokenLegs.size()>j+1 && _brokenLegs[j+1]!=leg)
+                    break;
+            }
+        }
+
+        pos.push_back(512*RAD2DYN*(_legsParams[leg][0]*M_PI/3+ _legsParams[leg][1]*M_PI/3*delayedPhase(t,_legsParams[leg][2])));
+        pos.push_back(512*RAD2DYN*(_legsParams[leg][3]*M_PI/3+_legsParams[leg][4]*delayedPhase(t,_legsParams[leg][5])));
+        pos.push_back(512*RAD2DYN*(_legsParams[leg][3]*M_PI/3+_legsParams[leg][4]*delayedPhase(t,_legsParams[leg][6])));
+
+        pos.push_back(512);
+        ++leg;
+    }
+    std::cout<<"taille pos:"<<pos.size()<<std::endl;
+  return pos;
+}
+
+
+
+
+float ControllerPhase::delayedPhase(float t, float phi)
+{
+
+    return tanh(sin(2*M_PI*t+phi*2*M_PI)*4);
+
+}

src/demos/controllerPhase.hpp

@@ -0,0 +1,51 @@
+#ifndef CONTROLLERPHASE_HPP
+#define CONTROLLERPAHSE_HPP
+
+#include <vector>
+#include <boost/shared_ptr.hpp>
+#include "robot/hexapod.hh"
+
+
+
+
+
+class ControllerPhase
+{
+
+protected :
+
+    std::vector< std::vector<float> > _legsParams;
+    std::vector<int> _brokenLegs;
+private:
+    float delayedPhase(float t, float phi);
+
+public :
+    typedef boost::shared_ptr<robot::Robot> robot_t;
+    ControllerPhase(const std::vector<float>& ctrl,std::vector<int> brokenLegs):_brokenLegs(brokenLegs)
+    {
+
+
+        assert(ctrl.size()==42);
+        for(int leg=0;leg<6;leg++)
+        {
+            std::vector<float> param;
+            param.push_back(ctrl[leg*7]*2-1);
+            param.push_back(ctrl[leg*7+1]);
+            param.push_back(ctrl[leg*7+2]);
+            param.push_back(ctrl[leg*7+3]*2-1);
+            param.push_back(ctrl[leg*7+4]);
+            param.push_back(ctrl[leg*7+5]);
+            param.push_back(ctrl[leg*7+6]);
+            _legsParams.push_back(param);
+        }
+
+    }
+
+    void moveRobot(robot_t& robot, float t);
+    std::vector<int> get_pos_dyna(float t);
+
+
+
+};
+
+#endif