Homec4science
Diffusion liboncilla (biorob-cmake-utils) Browse

liboncilla/sim/webots/controller/WebotsSynchronizer.cpp
a92ac2ba5450biorob-cmake-utils

Last Change
Raw File

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";
}
}
}
}
c4science · Help