imuIdentifierThread::imuIdentifierThread(int _rate, string _name, string _robot, int _v, int _nW, const ResourceFinder &_rf) :
RateThread(_rate), name(_name), robot(_robot), verbosity(_v), numWaypoints(_nW)
{
timeNow = yarp::os::Time::now();
ResourceFinder &rf = const_cast<ResourceFinder&>(_rf);
step = 0;
currentWaypoint = 0;
inIMUPort = new BufferedPort<Bottle>;
outPort = new BufferedPort<Bottle>;
w_old.resize(3,0.0);
posCtrlFlag = true;
//******************* ITERATIONS ******************
iterations=rf.check("iterations",Value(1)).asInt();
printf(("*** "+name+": number of iterations set to %g\n").c_str(),iterations);
//******************* WAYPOINTS ******************
wayPoints.push_back(wayPoint("START ")); // The first group is the home position
for (int j = 0; j < iterations; j++)
{
for (int i = 0; i < numWaypoints; i++)
{
string wayPointName = "WAYPOINT_"+int_to_string(i);
Bottle &b = rf.findGroup(wayPointName.c_str());
if (!b.isNull())
{
printMessage(1,"%s found: %s\n",wayPointName.c_str(),b.toString().c_str());
Vector _jntlims(3,0.0);
Vector _vels(3,0.0);
Bottle *bj = b.find("jntlims").asList();
Bottle *bv = b.find("vels").asList();
for (int j = 0; j < _jntlims.size(); j++)
{
_jntlims[j] = bj->get(j).asDouble();
_vels[j] = bv->get(j).asDouble();
}
wayPoints.push_back(wayPoint(wayPointName,_jntlims,_vels));
wayPoints.push_back(wayPoint("MIDDLE ")); // In order to come back every time
}
}
}
wayPoints.pop_back();
wayPoints.push_back(wayPoint("END ")); // The last group will be the home position as well
numWaypoints = wayPoints.size(); // The number of waypoints is simply the size of the vector
for (int i = 0; i < numWaypoints; i++)
{
wayPoints[i].print();
}
}
WayPointPtr RobotWorld::newWayPoint( const std::string& aName,
const Point& aPosition /*= Point(-1,-1)*/,
bool aNotifyObservers /*= true*/)
{
WayPointPtr wayPoint(new WayPoint( aName, aPosition));
wayPoints.push_back( wayPoint);
if (aNotifyObservers == true)
{
notifyObservers();
}
return wayPoint;
}
std::vector<Eigen::VectorXd> ParSmoother::smoothPath( double _maxVelArray[], double _maxAcelArray[], int _numIter, double _tol, std::vector<Eigen::VectorXd> &jerkyPath )
{
numIter = _numIter;
tol = _tol;
maxVel.resize(0); maxAcel.resize(0);
for( int i = 0; i < ndim; i++ )
{
maxVel.push_back( _maxVelArray[i] );
maxAcel.push_back( _maxAcelArray[i] );
}
std::vector< Vector > inputPath;
inputPath.resize(0);
Vector wayPoint( ndim );
//-- 1. Convert to a format we can use
for( int i = 0; i < jerkyPath.size(); i++ )
{
for( int j = 0; j < ndim; j++ )
{ wayPoint[j] = jerkyPath[i](j); }
inputPath.push_back( wayPoint );
}
// RampFeasibilityChecker checker( &mfc, tol );
DynamicPath traj;
traj.Init( maxVel, maxAcel );
traj.SetMilestones( inputPath );
printf("Initial path duration: %g\n",traj.GetTotalTime());
printf("Where is \n");
int res = traj.Shortcut( numIter, &mfc, tol );
printf("After shortcutting: %d shortcuts taken, duration %g\n", res, traj.GetTotalTime());
std::vector<Eigen::VectorXd> smoothedPath;
return smoothedPath;
}
void VehicleOne::add_waypoint(double x, double y) {
Point wayPoint(x,y);
waypointQueue.push_back(wayPoint);
}
