Diffusion liboncilla Webots Interface (master) Browse

OncillaWebotsSynchronizer.cpp
f37407b8fc4dmaster

OncillaWebotsSynchronizer.cpp
View Options

			/*
			* OncillaWebotsSynchronizer.cpp
			*
			* Created on: 15 d�c. 2011
			* Author: Alexandre Tuleu
			*/
			#include "OncillaWebotsSynchronizer.h"
			#include "WebotsNodeAdapters.h"
			#include <liboncilla/BackendFactory.h>

			#include <libwebots/communication/client.hh>
			#include <libwebots/plugin/modules/jointmanager/jointmanagerbuilder.hh>
			#include <libwebots/plugin/modules/debug/debugbuilder.hh>



			namespace c = liboncilla::webots::config;
			using namespace rci::oncilla;

			extern "C" {
			void liboncilla_webots_backend_entry(){
			BackendFactory::Instance().RegisterBackend(BackendFactory::SIMULATION,
			&OncillaWebotsSynchronizer::Create);
			}
			}

			OncillaWebotsSynchronizer::LegNames OncillaWebotsSynchronizer::s_webotsPrefix;

			rci::oncilla::OncillaSynchronizer * OncillaWebotsSynchronizer::Create(){
			return (new OncillaWebotsSynchronizer(new webots::Supervisor(),
			"Oncilla synchronizer"));
			}

			//OncillaWebotsSynchronizer::LegNames OncillaWebotsSynchronizer::s_webotsPrefix;


			OncillaWebotsSynchronizer::OncillaWebotsSynchronizer(webots::Supervisor * supervisor,
			const std::string & name)
			: rci::oncilla::OncillaSynchronizer(name)
			, d_supervisor(supervisor)
			, d_initialized(false){
			using namespace libwebots::communication;
			using namespace libwebots::plugin::modules;
			if(s_webotsPrefix.empty()){
			WebotsNodeWrapper::SetSupervisor(supervisor);
			s_webotsPrefix.push_back("LEFT_FORE");
			s_webotsPrefix.push_back("RIGHT_FORE");
			s_webotsPrefix.push_back("LEFT_HIND");
			s_webotsPrefix.push_back("RIGHT_HIND");
			}
			c::Config::Instance().LoadConfigFile();
			d_legParameters.assign(NUM_LEGS,LegParameters());
			Client & client = Client::Instance();
			client.Connect();
			libwebots::plugin::modules::messages::debug::Debug message;
			if(c::Config::Instance().Main().DebugLibwebots()){
			message.set_action(messages::debug::Action::Enable);
			message.set_type(messages::debug::Type::Info);
			client.Send(Messages::Create(messages::debug::debug,message));
			}

			for(ListOfLegParameters::iterator p = d_legParameters.begin();
			p != d_legParameters.end();
			++p){
			Leg l = static_cast<Leg> (p - d_legParameters.begin());
			ExtractLegParametersAndInitModel(l,*p);
			}
			}

			OncillaWebotsSynchronizer::~OncillaWebotsSynchronizer() {
			}

			void OncillaWebotsSynchronizer::processAsync(){

			while(d_supervisor->getTime() < c::Config::Instance().Main().InitTime()){
			Step();
			}

			//start the processing, we just send all commands
			for(ListOfAdpater::iterator a = d_adapters.begin();
			a != d_adapters.end();
			++a){
			(*a)->SendCommand();
			}
			}

			void OncillaWebotsSynchronizer::waitForProcessAsync(){
			//now we step and read the sensors.
			double ts = latestProcessLoopDurationPrimpl();
			Step();
			for(ListOfAdpater::iterator a = d_adapters.begin();
			a != d_adapters.end();
			++a){
			(*a)->Step(ts);
			(*a)->ReadSensors();
			}
			}


			double OncillaWebotsSynchronizer::latestProcessLoopDurationPrimpl() const {
			return std::max(1.0,d_supervisor->getBasicTimeStep()) / 1000.0;
			}


			bool OncillaWebotsSynchronizer::start(){

			bool toReturn =this->rci::Synchronizer::start();
			for(ListOfAdpater::iterator a = d_adapters.begin();
			a != d_adapters.end();
			++a){
			(*a)->Activate();
			}
			return toReturn;
			}



			void OncillaWebotsSynchronizer::registerTrunkNode(OncillaTrunkPtr){
			//TODO implement method
			}

			void OncillaWebotsSynchronizer::registerL0Node(Leg l,
			OncillaL0Ptr node){
			std::string servoName(s_webotsPrefix[l]);
			WebotsNodeAdapter * n = new WebotsL0Adapter(node,
			d_supervisor->getServo(servoName.append("_L_0")));
			d_adapters.push_back(WebotsNodeAdapterPtr(n));
			}

			void OncillaWebotsSynchronizer::registerL1Node(Leg l,
			OncillaL1Ptr node)
			{
			std::string servoName(s_webotsPrefix[l]);
			servoName.append("_L_1");
			d_adapters.push_back(WebotsNodeAdapterPtr(
			new WebotsL1Adapter(node,
			d_supervisor->getServo(servoName),
			WebotsNodeWrapper(servoName))));
			}

			void OncillaWebotsSynchronizer::registerL2Node(Leg l, OncillaL2Ptr node)
			{
			using namespace liboncilla::webots::config;
			std::string servoName(s_webotsPrefix[l]);

			d_adapters.push_back(WebotsNodeAdapterPtr(
			new WebotsL2Adapter(node,
			GetJointIdentifier(l,D1_D2),
			d_supervisor->getServo(servoName.append("_L_2")),
			LegConfigSection(l))));
			}

			void OncillaWebotsSynchronizer::registerL3Node(Leg l, OncillaL3Ptr node)
			{
			std::string servoName(s_webotsPrefix[l]);
			d_adapters.push_back(WebotsNodeAdapterPtr(
			new WebotsL3Adapter(node,
			d_supervisor->getServo(servoName.append("_L_3")))));
			}

			void OncillaWebotsSynchronizer::registerL4Node(Leg l, OncillaL4Ptr node)
			{
			d_adapters.push_back(WebotsNodeAdapterPtr(new WebotsL4Adapter(node,
			s_webotsPrefix[l] + "_FOOT")));
			}



			bool OncillaWebotsSynchronizer::stop(){
			for(ListOfAdpater::iterator a = d_adapters.begin();
			a != d_adapters.end();
			++a){
			(*a)->DeActivate();
			}
			return this->rci::Synchronizer::stop();
			}


			const LegParameters &
			OncillaWebotsSynchronizer::GetLegParameters(Leg l){
			if(l >= NUM_LEGS \|\| l < 0){
			return d_legParameters[NUM_LEGS];
			}

			LegParameters & to_ret = d_legParameters[l];
			if(to_ret.config == LegParameters::UNDEFINED){
			ExtractLegParametersAndInitModel(l,to_ret);
			}

			return to_ret;
			}



			void OncillaWebotsSynchronizer::ExtractLegParametersAndInitModel(Leg l,
			LegParameters & params){
			std::string l1Name(s_webotsPrefix[l]), l2Name(s_webotsPrefix[l]),
			l3Name(s_webotsPrefix[l]), footName(s_webotsPrefix[l]),
			p1Name(s_webotsPrefix[l]), d2Name(s_webotsPrefix[l]),
			p2Name(s_webotsPrefix[l]), d1Name(s_webotsPrefix[l]);


			l1Name.append("_L_1");
			l2Name.append("_L_2");
			l3Name.append("_L_3");
			p1Name.append("_P_1");
			p2Name.append("_P_2");
			footName.append("_FOOT");
			d1Name.append("_D_1");
			d2Name.append("_D_2");



			const c::LegSection & lConf = LegConfigSection(l);

			ExtractLegParameters(WebotsNodeWrapper(l1Name),
			WebotsNodeWrapper(l2Name),
			WebotsNodeWrapper(l3Name),
			WebotsNodeWrapper(p1Name),
			WebotsNodeWrapper(footName),
			d2Name,
			params);
			params.config = LegParameters::DEFINED;

			//Now we init the model
			params.jD1D2 = GetJointIdentifier(l,D1_D2);

			using namespace libwebots::communication;
			using namespace libwebots::plugin::modules::messages;
			using namespace libwebots::plugin::modules::messages;

			Client & client = Client::Instance();
			//create the missing joints
			client.Send(builders::JointManager::CreateHingeJoint(GetJointIdentifier(l,L2_L3),
			l2Name,
			l3Name,
			jointmanager::JointDefinition::CHILD,
			params.posOfD2inL3,
			params.l2L3AxisInL3));

			client.Send(builders::JointManager::CreateHingeJoint(GetJointIdentifier(l,D2_L3),
			d2Name,
			l3Name,
			jointmanager::JointDefinition::CHILD,
			params.posOfD2inL3,
			params.l2L3AxisInL3));

			client.Send(builders::JointManager::AddManagedjoint(GetJointIdentifier(l,D1_D2),
			d1Name,d2Name));

			lConf.Diagonal().SendJointDefinition(GetJointIdentifier(l,D1_D2));

			///\todo refactor this
			if(params.config & LegParameters::OPEN_PANTOGRAPH){
			client.Send(builders::JointManager::AddManagedjoint(GetJointIdentifier(l,P1_P2),
			p1Name,p2Name));
			lConf.Parallel().SendJointDefinition(GetJointIdentifier(l,P1_P2));

			}

			if(params.config & LegParameters::SPRING_FOOT){
			client.Send(builders::JointManager::AddManagedjoint(GetJointIdentifier(l,L3_FOOT),
			l3Name,footName));
			lConf.Foot().SendJointDefinition(GetJointIdentifier(l,L3_FOOT));
			}
			}



			void OncillaWebotsSynchronizer::ExtractLegParameters(const WebotsNodeWrapper & l1,
			const WebotsNodeWrapper & l2,
			const WebotsNodeWrapper & l3,
			const WebotsNodeWrapper & p1,
			const WebotsNodeWrapper & foot,
			const std::string & d2name,
			LegParameters & params){
			params.l1 = GetAbsMaxOfArray(l2.GetField<const double *>("translation"));
			params.l2 = GetAbsMaxOfArray(l3.GetField<const double *>("translation"));
			const double * fPos = foot.GetField<const double *>("translation");
			params.l3 = GetAbsMaxOfArray(fPos);

			params.l2L3AxisInL3 = Eigen::Vector3d(l3.GetField<webots::Field*>("rotation")->getSFRotation());
			params.deltaL = std::abs(GetAbsMaxOfArray(p1.GetField<const double *>("translation"))-params.l1);

			//now look for the maximal index of the position of the foot
			unsigned int index(0);
			bool posDirection= fPos[0] < 0.0;
			for(unsigned int i = 1; i < 3; ++i){
			if(std::abs(fPos[i]) > std::abs(fPos[index])){
			index = i;
			}
			}

			params.posOfD2inL3.setZero(3,1);
			double offset = params.l3 / 2.0 - params.deltaL;
			params.posOfD2inL3.coeffRef(index) = (posDirection ? 1.0 : -1.0) * offset;

			params.config = LegParameters::DEFINED;

			if(foot.Type() == webots::Node::SERVO){
			params.config = static_cast<LegParameters::Config>(params.config \| LegParameters::SPRING_FOOT);
			}

			if(!d_supervisor->getFromDef(d2name)){
			params.config = static_cast<LegParameters::Config>(params.config \| LegParameters::OPEN_PANTOGRAPH);
			}


			}


			const c::LegSection & OncillaWebotsSynchronizer::LegConfigSection(Leg l){
			if(l == LEFT_FORE \|\| l == RIGHT_FORE){
			return c::Config::Instance().ForeLeg();
			} else {
			return c::Config::Instance().HindLeg();
			}
			}


			void OncillaWebotsSynchronizer::Step(){
			d_supervisor->step(std::max(1.0,d_supervisor->getBasicTimeStep()));
			libwebots::communication::Client::Instance().ProcessMessages();
			}

liboncilla-webots/src/liboncilla-webots/OncillaWebotsSynchronizer.cppf37407b8fc4dmaster

OncillaWebotsSynchronizer.cppView Options

liboncilla-webots/src/liboncilla-webots/OncillaWebotsSynchronizer.cpp
f37407b8fc4dmaster

OncillaWebotsSynchronizer.cpp
View Options