diff --git a/src/liboncilla-webots/WebotsOncillaBackend.cpp b/src/liboncilla-webots/WebotsOncillaBackend.cpp index e08cb0b..27a4f51 100644 --- a/src/liboncilla-webots/WebotsOncillaBackend.cpp +++ b/src/liboncilla-webots/WebotsOncillaBackend.cpp @@ -1,299 +1,303 @@ /* * WebotsOncillaBackend.cpp * * Created on: Jan 22, 2013 * Author: tuleu */ #include "WebotsOncillaBackend.h" #include "Synchronizer.h" #include #include #include #include #include #include #include #include #include "WebotsNodeWrapper.h" #include using namespace boost; namespace ro = rci::oncilla; namespace low = liboncilla::webots; BIOROB_CPP_EXPORT_PLUGIN(OncillaBackend,WebotsOncillaBackend) const char * WebotsOncillaBackend::LEG_PREFIXES[4] = {"LEFT_FORE", "RIGHT_FORE", "LEFT_HIND", "RIGHT_HIND"}; //we don't accept any throw in this constructor !!!! WebotsOncillaBackend::WebotsOncillaBackend() : d_supervisor(new webots::Supervisor()) , d_synchronizer() , d_legParameters(){ //this constructor should not fail, never otherwise the exception is not //propagated to webots. Initialization is Lazily performed in InitIfNeeded() } WebotsOncillaBackend::~WebotsOncillaBackend(){ } shared_ptr WebotsOncillaBackend::CreateSynchronizer(){ InitIfNeeded(); return boost::static_pointer_cast(d_synchronizer); } boost::shared_ptr WebotsOncillaBackend::CreateL0(rci::oncilla::Leg leg , const std::string& name){ InitIfNeeded(); return ro::L0::Ptr(new low::L0(name, d_supervisor->getServo(LEG_PREFIXES[leg] + std::string("_L_0")))); } boost::shared_ptr WebotsOncillaBackend::CreateL1(rci::oncilla::Leg leg , const std::string& name){ InitIfNeeded(); std::string servoName (LEG_PREFIXES[leg]); servoName += "_L_1"; return ro::L1::Ptr(new low::L1(*d_synchronizer, name, d_supervisor->getServo(servoName), WebotsNodeWrapper(*d_supervisor, servoName))); } boost::shared_ptr WebotsOncillaBackend::CreateL2(rci::oncilla::Leg leg , const std::string& name){ InitIfNeeded(); std::string servoPrefix (LEG_PREFIXES[leg]); return ro::L2::Ptr(new low::L2(*d_synchronizer, name, GetJointIdentifier(leg,D1_D2), d_supervisor->getServo(servoPrefix + "_L_2"), d_supervisor->getServo(servoPrefix + "_D_2"), LegConfigSection(leg))); } boost::shared_ptr WebotsOncillaBackend::CreateL3(rci::oncilla::Leg leg , const std::string& name){ InitIfNeeded(); std::string prefix(LEG_PREFIXES[leg]); return ro::L3::Ptr(new low::L3(name, d_supervisor->getServo(prefix + "_L_3"))); } boost::shared_ptr WebotsOncillaBackend::CreateTrunk(){ InitIfNeeded(); return ro::Trunk::Ptr(new low::Trunk(d_supervisor->getFromDef("ONCILLA"))); } boost::shared_ptr WebotsOncillaBackend::CreateSupervisorTrunk(){ InitIfNeeded(); return ro::SupervisorTrunk::Ptr(new low::SupervisorTrunk(d_supervisor->getFromDef("ONCILLA"))); } boost::shared_ptr WebotsOncillaBackend::CreateSupervisorL4(rci::oncilla::Leg leg , const std::string& name){ InitIfNeeded(); - return ro::SupervisorL4::Ptr(new low::SupervisorL4(name)); + std::string prefix(LEG_PREFIXES[leg]); + return ro::SupervisorL4::Ptr(new low::SupervisorL4(name, + *d_supervisor, + d_config.Contact(), + prefix + "_FOOT")); } void WebotsOncillaBackend::InitIfNeeded(){ if(!d_synchronizer){ Init(); } } void WebotsOncillaBackend::Init(){ d_config.LoadConfigFile(); d_synchronizer = SynchronizerPtr(new low::Synchronizer(d_supervisor, d_config.Main())); d_legParameters.assign(rci::oncilla::NUM_LEGS,LegParameters()); for(ListOfLegParameters::iterator lp = d_legParameters.begin(); lp != d_legParameters.end(); ++lp){ rci::oncilla::Leg l = (rci::oncilla::Leg)(lp - d_legParameters.begin()); ExtractLegParametersAndInitModel(l,*lp); } } void WebotsOncillaBackend::ExtractLegParametersAndInitModel(rci::oncilla::Leg l, LegParameters & params){ if(l < 0 || l >= rci::oncilla::NUM_LEGS){ std::ostringstream os; os << "Leg id " << l << " is out of range ([0 , " << rci::oncilla::NUM_LEGS << "]."; throw std::runtime_error(os.str()); } namespace c = liboncilla::webots::config; std::string l1Name(LEG_PREFIXES[l]), l2Name(LEG_PREFIXES[l]), l3Name(LEG_PREFIXES[l]), footName(LEG_PREFIXES[l]), p1Name(LEG_PREFIXES[l]), d2Name(LEG_PREFIXES[l]), p2Name(LEG_PREFIXES[l]), d1Name(LEG_PREFIXES[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(*d_supervisor,l1Name), WebotsNodeWrapper(*d_supervisor,l2Name), WebotsNodeWrapper(*d_supervisor,l3Name), WebotsNodeWrapper(*d_supervisor,p1Name), WebotsNodeWrapper(*d_supervisor,footName), d2Name, params); log(debug, rci::oncilla::Oncilla::nameOfLeg(l), " leg has dimensions l1=", params.l1, " l2=" , params.l2, " l3=" , params.l3, " deltaL=", params.deltaL); 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; double drawSize = 0.04; 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, drawSize)); client.Send(builders::JointManager::CreateHingeJoint(GetJointIdentifier(l,D2_L3), d2Name, l3Name, jointmanager::JointDefinition::CHILD, params.posOfD2inL3, params.l2L3AxisInL3, drawSize)); 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)); } } template bool abs_compare( T a, T b){ return std::abs(a) < std::abs(b); } inline double GetAbsMaxOfArray(const double * val, unsigned int size){ return std::abs(*std::max_element(val,val+size,abs_compare)); } void WebotsOncillaBackend::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("translation"),3); params.l2 = GetAbsMaxOfArray(l3.GetField("translation"),3); const double * fPos = foot.GetField("translation"); params.l3 = GetAbsMaxOfArray(fPos,3); params.l2L3AxisInL3 = Eigen::Vector3d(l3.GetField("rotation")->getSFRotation()); params.deltaL = std::abs(GetAbsMaxOfArray(p1.GetField("translation"),3)-params.l1); //now look for the maximal index of the position of the foot unsigned int index(0); for(unsigned int i = 1; i < 3; ++i){ if(std::abs(fPos[i]) > std::abs(fPos[index])){ index = i; } } bool posDirection= fPos[index] < 0.0; 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(params.config | LegParameters::SPRING_FOOT); } if(!d_supervisor->getFromDef(d2name)){ params.config = static_cast(params.config | LegParameters::OPEN_PANTOGRAPH); } } uint32_t WebotsOncillaBackend::GetJointIdentifier(rci::oncilla::Leg l , JointIdentifier j){ return (j << 8) + l; } const liboncilla::webots::config::LegSection & WebotsOncillaBackend::LegConfigSection (rci::oncilla::Leg l){ using namespace rci::oncilla; if(l == LEFT_FORE || l == RIGHT_FORE){ return d_config.ForeLeg(); } else { return d_config.HindLeg(); } } diff --git a/src/liboncilla-webots/nodes/SupervisorL4.cpp b/src/liboncilla-webots/nodes/SupervisorL4.cpp index 154706b..c44d0b4 100644 --- a/src/liboncilla-webots/nodes/SupervisorL4.cpp +++ b/src/liboncilla-webots/nodes/SupervisorL4.cpp @@ -1,22 +1,132 @@ /** * \file SupervisorL4.cpp * * \date Jan 22, 2013 * \author tuleu */ #include "SupervisorL4.h" #include +#include +#include + namespace liboncilla { namespace webots { -SupervisorL4::SupervisorL4(const std::string & name) - : rci::oncilla::SupervisorL4(name){ +SupervisorL4::ContactManager * SupervisorL4::s_contactManager(NULL); +const char * SupervisorL4::ContactManager::GROUND_NAME= "GROUND"; + + + +SupervisorL4::SupervisorL4(const std::string & name, + ::webots::Supervisor & supervisor, + const liboncilla::webots::config::ContactSection & config, + const std::string & footNodeName) + : rci::oncilla::SupervisorL4(name) + , d_node(supervisor.getFromDef(footNodeName)){ + if(d_node == NULL){ + throw std::runtime_error("Could not find suitable node " + footNodeName + + " for SupervisorL4" + name); + } + + if(s_contactManager == NULL ){ + s_contactManager = new ContactManager(); + } + s_contactManager->RegisterNewContact(footNodeName,config,this); + } SupervisorL4::~SupervisorL4(){ + s_contactManager->Unregister(this); +} + +void SupervisorL4::PreStep(double){ + d_force.setZero(); +} + + +void SupervisorL4::PostStep(double){ + const double * P(d_node->getPosition()); + const double * R(d_node->getOrientation()); + + if(P == NULL || R == NULL){ + throw std::runtime_error("COuld not read position for " + this->nodename); + } + + updatePose(rci::Translation::fromMeters(P[0],P[1],P[2]), + rci::Orientation::fromRotationMatrix(R[0],R[1],R[2], + R[3],R[4],R[5], + R[6],R[7],R[8])); + + updateForces(rci::ForcesPtr(new rci::Forces(d_force[0],d_force[1],d_force[2]))); +} + + +SupervisorL4::ContactManager::ContactManager(){ + libwebots::communication::Client::Instance().AddHandler(*this, + libwebots::plugin::modules::messages::contact::contactmonitor, + &ContactManager::Handler); +} + +SupervisorL4::ContactManager::~ContactManager(){ +} + +void SupervisorL4::ContactManager::Handler(const MonitorMessage & m){ + NodeByName::const_iterator n = d_nodes.find(m.name()); + if( n == d_nodes.end()){ + n = d_nodes.find(m.other()); + } + + if( n == d_nodes.end()){ + return; + } + + unsigned int nbContact = m.force_size() / 3; + for(unsigned int i = 0; i < nbContact; ++i){ + + n->second->d_force += Eigen::Vector3d(m.force(3 * i), + m.force(3 * i + 1), + m.force(3 * i + 2)); + + } +} + + +void SupervisorL4::ContactManager::RegisterNewContact(const std::string & name, + const liboncilla::webots::config::ContactSection & config, + SupervisorL4 * node){ + if(d_nodes.insert(std::make_pair(name,node)).second == false){ + return; + } + if(d_names.insert(std::make_pair(node,name)).second == false){ + d_nodes.erase(name); + return; + } + + using namespace libwebots::communication; + using namespace libwebots::plugin::modules::messages::contact; + Contact m; + m.set_body1(name); + m.set_body2(GROUND_NAME); + m.set_coulombfriction(config.CoulombFriction()); + m.set_draw(config.Draw()); + m.set_monitor(true); + + + Client::Instance().Send(Messages::Create(contact,m)); + + +} + +void SupervisorL4::ContactManager::Unregister(SupervisorL4 * node){ + NameByNode::iterator n = d_names.find(node); + if(n == d_names.end()){ + return; + } + d_nodes.erase(n->second); + d_names.erase(n); } } /* namespace webots */ } /* namespace liboncilla */ diff --git a/src/liboncilla-webots/nodes/SupervisorL4.h b/src/liboncilla-webots/nodes/SupervisorL4.h index 1c0fef5..b494c1c 100644 --- a/src/liboncilla-webots/nodes/SupervisorL4.h +++ b/src/liboncilla-webots/nodes/SupervisorL4.h @@ -1,48 +1,61 @@ /** * \file SupervisorL4.h * * \date Jan 22, 2013 * \author tuleu */ #ifndef LIBONCILLA_WEBOTS_SUPERVISORL4_H_ #define LIBONCILLA_WEBOTS_SUPERVISORL4_H_ #include #include "NodeAdapter.h" #include - +#include +#include +#include namespace liboncilla { namespace webots { class SupervisorL4 : public rci::oncilla::SupervisorL4, public NodeAdapter { public: - SupervisorL4(const std::string & name); + SupervisorL4(const std::string & name, + ::webots::Supervisor & supervisor, + const liboncilla::webots::config::ContactSection & config, + const std::string & footNodeName); virtual ~SupervisorL4(); - - - //interface for PoseSensing Stubbed - virtual rci::PosePtr getPose() const{ - NOT_IMPLEMENTED_STUB(SupervisorL4,getPose); - } - virtual rci::TranslationPtr getTranslation() const{ - NOT_IMPLEMENTED_STUB(SupervisorL4,getTranslation); - } - virtual rci::OrientationPtr getOrientation() const{ - NOT_IMPLEMENTED_STUB(SupervisorL4,getOrientation); - } - - //interface for ForceSensing stubbed - virtual rci::ForcesPtr getForces() const{ - NOT_IMPLEMENTED_STUB(SupervisorL4,getForces); - } - - - + void PreStep(double ts); + + void PostStep(double ts); + +private : + class ContactManager { + public : + typedef ::libwebots::plugin::modules::messages::contact::Monitor MonitorMessage; + ContactManager(); + ~ContactManager(); + void Handler(MonitorMessage const & m); + void RegisterNewContact(const std::string & name, + const liboncilla::webots::config::ContactSection & config, + liboncilla::webots::SupervisorL4 * node); + void Unregister(liboncilla::webots::SupervisorL4 * node); + static const char * GROUND_NAME; + private : + typedef std::map NodeByName; + typedef std::map NameByNode; + + NodeByName d_nodes; + NameByNode d_names; + }; + friend class ContactManager; + + static ContactManager * s_contactManager; + ::webots::Node * d_node; + Eigen::Vector3d d_force; }; } /* namespace webots */ } /* namespace liboncilla */ #endif /* LIBONCILLA_WEBOTS_SUPERVISORL4_H_ */