Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F74788496
walkFourLegs2.cpp
No One
Temporary
Actions
Download File
Edit File
Delete File
View Transforms
Subscribe
Mute Notifications
Award Token
Subscribers
None
File Metadata
Details
File Info
Storage
Attached
Created
Mon, Jul 29, 17:16
Size
6 KB
Mime Type
text/x-c
Expires
Wed, Jul 31, 17:16 (1 d, 21 h)
Engine
blob
Format
Raw Data
Handle
19436645
Attached To
R6622 liboncilla
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;
}
Event Timeline
Log In to Comment