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); } }