bool goalReached (cargo_ants_msgs::Goal const & goal,
Eigen::Vector3d const & pose,
bool go_forward)
{
if (goal.dr > 0) {
double const dx (goal.gx - pose[0]);
double const dy (goal.gy - pose[1]);
if (sqrt (dx * dx + dy * dy) > goal.dr) {
return false;
}
}
if ((goal.dth > 0.0) && (goal.dth < M_PI)) {
double dth;
if (go_forward) {
dth = mod2pi (goal.gth - pose[2]);
}
else {
dth = mod2pi (M_PI + pose[2] - goal.dth);
}
if (fabs (dth) > goal.dth) {
return false;
}
}
return true;
}
/**************** C C | C ****************/
static double cc_c(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
static double toto;
a = x - radius*sin(phi);
b = y + radius*(cos(phi) -1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
va = 4.0 * radius;
if (u1 > va) return(infini);
else{
theta = atan2(b , a);
*u = acos(1.0 - (2.0 * u1 * u1)/(va*va));
toto = sin(*u);
if (FEQ(toto ,0.0 ,EPS3)) toto = 0.0;
/*BMIC*/
if (FEQ(toto ,0.0 ,EPS3) &&FEQ(u1 ,0.0 ,EPS3)) return(infini);
alpha = asin((2.0 * radius *toto)/(u1));
/*EMIC*/
*t = mod2pi(M_PI_2 + theta - alpha);
*v = mod2pi(*t - *u - phi);
long_rs = radius*(*t + *u + *v);
return(long_rs);
}
}
double reb_M_to_E(double e, double M){
double E;
if(e < 1.){
E = e < 0.8 ? M : M_PI;
double F = E - e*sin(M) - M;
for(int i=0; i<100; i++){
E = E - F/(1.-e*cos(E));
F = E - e*sin(E) - M;
if(fabs(F) < 1.e-16){
break;
}
}
E = mod2pi(E);
return E;
}
else{
E = M;
double F = E - e*sinh(E) - M;
for(int i=0; i<100; i++){
E = E - F/(1.0 - e*cosh(E));
F = E - e*sinh(E) - M;
if(fabs(F) < 1.e-16){
break;
}
}
E = mod2pi(E);
return E;
}
}
/**************** C | Cu Cu | C **************/
static double c_cucu_c(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
double toto;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) + 1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
if (u1 > 6.0 * radius) return(infini);
else{
theta = atan2(b , a);
va = 1.25 - (u1*u1) / (16.0 * radius * radius);
if ((va<0.)||(va>1.)) return(infini);
else{
*u = acos(va);
toto = sin(*u);
if (FEQ(toto ,0.0 ,EPS3)) toto = 0.;
alpha = asin(radius * toto * 2.0/u1);
*t = mod2pi(M_PI_2 + theta + alpha);
*v = mod2pi(*t - phi);
long_rs = radius*(2.0 *(*u) + *t + *v);
return(long_rs);
}
}
}
/**** C S C ****/
static double csca(double x , double y , double phi , double radius , double *t , double *u ,double* v)
{
double a , b , long_rs;
a = x - radius*sin(phi);
b = y + radius*(cos(phi) - 1.0);
*u = sqrt(a*a + b*b);
*t = mod2pi(atan2(b , a));
*v = mod2pi(phi - *t);
long_rs = radius*(*t + *v) + *u;
return(long_rs);
}
void CameraController::rotateRelative( float angle, float timeToArrive )
{
if ( timeToArrive > 0.0f)
{
m_isRotatingSmoothly = true;
m_startingAngle = mod2pi(m_rotation);
m_endingAngle = m_startingAngle + angle;
m_rotateTotalTimeToArrive = timeToArrive;
m_rotateTimeElapsed = 0.0;
}
else
{
m_rotation = mod2pi(m_rotation + angle);
}
}
KinematicControl::state_t KinematicControl::
run (double timestep, ostream & erros)
{
if ( ! enabled_) {
drive_->setSpeed (0.0, 0.0);
return RUNNING;
}
if ( ! have_goal_) {
drive_->setSpeed (0.0, 0.0);
return RUNNING;
}
Object const * obj (drive_->getParent());
double const dx (goal_.X() - obj->getGlobal().X());
double const dy (goal_.Y() - obj->getGlobal().Y());
double rho (sqrt (dx * dx + dy * dy));
double eps, gamma, delta;
if (fabs (rho) > 1.0e-5) { // a discontinuous way of avoiding numerical instability of atan2
eps = atan2 (dy, dx);
gamma = mod2pi (eps - obj->getGlobal().Theta());
delta = mod2pi (goal_.Theta() - gamma - obj->getGlobal().Theta());
}
else {
rho = 0.0;
eps = 0.0;
gamma = mod2pi (goal_.Theta() - obj->getGlobal().Theta());
delta = 0.0;
}
double vtrans (kr_ * rho);
double vrot (kg_ * gamma + kd_ * delta);
double strans (1.0);
if ((0.0 < vtrans_max_) && (fabs (vtrans) > vtrans_max_)) {
strans = vtrans_max_ / fabs (vtrans);
}
double srot (1.0);
if ((0.0 < vrot_max_) && (fabs (vrot) > vrot_max_)) {
srot = vrot_max_ / fabs (vrot);
}
double const sat (srot < strans ? srot : strans);
// printf ("%6.4f %6.4f %6.4f %6.4f %6.4f %6.4f %6.4f\n",
// strans, srot, sat, vtrans, vrot, sat * vtrans, sat * vrot);
drive_->setSpeed (sat * vtrans, sat * vrot);
return RUNNING;
}
int g2d_polygon_contains_point_ref(const zarray_t *poly, double q[2])
{
// use winding. If the point is inside the polygon, we'll wrap
// around it (accumulating 6.28 radians). If we're outside the
// polygon, we'll accumulate zero.
int psz = zarray_size(poly);
double acc_theta = 0;
double last_theta;
for (int i = 0; i <= psz; i++) {
double p[2];
zarray_get(poly, i % psz, &p);
double this_theta = atan2(q[1]-p[1], q[0]-p[0]);
if (i != 0)
acc_theta += mod2pi(this_theta - last_theta);
last_theta = this_theta;
}
return acc_theta > M_PI;
}
void CameraController::rotateAbsolute( float angle, float timeToArrive )
{
if ( timeToArrive > 0.0f)
{
m_isRotatingSmoothly = true;
m_startingAngle = mod2pi(m_rotation);
m_endingAngle = angle;
if ( fabs(m_startingAngle - m_endingAngle) > fabs(m_startingAngle-2*cPi - m_endingAngle) )
m_startingAngle -= 2*cPi;
m_rotateTotalTimeToArrive = timeToArrive;
m_rotateTimeElapsed = 0.0;
}
else
{
m_rotation = mod2pi(angle);
}
}
static double c_c2scb(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , long_rs , va;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) +1.0);
u1 = sqrt(a*a + b*b);
va = 2.0 * radius;
if (u1 < va) return(infini);
else{
theta = atan2(b , a);
*t = mod2pi(M_PI_2 + theta);
*u = u1 - va;
*v = mod2pi(phi - *t - M_PI_2);
long_rs = radius*(*t + M_PI_2 + *v) + *u;
return(long_rs);
}
}
//---------------------------------------------------------------------------
void comp_Heun()
{
E = getE(f,t);
double wtemp = w + E*Dt;
double ftemp = f + wtemp*Dt;
w += Dt*(E + getE(f,t+Dt))/2.;
f += Dt*(w + wtemp)/2.;
f = mod2pi(f);
}
static int
pose_listener (CTrans * ctrans, ctrans_update_type_t type, void *user)
{
if (type != CTRANS_POSE_UPDATE)
return 0;
RendererCar *self = (RendererCar*) user;
ViewHandler *vhandler = self->viewer->view_handler;
lcmtypes_pose_t pose;
ctrans_local_pose (self->ctrans, &pose);
double lastpos[3] = {0,0,0};
if (gu_ptr_circular_size(self->path))
memcpy(lastpos, gu_ptr_circular_index(self->path, 0),
3 * sizeof(double));
double diff[3];
vector_subtract_3d(pose.pos, lastpos, diff);
if (vector_magnitude_3d(diff) > 2.0) {
// clear the buffer if we jump
gu_ptr_circular_clear(self->path);
}
if (vector_magnitude_3d(diff) > 0.1 ||
gu_ptr_circular_size(self->path)==0) {
double *p = (double*) calloc(3, sizeof(double));
memcpy(p, pose.pos, sizeof(double)*3);
gu_ptr_circular_add(self->path, p);
}
if (vhandler && vhandler->update_follow_target && !self->teleport_car) {
vhandler->update_follow_target(vhandler, pose.pos, pose.orientation);
}
if (!self->did_teleport)
on_find_button(NULL, self);
self->did_teleport = 1;
int64_t dt = pose.utime - self->last_pose.utime;
double r = config_get_double_or_default (self->config,
"renderer_car.wheel_radius", 0.3);
if (self->last_pose.utime) {
int i;
for (i = 0; i < 4; i++) {
self->wheelpos[i] += self->wheelspeeds[i] * dt * 1e-6 / r;
self->wheelpos[i] = mod2pi (self->wheelpos[i]);
}
}
memcpy (&self->last_pose, &pose, sizeof (lcmtypes_pose_t));
viewer_request_redraw(self->viewer);
return 0;
}
static double cscb(double x , double y , double phi , double radius , double *t , double *u ,double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) + 1.0);
u1 = sqrt(a*a + b*b);
va = 2.0 * radius;
if (u1 < va) return(infini);
else{
theta = atan2(b , a);
*u = sqrt(u1*u1 - va*va);
alpha = atan2(va , *u);
*t = mod2pi(theta + alpha);
*v = mod2pi(*t - phi);
long_rs = radius*(*t + *v) + *u;
return(long_rs);
}
}
//---------------------------------------------------------------------------
void comp_Verlet_V()
{
if(!starteddrawing) E = getE(f,t); // solves problem when
// starting E is 0 or wierder
double oldE = E;
f = f + w*Dt + 0.5*E*Dt*Dt;
E = getE(f,t+Dt);
w = w + (E+oldE)*Dt/2.;
f = mod2pi(f);
}
/**** C | C C *****/
static double c_cc(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x - radius*sin(phi);
b = y + radius*(cos(phi) - 1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
if (u1 > 4*radius) return(infini);
else{
theta = atan2(b , a);
alpha = acos(u1/(4.0 * radius));
va = M_PI_2 + alpha;
*t = mod2pi(va + theta);
*u = mod2pi(2.0 * (M_PI - va));
*v = mod2pi(*t + *u - phi);
long_rs = radius*(*t + *u + *v);
return(long_rs);
}
}
/*** C Cu | Cu C ***/
static double ccu_cuc(double x , double y , double phi , double radius , double *t , double *u ,double* v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) + 1.0);
if (FEQ(a, 0.,EPS4)&&FEQ(b , 0.,EPS4)) return(infini);
u1 = sqrt(a*a + b*b);
va = 4.0 * radius;
if (u1 > va) return(infini);
else{
theta = atan2(b , a);
if (u1 > (2.0 *radius)) {
alpha = acos(u1/(4.0 * radius) - 0.5);
*t = mod2pi(M_PI_2 + theta - alpha);
*u = mod2pi(M_PI - alpha);
*v = mod2pi(phi - *t + 2.0 * (*u));
}
else{
alpha = acos(0.5 + u1/(4.0 * radius));
*t = mod2pi(M_PI_2 + theta + alpha);
*u = mod2pi(alpha);
*v = mod2pi(phi - *t + 2.0 * (*u));
}
long_rs = radius * (2.0 *(*u) + *t + *v);
return(long_rs);
}
}
/**************** C | C2 S C2 | C ***********/
static double c_c2sc2_c(double x , double y , double phi , double radius , double *t , double *u, double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x + radius*sin(phi);
b = y - radius*(cos(phi) + 1.0);
u1 = sqrt(a*a + b*b);
va = 4.0 * radius;
if (u1 < va) return(infini);
else{
theta = atan2(b , a);
*u = sqrt(u1*u1 - va*radius) - va;
if (*u < 0.0) return(infini);
else{
alpha = atan2(2*radius , (*u + va));
*t = mod2pi(M_PI_2 + alpha + theta);
*v = mod2pi(*t - phi);
long_rs = radius*(*t + M_PI + *v) + *u;
return(long_rs);
}
}
}
/**************** C S C2 | C ************/
static double csc2_ca(double x , double y , double phi , double radius , double *t , double *u , double *v)
{
double a , b , u1 , theta , alpha , long_rs , va;
a = x - radius*sin(phi);
b = y + radius*(cos(phi) -1.0);
u1 = sqrt(a*a + b*b);
va = 2.0 * radius;
if (u1 < va) return(infini);
else{
theta = atan2(b , a);
*u = sqrt(u1*u1 - va*va) - va;
if (*u < 0.0) return(infini);
else{
alpha = atan2((*u + va), va);
*t = mod2pi(M_PI_2 + theta - alpha);
*v = mod2pi(*t - M_PI_2 - phi);
long_rs = radius * (*t + M_PI_2 + *v) + *u;
return(long_rs);
}
}
}
static int
find_next_change_waypoint (gps_linearize_t * gpslin, double xy[2],
double theta, RndfOverlayLane * lane, int i)
{
if (i < 0 || !lane)
return -1;
while (i < lane->num_waypoints) {
RndfOverlayWaypoint * w = lane->waypoints + i;
double ll[2] = { w->waypoint->lat, w->waypoint->lon };
double wxy[2];
gps_linearize_to_xy (gpslin, ll, wxy);
double wtheta = atan2 (wxy[1] - xy[1], wxy[0] - xy[0]);
if (fabs (mod2pi (theta - wtheta)) < M_PI / 4)
return i;
i++;
}
return -1;
}
void g2d_polygon_make_ccw(zarray_t *poly)
{
// Step one: we want the points in counter-clockwise order.
// If the points are in clockwise order, we'll reverse them.
double total_theta = 0;
double last_theta = 0;
// Count the angle accumulated going around the polygon. If
// the sum is +2pi, it's CCW. Otherwise, we'll get -2pi.
int sz = zarray_size(poly);
for (int i = 0; i <= sz; i++) {
double p0[2], p1[2];
zarray_get(poly, i % sz, &p0);
zarray_get(poly, (i+1) % sz, &p1);
double this_theta = atan2(p1[1]-p0[1], p1[0]-p0[0]);
if (i > 0) {
double dtheta = mod2pi(this_theta-last_theta);
total_theta += dtheta;
}
last_theta = this_theta;
}
int ccw = (total_theta > 0);
// reverse order if necessary.
if (!ccw) {
for (int i = 0; i < sz / 2; i++) {
double a[2], b[2];
zarray_get(poly, i, a);
zarray_get(poly, sz-1-i, b);
zarray_set(poly, i, b, NULL);
zarray_set(poly, sz-1-i, a, NULL);
}
}
}
at::real MathUtil::mod2pi(at::real ref, at::real v) {
return ref + mod2pi(v-ref);
}
float CameraController::getCameraAngle() const
{
return mod2pi(m_rotation);
}
double reed_shepp48(Stconfig *c1 , Stconfig *c2, double radius , int numero , double *t_r , double *u_r , double *v_r)
{
double x , y , phi;
double t1 , u1 , v1;
double var , var_d, theta , alpha , dx , dy , r, longueur;
longueur = 0.0;
/* Numero compris entre 0 et 48 */
if((numero < 1) || (numero > 48)) {
*t_r = *u_r = *v_r = 0.0;
return(longueur);
}
/* Changement de repere,les courbes sont toujours calculees
de (0 0 0)--> (x , y , phi) */
dx = (double)(c2->x - c1->x);
dy = (double)(c2->y - c1->y);
var = (double)(c2->z -c1->z);
r = (double)radius;
theta = atan2(dy , dx);
alpha = theta - (double)(c1->z);
var_d = sqrt(dx*dx + dy*dy);
x = cos(alpha)*var_d;
y = sin(alpha)*var_d;
if (fabs(var) <= M_PI) phi = var;
else {
if (c2->z >= c1->z) phi = var - M_2PI;
else phi = mod2pi(var);}
t1 = u1 = v1 = 0.0;
switch(numero) {
/**** C | C | C ***/
case 1:
longueur = c_c_c(x,y,phi,r,&t1,&u1,&v1); /*l+ r- l+ */
break;
case 2:
longueur = c_c_c(-x,y,-phi,r,&t1,&u1,&v1); /*l- r+ l- */
break;
case 3:
longueur = c_c_c(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l- r+ */
break;
case 4:
longueur = c_c_c(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l+ r- */
break;
/**** C | C C ***/
case 5:
longueur = c_cc(x,y,phi,r,&t1,&u1,&v1); /*l+ r- l- */
break;
case 6:
longueur = c_cc(-x,y,-phi,r,&t1,&u1,&v1); /*l- r+ l+ */
break;
case 7:
longueur = c_cc(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l- r- */
break;
case 8:
longueur = c_cc(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l+ r+ */
break;
/**** C S C ****/
case 9:
longueur = csca(x,y,phi,r,&t1,&u1,&v1); /*l+ s+ l+ */
break;
case 10:
longueur = csca(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ s+ r+ */
break;
case 11:
longueur = csca(-x,y,-phi,r,&t1,&u1,&v1); /*l- s- l- */
break;
case 12:
longueur = csca(-x ,-y,phi,r,&t1,&u1,&v1); /*r- s- r- */
break;
case 13:
longueur = cscb(x,y,phi,r,&t1,&u1,&v1); /*l+ s+ r+ */
break;
case 14:
longueur = cscb(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ s+ l+ */
break;
case 15:
longueur = cscb(-x,y,-phi,r,&t1,&u1,&v1); /*l- s- r- */
break;
case 16:
longueur = cscb(-x ,-y,phi,r,&t1,&u1,&v1); /*r- s- l- */
break;
/*** C Cu | Cu C ***/
case 17:
longueur = ccu_cuc(x,y,phi,r,&t1,&u1,&v1); /*l+ r+ l- r-*/
break;
case 18:
longueur = ccu_cuc(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l+ r- l-*/
break;
case 19:
longueur = ccu_cuc(-x,y,-phi,r,&t1,&u1,&v1); /*l- r- l+ r+ */
break;
case 20:
longueur = ccu_cuc(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l- r+ l+ */
break;
/*** C | Cu Cu | C ***/
case 21:
longueur = c_cucu_c(x,y,phi,r,&t1,&u1,&v1); /*l+ r- l- r+ */
break;
case 22:
longueur = c_cucu_c(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l- r- l+ */
break;
case 23:
longueur = c_cucu_c(-x,y,-phi,r,&t1,&u1,&v1); /*l- r+ l+ r- */
break;
case 24:
longueur = c_cucu_c(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l+ r+ l- */
break;
/*** C | C2 S C ***/
case 25:
longueur = c_c2sca(x,y,phi,r,&t1,&u1,&v1); /*l+ r- s- l- */
break;
case 26:
longueur = c_c2sca(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l- s- r- */
break;
case 27:
longueur = c_c2sca(-x,y,-phi,r,&t1,&u1,&v1); /*l- r+ s+ l+ */
break;
case 28:
longueur = c_c2sca(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l+ s+ r+ */
break;
case 29:
longueur = c_c2scb(x,y,phi,r,&t1,&u1,&v1); /*l+ r- s- r- */
break;
case 30:
longueur = c_c2scb(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l- s- l- */
break;
case 31:
longueur = c_c2scb(-x,y,-phi,r,&t1,&u1,&v1); /*l- r+ s+ r+ */
break;
case 32:
longueur = c_c2scb(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l+ s+ l+ */
break;
/*** C | C2 S C2 | C ***/
case 33:
longueur = c_c2sc2_c(x,y,phi,r,&t1,&u1,&v1); /*l+ r- s- l- r+ */
break;
case 34:
longueur = c_c2sc2_c(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l- s- r- l+ */
break;
case 35:
longueur = c_c2sc2_c(-x,y,-phi,r,&t1,&u1,&v1); /*l- r+ s+ l+ r- */
break;
case 36:
longueur = c_c2sc2_c(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l+ s+ r+ l- */
break;
/*** C C | C ****/
case 37:
longueur = cc_c(x,y,phi,r,&t1,&u1,&v1); /*l+ r+ l- */
break;
case 38:
longueur = cc_c(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ l+ r- */
break;
case 39:
longueur = cc_c(-x,y,-phi,r,&t1,&u1,&v1); /*l- r- l+ */
break;
case 40:
longueur = cc_c(-x ,-y,phi,r,&t1,&u1,&v1); /*r- l- r+ */
break;
/*** C S C2 | C ***/
case 41:
longueur = csc2_ca(x,y,phi,r,&t1,&u1,&v1); /*l+ s+ r+ l- */
break;
case 42:
longueur = csc2_ca(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ s+ l+ r- */
break;
case 43:
longueur = csc2_ca(-x,y,-phi,r,&t1,&u1,&v1); /*l- s- r- l+ */
break;
case 44:
longueur = csc2_ca(-x ,-y,phi,r,&t1,&u1,&v1); /*r- s- l- r+ */
break;
case 45:
longueur = csc2_cb(x,y,phi,r,&t1,&u1,&v1); /*l+ s+ l+ r- */
break;
case 46:
longueur = csc2_cb(x ,-y,-phi,r,&t1,&u1,&v1); /*r+ s+ r+ l- */
break;
case 47:
longueur = csc2_cb(-x,y,-phi,r,&t1,&u1,&v1); /*l- s- l- r+ */
break;
case 48:
longueur = csc2_cb(-x ,-y,phi,r,&t1,&u1,&v1); /*r- s- r- l+ */
break;
}
*t_r = (double)t1; *u_r = (double)u1; *v_r = (double)v1;
return((double)longueur);
}
bool MoveBaseSBPL::makePlan(){
ROS_DEBUG("Planning for new goal...\n");
ompl::SBPLPlannerStatistics::entry & statsEntry(pStat_.top());
try {
const CostMap2D& cm = getCostMap();
// Copy out start and goal states to minimize locking requirement. Lock was not previously required because the
// planner and controller were running on the same thread and the only contention was for cost map updates on call
// backs. Note that cost map queries here are const methods that merely do co-ordinate transformations, so we do not need
// to lock protect those.
stateMsg.lock();
statsEntry.start = stateMsg.pos;
statsEntry.goal = stateMsg.goal;
stateMsg.unlock();
// Set start state based on global pose, updating statistics in the process.
cm.WC_MC(statsEntry.start.x, statsEntry.start.y, statsEntry.startIx, statsEntry.startIy);
env_->SetStart(statsEntry.start);
statsEntry.startState = env_->GetStateFromPose(statsEntry.start);
if (0 > statsEntry.startState) {
ROS_ERROR("invalid start state ID %d from pose (%+8.3f, %+8.3f): outside of map?\n",
statsEntry.startState, statsEntry.start.x, statsEntry.start.y);
return false;
}
int status(pMgr_->set_start(statsEntry.startState));
if (1 != status) {
ROS_ERROR("failed to set start state ID %d from (%ud, %ud): pMgr_->set_start() returned %d\n",
statsEntry.startState, statsEntry.startIx, statsEntry.startIy, status);
return false;
}
// Set goal state, updating statistics in the process.
cm.WC_MC(statsEntry.goal.x, statsEntry.goal.y, statsEntry.goalIx, statsEntry.goalIy);
env_->SetGoal(statsEntry.goal);
statsEntry.goalState = env_->GetStateFromPose(statsEntry.goal);
if (0 > statsEntry.goalState) {
ROS_ERROR("invalid goal state ID %d from pose (%+8.3f, %+8.3f): outside of map?\n",
statsEntry.goalState, statsEntry.goal.x, statsEntry.goal.y);
return false;
}
status = pMgr_->set_goal(statsEntry.goalState);
if (1 != status) {
ROS_ERROR("failed to set goal state ID %d from (%ud, %ud): pMgr_->set_goal() returned %d\n",
statsEntry.goalState, statsEntry.goalIx, statsEntry.goalIy, status);
return false;
}
// Invoke the planner, updating the statistics in the process.
std::vector<int> solutionStateIDs;
statsEntry.allocated_time_sec = plannerTimeLimit_;
statsEntry.status = pMgr_->replan(statsEntry.allocated_time_sec,
&statsEntry.actual_time_wall_sec,
&statsEntry.actual_time_user_sec,
&statsEntry.actual_time_system_sec,
&solutionStateIDs);
// Extract the solution, if available, and update statistics (as usual).
statsEntry.plan_length_m = 0;
statsEntry.plan_angle_change_rad = 0;
if ((1 == statsEntry.status) && (1 < solutionStateIDs.size())) {
std::list<std_msgs::Pose2DFloat32> plan;
double prevx(0), prevy(0), prevth(0);
prevth = 42.17; // to detect when it has been initialized (see 42 below)
for(std::vector<int>::const_iterator it = solutionStateIDs.begin(); it != solutionStateIDs.end(); ++it){
std_msgs::Pose2DFloat32 const waypoint(env_->GetPoseFromState(*it));
// update stats:
// - first round, nothing to do
// - second round, update path length only
// - third round, update path length and angular change
if (plan.empty()) {
prevx = waypoint.x;
prevy = waypoint.y;
}
else {
double const dx(waypoint.x - prevx);
double const dy(waypoint.y - prevy);
statsEntry.plan_length_m += sqrt(pow(dx, 2) + pow(dy, 2));
double const th(atan2(dy, dx));
if (42 > prevth) // see 42.17 above
statsEntry.plan_angle_change_rad += fabs(mod2pi(th - prevth));
prevx = waypoint.x;
prevy = waypoint.y;
prevth = th;
#warning 'add the cumulation of delta(waypoint.th) now that we can have 3D plans'
}
plan.push_back(waypoint);
}
// probably we should add the delta from the last theta to
// the goal theta onto statsEntry.plan_angle_change_rad here, but
// that depends on whether our planner handles theta for us,
// and needs special handling if we have no plan...
{
ostringstream prefix_os;
prefix_os << "[" << goalCount_ << "] ";
static size_t lastPlannedGoal(171717);
char const * title("\nREPLAN SUCCESS");
if (lastPlannedGoal != goalCount_) {
lastPlannedGoal = goalCount_;
title = "\nPLAN SUCCESS";
}
statsEntry.logFile(planStatsFile_.c_str(), title, prefix_os.str().c_str());
}
//// statsEntry.logInfo("move_base_sbpl: ");
pStat_.pushBack(pMgr_->getName(), env_->getName());
updatePlan(plan);
return true;
}
}
catch (std::runtime_error const & ee) {
ROS_ERROR("runtime_error in makePlan(): %s\n", ee.what());
return false;
}
ROS_ERROR("No plan found\n");
{
static size_t lastFailedGoal(424242);
if (lastFailedGoal != goalCount_) {
lastFailedGoal = goalCount_;
ostringstream prefix_os;
prefix_os << "[" << goalCount_ << "] ";
statsEntry.logFile(planStatsFile_.c_str(), "\nPLAN FAILURE", prefix_os.str().c_str());
}
}
return false;
}
static void my_draw( Viewer *viewer, Renderer *renderer )
{
RendererTextured *self = (RendererTextured*) renderer->user;
GtkuParamWidget *pw = GTKU_PARAM_WIDGET (self->pw);
int colormode = gtku_param_widget_get_enum(pw, COLOR_MENU);
if(colormode == COLOR_NONE)
return;
double total_theta = 0;
double last_ctheta = HUGE;
double pos[3];
if (!ctrans_have_pose(self->ctrans))
return;
ctrans_local_pos(self->ctrans, pos);
glEnable(GL_DEPTH_TEST);
glPointSize(2.0);
glBegin(GL_POINTS);
for (unsigned int cidx = 0; cidx < gu_ptr_circular_size(self->circular); cidx++) {
struct velodyne_data *vdata = (struct velodyne_data*) gu_ptr_circular_index(self->circular, cidx);
// do these returns still need to be projected
if (vdata->samples == NULL) {
vdata->samples = (velodyne_sample_t*) calloc(velodyne_decoder_estimate_samples(self->calib, vdata->data, vdata->datalen),
sizeof(velodyne_sample_t));
velodyne_decoder_t vdecoder;
velodyne_decoder_init(self->calib, &vdecoder, vdata->data, vdata->datalen);
while (!velodyne_decoder_next(self->calib, &vdecoder, &vdata->samples[vdata->num_samples])) {
velodyne_sample_t *vsample = &vdata->samples[vdata->num_samples++];
if (vsample->range < 0.25 && fabs(mod2pi(vsample->theta+M_PI/2)) < to_radians(20)) {
bad_samples[vsample->logical]++;
}
total_ranges[vsample->logical]++;
if (vsample->range < 0.01)
bad_ranges[vsample->logical]++;
double sensor_xyz[4] = { vsample->xyz[0], vsample->xyz[1], vsample->xyz[2], 1 };
double local_xyz[4];
matrix_vector_multiply_4x4_4d(vdata->m, sensor_xyz, local_xyz);
vsample->xyz[0] = local_xyz[0];
vsample->xyz[1] = local_xyz[1];
vsample->xyz[2] = local_xyz[2];
vdata->ctheta = vsample->ctheta;
/*
int thissign = sgn(mod2pi(vsample.ctheta));
if (thissign==1 && lastsign==-1) {
rotation_count++;
int ROTATION_COUNT_THRESH = 5;
if (rotation_count == ROTATION_COUNT_THRESH) {
double elapsed_time = (v->utime - rotation_utime)/1000000.0;
printf("velodyne %6s [%8d]: %5.3f Hz\n",
vdecoder.version_string, vdecoder.revolution_count,
rotation_count / elapsed_time);
rotation_count = 0;
rotation_utime = v->utime;
if (0) {
for (int i = 0; i < 64; i++)
printf("%3d : %6f\n", i, ((double)bad_ranges[i]*100.0)/total_ranges[i]);
}
}
}
*/
}
}
// have we plotted a whole revolution of data?
if (last_ctheta == HUGE)
last_ctheta = vdata->ctheta;
total_theta += fabs(mod2pi(last_ctheta - vdata->ctheta));
if (total_theta > self->numScans*2*M_PI)
break;
last_ctheta = vdata->ctheta;
//do motion comensation
//struct velodyne_data motion_compensated;
//motion_compensated.samples = (velodyne_sample_t*)malloc(vdata->num_samples);
//motion_compensate_scan(vdata, self, &motion_compensated);
// project these samples
for (unsigned int s = 0; s < vdata->num_samples; s++) {
velodyne_sample_t *vsample = &vdata->samples[s];
if(vsample->range > RANGE_THRESH)
{
switch (colormode)
{
case COLOR_INTENSITY:
{
double v = vsample->intensity;
glColor3fv(color_util_jet(v));
break;
}
case COLOR_RGB:
{
double rgb[3];
rgb[0] = (double)(vdata->rgbdata->rgb[s][0])/255;
rgb[1] = (double)(vdata->rgbdata->rgb[s][1])/255;
rgb[2] = (double)(vdata->rgbdata->rgb[s][2])/255;
glColor3dv(rgb);
break;
}
case COLOR_LASER:
glColor3d(self->laser_colors[vsample->logical][0],
self->laser_colors[vsample->logical][1],
self->laser_colors[vsample->logical][2]);
break;
case COLOR_DRAB:
glColor3d(0.3, 0.3, 0.3);
break;
case COLOR_Z:
{
double z = vsample->xyz[2] - pos[2];
double Z_MIN = -1, Z_MAX = 2;
double z_norm = (z - Z_MIN) / (Z_MAX - Z_MIN);
glColor3fv(color_util_jet(z_norm));
break;
}
case COLOR_COUNTED:
{
switch (vsample->logical%10)
{
case 0:
glColor3d(1, 1, 0.5); break;
case 5:
glColor3d(0.5, 1, 1); break;
default:
glColor3d(0.3, 0.3, 0.3); break;
}
}
break;
}
glVertex3dv(vsample->xyz);
}
}
}
glEnd();
}
double reed_shepp(Stconfig *c1 , Stconfig *c2, double radius , int* numero , double *t_r , double *u_r , double *v_r)
{
double x , y , phi;
double t , u , v , t1 , u1 , v1;
int num;
double var , var_d, theta , alpha , dx , dy , r, longueur;
/* Changement de repere,les courbes sont toujours calculees
de (0 0 0)--> (x , y , phi) */
dx = (double)(c2->x - c1->x);
dy = (double)(c2->y - c1->y);
var = (double)(c2->z -c1->z);
r = (double)radius;
theta = atan2(dy , dx);
alpha = theta - (double)(c1->z);
var_d = sqrt(dx*dx + dy*dy);
x = cos(alpha)*var_d;
y = sin(alpha)*var_d;
t1 = u1 = v1 = 0.0;
if (fabs(var) <= M_PI) phi = var;
else {
if (c2->z >= c1->z) phi = var - M_2PI;
else phi = mod2pi(var);}
if(FEQ(r,0.0,EPS4)) {
longueur = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
var = t1+v1; num = 9; t = t1; u = u1; v = v1;
csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if((t1+v1) < (t+v)){num = 10;t = t1; u = u1; v = v1;}
csca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- l- */
if((t1+v1) < (t+v)){num = 11;t = t1; u = u1; v = v1;}
csca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- r- */
if((t1+v1) < (t+v)){num = 12;t = t1; u = u1; v = v1;}
cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if((t1+v1) < (t+v)){num = 13;t = t1; u = u1; v = v1;}
cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if((t1+v1) < (t+v)){num = 14;t = t1; u = u1; v = v1;}
cscb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- r- */
if((t1+v1) < (t+v)){num = 15;t = t1; u = u1; v = v1;}
cscb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- l- */
if((t1+v1) < (t+v)){num = 16;t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
if(longueur == infini) PrintInfo(("infini0\n"));
return((double)longueur);
}
/**** C | C | C ***/
longueur = c_c_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- l+ */
num = 1; t = t1; u = u1; v = v1;
var = c_c_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ l- */
if (var < longueur){longueur = var; num = 2; t = t1; u = u1; v = v1;}
var = c_c_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- r+ */
if (var < longueur){longueur = var; num = 3; t = t1; u = u1; v = v1;}
var = c_c_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ r- */
if (var < longueur){longueur = var; num = 4; t = t1; u = u1; v = v1;}
/**** C | C C ***/
var = c_cc(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- l- */
if (var < longueur){longueur = var; num = 5; t = t1; u = u1; v = v1;}
var = c_cc(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ l+ */
if (var < longueur){longueur = var; num = 6; t = t1; u = u1; v = v1;}
var = c_cc(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- r- */
if (var < longueur){longueur = var; num = 7; t = t1; u = u1; v = v1;}
var = c_cc(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ r+ */
if (var < longueur){longueur = var; num = 8; t = t1; u = u1; v = v1;}
/**** C S C ****/
var = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
if (var < longueur){longueur = var; num = 9; t = t1; u = u1; v = v1;}
var = csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if (var < longueur){longueur = var; num = 10;t = t1; u = u1; v = v1;}
var = csca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- l- */
if (var < longueur){longueur = var; num = 11;t = t1; u = u1; v = v1;}
var = csca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- r- */
if (var < longueur){longueur = var; num = 12;t = t1; u = u1; v = v1;}
var = cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if (var < longueur){longueur = var; num = 13;t = t1; u = u1; v = v1;}
var = cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if (var < longueur){longueur = var; num = 14;t = t1; u = u1; v = v1;}
var = cscb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- r- */
if (var < longueur){longueur = var; num = 15;t = t1; u = u1; v = v1;}
var = cscb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- l- */
if (var < longueur){longueur = var; num = 16;t = t1; u = u1; v = v1;}
/*** C Cu | Cu C ***/
var = ccu_cuc(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r+ l- r- */
if (var < longueur){longueur = var; num = 17;t = t1; u = u1; v = v1;}
var = ccu_cuc(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l+ r- l- */
if (var < longueur){longueur = var; num = 18;t = t1; u = u1; v = v1;}
var = ccu_cuc(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r- l+ r+ */
if (var < longueur){longueur = var; num = 19;t = t1; u = u1; v = v1;}
var = ccu_cuc(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l- r+ l+ */
if (var < longueur){longueur = var; num = 20;t = t1; u = u1; v = v1;}
/*** C | Cu Cu | C ***/
var = c_cucu_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- l- r+ */
if (var < longueur){longueur = var;num = 21; t = t1; u = u1; v = v1;}
var = c_cucu_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- r- l+ */
if (var < longueur){longueur = var; num = 22;t = t1; u = u1; v = v1;}
var = c_cucu_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ l+ r- */
if (var < longueur){longueur = var;num = 23; t = t1; u = u1; v = v1;}
var = c_cucu_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ r+ l- */
if (var < longueur){longueur = var;num = 24; t = t1; u = u1; v = v1;}
/*** C | C2 S C ***/
var = c_c2sca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- s- l- */
if (var < longueur){longueur = var;num = 25; t = t1; u = u1; v = v1;}
var = c_c2sca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- s- r- */
if (var < longueur){longueur = var;num = 26; t = t1; u = u1; v = v1;}
var = c_c2sca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ s+ l+ */
if (var < longueur){longueur = var;num = 27; t = t1; u = u1; v = v1;}
var = c_c2sca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ s+ r+ */
if (var < longueur){longueur = var;num = 28; t = t1; u = u1; v = v1;}
var = c_c2scb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- s- r- */
if (var < longueur){longueur = var; num = 29; t = t1; u = u1; v = v1;}
var = c_c2scb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- s- l- */
if (var < longueur){longueur = var; num = 30; t = t1; u = u1; v = v1;}
var = c_c2scb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ s+ r+ */
if (var < longueur){longueur = var; num = 31; t = t1; u = u1; v = v1;}
var = c_c2scb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ s+ l+ */
if (var < longueur){longueur = var; num = 32; t = t1; u = u1; v = v1;}
/*** C | C2 S C2 | C ***/
var = c_c2sc2_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r- s- l- r+ */
if (var < longueur){longueur = var; num = 33; t = t1; u = u1; v = v1;}
var = c_c2sc2_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l- s- r- l+ */
if (var < longueur){longueur = var; num = 34; t = t1; u = u1; v = v1;}
var = c_c2sc2_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r+ s+ l+ r- */
if (var < longueur){longueur = var; num = 35; t = t1; u = u1; v = v1;}
var = c_c2sc2_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l+ s+ r+ l- */
if (var < longueur){longueur = var; num = 36; t = t1; u = u1; v = v1;}
/*** C C | C ****/
var = cc_c(x , y , phi , r , &t1 , &u1 , &v1); /* l+ r+ l- */
if (var < longueur){longueur = var; num = 37; t = t1; u = u1; v = v1;}
var = cc_c(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ l+ r- */
if (var < longueur){longueur = var; num = 38; t = t1; u = u1; v = v1;}
var = cc_c(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- r- l+ */
if (var < longueur){longueur = var; num = 39; t = t1; u = u1; v = v1;}
var = cc_c(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- l- r+ */
if (var < longueur){longueur = var; num = 40; t = t1; u = u1; v = v1;}
/*** C S C2 | C ***/
var = csc2_ca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ l- */
if (var < longueur){longueur = var; num = 41; t = t1; u = u1; v = v1;}
var = csc2_ca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ r- */
if (var < longueur){longueur = var; num = 42; t = t1; u = u1; v = v1;}
var = csc2_ca(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- r- l+ */
if (var < longueur){longueur = var; num = 43; t = t1; u = u1; v = v1;}
var = csc2_ca(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- l- r+ */
if (var < longueur){longueur = var; num = 44; t = t1; u = u1; v = v1;}
var = csc2_cb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ r- */
if (var < longueur){longueur = var; num = 45; t = t1; u = u1; v = v1;}
var = csc2_cb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ l- */
if (var < longueur){longueur = var; num = 46; t = t1; u = u1; v = v1;}
var = csc2_cb(-x , y , -phi , r , &t1 , &u1 , &v1); /* l- s- l- r+ */
if (var < longueur){longueur = var; num = 47; t = t1; u = u1; v = v1;}
var = csc2_cb(-x ,-y , phi , r , &t1 , &u1 , &v1); /* r- s- r- l+ */
if (var < longueur){longueur = var; num = 48; t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
return((double)longueur);
}
void invertangles(double* alpha, double* beta, double* gamma)
{
*alpha = mod2pi(*alpha - M_PI);
*beta = M_PI - *beta;
*gamma = mod2pi(M_PI - *gamma);
}
inline static double diffang(double a, double b)
{
return (M_PI - dabs(M_PI-mod2pi(a-b)));
}
double dubins(Stconfig *c1 , Stconfig *c2, double radius , int* numero , double *t_r , double *u_r , double *v_r)
{
double x , y , phi;
double t , u , v , t1 , u1 , v1;
int num;
double var , var_d, theta , alpha , dx , dy , r, longueur;
/* Changement de repere,les courbes sont toujours calculees
de (0 0 0)--> (x , y , phi) */
dx = (double)(c2->x - c1->x);
dy = (double)(c2->y - c1->y);
var = (double)(c2->z -c1->z);
r = (double)radius;
theta = atan2(dy , dx);
alpha = theta - (double)(c1->z);
var_d = sqrt(dx*dx + dy*dy);
x = cos(alpha)*var_d;
y = sin(alpha)*var_d;
t1 = u1 = v1 = 0.0;
if (fabs(var) <= M_PI) phi = var;
else {
if (c2->z >= c1->z) phi = var - M_2PI;
else phi = mod2pi(var);}
if(FEQ(r,0.0,EPS4)) {
longueur = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
var = t1+v1; num = 9; t = t1; u = u1; v = v1;
csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if((t1+v1) < (t+v)){num = 10;t = t1; u = u1; v = v1;}
cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if((t1+v1) < (t+v)){num = 13;t = t1; u = u1; v = v1;}
cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if((t1+v1) < (t+v)){num = 14;t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
if(longueur == infini) PrintInfo(("infini0\n"));
return((double)longueur);
}
/**** C S C ****/
longueur = csca(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ l+ */
num = 9; t = t1; u = u1; v = v1;
var = csca(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ r+ */
if (var < longueur){longueur = var; num = 10;t = t1; u = u1; v = v1;}
var = cscb(x , y , phi , r , &t1 , &u1 , &v1); /* l+ s+ r+ */
if (var < longueur){longueur = var; num = 13;t = t1; u = u1; v = v1;}
var = cscb(x ,-y , -phi , r , &t1 , &u1 , &v1); /* r+ s+ l+ */
if (var < longueur){longueur = var; num = 14;t = t1; u = u1; v = v1;}
*t_r = (double)t; *u_r = (double)u; *v_r = (double)v;
*numero = num;
return((double)longueur);
}
// radians [-pi, pi]
// speedfrac [0,1]
// torquefrac [0,1]
void
dynamixel_set_joint_goal_default(dynamixel_device_t *device,
int pmask,
double radians,
double speedfrac,
double torquefrac)
{
assert (!device->rotation_mode && (pmask == 0xfff || pmask == 0x3ff));
// Ensure proper ranges
radians = mod2pi(radians);
speedfrac = dmax(0.0, dmin(1.0, dabs(speedfrac)));
torquefrac = dmax(0.0, dmin(1.0, torquefrac));
double min = device->get_min_position_radians(device);
double max = device->get_max_position_radians(device);
radians = dmax(min, dmin(max, radians));
int stop = speedfrac < (1.0/0x3ff);
int posv = ((int) round((radians - min) / (max - min) * pmask)) & pmask;
// in joint-mode, speed == 0 --> maxspeed
int speedv = stop ? 0x1 : (int)(speedfrac * 0x3ff);
int torquev = (int)(torquefrac * 0x3ff);
dynamixel_msg_t *msg = dynamixel_msg_create(7);
msg->buf[0] = 0x1e;
msg->buf[1] = posv & 0xff;
msg->buf[2] = (posv >> 8) & 0xff;
msg->buf[3] = speedv & 0xff;
msg->buf[4] = (speedv >> 8) & 0xff;
msg->buf[5] = torquev & 0xff;
msg->buf[6] = (torquev >> 8) & 0xff;
dynamixel_msg_t *resp = device->write_to_RAM(device, msg, 1);
dynamixel_msg_destroy(msg);
if (resp != NULL);
dynamixel_msg_destroy(resp);
// Handle speed == 0 case (after slowing down, above) by relaying current
// position back to servo. Do not set torque == 0, b/c that is possibly not
// desired...
if (stop) {
msg = dynamixel_msg_create(2);
msg->buf[0] = 0x24;
msg->buf[1] = 2;
resp = device->bus->send_command(device->bus,
device->id,
INST_READ_DATA,
msg,
1);
dynamixel_msg_destroy(msg);
if (resp != NULL) {
dynamixel_msg_destroy(resp);
posv = (resp->buf[1] & 0xff) + ((resp->buf[2] & 0xff) << 8);
msg = dynamixel_msg_create(3);
msg->buf[0] = 0x1e;
msg->buf[1] = posv & 0xff;
msg->buf[2] = (posv > 8) & 0xff;
resp = device->write_to_RAM(device, msg, 1);
}
if (resp != NULL)
dynamixel_msg_destroy(resp);
}
}
