Homec4science
Diffusion liboncilla (dev) Browse

liboncilla/examples/walkFourLegs2.cpp
3606993d1160dev

Last Change
Raw File

walkFourLegs2.cpp
View Options

#include <iostream>
#include <fstream>
#include <stdlib.h>
#include <math.h>
#include <ctime>
#include <rsc/logging/LoggerFactory.h>
#include <boost/thread/recursive_mutex.hpp>
#include <boost/thread/condition.hpp>
#include <boost/timer.hpp>
#include <boost/date_time/posix_time/posix_time.hpp>
#include <boost/program_options.hpp>
#include <rsb/Informer.h>
#include <rsb/Handler.h>
#include <rsb/Factory.h>
#include <rsb/converter/Repository.h>
#include <rsb/converter/ProtocolBufferConverter.h>
#include <cca/Beat.h>
#include <cca/nodes/Junction.h>
#include <cca/nodes/Collector.h>
#include <cca/processing/RestlessProcessing.h>
#include <cca/processing/ModerateProcessing.h>
#include <rci/nodes/SimpleOscillator.h>
#include <rci/dto/JointAngles.h>
#include <rci/dto/converter/JointAnglesConverter.h>
using namespace std;
using namespace boost;
using namespace boost::program_options;
using namespace rsc;
using namespace rsc::logging;
using namespace rsb;
using namespace rsb::converter;
using namespace rst::converter::rci;
using namespace cca;
using namespace cca::rci;
using namespace cca::rci::driver;
/**
* This example needs AMARSi controller "cca2"
*/
int main(int ac, char **av) {
double cycletime = 10;
double amplitude = 0.5;
double offset = 0.0;
double frequency = 1;
LoggerFactory::getInstance().reconfigure(Logger::LEVEL_ERROR);
/**
* Command line handling
*/
options_description desc("Allowed options");
desc.add_options()("help", "Produce help message")("frequency",
value<double> (), "Frequency of oscillation in Hertz")("amplitude",
value<double> (), "Amplitude of the sine")("offset",
value<double> (), "Offset to the output value");
variables_map vm;
store(parse_command_line(ac, av, desc), vm);
notify(vm);
if (vm.count("help")) {
cout << desc << "\n";
return 1;
}
if (vm.count("frequency")) {
frequency = vm["frequency"].as<double> ();
}
cout << "Frequency set to " << frequency << " Hertz.\n";
if (vm.count("amplitude")) {
amplitude = vm["amplitude"].as<double> ();
}
cout << "Amplitude set to " << amplitude << ".\n";
if (vm.count("offset")) {
offset = vm["offset"].as<double> ();
}
cout << "Offset set to " << offset << ".\n";
/**
* Setup main beat
*/
GlobalBeatPtr heartbeat;
heartbeat = GlobalBeat::create(cycletime);
/**
* Setup the two oscillators
*/
CCANodePtr oscillator1 = SimpleOscillator<JointAngles>::create(
"Oscillator 1", 2 * frequency, amplitude, offset);
oscillator1->configureOutputPort(0,
PortConfiguration::LOCAL("/oscillate/1"));
oscillator1->setProcessingStrategy(TimedProcessing::create(1));
heartbeat->registerReceiver(oscillator1);
CCANodePtr oscillator2 = SimpleOscillator<JointAngles>::create(
"Oscillator 2", frequency, amplitude, offset, 0.5);
oscillator2->configureOutputPort(0,
PortConfiguration::LOCAL("/oscillate/2"));
oscillator2->setProcessingStrategy(TimedProcessing::create(1));
heartbeat->registerReceiver(oscillator2);
/**
* Set up two junctions
*/
CCANodePtr junction1 = Junction<JointAngles>::create(4);
junction1->setProcessingStrategy(RestlessProcessing::create());
CCANodePtr junction2 = Junction<JointAngles>::create(4);
junction2->setProcessingStrategy(RestlessProcessing::create());
/**
* Register converters for Joint Angles
*/
boost::shared_ptr<JointAnglesConverter> converter(
new JointAnglesConverter());
stringConverterRepository()->registerConverter(converter);
/**
* Connect oscillators to junction
*/
junction1->configureInputPort(0, PortConfiguration::LOCAL("/oscillate/1"));
junction1->configureOutputPort(0,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lfhip"));
junction1->configureOutputPort(1,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lhhip"));
junction1->configureOutputPort(2,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rfknee"));
junction1->configureOutputPort(3,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rhknee"));
junction2->configureInputPort(0, PortConfiguration::LOCAL("/oscillate/2"));
junction2->configureOutputPort(0,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rfhip"));
junction2->configureOutputPort(1,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rhhip"));
junction2->configureOutputPort(2,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lfknee"));
junction2->configureOutputPort(3,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lhknee"));
/**
* Collect all joint angles to send them at once
*/
CCANodePtr collector = Collector<JointAngles>::create(8);
collector->setProcessingStrategy(ModerateProcessing::create());
collector->configureInputPort(0,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lfhip"));
collector->configureInputPort(1,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rfhip"));
collector->configureInputPort(2,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lhhip"));
collector->configureInputPort(3,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rhhip"));
collector->configureInputPort(4,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lfknee"));
collector->configureInputPort(5,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rfknee"));
collector->configureInputPort(6,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/lhknee"));
collector->configureInputPort(7,
PortConfiguration::LOCAL("/oncilla/command/nodes/angles/rhknee"));
collector->configureOutputPort(0,
PortConfiguration::REMOTE("/oncilla/command/nodes/angles/all"));
std::cout << std::endl << "Two oscillators oscillating between " << (offset
- amplitude) << " and " << (offset + amplitude) << " with "
<< frequency << " Hertz ..." << std::endl;
heartbeat->run();
return EXIT_SUCCESS;
}
c4science · Help