diff --git a/src/liboncilla-webots/WebotsOncillaBackend.cpp b/src/liboncilla-webots/WebotsOncillaBackend.cpp
index 5246853..3916733 100644
--- a/src/liboncilla-webots/WebotsOncillaBackend.cpp
+++ b/src/liboncilla-webots/WebotsOncillaBackend.cpp
@@ -1,299 +1,299 @@
/*
* WebotsOncillaBackend.cpp
*
* Created on: Jan 22, 2013
* Author: tuleu
*/
#include "WebotsOncillaBackend.h"
#include "Synchronizer.h"
#include <liboncilla/Oncilla.h>
#include <liboncilla/nodes/Nodes.h>
#include <liboncilla/nodes/SupervisorNodes.h>
#include <liboncilla-webots/nodes/Nodes.h>
#include <libwebots/communication/client.hh>
#include <libwebots/plugin/modules/jointmanager/jointmanagerbuilder.hh>
#include <webots/Supervisor.hpp>
#include <webots/Node.hpp>
#include "WebotsNodeWrapper.h"
#include <biorob-cpp/log/Logger.h>
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<ro::Synchronizer> WebotsOncillaBackend::CreateSynchronizer(){
InitIfNeeded();
return boost::static_pointer_cast<ro::Synchronizer,low::Synchronizer>(d_synchronizer);
}
boost::shared_ptr<rci::oncilla::L0>
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<rci::oncilla::L1>
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<rci::oncilla::L2>
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<rci::oncilla::L3>
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<rci::oncilla::Trunk> WebotsOncillaBackend::CreateTrunk(){
InitIfNeeded();
- return ro::Trunk::Ptr(new low::Trunk());
+ return ro::Trunk::Ptr(new low::Trunk(d_supervisor->getFromDef("ONCILLA")));
}
boost::shared_ptr<rci::oncilla::SupervisorTrunk>
WebotsOncillaBackend::CreateSupervisorTrunk(){
InitIfNeeded();
return ro::SupervisorTrunk::Ptr(new low::SupervisorTrunk());
}
boost::shared_ptr<rci::oncilla::SupervisorL4>
WebotsOncillaBackend::CreateSupervisorL4(rci::oncilla::Leg leg ,
const std::string& name){
InitIfNeeded();
return ro::SupervisorL4::Ptr(new low::SupervisorL4(name));
}
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 <typename T>
bool abs_compare( T a, T b){
return std::abs<T>(a) < std::abs(b);
}
inline double GetAbsMaxOfArray(const double * val, unsigned int size){
return std::abs(*std::max_element(val,val+size,abs_compare<double>));
}
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<const double *>("translation"),3);
params.l2 = GetAbsMaxOfArray(l3.GetField<const double *>("translation"),3);
const double * fPos = foot.GetField<const double *>("translation");
params.l3 = GetAbsMaxOfArray(fPos,3);
params.l2L3AxisInL3 = Eigen::Vector3d(l3.GetField<webots::Field*>("rotation")->getSFRotation());
params.deltaL = std::abs(GetAbsMaxOfArray(p1.GetField<const double *>("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<LegParameters::Config>(params.config | LegParameters::SPRING_FOOT);
}
if(!d_supervisor->getFromDef(d2name)){
params.config = static_cast<LegParameters::Config>(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/Trunk.cpp b/src/liboncilla-webots/nodes/Trunk.cpp
index b1a1828..823ebe2 100644
--- a/src/liboncilla-webots/nodes/Trunk.cpp
+++ b/src/liboncilla-webots/nodes/Trunk.cpp
@@ -1,22 +1,60 @@
/**
* \file Trunk.cpp
*
* \date Jan 22, 2013
* \author tuleu
*/
#include "Trunk.h"
#include <liboncilla-webots/Common.h>
namespace liboncilla {
namespace webots {
-Trunk::Trunk(){
+Trunk::Trunk(::webots::Node * node)
+ : d_node(node)
+ , d_initialized(0){
+ if(node == NULL){
+ throw std::runtime_error("Coquld not find suitable node for teh trunk.");
+ }
}
Trunk::~Trunk(){
}
+void Trunk::PostStep(double ts){
+ const double * posVal(d_node->getPosition());
+ if(posVal == NULL){
+ throw std::runtime_error("Could not find Trunk current position.");
+ }
+ Eigen::Vector3d newPos(d_node->getPosition());
+ if(d_initialized == 2){
+ Eigen::Vector3d newVel = (newPos - d_position) / ts;
+ Eigen::Vector3d accel = (newVel - d_velocity) / ts;
+ d_velocity = newVel;
+ updateTranslationalAcceleration(rci::TranslationalAcceleration::fromM_s2(accel[0],accel[1],accel[2]));
+ } else if(d_initialized == 1){
+ d_velocity = (newPos - d_position) /ts;
+ updateTranslationalAcceleration(rci::TranslationalAcceleration::fromM_s2(0,0,0));
+ ++ d_initialized;
+ } else if (d_initialized == 0){
+ updateTranslationalAcceleration(rci::TranslationalAcceleration::fromM_s2(0,0,0));
+ ++d_initialized;
+ }
+ d_position = newPos;
+
+ const double * ori(d_node->getOrientation());
+ if(ori == NULL){
+ throw std::runtime_error("Could not find Trunk Orientation.");
+ }
+ updateOrientation(rci::Orientation::fromRotationMatrix(ori[0],ori[1],ori[2],
+ ori[3],ori[4],ori[5],
+ ori[6],ori[7],ori[8]));
+
+}
+
+
+
} /* namespace webots */
} /* namespace liboncilla */
diff --git a/src/liboncilla-webots/nodes/Trunk.h b/src/liboncilla-webots/nodes/Trunk.h
index 8a0d88d..0b6dbc7 100644
--- a/src/liboncilla-webots/nodes/Trunk.h
+++ b/src/liboncilla-webots/nodes/Trunk.h
@@ -1,36 +1,40 @@
/**
* \file Trunk.h
*
* \date Jan 22, 2013
* \author tuleu
*/
#ifndef LIBONCILLA_WEBOTS_TRUNK_H_
#define LIBONCILLA_WEBOTS_TRUNK_H_
#include <liboncilla/nodes/Trunk.h>
#include "NodeAdapter.h"
#include <liboncilla-webots/Common.h>
+#include <Eigen/Geometry>
+
+#include <webots/Node.hpp>
+
namespace liboncilla {
namespace webots {
class Trunk : public rci::oncilla::Trunk,
public NodeAdapter{
public:
- Trunk();
+ Trunk(::webots::Node * node);
virtual ~Trunk();
+ virtual void PostStep(double ts);
- virtual rci::TranslationalAccelerationPtr getTranslationalAcceleration() const{
- NOT_IMPLEMENTED_STUB(Trunk,getTranslationalAcceleration);
- }
-
- virtual rci::OrientationPtr getOrientation() const{
- NOT_IMPLEMENTED_STUB(Trunk,getOrientation);
- }
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(double,16);
+private :
+ ::webots::Node * d_node;
+ Eigen::Vector3d d_position;
+ Eigen::Vector3d d_velocity;
+ unsigned int d_initialized;
};
} /* namespace webots */
} /* namespace liboncilla */
#endif /* LIBONCILLA_WEBOTS_TRUNK_H_ */