diff --git a/data/checker.tga b/data/checker.tga new file mode 100644 index 0000000..9469655 Binary files /dev/null and b/data/checker.tga differ diff --git a/eigen2.py b/eigen2.py new file mode 100644 index 0000000..be54ff0 --- /dev/null +++ b/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') diff --git a/fonts/arial.ttf b/fonts/arial.ttf new file mode 100644 index 0000000..28208d2 Binary files /dev/null and b/fonts/arial.ttf differ diff --git a/ode.py b/ode.py new file mode 100644 index 0000000..5c96260 --- /dev/null +++ b/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 diff --git a/src/demos/basic.cc b/src/demos/basic.cc new file mode 100644 index 0000000..f447ded --- /dev/null +++ b/src/demos/basic.cc @@ -0,0 +1,46 @@ +#include + +#include +#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 robot; + std::vector 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; +} + diff --git a/src/demos/controllerPhase.cpp b/src/demos/controllerPhase.cpp new file mode 100644 index 0000000..60cbf76 --- /dev/null +++ b/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"<servos().size(); i+=3) + { + //std::cout<<"dans move"<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 ControllerPhase::get_pos_dyna(float t) +{ + std::vector pos; + //std::cout<<"debut move"<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:"< +#include +#include "robot/hexapod.hh" + + + + + +class ControllerPhase +{ + +protected : + + std::vector< std::vector > _legsParams; + std::vector _brokenLegs; +private: + float delayedPhase(float t, float phi); + +public : + typedef boost::shared_ptr robot_t; + ControllerPhase(const std::vector& ctrl,std::vector brokenLegs):_brokenLegs(brokenLegs) + { + + + assert(ctrl.size()==42); + for(int leg=0;leg<6;leg++) + { + std::vector 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 get_pos_dyna(float t); + + + +}; + +#endif diff --git a/src/demos/hexapod.cc b/src/demos/hexapod.cc new file mode 100644 index 0000000..ac3a844 --- /dev/null +++ b/src/demos/hexapod.cc @@ -0,0 +1,340 @@ +#include + +#include +#include "ode/environment_hexa.hh" +#include "robot/hexapod.hh" +#include "ode/box.hh" +#include "renderer/osg_visitor.hh" +#include "controllerPhase6D.hpp" +#define PERIOD 2.0f + +float func(float x) +{ + return tanh(sin(x) * 4); +} + +typedef boost::shared_ptr robot_t; + +int main() +{ + static const float step = 0.015; + std::vector< float > ctrl; + /* + // marche hexapode +//leg 0 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.5); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.25); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 +//leg 1 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.5); // Amp mot0 + ctrl.push_back(0.5); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.75); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 +//leg 2 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.5); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.25); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 +//leg 3 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.5); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.75); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 +//leg 4 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.5); // Amp mot0 + ctrl.push_back(0.5); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.25); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 +//leg 5 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.5); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.75); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + */ + /* + // marche quadripede + //leg 0 + ctrl.push_back(0.25); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.125); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + //leg 1 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.0); // Pinit mot 1 & 2 + ctrl.push_back(0.0); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.0); // Phase mot 2 + //leg 2 + ctrl.push_back(0.75); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.125); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.5); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + + + //leg 3 + ctrl.push_back(0.25); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.125); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 + + + + //leg 4 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.0); // Pinit mot 1 & 2 + ctrl.push_back(0.0); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.0); // Phase mot 2 + //leg 5 + ctrl.push_back(0.75); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.125); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.5); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 + + */ + + // marche TEST + + /* + //leg 0 + ctrl.push_back(0.625); // Pinit mot0 + ctrl.push_back(1 ); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.25); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 + //leg 1 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(1); // Amp mot0 + ctrl.push_back(0.5); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.225); // Amp mot 1 & 2 + ctrl.push_back(0.75); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + //leg 2 + ctrl.push_back(0.375); // Pinit mot0 + ctrl.push_back(1); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.25); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 + //leg 3 + ctrl.push_back(0.625); // Pinit mot0 + ctrl.push_back(1); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.75); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + //leg 4 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(1); // Amp mot0 + ctrl.push_back(0.5); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.25); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 + //leg 5 + ctrl.push_back(0.375); // Pinit mot0 + ctrl.push_back(1); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.25); // Pinit mot 1 & 2 + ctrl.push_back(0.25); // Amp mot 1 & 2 + ctrl.push_back(0.75); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + + + + */ + + + /* + // marche bipede + //leg 0 + ctrl.push_back(0.25); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.375); // Pinit mot 1 & 2 + ctrl.push_back(0.375); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.75); // Phase mot 2 + //leg 1 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.0); // Pinit mot 1 & 2 + ctrl.push_back(0.0); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.0); // Phase mot 2 + //leg 2 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.0); // Pinit mot 1 & 2 + ctrl.push_back(0.0); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.0); // Phase mot 2 + + + //leg 3 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.0); // Pinit mot 1 & 2 + ctrl.push_back(0.0); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.0); // Phase mot 2 + + + + //leg 4 + ctrl.push_back(0.5); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.0); // Pinit mot 1 & 2 + ctrl.push_back(0.0); // Amp mot 1 & 2 + ctrl.push_back(0.0); // Phase mot 1 + ctrl.push_back(0.0); // Phase mot 2 + //leg 5 + ctrl.push_back(0.75); // Pinit mot0 + ctrl.push_back(0.0); // Amp mot0 + ctrl.push_back(0.0); // Phase mot0 + ctrl.push_back(0.375); // Pinit mot 1 & 2 + ctrl.push_back(0.375); // Amp mot 1 & 2 + ctrl.push_back(0.5); // Phase mot 1 + ctrl.push_back(0.25); // Phase mot 2 + */ + std::vector< int > brokenLegs; + /* brokenLegs.push_back(0); + brokenLegs.push_back(1); + brokenLegs.push_back(2); + brokenLegs.push_back(3); + brokenLegs.push_back(4); + brokenLegs.push_back(5); + */ + dInitODE(); + + ctrl.push_back(0.25); + ctrl.push_back(0); + ctrl.push_back(0.5); + ctrl.push_back(0.5); + ctrl.push_back(0); + ctrl.push_back(0.25); + + renderer::OsgVisitor v;//(renderer::OsgVisitor::FIXED); + + boost::shared_ptr env2(new ode::Environment_hexa()); + boost::shared_ptr env(new ode::Environment_hexa()); + robot_t rob2 = robot_t(new robot::Hexapod(*env2, Eigen::Vector3d(0, 0, 0.1),brokenLegs)); + + ControllerPhase controller(ctrl,brokenLegs); + + // rob2->accept(v); + //controller.moveRobot(rob,0); + // low gravity to slow things down (eq. smaller timestep?) + env2->set_gravity(0, 0, -9.81); + bool stabilized = false; + int stab = 0; + for (size_t s = 0; s < 2000 && !stabilized; ++s) + { + Eigen::Vector3d prev_pos = rob2->pos(); + rob2->next_step(step); + env2->next_step(step); + //v.update(); + if ((rob2->pos() - prev_pos).norm() < 1e-5) + stab++; + else + stab = 0; + if (stab > 100) + stabilized = true; + // usleep(10000); + } + env2->set_gravity(0, 0, -9.81); + // assert(stabilized); + + + + robot_t rob = rob2->clone(*env); + rob->accept(v); + + struct timeval timev_init; // Initial absolute time (static) + struct timeval timev_diff; // Previous tick absolute time + struct timeval timev_cur; // Current absolute time + Eigen::Vector3d prev_pos = rob->pos(); + + gettimeofday(&timev_init, NULL); + + float t=0; + while (!v.done() && t<10 ) + { + + //for(int i=0;i< rob->servos().size();i++) + // rob->servos()[i]->set_angle(ode::Servo::DIHEDRAL, cos(t)); + + + //rob->servos()[0]->set_angle(ode::Servo::DIHEDRAL, cos(2*M_PI*t)); + //rob->servos()[4]->set_angle(ode::Servo::DIHEDRAL, cos(2*M_PI*t)); + //rob->servos()[8]->set_angle(ode::Servo::DIHEDRAL, cos(2*M_PI*t)); + + + controller.moveRobot(rob,t); + + rob->next_step(step); + env->next_step(step); + v.update(); + t += step; + usleep(10000); + + } + + gettimeofday(&timev_cur, NULL); + timersub(&timev_cur, &timev_init, &timev_diff); + + std::cout<<"time duration "<pos(); + //_covered_distance = fabs(next_pos[0] - prev_pos[0]); + std::cout<<"dist"< + +#include +#include "ode/environment.hh" +#include "robot/quadruped.hh" +#include "robot/hybrid.hh" +#include "ode/box.hh" +#include "renderer/osg_visitor.hh" + +int main() +{ + dInitODE(); + + renderer::OsgVisitor v; + ode::Environment env; + robot::Quadruped quad(env, Eigen::Vector3d(0, 0, 0)); + robot::Hybrid hyb(env, Eigen::Vector3d(2, 0, 0.3)); + float x = 0; + float ampl = 0.75; + static const float step = 0.005; + quad.accept(v); + hyb.accept(v); + + ode::Environment env2; + robot::Robot::ptr_t q2 = hyb.clone(env2); + q2->accept(v); + + std::vector g; + for (size_t i = 0; i < 20; ++i) + { + ode::Box::ptr_t + b(new ode::Box(env, Eigen::Vector3d(i*0.25,0,0), 1000, + 0.01, 1, 0.15)); + b->fix(); + b->accept(v); + env.add_to_ground(*b); + g.push_back(b); + } + // for (size_t i = 0; i < 4; ++i) + // hyb.motors()[i]->set_efficiency(0); + + while(!v.done()) + { + v.update(); + env.next_step(step); + env2.next_step(step); + quad.next_step(step); + hyb.next_step(step); + // std::cout<<" ----- "<get_pos()<<"\n --"<set_mode(ode::Servo::M_VEL); + quad.servos()[i]->set_vel(2, 0.1* cos(x+ i % 2)); + // quad.servos()[i]->set_angle(ode::Servo::TWIST, + // sin(x+i) * ampl); + hyb.servos()[i]->set_angle(ode::Servo::TWIST, sin(x+(i%4)) * ampl); + } + for (size_t i = 0; i < 4; ++i) + hyb.motors()[i]->set_vel(cos(x) * 2.5); + x += 0.001; + } + + return 0; +} + diff --git a/src/demos/rescue.cc b/src/demos/rescue.cc new file mode 100644 index 0000000..d35131d --- /dev/null +++ b/src/demos/rescue.cc @@ -0,0 +1,123 @@ +#include + +#include +#include "ode/environment.hh" +#include "ode/box.hh" +#include "ode/capped_cyl.hh" +#include "ode/sphere.hh" +#include "ode/motor.hh" +#include "robot/quadruped.hh" +#include "robot/hybrid.hh" +#include "ode/box.hh" +#include "renderer/osg_visitor.hh" + + +using namespace ode; +USING_PART_OF_NAMESPACE_EIGEN + +namespace robot +{ + class Rescue : public Robot + { + public: + Rescue(ode::Environment& env, const Eigen::Vector3d& pos) { _build(env, pos); } + protected: + void _build(ode::Environment& env, const Eigen::Vector3d& pos); + }; + + void Rescue :: _build(Environment& env, const Vector3d& pos) + { + static const double body_mass = 1; + static const double body_length = 0.75; + static const double body_width = 0.5; + static const double body_height = 0.2; + static const double leg_w = 0.05; + static const double leg_length = 0.5; + static const double leg_dist = 0.2; + static const double leg_mass = 0.2; + static const double wheel_rad = 0.1; + static const double wheel_mass = 0.05; + + _main_body = Object::ptr_t + (new Box(env, pos + Vector3d(0, 0, leg_length), + body_mass, body_length, body_width, body_height)); + _bodies.push_back(_main_body); + + for (size_t i = 0; i < 4; ++i) + { + int left_right = i > 1 ? -1 : 1; + int back_front = i % 2 == 0 ? -1 : 1; + + Object::ptr_t l1 + (new CappedCyl(env, pos + Vector3d(back_front * (leg_dist / 2 + leg_length / 2), + left_right * (body_width / 2 + leg_w), + leg_length), + leg_mass, leg_w, leg_length)); + l1->set_rotation(0, M_PI / 2.0, 0); + _bodies.push_back(l1); + + Servo::ptr_t s1 + (new Servo(env, pos + Vector3d(back_front * leg_dist / 2, + left_right * (body_width / 2 + leg_w), + leg_length), + *_main_body, *l1)); + _servos.push_back(s1); + + Object::ptr_t l11 + (new CappedCyl(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + leg_length / 2 + wheel_rad), + leg_mass, leg_w, leg_length - wheel_rad * 2)); + _bodies.push_back(l11); + Servo::ptr_t s2 + (new Servo(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + leg_length), + *l1, *l11)); + _servos.push_back(s2); + + Object::ptr_t wheel + (new Sphere(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + wheel_rad), + wheel_mass, wheel_rad)); + _bodies.push_back(wheel); + Motor::ptr_t s3 + (new Motor(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + wheel_rad), + Vector3d(0, 1, 0), + *l11, *wheel)); + _motors.push_back(s3); + } + for (size_t i = 0; i < _servos.size(); ++i) + for (size_t j = 0; j < 3; ++j) + _servos[i]->set_lim(j, -M_PI / 3, M_PI / 3); + } +} + + +int main() +{ + dInitODE(); + + renderer::OsgVisitor v; + ode::Environment env(0, 0.25, 0); + robot::Rescue hyb(env, Eigen::Vector3d(2, 0, 0.05)); + + float x = 0; + float step = 0.001; + hyb.accept(v); + while(!v.done()) + { + v.update(); + env.next_step(step); + + hyb.next_step(step); + for (size_t i = 0; i < 4; ++i) + hyb.motors()[i]->set_vel(cos(x) * 2.5); + } + + return 0; +} + diff --git a/src/demos/robotHexa.cpp b/src/demos/robotHexa.cpp new file mode 100644 index 0000000..21cec42 --- /dev/null +++ b/src/demos/robotHexa.cpp @@ -0,0 +1,204 @@ +#include +#include "robotHexa.hpp" + + +void RobotHexa :: init() +{ + _controller.open_serial("/dev/ttyUSB0"); + + // Scan actuators IDs + _controller.scan_ax12s(); + const std::vector& ax12_ids = _controller.ax12_ids(); + if (!ax12_ids.size()) + { + std::cerr<<"[ax12] no ax12 detected"< pos(_actuators_ids.size()); + + for (size_t i = 0; i < _actuators_ids.size(); ++i) + if (_actuators_ids[i] >= 30) // mx28 + pos[i] = 1024; + else + pos[i] = 512; + _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos)); + _controller.recv(READ_DURATION, _status); + + + pos.clear(); + pos.resize(_actuators_ids.size()); + sleep(1); + for (size_t i = 0; i < _actuators_ids.size(); ++i) + switch ((int)_actuators_ids[i]) + { + case 7: + pos[i] = 700; + break; + case 8: + pos[i] = 700; + break; + case 9: + pos[i] = 1023; + break; + case 10: + pos[i] = 00; + break; + case 11: + pos[i] = 300; + break; + case 12: + pos[i] = 300; + break; + + default: + if (_actuators_ids[i] >= 30) // mx28 + pos[i] = 1024; + else + pos[i] = 512; + break; + } + + + _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos)); + _controller.recv(READ_DURATION, _status); + + pos.clear(); + pos.resize(_actuators_ids.size()); + sleep(1); + for (size_t i = 0; i < _actuators_ids.size(); ++i) + switch ((int)_actuators_ids[i]) + { + case 7: + pos[i] = 700; + break; + case 8: + pos[i] = 700; + break; + case 9: + pos[i] = 1023; + break; + case 10: + pos[i] = 00; + break; + case 11: + pos[i] = 300; + break; + case 12: + pos[i] = 300; + break; + + default: + if (_actuators_ids[i] >= 30) // mx28 + pos[i] = 2048; + else + pos[i] = 512; + break; + } + + + _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos)); + _controller.recv(READ_DURATION, _status); + + + + pos.clear(); + pos.resize(_actuators_ids.size()); + sleep(1); + for (size_t i = 0; i < _actuators_ids.size(); ++i) + if (_actuators_ids[i] == 8) + pos[i] = 350; + else if (_actuators_ids[i] == 11) + pos[i] = 650; + else if (_actuators_ids[i] >= 30) // mx28 + pos[i] = 2048; + else + pos[i] = 512; + + + + + _controller.send(dynamixel::ax12::SetPositions(_actuators_ids, pos)); + _controller.recv(READ_DURATION, _status); + + + + std::cout << "done" << std::endl; + + + +} +void RobotHexa :: transfer(ControllerPhase& controller, float duration) +{ + + float t=0; + while (t +#include +#include +#include "controllerPhase.hpp" + +#define DEFAULT_SPEED 150 +#define READ_DURATION 0.02f +#define SAMPLING_FREQUENCY 20 + +class RobotHexa +{ +public: + typedef unsigned char byte_t; + + RobotHexa() + { + init(); + } + void init(); + + void relax() + { + std::cout << "relax..." << std::endl; + for (size_t i = 0; i < _actuators_ids.size(); ++i) + { + _controller.send(dynamixel::ax12::TorqueEnable(_actuators_ids[i], false)); + _controller.recv(READ_DURATION, _status); + } + std::cout << "done" << std::endl; + } + void enable() + { + for (size_t i = 0; i < _actuators_ids.size(); ++i) + { + _controller.send(dynamixel::ax12::TorqueEnable(_actuators_ids[i], true)); + _controller.recv(READ_DURATION, _status); + } + usleep(1e5); + } + void reset(); + void transfer(ControllerPhase& controller, float duration); + size_t nb_actuators() const + { + return _actuators_ids.size(); + } + size_t nb_wheels() const + { + return _wheels_ids.size(); + } +protected: + + dynamixel::Usb2Dynamixel _controller; + dynamixel::Status _status; + std::vector _wheels_ids; + + std::vector _actuators_ids; + +}; + +#endif diff --git a/src/demos/test_mx28.cc b/src/demos/test_mx28.cc new file mode 100644 index 0000000..a7134ca --- /dev/null +++ b/src/demos/test_mx28.cc @@ -0,0 +1,59 @@ +#include + +#include +#include "ode/environment.hh" +#include "ode/capped_cyl.hh" +#include "renderer/osg_visitor.hh" +#include "ode/mx28.hh" +#include "ode/box.hh" +int main() +{ + dInitODE(); + + + renderer::OsgVisitor v; + ode::Environment env; + env.set_gravity(0, 0,0); + std::vector robot; + std::vector servos; + + ode::Object::ptr_t p1 + (new ode::Box(env,Eigen::Vector3d(0, 0.0, 1), + 0.1,0.2, 0.1, 0.5)); + robot.push_back(p1); + + + + /* p1->set_rotation(Eigen::Vector3d(0,1 , 0), + Eigen::Vector3d(0, 0, 1));*/ + + + ode::Object::ptr_t p2 + (new ode::Box(env, Eigen::Vector3d(0, 0, 1 + 0.5), + 0.1, 0.2,0.1, 0.5)); + + robot.push_back(p2); + + ode::Servo::ptr_t s1 + (new ode::Mx28(env, Eigen::Vector3d(0, 0, 1 + 0.25), *p1, *p2)); + servos.push_back(s1); + s1->set_axis(0, Eigen::Vector3d(1,0,0)); + s1->set_axis(2, Eigen::Vector3d(0,1,0)); + + p1->set_rotation(0,1,0,M_PI/4); + p1->fix(); + 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; +} + diff --git a/src/demos/walking.cc b/src/demos/walking.cc new file mode 100644 index 0000000..638a3e1 --- /dev/null +++ b/src/demos/walking.cc @@ -0,0 +1,51 @@ +#include + +#include +#include +#include "ode/environment.hh" +#include "robot/quadruped.hh" +#include "robot/hybrid.hh" +#include "ode/box.hh" +#include "renderer/osg_visitor.hh" + +int main() +{ + dInitODE(); + + renderer::OsgVisitor v; + ode::Environment env; + robot::Hybrid hyb(env, Eigen::Vector3d(2, 0, 0.3)); + static const float step = 0.005; + hyb.accept(v); + + float init = 0; + float ampl = 0.3; + float center = 0.6; + static const int clockwise = -1; + static const int counter_clockwise = 1; + + using namespace boost::assign; + std::vector b_servos = list_of(0)(3)(4)(7); + + float x = 0; + while(!v.done()) + { + v.update(); + env.next_step(step); + hyb.next_step(step); + for (size_t i = 0; i < 8; ++i) + { + float phase = (i < 4 ? 0 : 1) * M_PI / 2.0f; + bool bot = std::find(b_servos.begin(), b_servos.end(), i) != b_servos.end(); + int c_shift = (bot ? -1 : 1); + hyb.servos()[i]->set_angle(ode::Servo::TWIST, center * c_shift); + // + (i % 2 ? -1 : 1) * sin(x + phase) * ampl); + } + // for (size_t i = 0; i < 4; ++i) + // hyb.motors()[i]->set_vel(cos(x) * 2.5); + x += 0.01; + } + + return 0; +} + diff --git a/src/ode/ax12.hh b/src/ode/ax12.hh new file mode 100644 index 0000000..a6406bf --- /dev/null +++ b/src/ode/ax12.hh @@ -0,0 +1,63 @@ +#ifndef AX12_HH_ +# define AX12_HH_ + +#include +#include +#include "object.hh" + +#include "servo.hh" + +namespace ode +{ + class Ax12 : public Servo + { + public: + typedef boost::shared_ptr ptr_t; + static const float angular_vel = 150.0 / 1024.0 * 11.9; + Ax12(Environment& env, + const Eigen::Vector3d& anchor, + Object& o1, Object& o2, + int mode = M_POS) : + Servo(env, anchor, o1, o2, mode) + { _init_ax12(); } + Ax12(const Ax12& s, Environment& env, Object& o1, Object& o2) : + Servo(s, env, o1, o2) { _init_ax12(); } + + boost::shared_ptr clone(Environment& env, Object& o1, Object &o2) const + { return ptr_t(new Ax12(*this, env, o1, o2)); } + protected: + void _init_ax12() + { + _lim_min = Eigen::Vector3d::Constant(-5 * M_PI / 6.0f); + _lim_max = Eigen::Vector3d::Constant(5 * M_PI / 6.0f); + dJointSetAMotorParam(_amotor, dParamLoStop, _lim_min[0]); + dJointSetAMotorParam(_amotor, dParamHiStop, _lim_max[0]); + dJointSetAMotorParam(_amotor, dParamLoStop2, _lim_min[1]); + dJointSetAMotorParam(_amotor, dParamHiStop2, _lim_max[1]); + dJointSetAMotorParam(_amotor, dParamLoStop3, _lim_min[2]); + dJointSetAMotorParam(_amotor, dParamHiStop3, _lim_max[2]); + + static const double fmax = 15; + dJointSetAMotorParam(_amotor, dParamFMax, fmax); + dJointSetAMotorParam(_amotor, dParamFMax2, fmax); + dJointSetAMotorParam(_amotor, dParamFMax3, fmax); + } + virtual void _asserv(unsigned i, float dt) + { + double cur_angle1 = dJointGetAMotorAngle(_amotor, i); + double error1 = _angles(i) - cur_angle1 - _offset(i); + double error2 = error1 / (5.0 * M_PI/3.0) * 1024.0; + int sign = (error1 > 0) * 2 - 1; + double vel = 0; + + if(fabs(error2) > 1) + vel = angular_vel * sign; + else + vel = 0; + dJointSetAMotorParam(_amotor, _vel_selector(i), vel); + } + }; +} + + +#endif diff --git a/src/ode/box.hh b/src/ode/box.hh new file mode 100644 index 0000000..559605b --- /dev/null +++ b/src/ode/box.hh @@ -0,0 +1,98 @@ +/* +** box.hh +** Login : +** Started on Thu Nov 25 22:50:53 2004 mandor +** $Id$ +** +** Copyright (C) 2004 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef BOX_HH_ +# define BOX_HH_ + +#include +#include "object.hh" + +namespace ode +{ + /// a simple wrapper around the ODE's box class + class Box : public Object + { + public: + static const float standard_mass = 1; + Box(Environment& env, + const Eigen::Vector3d& pos, + float mass, + float l, float w, float h) : + Object(env, pos), + _w(w), _h(h), _l(l), _mass(mass) + { + init(); + } + + Box(const Box& o, Environment& env) : + Object(env, o.get_pos()), + _w(o._w), _h(o._h), _l(o._l), + _mass(o._mass) + { + _copy(o); + _geom = dCreateBox(_env.get_space(), _l , _w, _h); + dGeomSetBody(_geom, _body); + } + + virtual Object::ptr_t clone(Environment& env) const + { return Object::ptr_t(new Box(*this, env)); } + + float get_length() const { return _l; } + float get_width() const { return _w; } + float get_height() const { return _h; } + + //set + void set_length(float l) { _l = l; init_again(); } + void set_width(float w) { _w = w; init_again(); } + void set_height(float h) { _h = h; init_again(); } + + void init_again() + { + if (_body) + dBodyDestroy(_body); + if (_geom) + dGeomDestroy(_geom); + init(); + } + + /// const visitor + virtual void accept (ConstVisitor &v) const + { + assert(_body); assert(_geom); + v.visit(*this); + } + protected: + void init() + { + Object::init(); + dMassSetBoxTotal (&_m, _mass, _l, _w, _h); + dBodySetMass (_body, &_m); + _geom = dCreateBox(_env.get_space(), _l , _w, _h); + dGeomSetBody(_geom, _body); + } + float _w, _h, _l; + float _mass; + }; +} + + +#endif /* !BOX_HH_ */ diff --git a/src/ode/capped_cyl.hh b/src/ode/capped_cyl.hh new file mode 100644 index 0000000..72f6f8b --- /dev/null +++ b/src/ode/capped_cyl.hh @@ -0,0 +1,104 @@ +/* +** cappedCyl.hh +** Login : +** Started on Wed Jan 4 16:54:53 2006 Jean-baptiste MOURET +** $Id$ +** +** Copyright (C) 2006 Jean-baptiste MOURET +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef CAPPEDCYL_HH_ +# define CAPPEDCYL_HH_ + +#include + +#include "object.hh" + + +namespace ode +{ + /// this class encapsulates the CappedCylinder class in ODE + /// WARNING: length include the cap! + class CappedCyl : public Object + { + public: + CappedCyl(Environment& env, + const Eigen::Vector3d& pos, + float mass, + float radius, float length) : + Object(env, pos), + _mass(mass), + _radius(radius), + _length(length) + { + init(); + } + CappedCyl(const CappedCyl& o, Environment& env) : + Object(env, o.get_pos()), + _mass(o._mass), + _radius(o._radius), + _length(o._length) + { + _copy(o); + _geom = dCreateCCylinder(_env.get_space(), _radius, _length - _radius * 2); + dGeomSetBody(_geom, _body); + } + + virtual Object::ptr_t clone(Environment& env) const + { return Object::ptr_t(new CappedCyl(*this, env)); } + + float get_radius() const + { + return _radius; + } + float get_length() const + { + return _length; + } + /// const visitor + virtual void accept (ConstVisitor &v) const + { + assert(_geom); + assert(_body); + v.visit(*this); + } + protected: + void init() + { + Object::init(); + _geom = dCreateCCylinder(_env.get_space(), _radius, _length - _radius * 2); + assert(_body); + assert(_geom); + /*dMassSetCylinderTotal(&_m, _mass, + 3, // direction = 3 (along z) + _radius, _length - _radius * 2);*/ + dMassSetCapsuleTotal(&_m, _mass, + 3, // direction = 3 (along z) + _radius, _length - _radius * 2); + dBodySetMass(_body, &_m); + dGeomSetBody(_geom, _body); + + } + // atributes + float _mass; + float _radius; + float _length; + }; +} + + + +#endif /* !CAPPEDCYL_HH_ */ diff --git a/src/ode/environment.cc b/src/ode/environment.cc new file mode 100644 index 0000000..dfe639e --- /dev/null +++ b/src/ode/environment.cc @@ -0,0 +1,131 @@ +/* +** environment.cc +** Login : +** Started on Mon Jun 14 16:04:28 2010 mandor +** $Id$ +** +** Copyright (C) 2010 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include +#include + +#include "environment.hh" +#include "object.hh" + +namespace ode +{ + void Environment::add_to_ground(ode::Object& o) + { + _ground_objects.insert(o.get_geom()); + } + void Environment::_init(bool add_ground, float _angle) + { + //create world + _world_id = dWorldCreate(); + //init gravity + dWorldSetGravity(_world_id, 0, 0, -cst::g); + //space + _space_id = dHashSpaceCreate(0); + //ground + if (add_ground) + { + angle=_angle; + Eigen::Vector3f normal(0, sin(-angle), cos(-angle)); + Eigen::Quaternion q1, q2; + q1 = Eigen::AngleAxis(_pitch, Eigen::Vector3f::UnitX()); + q2 = Eigen::AngleAxis(_roll, Eigen::Vector3f::UnitY()); + normal = q2 * q1 * normal; + _ground = dCreatePlane(_space_id, normal.x(), normal.y(), normal.z(), _z); + _ground_objects.insert(_ground); + } + //contact group1 + _contactgroup = dJointGroupCreate(0); + + // TODO : + // void dWorldSetContactMaxCorrectingVel (dWorldID, dReal vel); + // dReal dWorldGetContactMaxCorrectingVel (dWorldID); + dWorldSetContactMaxCorrectingVel(_world_id, 100); + // Set and get the maximum correcting velocity that contacts are allowed to generate. The default value is infinity (i.e. no limit). Reducing this value can help prevent "popping" of deeply embedded objects. + + // void dWorldSetContactSurfaceLayer (dWorldID, dReal depth); + // dReal dWorldGetContactSurfaceLayer (dWorldID);// 0.001 + } + void Environment::_collision(dGeomID o1, dGeomID o2) + { + int g1 = (_ground_objects.find(o1) != _ground_objects.end()); + int g2 = (_ground_objects.find(o2) != _ground_objects.end()); + + if (!(g1 ^ g2)) + return; + + static const int N = 10; + int i, n; + dContact contact[N]; + n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact)); + + dBodyID b = 0; + if (g1 && o2) + b = dGeomGetBody(o2); + else + if (o1) + b = dGeomGetBody(o1); + if (b) + { + Object*o = (Object *)dBodyGetData(b); + if (dBodyGetData(b)) + o->set_in_contact(true); + } + if (n > 0) + { + for (i = 0; i < n; i++) + { + contact[i].surface.mode = dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = dInfinity; + contact[i].surface.slip1 = 0.001;// 0.01; + contact[i].surface.slip2 = 0.001;//0.01; + contact[i].surface.soft_erp = 0.5; + contact[i].surface.soft_cfm = 0.05; //penetration/softness + + dJointID c = dJointCreateContact(get_world(), get_contactgroup(), + &contact[i]); + dJointAttach(c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2)); + + // grass + // dBodyID obj = 0; + // if (g1 && o1 == _ground) // g2 is our object + // obj = dGeomGetBody(o2); + // else if (g2 && o2 == _ground) + // obj = dGeomGetBody(o1); + // if (obj) + // { + // dVector3 vel; + // dBodyGetPointVel(obj, + // contact[i].geom.pos[0], + // contact[i].geom.pos[1], + // contact[i].geom.pos[2], + // vel); + // dBodyAddForce(obj, -vel[0]*200, -vel[1]*200, 0); + // std::cout<<"vel:"< +** Started on Tue Aug 3 15:10:20 2004 mandor +** $Id$ +** +** Copyright (C) 2004 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef ENVIRONMENT_HH_ +# define ENVIRONMENT_HH_ + +#include +#include +#include +#include +#include "misc.hh" + +namespace ode +{ + class Object; + //singleton : only one env + class Environment + { + public: + static const float time_step = 0.05; + // constructor + Environment() : + _ground(0x0), _pitch(0), _roll(0), _z(0) + { + _init(true); + } + Environment(float angle) : + _ground(0x0), _pitch(0), _roll(0), _z(0) + { + _init(true,angle); + } + Environment(float pitch, float roll, float z) : + _ground(0x0), _pitch(pitch), _roll(roll), _z(z) + { + _init(true); + } + + Environment(bool add_ground) : + _ground(0x0), _pitch(0), _roll(0), _z(0) + { + _init(add_ground); + } + ~Environment() + { + + if (_ground) + dGeomDestroy(_ground); + dSpaceDestroy(get_space()); + dWorldDestroy(get_world()); + dJointGroupDestroy(_contactgroup); + } + //interfaces + dWorldID get_world() const + { + return _world_id; + } + dSpaceID get_space() const + { + return _space_id; + } + dGeomID get_ground() const + { + return _ground; + } + dJointGroupID get_contactgroup() const + { + return _contactgroup; + } + //update sim + void next_step(double dt = time_step) + { + //check collisions + dSpaceCollide(_space_id, (void *)this, &_near_callback); + //next step + dWorldStep(_world_id, dt); + //dWorldQuickStep(_world_id, dt); + // remove all contact joints + dJointGroupEmpty(_contactgroup); + } + void disable_gravity() + { + dWorldSetGravity(_world_id, 0, 0, 0); + } + void set_gravity(float x, float y, float z) + { + dWorldSetGravity(_world_id, x, y, z); + } + void add_to_ground(ode::Object& o); + float get_pitch() const { return _pitch; } + float get_roll() const { return _roll; } + float get_z() const { return _z; } + protected: + std::set _ground_objects; + void _init(bool add_ground,float angle=0); + static void _near_callback(void *data, dGeomID o1, dGeomID o2) + { + Environment*env = reinterpret_cast(data); + env->_collision(o1, o2); + } + virtual void _collision(dGeomID o1, dGeomID o2); + public: // ?? + // attributes + dWorldID _world_id; + dSpaceID _space_id; + dGeomID _ground; + dJointGroupID _contactgroup; + float _pitch, _roll, _z; + float angle; + }; +} + + +#endif /* !ENVIRONMENT_HH_ */ diff --git a/src/ode/environment_hexa.cc b/src/ode/environment_hexa.cc new file mode 100644 index 0000000..8951bc3 --- /dev/null +++ b/src/ode/environment_hexa.cc @@ -0,0 +1,186 @@ + +/* +** environment.cc +** Login : +** Started on Mon Jun 14 16:04:28 2010 mandor +** $Id$ +** +** Copyright (C) 2010 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include +#include + +#include "environment_hexa.hh" +#include "object.hh" + +namespace ode +{ + void Environment_hexa::add_leg_object(int leg,ode::Object& o) + { + switch(leg) + { + case 0: + _leg0_objects.insert(o.get_geom()); + break; + case 1: + _leg1_objects.insert(o.get_geom()); + break; + case 2: + _leg2_objects.insert(o.get_geom()); + break; + case 3: + _leg3_objects.insert(o.get_geom()); + break; + case 4: + _leg4_objects.insert(o.get_geom()); + break; + case 5: + _leg5_objects.insert(o.get_geom()); + break; + } + } + + void Environment_hexa::_collision(dGeomID o1, dGeomID o2) + { + + + int g1 ; + if(_ground_objects.find(o1) != _ground_objects.end()) + g1=-1; + else if(_leg0_objects.find(o1) != _leg0_objects.end()) + g1=0; + else if(_leg1_objects.find(o1) != _leg1_objects.end()) + g1=1; + else if(_leg2_objects.find(o1) != _leg2_objects.end()) + g1=2; + else if(_leg3_objects.find(o1) != _leg3_objects.end()) + g1=3; + else if(_leg4_objects.find(o1) != _leg4_objects.end()) + g1=4; + else if(_leg5_objects.find(o1) != _leg5_objects.end()) + g1=5; + else + g1=6; //main body + int g2 ; + if(_ground_objects.find(o2) != _ground_objects.end()) //ground + g2=-1; + else if(_leg0_objects.find(o2) != _leg0_objects.end()) + g2=0; + else if(_leg1_objects.find(o2) != _leg1_objects.end()) + g2=1; + else if(_leg2_objects.find(o2) != _leg2_objects.end()) + g2=2; + else if(_leg3_objects.find(o2) != _leg3_objects.end()) + g2=3; + else if(_leg4_objects.find(o2) != _leg4_objects.end()) + g2=4; + else if(_leg5_objects.find(o2) != _leg5_objects.end()) + g2=5; + else + g2=6; // main body + + //if (!(g1==-1 ^ g2==-1)) + // return; + + if(g1==g2 || (g1==6 && g2!=-1) || (g1!=-1 && g2==6)) //contact between same part of the robot (or world) + return; + + + + static const int N = 10; + int i, n; + dContact contact[N]; + n = dCollide(o1, o2, N, &contact[0].geom, sizeof(dContact)); + + /*dBodyID b = 0; //don't work anymore with collision between leg detection + if (g1 && o2) + b = dGeomGetBody(o2); + else if (o1) + b = dGeomGetBody(o1); + if (b) + { + Object*o = (Object *)dBodyGetData(b); + if (dBodyGetData(b)) + o->set_in_contact(true); + }*/ + if (n > 0) + { + + if(g1!=-1 && g2!=-1) //colision between legs + { + //std::cout<<"colision "<set_in_contact(true); + + for (i = 0; i < n; i++) + { + contact[i].surface.mode = dContactSlip1 | dContactSlip2 | + dContactSoftERP | dContactSoftCFM | dContactApprox1; + contact[i].surface.mu = 0.8;//dInfinity; + contact[i].surface.slip1 = 0.01;// 0.01; + contact[i].surface.slip2 = 0.01;//0.01; + contact[i].surface.soft_erp = 0.1; + contact[i].surface.soft_cfm = 0.001; //penetration/softness + + + + dJointID c = dJointCreateContact(get_world(), get_contactgroup(), + &contact[i]); + dJointAttach(c, + dGeomGetBody(contact[i].geom.g1), + dGeomGetBody(contact[i].geom.g2)); + + // grass + // dBodyID obj = 0; + // if (g1 && o1 == _ground) // g2 is our object + // obj = dGeomGetBody(o2); + // else if (g2 && o2 == _ground) + // obj = dGeomGetBody(o1); + // if (obj) + // { + // dVector3 vel; + // dBodyGetPointVel(obj, + // contact[i].geom.pos[0], + // contact[i].geom.pos[1], + // contact[i].geom.pos[2], + // vel); + // dBodyAddForce(obj, -vel[0]*200, -vel[1]*200, 0); + // std::cout<<"vel:"< +** Started on Tue Aug 3 15:10:20 2004 mandor +** $Id$ +** +** Copyright (C) 2004 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef ENVIRONMENT_HEXA_HH_ +# define ENVIRONMENT_HEXA_HH_ + +#include +#include +#include +#include +#include "misc.hh" +#include "environment.hh" +namespace ode +{ + + + class Environment_hexa: public Environment + { + public: + Environment_hexa() : + _colision_between_legs(false) + { + _init(true); + } + Environment_hexa(float angle) : + _colision_between_legs(false) + { + _init(true,angle); + } + + Environment_hexa(const Environment_hexa& env) : + _colision_between_legs(false) + { + _init(true,env.angle); + _leg0_objects=env._leg0_objects; + _leg1_objects=env._leg1_objects; + _leg2_objects=env._leg2_objects; + _leg3_objects=env._leg3_objects; + _leg4_objects=env._leg4_objects; + _leg5_objects=env._leg5_objects; + + } + + + + void add_leg_object(int leg,ode::Object& o); + bool get_colision_between_legs(){return _colision_between_legs;} + protected: + void _collision(dGeomID o1, dGeomID o2); + std::set _leg0_objects; + std::set _leg1_objects; + std::set _leg2_objects; + std::set _leg3_objects; + std::set _leg4_objects; + std::set _leg5_objects; + bool _colision_between_legs ; + + + + + }; +} + + +#endif /* !ENVIRONMENT_HEXA_HH_ */ diff --git a/src/ode/misc.hh b/src/ode/misc.hh new file mode 100644 index 0000000..c5a612a --- /dev/null +++ b/src/ode/misc.hh @@ -0,0 +1,66 @@ +/* +** misc.hh +** Login : +** Started on Fri Jan 6 13:49:55 2006 Jean-baptiste MOURET +** $Id$ +** +** Copyright (C) 2006 Jean-baptiste MOURET +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef ODE_MISC_HH_ +# define ODE_MISC_HH_ +#include +#include + +namespace cst +{ + static const float rho = 1.225; + static const float g = 9.80665; + +} + +namespace ode +{ + /// from http://www.ode.org/ode-latest-userguide.html + /// ODE format : + /// store the matrix by rows, and each row is rounded up to + /// a multiple of 4 elements. The extra "padding" elements at the + /// end of each row/column must be set to 0. + inline Eigen::Matrix3d ode_to_matrixf(const dReal* dm) + { + assert(dm); + Eigen::Matrix3d mat(3, 3); + mat(0, 0) = dm[0]; + mat(0, 1) = dm[1]; + mat(0, 2) = dm[2]; + mat(1, 0) = dm[4]; + mat(1, 1) = dm[5]; + mat(1, 2) = dm[6]; + mat(2, 0) = dm[8]; + mat(2, 1) = dm[9]; + mat(2, 2) = dm[10]; + return mat; + } + + inline Eigen::Vector3d ode_to_vectorf(const dReal* v) + { + assert(v); + return Eigen::Vector3d(v[0], v[1], v[2]); + } +} + + +#endif /* !ODE_MISC_HH_ */ diff --git a/src/ode/motor.cc b/src/ode/motor.cc new file mode 100644 index 0000000..9e99277 --- /dev/null +++ b/src/ode/motor.cc @@ -0,0 +1,80 @@ +/* +** motor.cc +** +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include +#include "motor.hh" +namespace ode +{ + void Motor :: _init() + { + + _hinge = dJointCreateHinge(_env.get_world(), 0); + dJointAttach(_hinge, _o1.get_body(), _o2.get_body()); + dJointSetHingeAnchor(_hinge, _anchor.x(), _anchor.y(), _anchor.z()); + dJointSetHingeAxis(_hinge, _axis.x(), _axis.y(), _axis.z()); + // inifinite torque ? + dJointSetHingeParam (_hinge, dParamFMax, dInfinity); + dJointSetHingeParam (_hinge, dParamLoStop, -dInfinity); + dJointSetHingeParam (_hinge, dParamHiStop, dInfinity); + + _vel = 0; + + //_o2.add_servo(this);//TODO : add add_motor() function in object ? + //_o1.add_servo2(this); + } + Motor :: Motor(const Motor& o, Environment& env, Object& o1, Object& o2) : + _env(env), + _anchor(o._anchor), + _axis(o._axis), + _o1(o1), _o2(o2), + _vel(0), + _passive(o._passive), + _power(0), + _torque(0), + _efficiency(o._efficiency), + _pos(0) + { + _hinge = dJointCreateHinge(_env.get_world(), 0); + dJointAttach(_hinge, _o1.get_body(), _o2.get_body()); + dVector3 anchor; + dJointGetHingeAnchor(o._hinge, anchor); + _anchor = Eigen::Vector3d(anchor[0], anchor[1], anchor[2]); + dJointSetHingeAnchor(_hinge, _anchor.x(), _anchor.y(), _anchor.z()); + + dVector3 axis; + dJointGetHingeAxis(o._hinge, axis); + _axis = Eigen::Vector3d(axis[0], axis[1], axis[2]); + dJointSetHingeAxis(_hinge, _axis.x(), _axis.y(), _axis.z()); + + // inifinite torque ? + dJointSetHingeParam (_hinge, dParamFMax, dInfinity); + dJointSetHingeParam (_hinge, dParamLoStop, -dInfinity); + dJointSetHingeParam (_hinge, dParamHiStop, dInfinity); + + + _vel = 0; + + } + + void Motor :: next_step(float dt) + { + dJointSetHingeParam (_hinge, dParamVel, _vel * _efficiency); + _pos += _vel * dt; + } + +} diff --git a/src/ode/motor.hh b/src/ode/motor.hh new file mode 100644 index 0000000..b836b3f --- /dev/null +++ b/src/ode/motor.hh @@ -0,0 +1,117 @@ +/* +** motor.hh +** +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef MOTOR_HH_ +# define MOTOR_HH_ +#include +#include +#include "object.hh" + +namespace ode +{ + class Motor + { + public: + typedef boost::shared_ptr ptr_t; + Motor(Environment& env, + const Eigen::Vector3d& anchor, + const Eigen::Vector3d& axis, + Object& o1, Object& o2) : + _env(env), + _anchor(anchor), + _axis(axis), + _o1(o1), _o2(o2), + _vel(0), + _passive(false), + _power(0), + _torque(0), + _efficiency(1), + _pos(0) + { _init(); } + + ~Motor() + { + dJointDestroy(_hinge); + } + Motor(const Motor& s, Environment& env, Object& o1, Object& o2); + ptr_t clone(Environment& env, Object& o1, Object &o2) const + { return ptr_t(new Motor(*this, env, o1, o2)); } + + void next_step(float dt); + + /// desired angular velocity + void set_vel(float v) { _vel = v; } + + /// real angular velocity + float get_vel() const { return dJointGetHingeAngleRate(_hinge); } + + // effiency : real_velocity = efficiency * asked_vel + float get_efficiency() const { return _efficiency; } + void set_efficiency(float v) { _efficiency = v; } + + const Eigen::Vector3d& get_anchor() const { return _anchor; } + const Eigen::Vector3d& get_axis() const { return _axis; } + + void set_anchor(Eigen::Vector3d anchor) + { + _anchor = anchor; + dJointSetHingeAnchor(_hinge, _anchor.x(), _anchor.y(), _anchor.z()); + } + + void set_axis(Eigen::Vector3d axis) + { + _axis = axis; + dJointSetHingeAxis(_hinge, _axis.x(), _axis.y(), _axis.z()); + } + + void set_passive() + { + _passive = false; + dJointSetHingeParam (_hinge, dParamFMax, 0); + } + + const Object& get_o1() const { return _o1; } + const Object& get_o2() const { return _o2; } + float get_power() const { return _power; } + float get_torque() const { return _torque; } + // emulate an encoder, takes into account the "slippage" (via + // effiency) + // -> pos != real pos if efficiency != 1 + double get_pos() const { return _pos; } + + protected: + void _init(); + + // attributes + Environment& _env; + Eigen::Vector3d _anchor; + Eigen::Vector3d _axis; + Object& _o1; + Object& _o2; + dJointID _hinge; + float _vel; //desired angular velocity + bool _passive; + float _power; + float _torque; + float _efficiency; + double _pos; + }; +} + + +#endif /* !MOTOR_HH_ */ diff --git a/src/ode/mx28.hh b/src/ode/mx28.hh new file mode 100644 index 0000000..df0aad1 --- /dev/null +++ b/src/ode/mx28.hh @@ -0,0 +1,231 @@ +#ifndef MX28_HH_ +# define MX28_HH_ + +#include +#include +#include "object.hh" + +#include "servo.hh" + +namespace ode +{ + class Mx28 : public Servo + { + public: + typedef boost::shared_ptr ptr_t; + static const float angular_vel = 6.28; //45rpm + Mx28(Environment& env, + const Eigen::Vector3d& anchor, + Object& o1, Object& o2, + int mode = M_POS) : + Servo(env, anchor, o1, o2, mode,false) + { + _init_mx28(); + } + Mx28(const Mx28& s, Environment& env, Object& o1, Object& o2) : + Servo(s, env, o1, o2,false) + { + + _ball = dJointCreateHinge(_env.get_world(), 0); + dJointAttach(_ball, _o1.get_body(), _o2.get_body()); + //Eigen::Vector3d anchor=s.get_anchor(); + dVector3 anchor; + dJointGetHingeAnchor(s._ball, anchor); + _anchor = Eigen::Vector3d(anchor[0], anchor[1], anchor[2]); + dJointSetHingeAnchor(_ball, _anchor.x(), _anchor.y(), _anchor.z()); + + _amotor = dJointCreateAMotor(_env.get_world(), 0); + dJointAttach(_amotor, _o1.get_body(), _o2.get_body()); + + dJointSetAMotorNumAxes(_amotor, 1); + dVector3 axis; + + dJointGetHingeAxis(s._ball, axis); + dJointSetHingeAxis(_ball, axis[0], axis[1], axis[2]); + + dJointGetAMotorAxis(s._amotor, 0, axis); + dJointSetAMotorAxis(_amotor, 0, 1, axis[0], axis[1], axis[2]); + dJointGetAMotorAxis(s._amotor, 2, axis); + dJointSetAMotorAxis(_amotor, 2, 2, axis[0], axis[1], axis[2]); + // euler + dJointSetAMotorMode(_amotor, dAMotorEuler); + + + + /* _lim_min = Eigen::Vector3d::Constant(-5 * M_PI / 6.0f); + _lim_max = Eigen::Vector3d::Constant(5 * M_PI / 6.0f);*/ + dJointSetAMotorParam(_amotor, dParamLoStop, _lim_min[0]); + dJointSetAMotorParam(_amotor, dParamHiStop, _lim_max[0]); + dJointSetAMotorParam(_amotor, dParamLoStop2, _lim_min[1]); + dJointSetAMotorParam(_amotor, dParamHiStop2, _lim_max[1]); + dJointSetAMotorParam(_amotor, dParamLoStop3, _lim_min[2]); + dJointSetAMotorParam(_amotor, dParamHiStop3, _lim_max[2]); + + static const double fmax = 1.5; + dJointSetAMotorParam(_amotor, dParamFMax, fmax); + dJointSetAMotorParam(_amotor, dParamFMax2, fmax); + dJointSetAMotorParam(_amotor, dParamFMax3, fmax); + + //vels + dJointSetAMotorParam(_amotor, dParamVel, 0); + dJointSetAMotorParam(_amotor, dParamVel2, 0); + dJointSetAMotorParam(_amotor, dParamVel3, 0); + + for (size_t i = 0; i < 3; ++i) + { + float a = dJointGetAMotorAngle(s._amotor, i); + _offset[i] += a; + } + + // _angles(maths::_(0, _angles.size() - 1)) = 0; ?? + _angles = Eigen::Vector3d::Zero(); + + _o2.add_servo(this); + _o1.add_servo2(this); + + dJointSetFeedback(_amotor, &_feedback); + + + + + } + + boost::shared_ptr clone(Environment& env, Object& o1, Object &o2) const + { + return ptr_t(new Mx28(*this, env, o1, o2)); + } + void set_lim(unsigned i, float min, float max) + { + assert(i < 3); + _lim_min[i] = min; + _lim_max[i] = max; + dJointSetAMotorParam(_amotor, dParamLoStop, _lim_min[0]); + dJointSetAMotorParam(_amotor, dParamHiStop, _lim_max[0]); + dJointSetAMotorParam(_amotor, dParamLoStop2, _lim_min[1]); + dJointSetAMotorParam(_amotor, dParamHiStop2, _lim_max[1]); + dJointSetAMotorParam(_amotor, dParamLoStop3, _lim_min[2]); + dJointSetAMotorParam(_amotor, dParamHiStop3, _lim_max[2]); + + } + void set_axis(size_t a, const Eigen::VectorXd& ax) + { + if(a!=0) + return; + dJointSetAMotorAxis(_amotor, 0, 1, ax.x(), ax.y(), ax.z()); + if(ax.x()!=0 ||ax.y()!=0) + dJointSetAMotorAxis(_amotor, 2, 1, -ax.y(), ax.x(), 0); + else + dJointSetAMotorAxis(_amotor, 2, 1, ax.z(), 0, 0); + dVector3 axis; + dJointGetAMotorAxis(_amotor,0,axis); + dJointSetHingeAxis(_ball, axis[0], axis[1], axis[2]); + + } + /* void set_hinge_axis( const Eigen::VectorXd& ax) + { + dJointSetHingeAxis(_ball, ax.x(), ax.y(), ax.z()); + }*/ + void set_angle(unsigned i, float v) + { + // std::cout<angular_vel) + { + //std::cout<<"max vel"<-1e-5) + { + vel=0; + } + // std::cout< +** Started on Tue Jun 15 21:10:07 2010 mandor +** $Id$ +** +** Copyright (C) 2010 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include +#include "object.hh" + +namespace ode +{ + + //x-y-z convention + // http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles + // see also : http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler + // ode : [ w, x, y, z ], where w is the real part and (x, y, z) form the vector part. + Eigen::Vector3d Object::get_rot() const + { + const dReal* q = dBodyGetQuaternion(get_body()); + double + phi = 0, // bank + theta = 0, // attitude + psi = 0; // heading + double k = q[1] * q[2] + q[3] * q[0]; + + if (fabs(k - 0.5) < 1e-5) // north pole + { + psi = 2 * atan2(q[1], q[0]); + theta = M_PI / 2.0; + phi = 0; + } + else if ((k + 0.5) < 1e-5)// south pole + { + psi = -2 * atan2(q[1], q[0]); + theta = -M_PI / 2.0; + phi = 0; + } + else + { + phi = atan2(2 * (q[0] * q[1] + q[2] * q[3]), + 1 - 2 * (q[1] * q[1] + q[2] * q[2])); + theta = asin(2 * (q[0] * q[2] -q[3] * q[1])); + psi = atan2(2 * (q[0] * q[3] + q[1] * q[2]), + 1 - 2 * (q[2] * q[2] + q[3] * q[3])); + } + return Eigen::Vector3d(phi, theta, psi); + } +} diff --git a/src/ode/object.hh b/src/ode/object.hh new file mode 100644 index 0000000..21907aa --- /dev/null +++ b/src/ode/object.hh @@ -0,0 +1,219 @@ +/* +** object.hh +** Login : +** Started on Thu Nov 25 22:45:43 2004 mandor +** $Id$ +** +** Copyright (C) 2004 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef OBJECT_HH_ +# define OBJECT_HH_ + +#include +#include +#include +#include +#include + +#include "environment.hh" +#include "visitor.hh" + +#include +#include + + + +namespace ode +{ + static const float density = 1.0f; + class Servo; + + /// abstract class to represent a rigid body + class Object + { + public: + typedef boost::shared_ptr ptr_t; + Object(Environment & env, const Eigen::Vector3d & pos) : + _body(0x0), + _geom(0x0), + _init_pos(pos), + _env(env), + _servo(0x0), + _servo2(0x0), + _fix(0x0), + _bad_state(false), + _in_contact(false) + { + } + + virtual ~Object() + { + if (_body) + dBodyDestroy(_body); + if (_geom) + dGeomDestroy(_geom); + } + virtual ptr_t clone(Environment& env) const = 0; + + virtual dBodyID get_body() const { return _body; } + virtual dGeomID get_geom() const { return _geom; } + virtual float get_mass() const { return _m.mass; } + /// ask the current position to ode + /// Cf ode documentation for details + Eigen::Vector3d get_pos() const { return _get_ode_pos(); } + Eigen::Vector3d get_rot() const; + Eigen::Vector3d get_vel() const { return get_vground(); } + /// set an absolute rotation (euler angles) + void set_rotation(float phi, float theta, float psi) + { + dMatrix3 r; + dRFromEulerAngles(r, phi, theta, psi); + dBodySetRotation(get_body(), r); + } + /// set an absolute rotation (axis and angle) + void set_rotation(float ax, float ay, float az, float angle) + { + dMatrix3 r; + dRFromAxisAndAngle(r, ax, ay, az, angle); + dBodySetRotation(get_body(), r); + } + void set_rotation(const Eigen::Vector3d& a1, const Eigen::Vector3d &a2) + { + dMatrix3 r; + dRFrom2Axes (r, a1.x(), a1.y(), a1.z(), a2.x(), a2.y(), a2.z()); + dBodySetRotation(get_body(), r); + } + /// const visitor, useful for example for a 3d renderer + virtual void accept(ConstVisitor& v) const = 0; + + /// connect a servo. Used by Panels (called by the constructor) to + /// change their shape according to their sweep angle + void add_servo(Servo*servo) { _servo = servo; } + void add_servo2(Servo*servo2) { _servo2 = servo2; } + + const Servo& get_servo(void) const { assert(_servo); return *_servo; } + const Servo& get_servo2(void) const { assert(_servo2); return *_servo2; } + + /// true in case of simulation errors (nan, ...) + bool get_bad_state() const { return _bad_state;} + + + /// ask to ode the speed of a given point in the object + /// the given point must be in world + /// the result is in world coordinate + Eigen::Vector3d get_vground(const Eigen::Vector3d& v) const + { + dReal res[3]; + dBodyGetPointVel(get_body(), v.x(), v.y(), v.z(), res); + return ode_to_vectorf(res); + } + /// get_vground() at CG + Eigen::Vector3d get_vground() const + { + dReal res[3]; + const dReal*pos = dBodyGetPosition(get_body()); + dBodyGetPointVel(get_body(), pos[0], pos[1], pos[2], res); + return ode_to_vectorf(res); + } + + + ///attach the object to the static environment + void fix() + { + if (_fix) + return; + fix_along_axis(Eigen::Vector3d(0, 0, 1)); + dJointSetSliderParam(_fix, dParamLoStop, 0); + dJointSetSliderParam(_fix, dParamHiStop, 0); + } + /// detach the object + void unfix() + { + if (!_fix) + return; + dJointDestroy(_fix); + _fix = 0x0; + } + + /// attach to the static environment along the axis 'axis' + /// (the object will be able to move only along this axis) + void fix_along_axis(const Eigen::Vector3d& axis) + { + if (_fix) + return; + _fix = dJointCreateSlider(_env.get_world(), 0); + dJointAttach(_fix, _body, 0); + dJointSetSliderAxis(_fix, axis[0], axis[1], axis[2]); + } + + /// true if fixed + bool get_fix() const { return _fix == 0; } + const Environment& get_env() const { return _env; } + void set_in_contact(bool b) { _in_contact = b; } + bool get_in_contact() const { return _in_contact; } + protected: + void init() + { + _body = dBodyCreate(_env.get_world()); + dBodySetPosition(_body, + _init_pos.x(), + _init_pos.y(), + _init_pos.z()); + dBodySetData(_body, this); + } + // does not copy servos (they must be copied later) + void _copy(const Object& o) + { + _body = dBodyCreate(_env.get_world()); + // + dQuaternion quat; + dBodyCopyQuaternion(o._body, quat); + dVector3 pos; + dBodyCopyPosition(o._body, pos); + // + dBodySetPosition(_body, pos[0], pos[1], pos[2]); + dBodySetQuaternion(_body, quat); + // + dBodyGetMass(o._body, &_m); + dBodySetMass(_body, &_m); + // + _fix = o._fix; + _init_pos = o.get_pos(); + // + dBodySetData(_body, this); + } + Eigen::Vector3d _get_ode_pos() const + { + assert(get_body()); + const dReal*pos; + pos = dBodyGetPosition(get_body()); + return Eigen::Vector3d(pos[0], pos[1], pos[2]); + } + dMass _m; + dBodyID _body; + dGeomID _geom; + Eigen::Vector3d _init_pos; + Environment& _env; + Servo*_servo; + Servo*_servo2; + dJointID _fix; + bool _bad_state; + bool _in_contact; + }; +} + +#endif /* !OBJECT_HH_ */ diff --git a/src/ode/servo.cc b/src/ode/servo.cc new file mode 100644 index 0000000..e2d9559 --- /dev/null +++ b/src/ode/servo.cc @@ -0,0 +1,203 @@ +/* +** servo.cc +** Login : +** Started on Fri Sep 8 19:48:06 2006 Jeanbaptiste MOURET +** $Id$ +** +** Copyright (C) 2006 Jeanbaptiste MOURET +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#include +#include +#include "servo.hh" +namespace ode +{ + + void Servo :: _init() + { + _ball = dJointCreateBall(_env.get_world(), 0); + dJointAttach(_ball, _o1.get_body(), _o2.get_body()); + dJointSetBallAnchor(_ball, _anchor.x(), _anchor.y(), _anchor.z()); + + _amotor = dJointCreateAMotor(_env.get_world(), 0); + dJointAttach(_amotor, _o1.get_body(), _o2.get_body()); + //axis + dJointSetAMotorAxis(_amotor, 0, 1, 1, 0, 0); + dJointSetAMotorAxis(_amotor, 2, 2, 0, 1, 0); + // euler + dJointSetAMotorMode(_amotor, dAMotorEuler); + + //stops + dJointSetAMotorParam(_amotor, dParamLoStop, _lim_min[0]); + dJointSetAMotorParam(_amotor, dParamHiStop, _lim_max[0]); + dJointSetAMotorParam(_amotor, dParamLoStop2, _lim_min[1]); + dJointSetAMotorParam(_amotor, dParamHiStop2, _lim_max[1]); + dJointSetAMotorParam(_amotor, dParamLoStop3, _lim_min[2]); + dJointSetAMotorParam(_amotor, dParamHiStop3, _lim_max[2]); + + //fmax + dJointSetAMotorParam(_amotor, dParamFMax, DEFAULT_FMAX); + dJointSetAMotorParam(_amotor, dParamFMax2, DEFAULT_FMAX); + dJointSetAMotorParam(_amotor, dParamFMax3, DEFAULT_FMAX); + //vels + dJointSetAMotorParam(_amotor, dParamVel, 0); + dJointSetAMotorParam(_amotor, dParamVel2, 0); + dJointSetAMotorParam(_amotor, dParamVel3, 0); + + // _angles(maths::_(0, _angles.size() - 1)) = 0; ?? + _angles = Eigen::Vector3d::Zero(); + + _o2.add_servo(this); + _o1.add_servo2(this); + + dJointSetFeedback(_amotor, &_feedback); + } + + Servo :: Servo(const Servo& o, Environment& env, Object& o1, Object& o2,bool init) : + _env(env), + _anchor(o._anchor), + _o1(o1), _o2(o2), + _angles(o._angles), + _passive(o._passive), + _power(0), + _torque(0), + _vrot(0), + _mode(o._mode), + _vel(o._vel), + _lim_min(o._lim_min), + _lim_max(o._lim_max), + _blocked(o._blocked), + _p(o._p), + _offset(o._offset) + { + if(!init) + return; + _ball = dJointCreateBall(_env.get_world(), 0); + dJointAttach(_ball, _o1.get_body(), _o2.get_body()); + + + dVector3 anchor; + dJointGetBallAnchor(o._ball, anchor); + _anchor = Eigen::Vector3d(anchor[0], anchor[1], anchor[2]); + dJointSetBallAnchor(_ball, _anchor.x(), _anchor.y(), _anchor.z()); + + _amotor = dJointCreateAMotor(_env.get_world(), 0); + dJointAttach(_amotor, _o1.get_body(), _o2.get_body()); + //axis + dVector3 axis; + dJointGetAMotorAxis(o._amotor, 0, axis); + dJointSetAMotorAxis(_amotor, 0, 1, axis[0], axis[1], axis[2]); + dJointGetAMotorAxis(o._amotor, 2, axis); + dJointSetAMotorAxis(_amotor, 2, 2, axis[0], axis[1], axis[2]); + // euler + dJointSetAMotorMode(_amotor, dAMotorEuler); + + //stops + dJointSetAMotorParam(_amotor, dParamLoStop, _lim_min[0]); + dJointSetAMotorParam(_amotor, dParamHiStop, _lim_max[0]); + dJointSetAMotorParam(_amotor, dParamLoStop2, _lim_min[1]); + dJointSetAMotorParam(_amotor, dParamHiStop2, _lim_max[1]); + dJointSetAMotorParam(_amotor, dParamLoStop3, _lim_min[2]); + dJointSetAMotorParam(_amotor, dParamHiStop3, _lim_max[2]); + + //fmax + dJointSetAMotorParam(_amotor, dParamFMax, DEFAULT_FMAX); + dJointSetAMotorParam(_amotor, dParamFMax2, DEFAULT_FMAX); + dJointSetAMotorParam(_amotor, dParamFMax3, DEFAULT_FMAX); + //vels + dJointSetAMotorParam(_amotor, dParamVel, 0); + dJointSetAMotorParam(_amotor, dParamVel2, 0); + dJointSetAMotorParam(_amotor, dParamVel3, 0); + + for (size_t i = 0; i < 3; ++i) + { + float a = dJointGetAMotorAngle(o._amotor, i); + _offset[i] += a; + } + + // _angles(maths::_(0, _angles.size() - 1)) = 0; ?? + _angles = Eigen::Vector3d::Zero(); + + _o2.add_servo(this); + _o1.add_servo2(this); + + dJointSetFeedback(_amotor, &_feedback); + } + + void Servo :: _asserv(unsigned i, float dt) + { + //std::cout<<"\nServo::_asserv dt ="< 1)//cap velocity + // vel = 1; + // if (vel < -1) + // vel = -1; + dJointSetAMotorParam(_amotor, _vel_selector(i), vel); + } + + void Servo :: next_step(float dt) + { + if (!_blocked) + { + if (_mode == M_POS) + for (unsigned i = 0; i < _angles.size(); ++i) + _asserv(i, dt); + else + for (unsigned i = 0; i < _vel.size(); ++i) + if ((_vel(i) < 0 && get_angle(i) > _lim_min(i)) || + (_vel(i) > 0 && get_angle(i) < _lim_max(i))) + dJointSetAMotorParam(_amotor, _vel_selector(i), _vel(i)); + else + dJointSetAMotorParam(_amotor, _vel_selector(i), 0); + } + + //probably useless + dJointGetFeedback(_amotor); + const dReal*r1 = dBodyGetAngularVel(_o1.get_body()); + Eigen::Vector3d v1(r1[0], r1[1], r1[2]); + const dReal*r2 = dBodyGetAngularVel(_o2.get_body()); + Eigen::Vector3d v2(r2[0], r2[1], r2[2]); + /*dVector3 p; + dJointGetBallAnchor(_ball, p); + Eigen::Vector3d pos(p[0], p[1], p[2]); + Eigen::Vector3d v = v1 - v2; + + //get torques + Eigen::Vector3d t1(_feedback.t1[0], _feedback.t1[1], _feedback.t1[2]); + Eigen::Vector3d t2(_feedback.t2[0], _feedback.t2[1], _feedback.t2[2]); + //forces + Eigen::Vector3d f1(_feedback.f1[0], _feedback.f1[1], _feedback.f1[2]); + Eigen::Vector3d f2(_feedback.f2[0], _feedback.f2[1], _feedback.f2[2]); + + t1 = t1 + (f1.cross(pos - _o1.get_pos())); + t2 = t2 + (f2.cross(pos - _o2.get_pos())); + + // plot torque = f(t) (approximation : projected on global x axis + Eigen::Vector3d xaxis(1, 0, 0); + _torque = t1.dot(xaxis); + + // plot vrot = f(t) projected on global x axis + _vrot = v.dot(xaxis); + + // real power computation + _power = fabs(t1.dot(v)); //both positive and negative work added ...? ... + //_power = t1 * v; // essai decompte travail négatif*/ + } + +} diff --git a/src/ode/servo.hh b/src/ode/servo.hh new file mode 100644 index 0000000..f063221 --- /dev/null +++ b/src/ode/servo.hh @@ -0,0 +1,169 @@ +#ifndef SERVO_HH_ +# define SERVO_HH_ + +#include +#include +#include "object.hh" + +#define DEFAULT_STOP (1 * M_PI / 2.1f) + //#define DEFAULT_STOP (dInfinity) +#define DEFAULT_FMAX (1500) + +namespace ode +{ + class Servo + { + public: + typedef boost::shared_ptr ptr_t; + enum { DIHEDRAL = 0, SWEEP = 1, TWIST = 2 }; + enum { M_POS = 0, M_VEL }; + /// Object order can matter ! + /// (object1 is the fixed one, object2 the moving one => object2 + /// has the pointer to the servo) + Servo(Environment & env, + const Eigen::Vector3d & anchor, + Object & o1, Object & o2, + int mode = M_POS,bool init=true) : + _env(env), + _anchor(anchor), + _o1(o1), _o2(o2), + _angles(Eigen::Vector3d::Zero()), + _passive(false), + _power(0), + _torque(0), + _vrot(0), + _mode(mode), + _vel(Eigen::Vector3d::Zero()), + _lim_min(Eigen::Vector3d::Constant(-DEFAULT_STOP)), + _lim_max(Eigen::Vector3d::Constant(DEFAULT_STOP)), + _blocked(false), + _p(1.0), + _offset(Eigen::Vector3d::Zero()) + { + if( init) + _init(); + } + + ~Servo() + { + dJointDestroy(_ball); + dJointDestroy(_amotor); + } + Servo(const Servo &s, Environment & env, Object & o1, Object & o2,bool init=true); + virtual ptr_t clone(Environment& env, Object& o1, Object& o2) const + { return ptr_t(new Servo(*this, env, o1, o2)); } + void next_step(float dt); + /// desired angle + virtual void set_angle(unsigned i, float v) + { + assert(_mode == M_POS); + v = std::min((double)v, DEFAULT_STOP); + v = std::max((double)v, -DEFAULT_STOP); + _angles(i) = v; + } + void set_vel(unsigned i, float v) + { + assert(_mode == M_VEL); + _vel(i) = v; + } + virtual void set_lim(unsigned i, float min, float max) + { + assert(i < 3); + _lim_min[i] = min; + _lim_max[i] = max; + } + void set_offset(const Eigen::Vector3d& offset) { _offset = offset; } + const Eigen::Vector3d& get_offset() const { return _offset; } + void set_mode(int m) + { + assert(m == M_POS || m == M_VEL); + _mode = m; + } + /// real angle + offset + float get_angle(unsigned i) const + { + return dJointGetAMotorAngle(_amotor, i) + _offset[i]; + } + const Eigen::Vector3d& get_anchor() const + { + return _anchor; + } + void set_anchor(Eigen::Vector3d anchor) + { + _anchor = anchor; + dJointSetBallAnchor(_ball, _anchor.x(), _anchor.y(), _anchor.z()); + } + virtual void set_passive() + { + _passive = false; + dJointSetAMotorParam(_amotor, dParamFMax, 0); + dJointSetAMotorParam(_amotor, dParamFMax2, 0); + dJointSetAMotorParam(_amotor, dParamFMax3, 0); + } + virtual void set_axis(size_t a, const Eigen::VectorXd& ax) + { + + dJointSetAMotorAxis(_amotor, a, 1, ax.x(), ax.y(), ax.z()); + } + void set_blocked(bool b) { _blocked = b; } + bool get_blocked() const { return _blocked; } + void set_p(float p) { _p = p; } + const Object& get_o1() const { return _o1; } + const Object& get_o2() const { return _o2; } + float get_power() const { return _power; } + float get_torque() const { return _torque; } + float get_vrot() const { return _vrot; } + protected: + void _init(); + + // http://ode.org/cgi-bin/wiki.pl?ServoSimulation + // simple P loop + virtual void _asserv(unsigned i, float dt); + void _forced_movement(unsigned i) + { + dJointSetAMotorParam(_amotor, dParamLoStop, _angles(0)); + dJointSetAMotorParam(_amotor, dParamHiStop, _angles(0)); + dJointSetAMotorParam(_amotor, dParamLoStop2, _angles(1)); + dJointSetAMotorParam(_amotor, dParamHiStop2, _angles(1)); + dJointSetAMotorParam(_amotor, dParamLoStop3, _angles(2)); + dJointSetAMotorParam(_amotor, dParamHiStop3, _angles(2)); + } + int _vel_selector(int i) + { + switch (i) + { + case 0: + return dParamVel; + case 1: + return dParamVel2; + case 2: + return dParamVel3; + default: + assert(0); + } + return dParamVel; + } + // attributes + Environment& _env; + Eigen::Vector3d _anchor; + Object& _o1; + Object& _o2; + dJointID _ball; + dJointID _amotor; + Eigen::Vector3d _angles; + bool _passive; + dJointFeedback _feedback; + float _power; + float _torque; + float _vrot; + int _mode; + Eigen::Vector3d _vel; + Eigen::Vector3d _lim_min, _lim_max; + bool _blocked; + float _p; + Eigen::Vector3d _offset; + }; +} + + +#endif /* !SERVO_HH_ */ diff --git a/src/ode/sphere.hh b/src/ode/sphere.hh new file mode 100644 index 0000000..5e95529 --- /dev/null +++ b/src/ode/sphere.hh @@ -0,0 +1,90 @@ +/* +** sphere.hh +** Login : +** Started on Thu Nov 25 22:50:53 2004 mandor +** $Id$ +** +** Copyright (C) 2004 mandor +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef SPHERE_HH_ +# define SPHERE_HH_ + +#include +#include "object.hh" + +namespace ode +{ + /// a simple wrapper around the ODE's sphere class + class Sphere : public Object + { + public: + static const float standard_mass = 1; + Sphere(Environment& env, + const Eigen::Vector3d& pos, + float mass, + float rad) : + Object(env, pos), + _rad(rad), _mass(mass) + { + init(); + } + Sphere(const Sphere& o, Environment& env) : + Object(env, o.get_pos()), + _rad(o._rad), + _mass(o._mass) + { + _copy(o); + _geom = dCreateSphere(_env.get_space(), _rad); + dGeomSetBody(_geom, _body); + } + + virtual Object::ptr_t clone(Environment& env) const + { return Object::ptr_t(new Sphere(*this, env)); } + + float get_rad() const { return _rad; } + + void init_again() + { + if (_body) + dBodyDestroy(_body); + if (_geom) + dGeomDestroy(_geom); + init(); + } + + /// const visitor + virtual void accept (ConstVisitor &v) const + { + assert(_body); assert(_geom); + v.visit(*this); + } + protected: + void init() + { + Object::init(); + dMassSetSphereTotal(&_m, _mass, _rad); + dBodySetMass(_body, &_m); + _geom = dCreateSphere(_env.get_space(), _rad); + dGeomSetBody(_geom, _body); + } + float _rad; + float _mass; + }; +} + + +#endif /* !SPHERE_HH_ */ diff --git a/src/ode/visitor.hh b/src/ode/visitor.hh new file mode 100644 index 0000000..91c70bf --- /dev/null +++ b/src/ode/visitor.hh @@ -0,0 +1,91 @@ +/* +** visitor.hh +** Login : +** Started on Wed Jan 4 14:53:53 2006 Jean-baptiste MOURET +** $Id$ +** +** Copyright (C) 2006 Jean-baptiste MOURET +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef VISITOR_HH_ +# define VISITOR_HH_ +#include +#include +#include + +namespace traits +{ + + /// const selectors + template + struct constify_traits + { + typedef const T type; + }; + + template + struct id_traits + { + typedef T type; + }; + + //select iterator + template + struct select_iterator + { + typedef typename T::iterator type; + }; + + template + struct select_iterator + { + typedef typename T::const_iterator type; + }; +} //namespace traits + + +//forward declaration +namespace ode +{ + class Box; + class Object; + class CappedCyl; + class Sphere; +} + +/// abstract visitor +/// check the book "design patterns" from Gamma et al for general +/// explanation about the visitor pattern +/// The template trick allows to declare const and non-const visitor +/// at the same time. +namespace ode +{ + template < template class Const > + class GenVisitor + { + public: + virtual void visit(typename Const::type& e) {}; + virtual void visit(typename Const::type& e) {} + virtual void visit(typename Const::type& e) {} + typedef boost::shared_ptr obj_ptr_t; + virtual void visit(typename Const >::type& l) {} + virtual ~GenVisitor () {} + }; + + typedef GenVisitor ConstVisitor; + typedef GenVisitor Visitor; +} +#endif /* !VISITOR_HH_ */ diff --git a/src/renderer/keyboard.cc b/src/renderer/keyboard.cc new file mode 100644 index 0000000..230568a --- /dev/null +++ b/src/renderer/keyboard.cc @@ -0,0 +1,86 @@ +#include "keyboard.hh" + +namespace renderer +{ + bool KeyboardEventHandler::addFunction(int whatKey, functionType newFunction) + { + if ( keyFuncMap.end() != keyFuncMap.find( whatKey )) + { + std::cout << "duplicate key '" << whatKey << "' ignored." << std::endl; + return false; + } + else + { + keyFuncMap[whatKey].keyFunction = newFunction; + return true; + } + } + + bool KeyboardEventHandler::addFunction (int whatKey, keyStatusType keyPressStatus, functionType newFunction) + { + if (keyPressStatus == KEY_DOWN) + { + return addFunction(whatKey,newFunction); + } + else + { + if ( keyUPFuncMap.end() != keyUPFuncMap.find( whatKey )) + { + std::cout << "duplicate key '" << whatKey << "' ignored." << std::endl; + return false; + } + else + { + keyUPFuncMap[whatKey].keyFunction = newFunction; + return true; + } + } // KEY_UP + } + + bool KeyboardEventHandler::handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter& aa) + { + bool newKeyDownEvent = false; + bool newKeyUpEvent = false; + + switch(ea.getEventType()) + { + case(osgGA::GUIEventAdapter::KEYDOWN): + { + keyFunctionMap::iterator itr = keyFuncMap.find(ea.getKey()); + if (itr != keyFuncMap.end()) + { + if ( (*itr).second.keyState == KEY_UP ) + { + (*itr).second.keyState = KEY_DOWN; + newKeyDownEvent = true; + } + if (newKeyDownEvent) + { + (*itr).second.keyFunction(); + newKeyDownEvent = false; + } + return true; + } + return false; + } + case(osgGA::GUIEventAdapter::KEYUP): + { + keyFunctionMap::iterator itr = keyFuncMap.find(ea.getKey()); + if (itr != keyFuncMap.end() ) + { + (*itr).second.keyState = KEY_UP; + } + itr = keyUPFuncMap.find(ea.getKey()); + if (itr != keyUPFuncMap.end()) + { + (*itr).second.keyFunction(); + return true; + } + return false; + } + default: + return false; + } + } + +} diff --git a/src/renderer/keyboard.hh b/src/renderer/keyboard.hh new file mode 100644 index 0000000..8ec8eca --- /dev/null +++ b/src/renderer/keyboard.hh @@ -0,0 +1,37 @@ + + +#ifndef KEYBOARD_H +#define KEYBOARD_H + +#include +#include + +namespace renderer +{ + class KeyboardEventHandler : public osgGA::GUIEventHandler + { + public: + + typedef void (*functionType) (); + + enum keyStatusType { KEY_UP, KEY_DOWN }; + + struct functionStatusType + { + functionStatusType() {keyState = KEY_UP; keyFunction = NULL;} + functionType keyFunction; + keyStatusType keyState; + }; + + bool addFunction(int whatKey, functionType newFunction); + bool addFunction(int whatKey, keyStatusType keyPressStatus, functionType newFunction); + virtual bool handle(const osgGA::GUIEventAdapter& ea,osgGA::GUIActionAdapter&); + virtual void accept(osgGA::GUIEventHandlerVisitor& v) { v.visit(*this); }; + protected: + typedef std::map keyFunctionMap; + keyFunctionMap keyFuncMap; + keyFunctionMap keyUPFuncMap; + }; +} + +#endif diff --git a/src/renderer/osg_hud.cc b/src/renderer/osg_hud.cc new file mode 100644 index 0000000..1e481e4 --- /dev/null +++ b/src/renderer/osg_hud.cc @@ -0,0 +1,113 @@ +#include "osg_hud.hh" + +using namespace osg; + +namespace renderer +{ + OsgHud :: OsgHud(Group* groupe) + { + + Geode *HUDGeode = new Geode(); + // Text instance for HUD: + _text = new osgText::Text(); + // Projection node for defining view frustrum for HUD: + Projection* HUDProjectionMatrix(new Projection); + + HUDProjectionMatrix->setMatrix(Matrix::ortho2D(0,1024,0,768)); + + // For the HUD model view matrix use an identity matrix: + MatrixTransform* HUDModelViewMatrix = new MatrixTransform; + HUDModelViewMatrix->setMatrix(Matrix::identity()); + + // Make sure the model view matrix is not affected by any transforms + // above it in the scene graph: + HUDModelViewMatrix->setReferenceFrame(Transform::ABSOLUTE_RF); + + // Add the HUD projection matrix as a child of the root node + // and the HUD model view matrix as a child of the projection matrix + // Anything under this node will be view using this projection matrix + // and positioned with this model view matrix. + groupe->addChild(HUDProjectionMatrix); + HUDProjectionMatrix->addChild(HUDModelViewMatrix); + + // Add the Geometry node to contain HUD geometry as a child of the + // HUD model view matrix. + // (See figure "n") + HUDModelViewMatrix->addChild(HUDGeode); + + // Add the text (Text class is derived from drawable) to the geode: + HUDGeode->addDrawable(_text); + + // Set up the parameters for the text we'll add to the HUD: + _text->setCharacterSize(15); + + _text->setAxisAlignment(osgText::Text::SCREEN); + _text->setPosition(Vec3(10,10,-1.5) ); + _text->setColor(Vec4(199, 77, 15, 1) ); + _text->setFont("fonts/arial.ttf"); + + // Set up geometry for the HUD and add it to the HUD + // geometry node, see tutorial "n" for details: + Geometry* HUDBackgroundGeometry = new Geometry(); + + Vec3Array* HUDBackgroundVertices = new Vec3Array; + + HUDBackgroundVertices->push_back(Vec3(0, 0,-1) ); + HUDBackgroundVertices->push_back(Vec3(1024, 0,-1) ); + HUDBackgroundVertices->push_back(Vec3(1024,30,-1) ); + HUDBackgroundVertices->push_back(Vec3(0,30,-1) ); + + DrawElementsUInt* HUDBackgroundIndices = + new DrawElementsUInt(PrimitiveSet::POLYGON, 0); + HUDBackgroundIndices->push_back(0); + HUDBackgroundIndices->push_back(1); + HUDBackgroundIndices->push_back(2); + HUDBackgroundIndices->push_back(3); + + Vec4Array* HUDcolors = new Vec4Array; + HUDcolors->push_back(Vec4(0.8f,0.8f,0.8f,0.8f)); + + Vec2Array* texcoords = new Vec2Array(4); + (*texcoords)[0].set(0.0f,0.0f); + (*texcoords)[1].set(1.0f,0.0f); + (*texcoords)[2].set(1.0f,1.0f); + (*texcoords)[3].set(0.0f,1.0f); + HUDBackgroundGeometry->setTexCoordArray(0,texcoords); + Texture2D* HUDTexture = new Texture2D; + HUDTexture->setDataVariance(Object::DYNAMIC); + Image* hudImage; + hudImage = osgDB::readImageFile("HUD.png"); + HUDTexture->setImage(hudImage); + Vec3Array* HUDnormals = new Vec3Array; + HUDnormals->push_back(Vec3(0.0f,0.0f,1.0f)); + HUDBackgroundGeometry->setNormalArray(HUDnormals); + HUDBackgroundGeometry->setNormalBinding(Geometry::BIND_OVERALL); + + HUDBackgroundGeometry->addPrimitiveSet(HUDBackgroundIndices); + HUDBackgroundGeometry->setVertexArray(HUDBackgroundVertices); + HUDBackgroundGeometry->setColorArray(HUDcolors); + HUDBackgroundGeometry->setColorBinding(Geometry::BIND_OVERALL); + + HUDGeode->addDrawable(HUDBackgroundGeometry); + + // Create and set up a state set using the texture from above: + StateSet* HUDStateSet = new StateSet(); + HUDGeode->setStateSet(HUDStateSet); + HUDStateSet-> + setTextureAttributeAndModes(0,HUDTexture,StateAttribute::ON); + + // For this state set, turn blending on (so alpha texture looks right) + HUDStateSet->setMode(GL_BLEND,StateAttribute::ON); + + // Disable depth testing so geometry is draw regardless of depth values + // of geometry already draw. + HUDStateSet->setMode(GL_DEPTH_TEST,StateAttribute::OFF); + HUDStateSet->setRenderingHint(StateSet::TRANSPARENT_BIN ); + + // Need to make sure this geometry is draw last. RenderBins are handled + // in numerical order so set bin number to 11 + HUDStateSet->setRenderBinDetails(99, "RenderBin"); + + } + +} diff --git a/src/renderer/osg_hud.hh b/src/renderer/osg_hud.hh new file mode 100644 index 0000000..2fbec79 --- /dev/null +++ b/src/renderer/osg_hud.hh @@ -0,0 +1,44 @@ +#ifndef _OSGHUD_H +#define _OSGHUD_H + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + +#include + + +namespace renderer +{ + + class OsgHud + { + public: + OsgHud(osg::Group*); + + void set_text(const std::string& text) + { + //_text->setText(text); + } + protected: + // Text instance for HUD: + osgText::Text* _text; + + // points2 + osg::Vec3* _p2; + + }; +} +#endif diff --git a/src/renderer/osg_visitor.cc b/src/renderer/osg_visitor.cc new file mode 100644 index 0000000..d1a8a0a --- /dev/null +++ b/src/renderer/osg_visitor.cc @@ -0,0 +1,327 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "osg_visitor.hh" +#include "ode/capped_cyl.hh" +#include "ode/box.hh" +#include "ode/sphere.hh" + +#include +#include + +#include "shadows.hh" + +using namespace osg; +namespace renderer +{ + inline Quat _osg_quat(const ode::Object& b) + { + const dReal*q = dBodyGetQuaternion(b.get_body()); + return Quat(q[1], q[2], q[3], q[0]); + } + + inline Vec3 _osg_pos(const ode::Object& b) + { + Eigen::Vector3d p = b.get_pos(); + return Vec3(p.x(), p.y(), p.z()); + } + + + // apply ode transformation + class UpdateCallback : public NodeCallback + { + public: + UpdateCallback(const ode::Object & obj) : _obj(obj){ + } + + virtual void operator() (Node * node, NodeVisitor * nv) + { + PositionAttitudeTransform *pat = + dynamic_cast(node); + pat->setPosition(_osg_pos(_obj)); + pat->setAttitude(_osg_quat(_obj)); + traverse(node, nv); + } + protected: + const ode::Object& _obj; + }; + + struct ImgPostDrawCallback : public osg::Camera::DrawCallback + { + ImgPostDrawCallback(const std::string& prefix): + _prefix(prefix), + _image(new osg::Image), + _k(0) + {} + + virtual void operator () (const osg::Camera &camera) const + { + int x = (int) camera.getViewport()->x(); + int y = (int) camera.getViewport()->y(); + int width = (int) camera.getViewport()->width(); + int height = (int )camera.getViewport()->height(); + + _image->readPixels(x, y, width, height, GL_RGB, GL_UNSIGNED_BYTE); + osgDB::writeImageFile(*_image, + _prefix + boost::str(boost::format("%1%")%boost::io::group(std::setfill('0'),std::setw(3),_k++))+".png"); + + } + private: + std::string _prefix; + osg::ref_ptr _image; + mutable size_t _k; + }; + + + + void OsgVisitor :: enable_dump(const std::string& prefix) + { + _viewer.getCamera()->setPostDrawCallback(new ImgPostDrawCallback(prefix)); + } + + + Texture2D *OsgVisitor :: _load_texture(const std::string& fname) + { + Texture2D*texture = new Texture2D(); + Image*image = osgDB::readImageFile(fname); + assert(image); + texture->setImage(image); + texture->setWrap(osg::Texture::WRAP_S, osg::Texture::REPEAT); + texture->setWrap(osg::Texture::WRAP_T, osg::Texture::REPEAT); + texture->setDataVariance(osg::Object::DYNAMIC); + return texture; + } + + void OsgVisitor :: visit(const ode::Box& b) + { + if (!_first) + return; + ref_ptr geode(new Geode); + geode-> + addDrawable(new ShapeDrawable + (new Box(Vec3f(), + b.get_length(), + b.get_width(), + b.get_height()))); + ref_ptr pat(new PositionAttitudeTransform()); + pat->addChild(geode.get()); + ref_ptr cb(new UpdateCallback(b)); + pat->setUpdateCallback(cb.get()); + _root->addChild(pat.get()); + _set_tm(geode.get()); + } + + void OsgVisitor :: visit(const ode::Sphere& b) + { + float radius = b.get_rad(); + ref_ptr geode(new Geode); + geode-> + addDrawable(new ShapeDrawable + (new Sphere(Vec3f(), + radius))); + + ref_ptr pat(new PositionAttitudeTransform()); + pat->addChild(geode.get()); + + ref_ptr cb(new UpdateCallback(b)); + pat->setUpdateCallback(cb.get()); + _root->addChild(pat.get()); + } + + + + ref_ptr OsgVisitor :: _create_sqr(float width, float length) + { + ref_ptr sqr(new Geometry); + ref_ptr geode_sqr(new Geode); + ref_ptr sqr_v(new Vec3Array); + + geode_sqr->addDrawable(sqr.get()); + float x = width; + float y = length; + sqr_v->push_back(Vec3(0, 0, 0)); + sqr_v->push_back(Vec3(0, y, 0)); + sqr_v->push_back(Vec3(x, y, 0)); + sqr_v->push_back(Vec3(x, 0, 0)); + sqr->setVertexArray(sqr_v.get()); + + ref_ptr + quad(new DrawElementsUInt(PrimitiveSet::QUADS, 0)); + quad->push_back(3); + quad->push_back(2); + quad->push_back(1); + quad->push_back(0); + + sqr->addPrimitiveSet(quad.get()); + + ref_ptr texcoords(new Vec2Array(4)); + float rep = 5; + (*texcoords)[0].set(0.0f, 0.0f); + (*texcoords)[1].set(0.0f, rep); + (*texcoords)[2].set(rep, rep); + (*texcoords)[3].set(rep, 0.0f); + sqr->setTexCoordArray(0, texcoords.get()); + + ref_ptr normals(new Vec3Array); + normals->push_back(osg::Vec3(0, 0, 1)); + sqr->setNormalArray(normals.get()); + sqr->setNormalBinding(Geometry::BIND_OVERALL); + + return geode_sqr; + } + + void OsgVisitor :: _create_ground(const ode::Environment &env) + { + static const float ground_x = 20; + static const float ground_y = ground_x; + static const float ground_z = 0; + + ref_ptr patg(new PositionAttitudeTransform()); + patg->setAttitude(osg::Quat(env.get_roll(), osg::Vec3(0, 1, 0), + env.get_pitch(), osg::Vec3(1, 0, 0), + 0, osg::Vec3(0, 0, 1))); + patg->setPosition(osg::Vec3(0, 0, env.get_z())); + + _ground = new Group; + patg->addChild(_ground); + + static const float nb = 1; + for (size_t i = 0; i < nb; ++i) + for (size_t j = 0; j < nb; ++j) + { + ref_ptr geode_sqr = _create_sqr(ground_x, ground_y); + ref_ptr pat(new PositionAttitudeTransform()); + pat->setPosition(Vec3((i - nb / 2) * ground_x, + (j - nb / 2) * ground_y, 0)); + pat->addChild(geode_sqr.get()); + + ref_ptr ss_checker(new StateSet()); + ss_checker->setTextureAttributeAndModes(0, _load_texture("data/checker.tga")); + geode_sqr->setStateSet(ss_checker.get()); + _ground->addChild(pat.get()); + } + + Vec3 center(0.0f, 0.0f, 0.0f); + float radius = 500; + Vec3 light_position(center + Vec3(0.0f, 0.0f, 10)); + + + + osg::ref_ptr shadowedScene = new osgShadow::ShadowedScene; + // osgShadow::MinimalDrawBoundsShadowMap* sm = new + // osgShadow::MinimalDrawBoundsShadowMap; + osg::ref_ptr sm = new + osgShadow::MinimalCullBoundsShadowMap; + //osg::ref_ptr sm = new osgShadow::ShadowMap; + // osg::ref_ptr sm = + // new osgShadow::ParallelSplitShadowMap; + + // osg::ref_ptr sm = new osgShadow::SoftShadowMap; + //sm->setTextureSize(osg::Vec2s(1024*2, 1024*2)); + //sm->setAmbientBias(osg::Vec2(0.9, 0.9)); + // sm->setMaxFarDistance(20); + // sm->setPolygonOffset(osg::Vec2(0, 0)); + //sm->enableShadowGLSLFiltering(false); + shadowedScene->setShadowTechnique(sm.get()); + LightSource*ls = new LightSource; + ls->getLight()->setPosition(osg::Vec4(light_position, 1)); + ls->getLight()->setAmbient(Vec4(1, 1, 1, 1.0)); + ls->getLight()->setDiffuse(Vec4(0.9, 0.9, 0.9, 1.0)); + + if (env.get_ground() != 0x0) + _root->addChild(patg); + shadowedScene->addChild(_root.get()); + shadowedScene->addChild(ls); + + ref_ptr matirial = new Material; + matirial->setColorMode(Material::DIFFUSE); + matirial->setAmbient(Material::FRONT_AND_BACK, Vec4(0.2, 0.2, 0.2, 1)); + matirial->setSpecular(Material::FRONT_AND_BACK, Vec4(0.2, 0.2, 0.2, 1)); + matirial->setShininess(Material::FRONT_AND_BACK, 64); + _root->getOrCreateStateSet()->setAttributeAndModes(matirial.get(), StateAttribute::ON); + + // _shadowed_scene = + // create_shadowed_scene(_root.get(), ground.get(), + // light_position, radius / 100.0f, 1); + _shadowed_scene = shadowedScene; + + _viewer.setSceneData(_shadowed_scene.get()); + //_viewer.setSceneData(_root);//_shadowed_scene.get()); + } + + void OsgVisitor::visit(const std::vector& v) + { + assert(v.size());//hack.. + if (!_viewer.getSceneData()) + _create_ground(v[0]->get_env()); + BOOST_FOREACH(ode::Object::ptr_t o, v) + o->accept(*this); + } + void OsgVisitor::_update_traj() + { + osg::Vec3 pos = _node_ref->getBound().center() * osg::computeLocalToWorld(_node_ref->getParentalNodePaths()[0]); + if ((pos - _prev_pos).length() > 0.02) + { + _prev_pos = pos; + ref_ptr geode(new Geode); + osg::ref_ptr sphere = + new ShapeDrawable(new Sphere(osg::Vec3(pos.x(), pos.y(), 0), 0.01)); + sphere->setColor(Vec4(1, 0, 0, 0.5)); + sphere->getOrCreateStateSet()->setMode(GL_BLEND, osg::StateAttribute::ON); + geode->addDrawable(sphere.get()); + + _shadowed_scene->addChild(geode.get()); + } + } + + + void OsgVisitor :: visit(const ode::CappedCyl& e) + { + if (_first) + { + ref_ptr geode(new Geode); + geode-> + addDrawable(new ShapeDrawable + (new Capsule(Vec3f(), + e.get_radius(), + e.get_length()))); + ref_ptr pat(new PositionAttitudeTransform()); + pat->addChild(geode.get()); + ref_ptr cb(new UpdateCallback(e)); + pat->setUpdateCallback(cb.get()); + _root->addChild(pat.get()); + + _set_tm(geode.get()); + } + } + + // apply ode transformation position only + class PositionCallback : public NodeCallback + { + public: + PositionCallback(const ode::Object & obj) : _obj(obj){ + } + + virtual void operator() (Node * node, NodeVisitor * nv) + { + PositionAttitudeTransform *pat = + dynamic_cast(node); + pat->setPosition(_osg_pos(_obj)); + traverse(node, nv); + } + protected: + const ode::Object& _obj; + }; +} diff --git a/src/renderer/osg_visitor.hh b/src/renderer/osg_visitor.hh new file mode 100644 index 0000000..138f8bb --- /dev/null +++ b/src/renderer/osg_visitor.hh @@ -0,0 +1,127 @@ +/* +** osgVisitor.hh +** Login : +** Started on Wed Jan 4 15:14:31 2006 Jean-baptiste MOURET +** $Id$ +** +** Copyright (C) 2006 Jean-baptiste MOURET +** This program is free software; you can redistribute it and/or modify +** it under the terms of the GNU General Public License as published by +** the Free Software Foundation; either version 2 of the License, or +** (at your option) any later version. +** +** This program is distributed in the hope that it will be useful, +** but WITHOUT ANY WARRANTY; without even the implied warranty of +** MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +** GNU General Public License for more details. +** +** You should have received a copy of the GNU General Public License +** along with this program; if not, write to the Free Software +** Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA +*/ + +#ifndef OSGVISITOR_HH_ +# define OSGVISITOR_HH_ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ode/servo.hh" +#include "keyboard.hh" +#include "osg_hud.hh" + +namespace renderer +{ + class OsgVisitor : public ode::ConstVisitor + { + public: + typedef enum { FREE, FOLLOW, FIXED} camera_t; + OsgVisitor(camera_t cam = FOLLOW) : + _root(new osg::Group()), + _tm(new osgGA::NodeTrackerManipulator), + _keh(new KeyboardEventHandler()), + _first(true), + _camera_type(cam), + _node_ref(NULL) + { + _viewer.realize(); + osgViewer::WindowSizeHandler *wsh=new osgViewer::WindowSizeHandler; + _viewer.addEventHandler(wsh); + //wsh->setToggleFullscreen(false); // ne marche pas... + //std::cout<<"ToggleFullscreen="<getToggleFullscreen()<setRotationMode(osgGA::NodeTrackerManipulator::TRACKBALL); + _tm->setTrackerMode(osgGA::NodeTrackerManipulator::NODE_CENTER); + + // _create_ground(); + if (_camera_type != FIXED) + _viewer.setCameraManipulator(new osgGA::TrackballManipulator()); + _viewer.addEventHandler(new osgViewer::StatsHandler); + _viewer.addEventHandler(_keh.get()); + + // _hud = new OsgHud(_shadowed_scene.get()); + } + void init() {} + // update must be called after the first visit + void update() + { + _first = false; + if (_camera_type == FIXED) + _viewer.getCamera()->setViewMatrixAsLookAt(osg::Vec3d(0, -2, 1.25), + osg::Vec3d(0, 0, 0), + osg::Vec3d(0, 0, 1)); + _viewer.frame(); + _update_traj(); + } + bool done() { return _viewer.done(); } + virtual void visit(const std::vector& v); + virtual void visit(const ode::Box&); + virtual void visit(const ode::CappedCyl&); + virtual void visit(const ode::Sphere&); + + void enable_dump(const std::string& prefix); + + KeyboardEventHandler* get_keh() { return _keh.get(); } + OsgHud* get_hud() { return _hud; } + protected: + osgViewer::Viewer _viewer; + osg::ref_ptr _root; + osg::ref_ptr _shadowed_scene; + osgGA::NodeTrackerManipulator* _tm; + osg::ref_ptr _keh; + OsgHud* _hud; + bool _first; + camera_t _camera_type; + void _update_traj(); + void _set_tm(osg::Node* g) + { + if (!_node_ref) + _node_ref = g; + if (!_tm->getTrackNode() && _camera_type == FOLLOW) + { + _tm->setTrackNode(g); + _tm->setNode(g); + + _tm->setHomePosition(osg::Vec3(-4, -3, 2), + osg::Vec3(0, 0, 0), + osg::Vec3(0, 0, 1)); + _viewer.setCameraManipulator(_tm); + } + } + void _create_ground(const ode::Environment& env); + osg::ref_ptr _create_sqr(float width, float length); + osg::Texture2D* _load_texture(const std::string& fname); + osg::ref_ptr _ground; + osg::Vec3 _prev_pos; + osg::Node* _node_ref; + }; +} + + + +#endif /* !OSGVISITOR_HH_ */ diff --git a/src/renderer/shadows.cc b/src/renderer/shadows.cc new file mode 100644 index 0000000..88f8692 --- /dev/null +++ b/src/renderer/shadows.cc @@ -0,0 +1,179 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "shadows.hh" + +using namespace osg; + +class UpdateCameraAndTexGenCallback : public osg::NodeCallback +{ +public: + + UpdateCameraAndTexGenCallback(const osg::Vec3& position + , osg::CameraNode* cameraNode, osg::TexGenNode* texgenNode): + _position(position), + _cameraNode(cameraNode), + _texgenNode(texgenNode) + { + } + + virtual void operator()(osg::Node* node, osg::NodeVisitor* nv) + { + // first update subgraph to make sure objects are all moved into postion + traverse(node,nv); + + // now compute the camera's view and projection matrix to point at the shadower (the camera's children) + osg::BoundingSphere bs; + for(unsigned int i=0; i<_cameraNode->getNumChildren(); ++i) + { + bs.expandBy(_cameraNode->getChild(i)->getBound()); + } + + if (!bs.valid()) + { + osg::notify(osg::WARN) << "bb invalid"<<_cameraNode.get()<setReferenceFrame(osg::CameraNode::ABSOLUTE_RF); + _cameraNode->setProjectionMatrixAsFrustum(-right,right,-top,top,znear,zfar); + _cameraNode->setViewMatrixAsLookAt(_position,bs.center(),osg::Vec3(0.0f,1.0f,0.0f)); + + // compute the matrix which takes a vertex from local coords into tex coords + // will use this later to specify osg::TexGen.. + osg::Matrix MVPT = _cameraNode->getViewMatrix() * + _cameraNode->getProjectionMatrix() * + osg::Matrix::translate(1.0,1.0,1.0) * + osg::Matrix::scale(0.5f,0.5f,0.5f); + + _texgenNode->getTexGen()->setMode(osg::TexGen::EYE_LINEAR); + _texgenNode->getTexGen()->setPlanesFromMatrix(MVPT); + + } + +protected: + + virtual ~UpdateCameraAndTexGenCallback() {} + + osg::Vec3 _position; + osg::ref_ptr _cameraNode; + osg::ref_ptr _texgenNode; + +}; + + +osg::Group* create_shadowed_scene(osg::Node* shadower, + osg::Node* shadowed, + const osg::Vec3& lightPosition, + float radius,unsigned int unit) +{ + osg::Group* group = new osg::Group; + + // add light source + { + osg::LightSource* lightSource = new osg::LightSource; + lightSource->getLight()->setPosition(osg::Vec4(lightPosition,1.0f)); + lightSource->getLight()->setLightNum(0); + + group->addChild(lightSource); + + osg::Geode* lightgeode = new osg::Geode; + lightgeode->getOrCreateStateSet()->setMode(GL_LIGHTING,osg::StateAttribute::OFF); + lightgeode->addDrawable(new osg::ShapeDrawable(new osg::Sphere(lightPosition,radius))); + group->addChild(lightgeode); + } + + osg::Vec4 ambientLightColor(0.2,0.2f,0.2f,1.0f); + + // add the shadower and shadowed. + group->addChild(shadower); + group->addChild(shadowed); + + + unsigned int tex_width = 512; + unsigned int tex_height = 512; + + osg::Texture2D* texture = new osg::Texture2D; + texture->setTextureSize(tex_width, tex_height); + texture->setInternalFormat(GL_RGB); + texture->setFilter(osg::Texture2D::MIN_FILTER,osg::Texture2D::LINEAR); + texture->setFilter(osg::Texture2D::MAG_FILTER,osg::Texture2D::LINEAR); + texture->setWrap(osg::Texture2D::WRAP_S,osg::Texture2D::CLAMP_TO_BORDER); + texture->setWrap(osg::Texture2D::WRAP_T,osg::Texture2D::CLAMP_TO_BORDER); + texture->setBorderColor(osg::Vec4(1.0f,1.0f,1.0f,1.0f)); + + // set up the render to texture camera. + { + + // create the camera + osg::CameraNode* camera = new osg::CameraNode; + + camera->setClearColor(osg::Vec4(1.0f,1.0f,1.0f,1.0f)); + + // set viewport + camera->setViewport(0,0,tex_width,tex_height); + + // set the camera to render before the main camera. + camera->setRenderOrder(osg::CameraNode::PRE_RENDER); + + // tell the camera to use OpenGL frame buffer object where supported. + camera->setRenderTargetImplementation(osg::CameraNode::FRAME_BUFFER_OBJECT); + + // attach the texture and use it as the color buffer. + camera->attach(osg::CameraNode::COLOR_BUFFER, texture); + + // add subgraph to render + camera->addChild(shadower); + + osg::StateSet* stateset = camera->getOrCreateStateSet(); + + // make the material black for a shadow. + osg::Material* material = new osg::Material; + material->setAmbient(osg::Material::FRONT_AND_BACK,osg::Vec4(0.0f,0.0f,0.0f,1.0f)); + material->setDiffuse(osg::Material::FRONT_AND_BACK,osg::Vec4(0.0f,0.0f,0.0f,1.0f)); + material->setEmission(osg::Material::FRONT_AND_BACK,ambientLightColor); + material->setShininess(osg::Material::FRONT_AND_BACK,0.0f); + stateset->setAttribute(material,osg::StateAttribute::OVERRIDE); + + group->addChild(camera); + + // create the texgen node to project the tex coords onto the subgraph + osg::TexGenNode* texgenNode = new osg::TexGenNode; + texgenNode->setTextureUnit(unit); + group->addChild(texgenNode); + + // set an update callback to keep moving the camera and tex gen in the right direction. + group->setUpdateCallback(new UpdateCameraAndTexGenCallback(lightPosition, camera, texgenNode)); + } + + // set the shadowed subgraph so that it uses the texture and tex gen settings. + { + osg::StateSet* stateset = shadowed->getOrCreateStateSet(); + stateset->setTextureAttributeAndModes(unit,texture,osg::StateAttribute::ON); + stateset->setTextureMode(unit,GL_TEXTURE_GEN_S,osg::StateAttribute::ON); + stateset->setTextureMode(unit,GL_TEXTURE_GEN_T,osg::StateAttribute::ON); + stateset->setTextureMode(unit,GL_TEXTURE_GEN_R,osg::StateAttribute::ON); + stateset->setTextureMode(unit,GL_TEXTURE_GEN_Q,osg::StateAttribute::ON); + } + + + return group; +} diff --git a/src/renderer/shadows.hh b/src/renderer/shadows.hh new file mode 100644 index 0000000..5a42154 --- /dev/null +++ b/src/renderer/shadows.hh @@ -0,0 +1,12 @@ +#ifndef CREATESHADOWEDSCENE_H +#define CREATESHADOWEDSCENE_H + +#include +#include + +// function to create a lightsource which contain a shadower and showed subgraph, +// the showadowed subgrph has a cull callback to fire off a pre render to texture +// of the shadowed subgraph. +extern osg::Group* create_shadowed_scene(osg::Node* shadower,osg::Node* shadowed,const osg::Vec3& lightPosition,float radius,unsigned int textureUnit=1); + +#endif diff --git a/src/robot/hexapod.cc b/src/robot/hexapod.cc new file mode 100644 index 0000000..1e68b71 --- /dev/null +++ b/src/robot/hexapod.cc @@ -0,0 +1,245 @@ + +#include "hexapod.hh" +#include "ode/box.hh" +#include "ode/capped_cyl.hh" +#include "ode/sphere.hh" +#include "ode/motor.hh" +#include "ode/mx28.hh" +#include "ode/ax12.hh" + + + + +using namespace ode; +USING_PART_OF_NAMESPACE_EIGEN + +namespace robot +{ + static int sign(float x) + { + return x > 0 ? 1 : -1; + } + void Hexapod :: _build(Environment_hexa& env, const Vector3d& pos) + { + + /// Definition of robot's params + // length in meter + // mass in KG + static const double body_mass = 1.5; + static const double body_length = 0.20; + static const double body_width = 0.24; + static const double body_height =0.04; + + static const double legP1_w = 0.02; + static const double legP1_length = 0.06; + static const double legP1_dist = 0.2; + static const double legP1_mass = 0.020; + + static const double legP2_w = 0.02; + static const double legP2_length = 0.085; + static const double legP2_dist = 0.2; + static const double legP2_mass = 0.180; + + + static const double legP3_w = 0.025; + static const double legP3_length = 0.145; + static const double legP3_dist = 0.2; + static const double legP3_mass = 0.060; + + + + /// creation of robot's body + _main_body = Object::ptr_t(new Box(env, pos + Vector3d(0, 0, legP3_length+0.01), + body_mass, body_length, body_width, body_height)); + _bodies.push_back(_main_body); + + + + + for (size_t i = 0; i <6; ++i) // for each legs + { + for(int j=0;j<_brokenLegs.size();j++) + { + if(i==_brokenLegs[j]) + { + i++; + if(_brokenLegs.size()>j+1 && _brokenLegs[j+1]!=i) + break; + } + } + if(i>=6) + return; + // selecting an angle corresponding to number of the leg + // float angle = i<3 ? M_PI / 2.0f : -M_PI / 2.0f;// + M_PI / 6; + float angle; + float xStart=0; // selection of the start of the first joint + float yStart=0; + switch(i) + { + case 0: + case 5: + { + xStart=i<3 ? 0.06 : -0.06; + yStart=0.12; + angle=i<3? 3*M_PI/8:-3*M_PI/8; + break; + } + + case 1: + case 4: + { + xStart=i<3 ? 0.10 : -0.10; + yStart=0; + angle=i<3? M_PI/2:-M_PI/2; + break; + } + + case 2: + case 3: + { + xStart=i<3 ? 0.06 : -0.06; + yStart=-0.12; + angle=i<3? 5*M_PI/8:-5*M_PI/8; + break; + } + } + + /// first part + Object::ptr_t l1( + /* new Box(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length / 2), + yStart+cos(angle) * (legP1_length / 2), + legP3_length+0.01), + legP1_mass,0.04, legP1_w, legP1_length)); + +*/ + + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length / 2), + yStart+cos(angle) * (legP1_length / 2), + legP3_length+0.01), + legP1_mass, legP1_w, legP1_length)); + + + l1->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(0, 0, 1)); + + _bodies.push_back(l1); + env.add_leg_object(i,*l1); + Mx28::ptr_t s1(new Mx28(env, pos + + Vector3d(xStart, + yStart, + /*legP2_length+*/legP3_length+0.01), + *_main_body, *l1)); + + + + //s1->set_axis(0, Vector3d(cos(-angle), sin(-angle), 0)); + s1->set_axis(0, Vector3d(0,0, 1)); + s1->set_axis(2, Vector3d(1,0, 0)); + //s1->set_hinge_axis(Vector3d(0,0, 1)); + //s1->set_axis(2, Vector3d(0, 0, -1)); + + + + s1->set_lim(0, -M_PI/8, M_PI/8 ); + + _servos.push_back(s1); + + /// second part + Object::ptr_t l2( + /* new Box(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length/2), + yStart+cos(angle) * (legP1_length+legP2_length/2), + legP3_length+0.01), + legP2_mass,0.04, legP2_w, legP2_length));*/ + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length/2), + yStart+cos(angle) * (legP1_length+legP2_length/2), + legP3_length+0.01), + legP2_mass, legP2_w, legP2_length)); + + /*l2->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(sin(-angle), -cos(-angle), 0));*/ + + + l2->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(0, 0, 1)); + + + _bodies.push_back(l2); + + env.add_leg_object(i,*l2); + + Mx28::ptr_t s2(new Mx28(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length), + yStart+cos(angle) * (legP1_length), + legP3_length+0.01), + *l1, *l2)); + s2->set_axis(0, Vector3d(cos(-angle), sin(-angle), 0)); + s2->set_axis(2, Vector3d(sin(-angle), -cos(-angle), 0)); + + + //s2->set_axis(0, Vector3d(0, 1, 0)); + //s2->set_axis(2, Vector3d(0, 0, 1)); + + + + s2->set_lim(0, -M_PI/4, M_PI/4 ); + _servos.push_back(s2); + + + /// third part + Object::ptr_t l3( + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length), + yStart+cos(angle) * (legP1_length+legP2_length), + legP3_length/2+0.01), + legP3_mass, legP3_w, legP3_length)); + + //l3->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + // Vector3d(sin(-angle), -cos(-angle), 0)); + + + _bodies.push_back(l3); + env.add_leg_object(i,*l3); + Mx28::ptr_t s3(new Mx28(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length), + yStart+cos(angle) * (legP1_length+legP2_length), + legP3_length+0.01), + *l2, *l3)); + s3->set_axis(0, Vector3d(cos(-angle), sin(-angle), 0)); + s3->set_axis(2, Vector3d(sin(-angle), -cos(-angle), 0)); + + + //s3->set_axis(0, Vector3d(1, 0, 0)); + //s3->set_axis(2, Vector3d(0, 1, 0)); + + + // dBodyVectorToWorld(l2->get_body(),1,0,0,axis); + + //s3->set_hinge_axis(Vector3d(axis[0],axis[1],axis[2]));//Vector3d(0,1,0)); + + + + + s3->set_lim(0, -M_PI/4, M_PI/4 ); + _servos.push_back(s3); + + + + } + + + + for (size_t i = 0; i < _servos.size(); ++i) + for (size_t j = 0; j < 3; ++j) + _servos[i]->set_lim(j, -M_PI/2, M_PI/2 ); + /*for (size_t i = 0; i < _servos.size(); i++) + { + for (size_t j = 0; j < 3; ++j) + _servos[i]->set_lim(j, 0, 0 ); + }*/ + } +} + diff --git a/src/robot/hexapod.hh b/src/robot/hexapod.hh new file mode 100644 index 0000000..af8bcbc --- /dev/null +++ b/src/robot/hexapod.hh @@ -0,0 +1,40 @@ +#ifndef ROBOT_HEXAPOD_HPP +#define ROBOT_HEXAPOD_HPP +#include "robot/robot.hh" +#include "ode/environment_hexa.hh" +namespace robot +{ + class Hexapod : public Robot + { + public: + Hexapod(ode::Environment_hexa & env, const Eigen::Vector3d & pos,std::vector brokenLegs):_brokenLegs(brokenLegs) + { + + _build(env, pos); + + /*std::cout<<_bodies.size()<get_pos()< clone(ode::Environment_hexa& env) const + { + + return boost::shared_ptr(new Hexapod(*this, env)); + } + protected: + std::vector _brokenLegs; + void _build(ode::Environment_hexa& env, const Eigen::Vector3d& pos); + }; +} + +#endif diff --git a/src/robot/hexapod_wheel.cc b/src/robot/hexapod_wheel.cc new file mode 100644 index 0000000..bb4bad9 --- /dev/null +++ b/src/robot/hexapod_wheel.cc @@ -0,0 +1,237 @@ +#include "hexapod.hh" +#include "ode/box.hh" +#include "ode/capped_cyl.hh" +#include "ode/sphere.hh" +#include "ode/motor.hh" +#include "ode/ax12.hh" + + + +using namespace ode; +USING_PART_OF_NAMESPACE_EIGEN + +namespace robot +{ + static int sign(float x) + { + return x > 0 ? 1 : -1; + } + void Hexapod :: _build(Environment_hexa& env, const Vector3d& pos) + { + + /// Definition of robot's params + // length in meter + // mass in KG + static const double body_mass = 0.426; + static const double body_length = 0.20; + static const double body_width = 0.24; + static const double body_height = 0.04; + + static const double legP1_w = 0.02; + static const double legP1_length = 0.06; + static const double legP1_dist = 0.2; + static const double legP1_mass = 0.0288; + + static const double legP2_w = 0.02; + static const double legP2_length = 0.085; + static const double legP2_dist = 0.2; + static const double legP2_mass = 0.141; + + static const double legP3_w = 0.025; + static const double legP3_length = 0.14; + static const double legP3_dist = 0.2; + static const double legP3_mass = 0.088; + + static const double legP4_w = 0.02; + static const double legP4_length = 0.045; + static const double legP4_dist = 0.2; + static const double legP4_mass = 0.0615; + + static const double wheel_rad = 0.025; + static const double wheel_mass = 0.0152; + + /// creation of robot's body + _main_body = Object::ptr_t(new Box(env, pos + Vector3d(0, 0, legP3_length+wheel_rad), + body_mass, body_length, body_width, body_height)); + _bodies.push_back(_main_body); + + + + + for (size_t i = 0; i <6; ++i) // for each legs + { + for(int j=0;j<_brokenLegs.size();j++) + { + if(i==_brokenLegs[j]) + { + i++; + if(_brokenLegs.size()>j+1 && _brokenLegs[j+1]!=i) + break; + } + } + if(i>=6) + return; + // selecting an angle corresponding to number of the leg + float angle = i<3 ? M_PI / 4.0f*(1+i) : M_PI / 4.0f*(2+i);// + M_PI / 6; + + float xStart=0; // selection of the start of the first joint + float yStart=0; + switch(i) + { + case 0: + case 5: + { + xStart=i<3 ? 0.06 : -0.06; + yStart=0.12; + break; + } + + case 1: + case 4: + { + xStart=i<3 ? 0.10 : -0.10; + yStart=0; + break; + } + + case 2: + case 3: + { + xStart=i<3 ? 0.06 : -0.06; + yStart=-0.12; + break; + } + } + + /// first part + Object::ptr_t l1( + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length / 2), + yStart+cos(angle) * (legP1_length / 2), + legP3_length+wheel_rad), + legP1_mass, legP1_w, legP1_length)); + + l1->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(0, 0, -1)); + + _bodies.push_back(l1); + env.add_leg_object(i,*l1); + Ax12::ptr_t s1(new Ax12(env, pos + + Vector3d(xStart, + yStart, + legP2_length+legP3_length+wheel_rad), + *_main_body, *l1)); + + //s1->set_axis(0, Vector3d(cos(-angle), sin(-angle), 0)); + s1->set_axis(0, Vector3d(0,0, 1)); + //s1->set_axis(2, Vector3d(0, 0, -1)); + + _servos.push_back(s1); + + /// second part + Object::ptr_t l2( + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length/2), + yStart+cos(angle) * (legP1_length+legP2_length/2), + legP3_length+wheel_rad), + legP2_mass, legP2_w, legP2_length)); + + /*l2->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(sin(-angle), -cos(-angle), 0));*/ + l2->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(0, 0, -1)); + _bodies.push_back(l2); + env.add_leg_object(i,*l2); + + + Ax12::ptr_t s2(new Ax12(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length), + yStart+cos(angle) * (legP1_length), + legP3_length+wheel_rad), + *l1, *l2)); + s2->set_axis(0, Vector3d(cos(-angle), sin(-angle), 0)); + s2->set_axis(2, Vector3d(sin(-angle), -cos(-angle), 0)); + + _servos.push_back(s2); + + + /// third part + Object::ptr_t l3( + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length), + yStart+cos(angle) * (legP1_length+legP2_length), + legP3_length/2+wheel_rad), + legP3_mass, legP3_w, legP3_length)); + + l3->set_rotation(Vector3d(cos(-angle), sin(-angle), 0), + Vector3d(sin(-angle), -cos(-angle), 0)); + + _bodies.push_back(l3); + env.add_leg_object(i,*l3); + Ax12::ptr_t s3(new Ax12(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length), + yStart+cos(angle) * (legP1_length+legP2_length), + legP3_length+wheel_rad), + *l2, *l3)); + s3->set_axis(0, Vector3d(cos(-angle), sin(-angle), 0)); + s3->set_axis(2, Vector3d(sin(-angle), -cos(-angle), 0)); + + + _servos.push_back(s3); + + /// fourth part + Object::ptr_t l4( + new CappedCyl(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length-legP4_length/2), + yStart+cos(angle) * (legP1_length+legP2_length-legP4_length/2), + wheel_rad), + legP4_mass, legP4_w, legP4_length)); + + l4->set_rotation(Vector3d(0,0 , -1), + Vector3d(cos(-angle), sin(-angle), 0)); + + _bodies.push_back(l4); + env.add_leg_object(i,*l4); + Ax12::ptr_t s4(new Ax12(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length+legP3_w), + yStart+cos(angle) * (legP1_length+legP2_length+legP3_w), + wheel_rad), + *l3, *l4)); + // problem + s4->set_axis(0, Vector3d(0,0,1)); + s4->set_axis(2, Vector3d(sin(-angle), -cos(-angle), 0)); + + _servos.push_back(s4); + + + /// wheel + + Object::ptr_t wheel + (new Sphere(env, pos + + Vector3d(xStart+sin(angle) * (legP1_length+legP2_length-legP4_length), + yStart+cos(angle) * (legP1_length+legP2_length-legP4_length), + wheel_rad), + wheel_mass, wheel_rad)); + + + + _bodies.push_back(wheel); + env.add_leg_object(i,*wheel); + Motor::ptr_t m1 + (new Motor(env, pos + + Vector3d(xStart+ sin(angle) * ( legP1_length+legP2_length-legP4_length), + yStart+ cos(angle) * ( legP1_length+legP2_length-legP4_length), + wheel_rad), + Vector3d(cos(-angle), sin(-angle), 0), + *l4, *wheel)); + _motors.push_back(m1); + + } + + + + for (size_t i = 0; i < _servos.size(); ++i) + for (size_t j = 0; j < 3; ++j) + _servos[i]->set_lim(j, -M_PI/3, M_PI/3 ); + } +} diff --git a/src/robot/hexapod_wheel.hh b/src/robot/hexapod_wheel.hh new file mode 100644 index 0000000..0bb1375 --- /dev/null +++ b/src/robot/hexapod_wheel.hh @@ -0,0 +1,28 @@ +#ifndef ROBOT_HEXAPOD_HPP +#define ROBOT_HEXAPOD_HPP +#include "robot/robot.hh" +#include "ode/environment_hexa.hh" +namespace robot +{ + class Hexapod : public Robot + { + public: + Hexapod(ode::Environment_hexa & env, const Eigen::Vector3d & pos,std::vector brokenLegs):_brokenLegs(brokenLegs) + { + _build(env, pos); + } + Hexapod(const Hexapod &o, ode::Environment_hexa & env) : + Robot(o, env) + { + } + boost::shared_ptr clone(ode::Environment_hexa& env) const + { + return boost::shared_ptr(new Hexapod(*this, env)); + } + protected: + std::vector _brokenLegs; + void _build(ode::Environment_hexa& env, const Eigen::Vector3d& pos); + }; +} + +#endif diff --git a/src/robot/hybrid.cc b/src/robot/hybrid.cc new file mode 100644 index 0000000..beccadf --- /dev/null +++ b/src/robot/hybrid.cc @@ -0,0 +1,82 @@ +#include "hybrid.hh" + +#include "ode/box.hh" +#include "ode/capped_cyl.hh" +#include "ode/sphere.hh" +#include "ode/motor.hh" + +using namespace ode; +USING_PART_OF_NAMESPACE_EIGEN + +namespace robot +{ + void Hybrid :: _build(Environment& env, const Vector3d& pos) + { + static const double body_mass = 1; + static const double body_length = 0.75; + static const double body_width = 0.5; + static const double body_height = 0.2; + static const double leg_w = 0.05; + static const double leg_length = 0.5; + static const double leg_dist = 0.2; + static const double leg_mass = 0.2; + static const double wheel_rad = 0.1; + static const double wheel_mass = 0.05; + + _main_body = Object::ptr_t + (new Box(env, pos + Vector3d(0, 0, leg_length), + body_mass, body_length, body_width, body_height)); + _bodies.push_back(_main_body); + + for (size_t i = 0; i < 4; ++i) + { + int left_right = i > 1 ? -1 : 1; + int back_front = i % 2 == 0 ? -1 : 1; + + Object::ptr_t l1 + (new CappedCyl(env, pos + Vector3d(back_front * (leg_dist / 2 + leg_length / 2), + left_right * (body_width / 2 + leg_w), + leg_length), + leg_mass, leg_w, leg_length)); + l1->set_rotation(0, M_PI / 2.0, 0); + _bodies.push_back(l1); + + Servo::ptr_t s1 + (new Servo(env, pos + Vector3d(back_front * leg_dist / 2, + left_right * (body_width / 2 + leg_w), + leg_length), + *_main_body, *l1)); + _servos.push_back(s1); + + Object::ptr_t l11 + (new CappedCyl(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + leg_length / 2 + wheel_rad), + leg_mass, leg_w, leg_length - wheel_rad * 2)); + _bodies.push_back(l11); + Servo::ptr_t s2 + (new Servo(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + leg_length), + *l1, *l11)); + _servos.push_back(s2); + + Object::ptr_t wheel + (new Sphere(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + wheel_rad), + wheel_mass, wheel_rad)); + _bodies.push_back(wheel); + Motor::ptr_t s3 + (new Motor(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + wheel_rad), + Vector3d(0, 1, 0), + *l11, *wheel)); + _motors.push_back(s3); + } + for (size_t i = 0; i < _servos.size(); ++i) + for (size_t j = 0; j < 3; ++j) + _servos[i]->set_lim(j, -M_PI / 3, M_PI / 3); + } +} diff --git a/src/robot/hybrid.hh b/src/robot/hybrid.hh new file mode 100644 index 0000000..4dc47a8 --- /dev/null +++ b/src/robot/hybrid.hh @@ -0,0 +1,17 @@ +#ifndef ROBOT_HYBRYD_HPP +#define ROBOT_HYBRYD_HPP +#include "robot/robot.hh" + +namespace robot +{ + /// a basic hylos-like quadruped hybrid + class Hybrid : public Robot + { + public: + Hybrid(ode::Environment& env, const Eigen::Vector3d& pos) { _build(env, pos); } + protected: + void _build(ode::Environment& env, const Eigen::Vector3d& pos); + }; +} + +#endif diff --git a/src/robot/quadruped.cc b/src/robot/quadruped.cc new file mode 100644 index 0000000..3c63932 --- /dev/null +++ b/src/robot/quadruped.cc @@ -0,0 +1,63 @@ +#include "quadruped.hh" + +#include "ode/box.hh" +#include "ode/capped_cyl.hh" + +using namespace ode; +USING_PART_OF_NAMESPACE_EIGEN + +namespace robot +{ + void Quadruped :: _build(Environment& env, const Vector3d& pos) + { + static const double body_mass = 1; + static const double body_length = 0.75; + static const double body_width = 0.5; + static const double body_height = 0.2; + static const double leg_w = 0.05; + static const double leg_length = 0.5; + static const double leg_dist = 0.2; + static const double leg_mass = 0.2; + + _main_body = Object::ptr_t + (new Box(env, pos + Vector3d(0, 0, leg_length), + body_mass, body_length, body_width, body_height)); + _bodies.push_back(_main_body); + + for (size_t i = 0; i < 4; ++i) + { + int left_right = i > 1 ? -1 : 1; + int back_front = i % 2 == 0 ? -1 : 1; + Object::ptr_t l1 + (new CappedCyl(env, pos + Vector3d(back_front * (leg_dist / 2 + leg_length / 2), + left_right * (body_width / 2 + leg_w), + leg_length), + leg_mass, leg_w, leg_length)); + l1->set_rotation(0, M_PI / 2.0, 0); + _bodies.push_back(l1); + + Ax12::ptr_t s1 + (new Ax12(env, pos + Vector3d(back_front * leg_dist / 2, + left_right * (body_width / 2 + leg_w), + leg_length), + *_main_body, *l1)); + _servos.push_back(s1); + + Object::ptr_t l11 + (new CappedCyl(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + leg_length / 2), + leg_mass, leg_w, leg_length)); + _bodies.push_back(l11); + Ax12::ptr_t s2 + (new Ax12(env, pos + Vector3d(back_front * (leg_length + leg_dist / 2), + left_right * (body_width / 2 + leg_w), + leg_length), + *l1, *l11)); + _servos.push_back(s2); + } + for (size_t i = 0; i < _servos.size(); ++i) + for (size_t j = 0; j < 3; ++j) + _servos[i]->set_lim(j, -M_PI / 3, M_PI / 3); + } +} diff --git a/src/robot/quadruped.hh b/src/robot/quadruped.hh new file mode 100644 index 0000000..d8e676b --- /dev/null +++ b/src/robot/quadruped.hh @@ -0,0 +1,18 @@ +#ifndef ROBOT_QUADRUPED_HPP +#define ROBOT_QUADRUPED_HPP +#include "robot/robot.hh" +#include "ode/ax12.hh" + +namespace robot +{ + /// a basic hylos-like quadruped + class Quadruped : public Robot + { + public: + Quadruped(ode::Environment& env, const Eigen::Vector3d& pos) { _build(env, pos); } + protected: + void _build(ode::Environment& env, const Eigen::Vector3d& pos); + }; +} + +#endif diff --git a/src/robot/robot.hh b/src/robot/robot.hh new file mode 100644 index 0000000..0b92865 --- /dev/null +++ b/src/robot/robot.hh @@ -0,0 +1,74 @@ +#ifndef ROBOT_H_ +#define ROBOT_H_ +#include +#include +#include + +#include "ode/servo.hh" +#include "ode/motor.hh" +#include "ode/object.hh" +#include "ode/visitor.hh" + +namespace robot +{ + class Robot + { + public: + typedef boost::shared_ptr ptr_t; + Robot() {} + Robot(const Robot& o, ode::Environment& env) + { + using namespace ode; + std::map old_to_new; + BOOST_FOREACH(Object::ptr_t b, o._bodies) + { + ode::Object::ptr_t b2 = b->clone(env); + old_to_new[b.get()] = b2; + _bodies.push_back(b2); + } + BOOST_FOREACH(Servo::ptr_t s, o._servos) + _servos.push_back(s->clone(env, + *old_to_new[&s->get_o1()], + *old_to_new[&s->get_o2()])); + + BOOST_FOREACH(Motor::ptr_t s, o._motors) + _motors.push_back(s->clone(env, + *old_to_new[&s->get_o1()], + *old_to_new[&s->get_o2()])); + _main_body = old_to_new[o._main_body.get()]; + } + + virtual ptr_t clone(ode::Environment& env) const { return ptr_t(new Robot(*this, env)); } + + const std::vector& bodies() const { return _bodies; } + std::vector& bodies() { return _bodies; } + + const std::vector& servos() const { return _servos; } + std::vector& servos() { return _servos; } + + const std::vector& motors() const { return _motors; } + std::vector& motors() { return _motors; } + + Eigen::Vector3d pos() const { return _main_body->get_pos(); } + Eigen::Vector3d rot() const { return _main_body->get_rot(); } + Eigen::Vector3d vel() const { return _main_body->get_vel(); } + virtual void accept (ode::ConstVisitor &v) const { v.visit(_bodies); } + + virtual void next_step(double dt = ode::Environment::time_step) + { + BOOST_FOREACH(ode::Object::ptr_t s, _bodies) + s->set_in_contact(false); + BOOST_FOREACH(ode::Servo::ptr_t s, _servos) + s->next_step(dt); + BOOST_FOREACH(ode::Motor::ptr_t s, _motors) + s->next_step(dt); + } + protected: + std::vector _bodies; + std::vector _servos; + std::vector _motors; + ode::Object::ptr_t _main_body; + }; +} + +#endif diff --git a/src/wscript b/src/wscript new file mode 100644 index 0000000..bc773d3 --- /dev/null +++ b/src/wscript @@ -0,0 +1,96 @@ +#! /usr/bin/env python +import os, glob +import Options + +def build(bld): + osg = not bld.all_envs['default']['NO_OSG'] + + # librobdyn + obj = bld.new_task_gen('cxx', 'staticlib') + obj.source = 'ode/motor.cc \ + ode/servo.cc \ + ode/object.cc \ + ode/environment.cc \ + ode/environment_hexa.cc\ + robot/quadruped.cc \ + robot/hybrid.cc \ + robot/hexapod.cc' + obj.includes = '.' + obj.target = 'robdyn' + obj.want_libtool = 1 + obj.uselib = 'ODE BOOST EIGEN2' + + # viewer + if osg: + obj = bld.new_task_gen('cxx', 'staticlib') + obj.source = 'renderer/osg_visitor.cc \ + renderer/osg_hud.cc \ + renderer/keyboard.cc \ + renderer/shadows.cc' + obj.includes = '.' + obj.target = 'robdyn_osgvisitor' + obj.want_libtool = 1 + obj.uselib = ' ODE BOOST EIGEN2 OSG' + + # demo programs are built only if osg is detected + obj = bld.new_task_gen('cxx', 'program') + obj.source = "demos/basic.cc" + obj.includes = '.' + obj.target = 'basic' + obj.uselib = ' ODE BOOST EIGEN2 OSG' + obj.uselib_local = 'robdyn robdyn_osgvisitor' + + obj = bld.new_task_gen('cxx', 'program') + obj.source = "demos/test_mx28.cc" + obj.includes = '.' + obj.target = 'test_mx28' + obj.uselib = ' ODE BOOST EIGEN2 OSG' + obj.uselib_local = 'robdyn robdyn_osgvisitor' + + + obj = bld.new_task_gen('cxx', 'program') + obj.source = "demos/rescue.cc" + obj.includes = '.' + obj.target = 'rescue' + obj.uselib = 'ODE BOOST EIGEN2 OSG' + obj.uselib_local = 'robdyn robdyn_osgvisitor' + + + obj = bld.new_task_gen('cxx', 'program') + obj.source = "demos/quadruped.cc" + obj.includes = '.' + obj.target = 'quadruped' + obj.uselib = 'ODE BOOST EIGEN2 OSG' + obj.uselib_local = 'robdyn robdyn_osgvisitor' + + obj = bld.new_task_gen('cxx', 'program') + obj.source = "demos/walking.cc" + obj.includes = '.' + obj.target = 'walking' + obj.uselib = 'ODE BOOST EIGEN2 OSG' + obj.uselib_local = 'robdyn robdyn_osgvisitor' + + obj = bld.new_task_gen('cxx', 'program') + obj.source = "demos/hexapod.cc demos/controllerPhase6D.cpp " + obj.includes = '.' + obj.target = 'hexapod' + obj.uselib = 'ODE BOOST EIGEN2 OSG' + obj.uselib_local = 'robdyn robdyn_osgvisitor' + + # install all includes (custom-made) + import os, glob, types + p = bld.srcnode.abspath() + '/src/' + r = glob.glob(p + '*/*.hh') + glob.glob(p + '*.hh') + for i in r: + k = os.path.split(i) + d = os.path.split(k[0]) + if d[0][len(d[0]) - 3:len(d[0])] == "src": + bld.install_files('${PREFIX}/include/robdyn/' + d[1], i) + else: + bld.install_files('${PREFIX}/include/robdyn/', i) + #libs + p = bld.srcnode.abspath() + '/bld/default/' + r = glob.glob(p + '*/*.a') + for i in r: + bld.install_files('${PREFIX}/lib/', i) + diff --git a/waf b/waf new file mode 100755 index 0000000..b3aae13 Binary files /dev/null and b/waf differ diff --git a/wscript b/wscript new file mode 100644 index 0000000..f622532 --- /dev/null +++ b/wscript @@ -0,0 +1,82 @@ +#! /usr/bin/env python +VERSION='0.0.1' +APPNAME='robdyn' + +srcdir = '.' +blddir = 'bld' + +import copy +import TaskGen +import Options +import os, sys + +def init(): + pass + +def set_options(opt): + opt.tool_options('compiler_cxx') + opt.tool_options('boost') + opt.tool_options('ode') + opt.tool_options('eigen2') + + opt.add_option('--disable_osg', type='string', help='disable open scene graph support (no visualization)', dest='disable_osg') + opt.add_option('--64bits', type='string', help='enable 64 bits support', dest='bits64') + +def configure(conf): + # log configure options + fname = blddir + '/configure.options' + args = open(fname, 'a') + for i in sys.argv: + args.write(i + ' ') + args.write("\n") + args.close() + + conf.check_tool('compiler_cxx') + ode_found = conf.check_tool('ode') + eigen2_found = conf.check_tool('eigen2') + conf.check_tool('boost') + conf.check_boost(lib='', min_version='1.35') + conf.env['LIB_OSG'] = [ 'GLU', 'osg', 'osgDB', 'osgUtil', 'osgGA', + 'osgViewer', 'OpenThreads', + 'osgFX', 'osgShadow', 'osgTerrain', 'GL'] + + if Options.options.disable_osg == "yes": + osg_flag = ' -DNO_OSG' + conf.env['NO_OSG'] = True + else: + osg_flag = '' + conf.env['NO_OSG'] = False + + if Options.options.bits64: + osg_flag += ' -m64 -DBOOST_NO_INTRINSIC_INT64_T ' + conf.env.append_value("LINKFLAGS", "-m64") + + common_flags = "-D_REENTRANT -Wall -fPIC -ftemplate-depth-128 -Wno-sign-compare -Wno-deprecated -Wno-unused " + osg_flag + cxxflags = conf.env['CXXFLAGS'] + + # release + conf.setenv('default') + opt_flags = common_flags + ' -DNDEBUG -O3 -fomit-frame-pointer -finline-functions -ftracer -funroll-loops -fvariable-expansion-in-unroller -fstrict-aliasing -ffast-math' + + opt_flags += ' -mfpmath=sse -march=core2 -msse2' + + conf.env['CXXFLAGS'] = cxxflags + opt_flags.split(' ') + + # debug + env = conf.env.copy() + env.set_variant('debug') + conf.set_env_name('debug', env) + conf.setenv('debug') + debug_flags = common_flags + '-O0 -ggdb3 -DDBG_ENABLED' + conf.env['CXXFLAGS'] = cxxflags + debug_flags.split(' ') + + + +def build(bld): + print ("Entering into directory " + os.getcwd()) + bld.add_subdirs('src') + for obj in copy.copy(bld.all_task_gen): + obj.clone('debug') + +def shutdown (): + pass