main(){
double brdf;
double mvbp1_();
Verstraete_model verstraete;
verstraete.theta[0]=DTOR(10.0);
verstraete.theta[1]=DTOR(20.0);
verstraete.theta[2]=DTOR(0.0);
verstraete.model_type = 1;
verstraete.kappa_flag = 1;
verstraete.phase_model = 1;
verstraete.scattering_model = 1;
verstraete.kappa[0] = 0.01;
verstraete.kappa[1] = 1.0;
verstraete.kappa[2] = 0.2;
verstraete.kappa[3] = 0.0;
verstraete.optics[0] = 0.8;
verstraete.optics[1] = -0.4;
verstraete.optics[2] = 0.0;
init_verstraete(&verstraete);
brdf = mvbp1_(&verstraete);
fprintf(stdout,"%f\n",(float)brdf);
}
void
graph_error_func (struct experiment_params *params)
{
double speed, angle, err;
FILE *f;
gsl_vector *args;
args = gsl_vector_alloc (params->minimizer_dimen);
if ((f = fopen ("err.dat", "w")) == NULL) {
fprintf (stderr, "can't create err.dat\n");
exit (1);
}
for (speed = 0; speed < 80; speed += 1) {
for (angle = DTOR (0); angle < DTOR (50); angle += DTOR (1)) {
gsl_vector_set (args, 0, speed);
gsl_vector_set (args, 1, angle);
err = compute_error_func (args, params);
fprintf (f, "%.14g %.14g %.14g\n",
speed, RTOD (angle), err);
}
fprintf (f, "\n");
}
fclose (f);
}
double Particle::getMapBlockAtAngle(double angle){
for(double i=0;i<LASER_MAX_RANGE;i+=0.1){
int xTest = this->_xVal/139.0*550.0+ (cos(DTOR(angle + this->_yawVal))) * i*10.0;
int yTest = this->_yVal/95.0*380.0+ (sin(DTOR(angle + this->_yawVal))) * i*10.0;
if(_map->IsPixelIsBlack(xTest,yTest)==false){
cout<<"testing x"<<xTest<<","<<yTest<<" for "<<angle<<endl;
return i;
}
}
return LASER_MAX_RANGE;
}
str_grid_edge_data_t* str_grid_edge_data_with_buffer(str_grid_t* grid,
int num_components,
void* buffer)
{
ASSERT(num_components > 0);
// Allocate storage.
int num_patches = 3 * str_grid_num_patches(grid);
size_t patches_size = sizeof(str_grid_patch_t*) * num_patches;
size_t edge_data_size = sizeof(str_grid_edge_data_t) + patches_size;
str_grid_edge_data_t* edge_data = polymec_malloc(edge_data_size);
edge_data->grid = grid;
edge_data->nc = num_components;
str_grid_get_extents(grid, &edge_data->nx, &edge_data->ny, &edge_data->nz);
edge_data->x_patches = int_ptr_unordered_map_new();
edge_data->y_patches = int_ptr_unordered_map_new();
edge_data->z_patches = int_ptr_unordered_map_new();
edge_data->patch_offsets = polymec_malloc(sizeof(int) * (3*num_patches+1));
edge_data->buffer = NULL;
// Now populate the patches for x-, y-, and z-edges.
int px, py, pz;
str_grid_get_patch_size(grid, &px, &py, &pz);
edge_data->patch_lx = 1.0 / edge_data->nx;
edge_data->patch_ly = 1.0 / edge_data->ny;
edge_data->patch_lz = 1.0 / edge_data->nz;
int pos = 0, i, j, k;
while (str_grid_next_patch(grid, &pos, &i, &j, &k))
{
int index = patch_index(edge_data, i, j, k);
int_ptr_unordered_map_insert_with_v_dtor(edge_data->x_patches, index,
str_grid_patch_with_buffer(px+1, py, pz, num_components, 0, NULL),
DTOR(str_grid_patch_free));
int_ptr_unordered_map_insert_with_v_dtor(edge_data->y_patches, index,
str_grid_patch_with_buffer(px, py+1, pz, num_components, 0, NULL),
DTOR(str_grid_patch_free));
int_ptr_unordered_map_insert_with_v_dtor(edge_data->z_patches, index,
str_grid_patch_with_buffer(px, py, pz+1, num_components, 0, NULL),
DTOR(str_grid_patch_free));
}
count_edges(edge_data);
// Set the buffer.
str_grid_edge_data_set_buffer(edge_data, buffer, false);
return edge_data;
}
void
movement (void)
{
double now, dt;
now = get_secs ();
dt = now - player.lasttime;
if (arrowkey[LEFT] != arrowkey[RIGHT] && mousebutton[3] == 0
&& mousebutton[2] == 0) {
if (arrowkey[LEFT]) {
player.theta = player.theta + player.turnspeed * dt;
}
if (arrowkey[RIGHT]) {
player.theta = player.theta - player.turnspeed * dt;
}
if (player.theta < 0) {
player.theta = player.theta + DTOR (360);
}
}
player.theta = fmod (player.theta, DTOR (360));
player.moving = NO;
if (arrowkey[UP] == 1)
player.moving = FORW;
if (arrowkey[DOWN] == 1)
player.moving = BACK;
if (arrowkey[Q] == 1 || arrowkey[FAKE_Q] == 1)
player.moving = SIDE_Q;
if (arrowkey[E] == 1 || arrowkey[FAKE_E] == 1)
player.moving = SIDE_E;
if ((arrowkey[Q] == 1 && arrowkey[UP] == 1)
|| (arrowkey[FAKE_Q] == 1 && arrowkey[UP] == 1)
|| (arrowkey[Q] == 1 && mousebutton[2] == 1)
|| (arrowkey[FAKE_Q] == 1 && mousebutton[2] == 1))
player.moving = FORW_Q;
if ((arrowkey[E] == 1 && arrowkey[UP] == 1)
|| (arrowkey[FAKE_E] == 1 && arrowkey[UP] == 1)
|| (arrowkey[E] == 1 && mousebutton[2] == 1)
|| (arrowkey[FAKE_E] == 1 && mousebutton[2] == 1))
player.moving = FORW_E;
if ((arrowkey[Q] == 1 && arrowkey[DOWN] == 1)
|| (arrowkey[FAKE_Q] == 1 && arrowkey[DOWN] == 1))
player.moving = BACK_Q;
if ((arrowkey[E] == 1 && arrowkey[DOWN] == 1)
|| (arrowkey[FAKE_E] == 1 && arrowkey[DOWN] == 1))
player.moving = BACK_E;
}
TE::TE(ConfigFile* cf, int section)
: Driver(cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN, PLAYER_POSITION2D_CODE) {
dist_eps = cf->ReadTupleLength(section, "goal_tol", 0, 0.5);
ang_eps = cf->ReadTupleAngle(section, "goal_tol", 1, DTOR(10.0));
vx_max = cf->ReadFloat(section, "max_speed", 0.3);
va_max = cf->ReadFloat(section, "max_rot", DTOR(45.0));
vx_min = cf->ReadFloat(section, "min_speed", 0.05);
va_min = cf->ReadFloat(section, "min_rot", DTOR(10.0));
min_dist = cf->ReadLength(section, "min_dist", 0.3);
warning_dist_on = cf->ReadLength(section, "warning_dist_on", 0.4);
warning_dist_off = cf->ReadLength(section, "warning_dist_off", 0.41);
obs_dist = cf->ReadLength(section, "obs_dist", 0.7);
laser_min_angle = cf->ReadFloat(section, "laser_min_angle",
-DTOR(90));
laser_max_angle = cf->ReadFloat(section, "laser_max_angle",
DTOR(90));
verbose = cf->ReadBool(section, "verbose", false);
k_a = cf->ReadFloat(section, "k_a", 1);
odom = NULL;
if (cf->ReadDeviceAddr(&odom_addr, section, "requires",
PLAYER_POSITION2D_CODE, -1, "output") != 0) {
SetError(-1);
return;
}
localize = NULL;
if (cf->ReadDeviceAddr(&localize_addr, section, "requires",
PLAYER_POSITION2D_CODE, -1, "input") != 0) {
SetError(-1);
return;
}
laser = NULL;
memset(&laser_addr, 0, sizeof (player_devaddr_t));
cf->ReadDeviceAddr(&laser_addr, section, "requires",
PLAYER_LASER_CODE, -1, NULL);
if (laser_addr.interf) {
laser_buffer = cf->ReadInt(section, "laser_buffer", 10);
}
return;
}
/**
* Destructor
*/
MOS6581::~MOS6581()
{
// Close the renderer
open_close_renderer(ThePrefs.SIDType, SIDTYPE_NONE);
DTOR(MOS6581);
}
void Robot::SetOdometryByPixelCoord(double dX, double dY, double dYaw, double resolution, double mapWidth, double mapHeight) {
SetPosition(Convert::PixelXCoordToRobotRelativeXPos(dX, CM_TO_METERS(resolution), mapWidth),
Convert::PixelYCoordToRobotRelativeYPos(dY, CM_TO_METERS(resolution), mapHeight),
DTOR(dYaw), true);
_localization->CreateParticle(_dX, _dY, _dYaw, 1);
}
static void do_local_copies(amr_grid_t* grid, amr_grid_data_t* data)
{
// Make sure we have a local patch buffer to use with the right number of components.
amr_grid_data_centering_t centering = amr_grid_data_centering(data);
int num_comp = amr_grid_data_num_components(data);
int num_ghosts = amr_grid_data_num_ghosts(data);
ptr_array_t* local_buffers = grid->local_buffers[centering];
if (num_comp > local_buffers->size)
{
int old_size = local_buffers->size;
ptr_array_resize(local_buffers, num_comp);
for (int i = old_size; i < num_comp; ++i)
local_buffers->data[i] = NULL;
}
if (local_buffers->data[num_comp-1] == NULL)
{
patch_buffer_t* new_buffer = local_patch_buffer_new(grid, centering, num_comp, num_ghosts);
ptr_array_assign_with_dtor(local_buffers, num_comp-1, new_buffer, DTOR(patch_buffer_free));
}
patch_buffer_t* buffer = local_buffers->data[num_comp-1];
// Copy the data into our local patch buffer and then out again.
patch_buffer_copy_in(buffer, data);
// Copy out.
patch_buffer_copy_out(buffer, data);
}
Astar::Astar(ros::NodeHandle & n,Robot *rob,double dG, double cT,QString heuristicT):
nh(n),
distGoal(dG),
covTolerance(cT),
robot(rob),
root(NULL),
test(NULL),
path(NULL),
p(NULL),
openList(NULL),
closedList(NULL),
globalcount(0),
debug(false)
{
try
{
heuristic = Heuristic::factory(heuristicT,debug);
}
catch(SSPPException e)
{
cout<<e.what()<<endl;
}
orientation2Goal = DTOR(60);//for distance hueristic
obj = new OcclusionCullingGPU(nh, "etihad_nowheels_densed.pcd");
covFilteredCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud <pcl::PointXYZ>);
}
void
moving (void)
{
struct groundtri *gt;
if ((gt = detect_plane (&player.p)) == NULL) {
printf ("Can't find ground, moving player to start position "
"to avoid seg fault\n");
player.p.x = 0;
player.p.y = 0;
player.p.z = ground_height (&player.p);
player.movtype = GROUNDED;
gt = detect_plane (&player.p);
}
player.loc = gt;
if (gt->theta < DTOR (50))
player.lastflat = gt;
switch (player.movtype) {
case GROUNDED:
grounded_moving ();
break;
case FALLING:
falling_moving ();
break;
case FLYING:
flying_moving ();
break;
}
}
Astar::Astar():
distGoal(0.01),
covTolerance(0.05),
heuristic(NULL),
root(NULL),
test(NULL),
path(NULL),
p(NULL),
openList(NULL),
closedList(NULL),
globalcount(0),
debug(true)
{
try
{
heuristic = Heuristic::factory("Distance",debug);
}
catch(SSPPException e)
{
cout<<e.what()<<endl;
}
distGoal = 1;//for distance hueristic
orientation2Goal = DTOR(180);//for distance hueristic
obj = new OcclusionCullingGPU("etihad_nowheels_densed.pcd");
covFilteredCloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud <pcl::PointXYZ>);
}
/*!
* \brief Create a new joint ::P3D_BASE with angle in degree
*
* Note: For compatibility reason the orientation of the joint
* couldn't be change by \a pos.
*
* \param pos: the position matrix of the joint
* \param v: the values of the degree of freedom for the joint
* (with angle in degree)
* \param vmin & vmax: the bounds values of
* the degree of freedom for the joint
* (with angle in degree)
* \param vmin_rand & vmax_rand: the random (or user) bounds values of
* the degree of freedom for the joint
* (with angle in degree)
* \param param: the array of the parameters for the joint (NULL here)
*
* \return the new joint.
*
* \internal
*/
p3d_jnt * p3d_jnt_base_create_deg(p3d_matrix4 pos, double * v,
double * vmin, double * vmax,
double * vmin_rand, double * vmax_rand,
double *velocity_max, double *acceleration_max, double *jerk_max, double * param)
{
int i;
for(i=3; i<6; i++) {
v[i] = DTOR(v[i]);
vmin[i] = DTOR(vmin[i]);
vmax[i] = DTOR(vmax[i]);
vmin_rand[i] = DTOR(vmin_rand[i]);
vmax_rand[i] = DTOR(vmax_rand[i]);
}
return p3d_jnt_base_create(pos, v, vmin, vmax, vmin_rand, vmax_rand, velocity_max, acceleration_max, jerk_max,
param);
}
void
middleclickdown (void)
{
arrowkey[UP] = 1;
oldmouse_x = mouse_x;
oldmouse_y = mouse_y;
player.theta = playercamera.theta + DTOR (180);
SDL_WarpMouse (WIDTH / 2, HEIGHT / 2);
}
void define_units_system(int units_system_name)
{
if (units_systems == NULL)
units_systems = int_ptr_unordered_map_new();
if (int_ptr_unordered_map_contains(units_systems, units_system_name))
polymec_error("define_units_system: units system %d is already defined.", units_system_name);
int_ptr_unordered_map_insert_with_v_dtor(units_systems,
units_system_name,
int_real_unordered_map_new(),
DTOR(int_real_unordered_map_free));
}
float Particle::ProbByScan(float laserArray[]) {
int matchCount = 0;
int countCheck = 0;
int mapx = 0;
int mapy = 0;
int scanCount = laserProxy->GetCount();
for (int i = 0; i < scanCount; i += 5) {
countCheck++;
int angle = laserProxy->GetBearing(i);
double distanceFromLaserInPx = Utils::MathUtil::cmToPx(
(double) laserArray[i] * 100.0);
mapx = round(
cos(DTOR(this->yaw) + angle) * distanceFromLaserInPx
+ (double) this->x);
mapy = round(
sin(DTOR(this->yaw) + angle) * distanceFromLaserInPx
+ (double) this->y);
Cell* cell = this->map->getResizedCell(mapx, mapy);
if (cell != NULL) {
if (laserArray[i] < MAX_LEASER_DISTANCE
&& cell->Cell_Cost == CellType::WALL) {
matchCount++;
}
else if (cell->Cell_Cost != CellType::WALL) {
matchCount++;
}
}
}
return matchCount / countCheck;
}
Location* Particle::getMeterLocation(){
Location* location;
// Convert To Meters *10->CM /100>Meter
//for x 0 139 > -6.875 6.875
//for y 0 95 > -4.25 4.25
double x = GetX()/139*13.75-6.875;
double y = 9.5-(GetY()/95*9.5-4.25);
//Set Location To Manager
location->setX(x);
location->setY(y);
location->setYaw(DTOR(GetYaw()));
//cout << "robot location by Meters X: " << x << " y: " << y<< " Yaw: " << best->GetYaw() << endl;
return location;
}
////////////////////////////////////////////////////////////////////////////////
// Constructor. Retrieve options from the configuration file and do any
// pre-Setup() setup.
PlayerNIMU::PlayerNIMU (ConfigFile* cf, int section)
: Driver (cf, section, false, PLAYER_MSGQUEUE_DEFAULT_MAXLEN,
PLAYER_IMU_CODE)
{
// raw values
dataType = cf->ReadInt (section, "data_packet_type", 2);
AccelRange = 9.81*cf->ReadFloat(section, "accel_range", DEFAULT_ACCEL_RANGE);
GyroRange = DTOR(cf->ReadFloat(section, "gyro_range", DEFAULT_GYRO_RANGE));
MagRange = cf->ReadFloat(section, "mag_range", DEFAULT_MAG_RANGE);
return;
}
void
rightclickdown (void)
{
oldmouse_x = mouse_x;
oldmouse_y = mouse_y;
player.theta = playercamera.theta + DTOR (180);
SDL_WarpMouse (WIDTH / 2, HEIGHT / 2);
if (mousebutton[1]) {
middleclickdown ();
mousebutton[2] = 1;
}
if (arrowkey[LEFT])
arrowkey[FAKE_Q] = 1;
if (arrowkey[RIGHT])
arrowkey[FAKE_E] = 1;
}
static int start_remote_copies(amr_grid_t* grid, amr_grid_data_t* data)
{
// Make a new remote data thingy.
remote_data_t* remote_data = remote_data_new(data);
// Copy the grid data into the thingy's patch buffer.
patch_buffer_t* buffer = remote_data->buffer;
patch_buffer_copy_in(buffer, data);
// Initiate the data transfer and receive a token for the exchange.
int num_comp = amr_grid_data_num_components(data);
int token = exchanger_start_exchange(remote_data->ex, buffer->data, num_comp, 0, MPI_REAL_T);
// Stash the remote data into our set of pending data.
int_ptr_unordered_map_insert_with_v_dtor(grid->pending_data, token, remote_data, DTOR(remote_data_free));
return token;
}
str_grid_node_data_t* str_grid_node_data_with_buffer(str_grid_t* grid,
int num_components,
void* buffer)
{
ASSERT(num_components > 0);
// Allocate storage.
int num_patches = str_grid_num_patches(grid);
size_t patches_size = sizeof(str_grid_patch_t*) * num_patches;
size_t node_data_size = sizeof(str_grid_node_data_t) + patches_size;
str_grid_node_data_t* node_data = polymec_malloc(node_data_size);
node_data->grid = grid;
node_data->nc = num_components;
str_grid_get_extents(grid, &node_data->nx, &node_data->ny, &node_data->nz);
node_data->patches = int_ptr_unordered_map_new();
node_data->patch_offsets = polymec_malloc(sizeof(int) * (num_patches+1));
node_data->buffer = NULL;
// Now populate the patches (with NULL buffers).
int px, py, pz;
str_grid_get_patch_size(grid, &px, &py, &pz);
node_data->patch_lx = 1.0 / node_data->nx;
node_data->patch_ly = 1.0 / node_data->ny;
node_data->patch_lz = 1.0 / node_data->nz;
int pos = 0, i, j, k, l = 0;
while (str_grid_next_patch(grid, &pos, &i, &j, &k))
{
int index = patch_index(node_data, i, j, k);
int_ptr_unordered_map_insert_with_v_dtor(node_data->patches, index,
str_grid_patch_with_buffer(px+1, py+1, pz+1, num_components, 0, NULL),
DTOR(str_grid_patch_free));
++l;
}
count_nodes(node_data);
// Set the buffer.
str_grid_node_data_set_buffer(node_data, buffer, false);
return node_data;
}
void fe_mesh_add_block(fe_mesh_t* mesh,
const char* name,
fe_block_t* block)
{
ptr_array_append_with_dtor(mesh->blocks, block, DTOR(fe_block_free));
string_array_append_with_dtor(mesh->block_names, string_dup(name), string_free);
int num_block_elements = fe_block_num_elements(block);
int num_elements = mesh->block_elem_offsets->data[mesh->block_elem_offsets->size-1] + num_block_elements;
int_array_append(mesh->block_elem_offsets, num_elements);
// If we are adding a polyhedral block, read off the maximum face and use
// that to infer the number of faces in the mesh.
if (block->elem_faces != NULL)
{
int max_face = mesh->num_faces - 1;
for (int i = 0; i < block->elem_face_offsets[num_block_elements]; ++i)
max_face = MAX(max_face, block->elem_faces[i]);
mesh->num_faces = max_face + 1;
}
}
amr_data_hierarchy_t* amr_data_hierarchy_new(amr_grid_hierarchy_t* grids,
int num_components,
int num_ghosts)
{
ASSERT(num_components > 0);
amr_data_hierarchy_t* data = polymec_malloc(sizeof(amr_data_hierarchy_t));
data->grids = grids;
data->num_components = num_components;
data->num_ghosts = num_ghosts;
data->grid_data = ptr_array_new();
int pos = 0;
amr_grid_t* level;
while (amr_grid_hierarchy_next_coarsest(grids, &pos, &level))
{
amr_grid_data_t* grid_data = amr_grid_data_new(level, AMR_GRID_CELL, data->num_components, data->num_ghosts);
ptr_array_append_with_dtor(data->grid_data, grid_data, DTOR(amr_grid_data_free));
}
return data;
}
int
pt_in_gt (struct pt *p, struct groundtri *gt)
{
double theta;
struct vect v1, v2;
theta = 0;
psub (&v1, >->p1, p);
vnorm (&v1, &v1);
psub (&v2, >->p2, p);
vnorm (&v2, &v2);
theta += acos (vdot (&v1, &v2));
psub (&v1, >->p2, p);
vnorm (&v1, &v1);
psub (&v2, >->p3, p);
vnorm (&v2, &v2);
theta += acos (vdot (&v1, &v2));
psub (&v1, >->p3, p);
vnorm (&v1, &v1);
psub (&v2, >->p1, p);
vnorm (&v2, &v2);
theta += acos (vdot (&v1, &v2));
if (theta >= DTOR ((360 - 1e-7))) {
return (1);
}
return (0);
}
int
main(int argc, const char **argv)
{
int i;
playerc_client_t *client;
playerc_position2d_t *position2d;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Make the robot spin!
if (0 != playerc_position2d_set_cmd_vel(position2d, 0.25, 0, DTOR(40.0), 1))
return -1;
for (i = 0; i < 200; i++)
{
// Wait for new data from server
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
// Shutdown
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
int str_grid_cell_filler_start(str_grid_cell_filler_t* cell_filler, str_grid_cell_data_t* data)
{
ASSERT(str_grid_cell_data_grid(data) == cell_filler->grid);
// Dream up a new token for ghost fills.
int token = 0;
while (int_ptr_unordered_map_contains(cell_filler->tokens, token))
++token;
// Map our token to a list of underlying operation tokens.`
int_array_t* op_tokens = int_array_new();
int_ptr_unordered_map_insert_with_v_dtor(cell_filler->tokens, token, op_tokens, DTOR(int_array_free));
// Begin the ghost fill operations and gather tokens.
int pos = 0, patch_index;
ptr_array_t* fillers;
while (int_ptr_unordered_map_next(cell_filler->patch_fillers, &pos, &patch_index, (void**)&fillers))
{
int i, j, k;
get_patch_indices(cell_filler, patch_index, &i, &j, &k);
// Go through all the fillers for this patch.
for (int l = 0; l < fillers->size; ++l)
{
str_grid_patch_filler_t* filler = fillers->data[l];
int op_token = str_grid_patch_filler_start(filler, i, j, k, data);
ASSERT((op_token >= 0) || (op_token == -1));
if (op_token != -1) // -1 <-> no finish operation.
{
// We stash both the patch index and the op token.
int_array_append(op_tokens, patch_index);
int_array_append(op_tokens, op_token);
}
}
}
return token;
}
serializer_t* exchanger_serializer()
{
return serializer_new("exchanger", ex_size, ex_read, ex_write, DTOR(exchanger_free));
}
int
main(int argc, const char **argv)
{
int i;
playerc_client_t *client;
player_pose2d_t position2d_vel;
player_pose2d_t position2d_target;
playerc_position2d_t *position2d;
// Create a client and connect it to the server.
client = playerc_client_create(NULL, "localhost", 6665);
if (0 != playerc_client_connect(client))
return -1;
// Create and subscribe to a position2d device.
position2d = playerc_position2d_create(client, 0);
if (playerc_position2d_subscribe(position2d, PLAYER_OPEN_MODE))
return -1;
// Make the robot spin!
if (0 != playerc_position2d_set_cmd_vel(position2d, 0.25, 0, DTOR(40.0), 1))
return -1;
for (i = 0; i < 50; i++)
{
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
/* con el comando playerc_position2d_set_cmd_vel() simplemente se establece una
* consigna de velocidad la cual es independiente de la posición en la que se
* encuentra el robot */
position2d_target.px = 2;
position2d_target.py = -3;
position2d_target.pa = 0;
// Move to pose
playerc_position2d_set_cmd_pose(position2d, position2d_target.px , position2d_target.py, position2d_target.pa , 1);
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
// Stop when reach the target
while (sqrt(pow(position2d->px - position2d_target.px,2.0) + pow(position2d->py - position2d_target.py,2.0)) > 0.05 )
{
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
/* Con el comando position2d_set_cmd_pose() se establece una pose de destino.
* Cuando se utiliza este método la información obtenida de la odometría sí que
* afecta a la trayectoria seguida y al punto final */
position2d_target.px = 0;
position2d_target.py = -3;
position2d_target.pa = 0;
position2d_vel.px = 0.6;
position2d_vel.py = 0;
position2d_vel.pa = 0;
// Move to pose
playerc_position2d_set_cmd_pose_with_vel(position2d, position2d_target, position2d_vel, 1);
// Stop when reach the target
while (sqrt(pow(position2d->px - position2d_target.px,2.0) + pow(position2d->py - position2d_target.py,2.0)) > 0.05 )
{
playerc_client_read(client);
// Print current robot pose
printf("position2d : %f %f %f\n",
position2d->px, position2d->py, position2d->pa);
}
/* Con el comando playerc_position2d_set_cmd_pose_with_vel se hace lo mismo que
* con playerc_position2d_set_cmd_pose() pero además se puede indicar una
* consigna de velocidad */
// Shutdown
playerc_position2d_unsubscribe(position2d);
playerc_position2d_destroy(position2d);
playerc_client_disconnect(client);
playerc_client_destroy(client);
return 0;
}
*
* @class FollowUpCurvePlot
*
* Plot of the follow-up curves of the global Br2Hdr::Instance().
*
*=====================================================================*/
class FollowUpCurvePlot : public br::CurvePlotClass, public br::EventReceiver
{
private:
int channel_;
br::CurvePlotClass & plot_; // synonym to make the code clearer
public:
FollowUpCurvePlot (int X, int Y, int W, int H, const char* la=0);
~FollowUpCurvePlot() {DTOR(label())}
void set_channel (int c) {channel_ = c;}
int channel() const {return channel_;} // current channel to plot
void build();
void update() {build(); redraw();}
void handle_Event (br::Br2Hdr::Event); // virtual EventReceiver inheritance
};
#endif // FllwUpCrvPlot.hpp
// END OF FILE
void
activate_muscle (HEAD *face, float *vt, float *vh, float fstart, float fin, float ang, float val)
{
float newp[3], va[3], vb[3] ;
int i,j,k,l ;
float valen, vblen ;
float cosa, cosv, dif, tot, percent, thet, newv, the, radf ;
radf = 180.0/ M_PI ;
the = ang / radf ; ;
thet = cos ( the ) ;
cosa = 0.0 ;
/* find the length of the muscle */
for (i=0; i<3; i++)
va[i] = vt[i] - vh[i] ;
valen = VecLen ( va ) ;
/* loop for all polygons */
for (i=0; i<face->npolygons; i++) {
/* loop for all vertices */
for (j=0; j<3; j++) {
/* find the length of the muscle head to the mesh node */
for (k=0; k<3; k++)
vb[k] = face->polygon[i]->vertex[j]->xyz[k] - vh[k] ;
vblen = VecLen ( vb ) ;
if ( valen > 0.0 && vblen > 0.0) {
cosa = CosAng ( va, vb ) ;
if ( cosa >= thet ) {
if ( vblen <= fin ) {
cosv = val * ( 1.0 - (cosa/thet) ) ;
if ( vblen >= fstart && vblen <= fin) {
dif = vblen - fstart ;
tot = fin - fstart ;
percent = dif/tot ;
newv = cos ( DTOR(percent*90.0) ) ;
for ( l=0; l<3; l++)
newp[l] = (vb[l] * cosv) * newv ;
}
else {
for ( l=0; l<3; l++)
newp[l] = vb[l] * cosv ;
} /* endif vblen>fin */
for (l=0; l<3; l++)
face->polygon[i]->vertex[j]->xyz[l] += newp[l] ;
} /* endif vblen>fin */
} /* endif cosa>thet */
} /* endif mlen&&tlen */
} /* end for j vertices */
} /* end for i polygon */
}
