Page Menu
Home
c4science
Search
Configure Global Search
Log In
Files
F88014208
WebotsSynchronizer.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
Wed, Oct 16, 07:57
Size
5 KB
Mime Type
text/x-c
Expires
Fri, Oct 18, 07:57 (2 d)
Engine
blob
Format
Raw Data
Handle
21699212
Attached To
R6622 liboncilla
WebotsSynchronizer.cpp
View Options
/**
* \file WebotsSynchronizer.cpp
*
* \date Aug 22, 2010
* \author tuleu
* \author anordman
*/
//#define TEST_ACTUATOR_IH_PRESENCE
#include <libamarsi-quad/utils/PreferenceReader.h>
#include "WebotsSynchronizer.h"
#include <cmath>
#include <iostream>
#include <boost/format.hpp>
#define PI 3.1415265
using namespace std;
using namespace boost;
using namespace amarsi;
namespace cca {
namespace rci {
namespace driver {
WebotsSynchronizer::WebotsSynchronizer(int& argc, char** argv) :
Synchronizer("Webots Synchronizer"), Controller(argc, argv),
_running(true), d_amplitudeHip(0.3), d_offsetHip(0),
d_amplitudeKnee(0.4), d_offsetKnee(0.6), d_frequency(0.5),
_actuatorNames(amarsi::NUMBER_OF_ACTUATORS),
_servoNames(amarsi::NUMBER_OF_ACTUATORS) {
// FIXME: 1000* is nonsense
this->_nodes.reserve(1000 * (unsigned int) amarsi::NUMBER_OF_ACTUATORS); // Number of actuators is number of nodes in general
this->prepareNodeWebotsMapping();
_primpleAmarsi
= static_cast<ControllerPrimpleAmarsi*> (Controller::pimpController_);
// Initialize simulation
Supervisor::instance().initSimulation();
cout << "[WebotsSynchronizer] Webots simulator initialized." << endl;
}
WebotsSynchronizer::~WebotsSynchronizer() {
Supervisor::instance().simulationQuit();
cout << "[WebotsSynchronizer] Ended simulation" << endl;
}
void WebotsSynchronizer::userInitSimulation() {
//add some motion restriction
Supervisor::instance().configureMotionRestriction(
"0.0:SAGITAL_FIXED_PITCH|2.0:SAGITAL_FREE_PITCH|4.0:FREE");
Supervisor::instance().setTrackingOn();
Supervisor::instance().setDisplayMode(amarsi::SHOW_GROUND_REACTION_FORCES);
}
void WebotsSynchronizer::userInitEnvironment() {
//put the robot on a slope of angle d_angle in the front direction.
Supervisor::instance().setSlope(true, d_angle);
}
int WebotsSynchronizer::run() {
while (true) {
step();
}
return 0;
}
/* CCA-specific */
CCANodePtr WebotsSynchronizer::create(int &argc, char** argv) {
return CCANodePtr(new WebotsSynchronizer(argc, argv));
}
std::string WebotsSynchronizer::print() const {
format fmter("<Synchronizer \"%1%\">");
fmter % this->getName();
return fmter.str();
}
void WebotsSynchronizer::send() {
double value;
if (this->_running) {
// Sending commands to the hip servos
for (unsigned int index = (unsigned int) amarsi::LEFT_FORE_HIP; index
<= (unsigned int) amarsi::RIGHT_HIND_HIP; ++index) {
value
= static_pointer_cast<OncillaHip> (
this->_nodes[(unsigned int) index])->getLastPositionCommand()->asDouble(
0);
actuator(static_cast<amarsi::ActuatorID> (index)).setCommand(value);
}
// Sending commands to the knee servos
for (unsigned int index = (unsigned int) amarsi::LEFT_FORE_KNEE; index
<= (unsigned int) amarsi::RIGHT_HIND_KNEE; ++index) {
value
= static_pointer_cast<OncillaKnee> (
this->_nodes[(unsigned int) index])->getLastPositionCommand()->asDouble(
0);
actuator(static_cast<amarsi::ActuatorID> (index)).setCommand(value);
}
}
}
void WebotsSynchronizer::receive() {
double value;
if (this->_running) {
// Reading encoder values from the hips
for (unsigned int index = (unsigned int) amarsi::LEFT_FORE_HIP; index
<= (unsigned int) amarsi::RIGHT_HIND_HIP; ++index) {
value = this->_primpleAmarsi->getWebotsServo(
this->_servoNames[(unsigned int) index])->getPosition();
static_pointer_cast<OncillaHip> (
this->_nodes[(unsigned int) index])->updateJointPosition(
JointAngles::fromValue(value));
}
// Reading encoder values from the knees
for (unsigned int index = (unsigned int) amarsi::LEFT_FORE_KNEE; index
<= (unsigned int) amarsi::RIGHT_HIND_KNEE; ++index) {
value = this->_primpleAmarsi->getWebotsServo(
this->_servoNames[(unsigned int) index])->getPosition();
static_pointer_cast<OncillaKnee> (
this->_nodes[(unsigned int) index])->updateJointPosition(
JointAngles::fromValue(value));
}
}
}
void WebotsSynchronizer::registerQuadNode(CCANodePtr node, ActuatorID index) {
// Try to register node
ResourceNodePtr rn = static_pointer_cast<ResourceNode> (node);
std::cout << "[WebotsSynchronizer] Register node " << node->getName()
<< " at position #" << (unsigned int) index << std::endl;
this->registerNode(rn, (unsigned int) index);
}
void WebotsSynchronizer::prepareNodeWebotsMapping() {
this->_actuatorNames.reserve((unsigned int) amarsi::NUMBER_OF_ACTUATORS);
this->_servoNames.reserve((unsigned int) amarsi::NUMBER_OF_ACTUATORS);
// Hip Servos
this->_servoNames[(unsigned int) amarsi::LEFT_FORE_HIP]
= "LEFT_FORE_hip_servo";
this->_servoNames[(unsigned int) amarsi::RIGHT_FORE_HIP]
= "RIGHT_FORE_hip_servo";
this->_servoNames[(unsigned int) amarsi::LEFT_HIND_HIP]
= "LEFT_HIND_hip_servo";
this->_servoNames[(unsigned int) amarsi::RIGHT_HIND_HIP]
= "RIGHT_HIND_hip_servo";
// Knee Servos
this->_servoNames[(unsigned int) amarsi::LEFT_FORE_KNEE]
= "LEFT_FORE_FRONT_KNEE_SERVO";
this->_servoNames[(unsigned int) amarsi::RIGHT_FORE_KNEE]
= "RIGHT_FORE_FRONT_KNEE_SERVO";
this->_servoNames[(unsigned int) amarsi::LEFT_HIND_KNEE]
= "LEFT_HIND_FRONT_KNEE_SERVO";
this->_servoNames[(unsigned int) amarsi::RIGHT_HIND_KNEE]
= "RIGHT_HIND_FRONT_KNEE_SERVO";
}
}
}
}
Event Timeline
Log In to Comment