/** *@brief 初期手先位置を設定 * @param s_p 初期手先位置 * @param the 初期手先姿勢 * @param maxSpeedCartesianTrans 手先の最大速度 * @param maxSpeedCartesianRot 手先姿勢の最大速度 * @param minTime 到達時間の最小値 */ void TargetPos::setStartPoint(Vector3d s_p, double the, double maxSpeedCartesianTrans, double maxSpeedCartesianRot, double minTime) { start_pos(0) = s_p(0); start_pos(1) = s_p(1); start_pos(2) = s_p(2); start_theta = the; if(end_time <= 0) { double dx = target_pos(0)-start_pos[0]; double dy = target_pos(1)-start_pos[1]; double dz = target_pos(2)-start_pos[2]; double ST = sqrt(dx*dx+dy*dy+dz*dz); end_time = ST/maxSpeedCartesianTrans; double dt = target_theta-start_theta; dt = sqrt(dt*dt)/maxSpeedCartesianRot; if(end_time < dt) end_time = dt; if(end_time < minTime) end_time = minTime; std::cout << end_time << std::endl; } }
/** *@brief 目標手先位置を設定 * @param t 到達時間(0以下に設定した場合は初期位置と目標位置との距離から自動的に計算) * @param t_p 手先位置 * @param the 手先姿勢 */ void TargetPos::setPoint(double t, Vector3d t_p, double the) { target_pos(0) = t_p(0); target_pos(1) = t_p(1); target_pos(2) = t_p(2); target_theta = the; type = Point; end_time = t; }
void WarkeyDlg::OnMotion(wxMouseEvent &evt) { if (evt.LeftIsDown() && evt.Dragging()) { wxPoint target_pos(evt.GetPosition()); auto delta = target_pos - orig_mouse_pos_; orig_wnd_pos_ += delta; Move(orig_wnd_pos_); } }
void SGSHero::onMoveTo(SGMessage* message) { int x, y; message->getInt("x", &x); message->getInt("y", &y); SGSPoint target_pos(x,y); SGSPointList& path = __terrain->calcShortestPath(this, target_pos); if (path.empty()) { log("Failed to find available path"); return; } else { path.erase(path.begin()); } moveOneStep(&path); }
int main () { // file to write ofstream outputfile("tracking.csv"); //make instance double m_dt=0.001; interpolator* i = new interpolator(1, m_dt,interpolator::HOFFARBIB,0.5); double start=0.0,goal=1.0,goalv=0; double test_time=10; double t=0; double x=0,v=0,a=0; double rt; //i->load("data.csv"); i->set(&start); for (int j=0;j<test_time/m_dt;j++){ // set goal double target,c_target; double interpolation_time; double interpolation_gain=10; double Kp=0.1; target=target_pos(t); c_target=Kp*(target-x) +x; interpolation_time= 0.1;//interpolation_gain*fabs(x-target); //std::cout << fabs(x-target) << std::endl; // std::cout << interpolation_time << std::endl; i->setGoal(&c_target, interpolation_time, true); // get interpolation val if (i->isEmpty()) std::cout << "empty!" << std::endl; i->get(&x,&v,&a,true); outputfile << t << " " << x << " " << v << " "<< a << " " << target <<std::endl; t+=i->deltaT(); } outputfile.close(); return 0; };
void Simu ::_make_robot_init(float duration) { robot_t rob = this->robot(); Eigen::Vector3d rot=rob->rot(); _arrival_angle= atan2( cos(rot[2])* sin(rot[1])* sin(rot[0]) + sin(rot[2])* cos(rot[0]), cos(rot[2])* cos(rot[1]))*180/M_PI; //std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl; // std::cout<<"initial angle "<<_arrival_angle<<std::endl; Eigen::Vector3d target_pos(0,2,0.2); _controller.moveRobot(rob,0); /* float t = 0.0f; while (t < 1) { t += step; next_step(); }*/ /*if(!stabilize_robot()) { _energy=1e20; _covered_distance=1e10; std::cout<<"non stab"<<std::endl; return; }*/ Eigen::Vector3d prev_pos = rob->pos(); float t=0; int index = 0; #ifdef GRAPHIC while (t < duration && !_visitor.done()) #else while (t < duration) #endif { _controller.moveRobot(rob,t); if(_robot->bodies()[0]->get_in_contact() || _env->get_colision_between_legs() || _robot->pos()[2]>0.5) { #ifdef GRAPHIC std::cout<<"mort subite"<<std::endl; #endif _covered_distance=-10002; return; } int nbCassee=0; if (index%2==0) for (unsigned i = 0; i < 6; ++i) { switch (i) { case 0: if (_controller.isBroken(i)) { _behavior_contact_0.push_back(0); nbCassee++; } else { _behavior_contact_0.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() ); } break; case 1: if (_controller.isBroken(i)) { _behavior_contact_1.push_back(0); nbCassee++; } else { _behavior_contact_1.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() ); } break; case 2: if (_controller.isBroken(i)) { _behavior_contact_2.push_back(0); nbCassee++; } else { _behavior_contact_2.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() ); } break; case 3: if (_controller.isBroken(i)) { _behavior_contact_3.push_back(0); nbCassee++; } else { _behavior_contact_3.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() ); } break; case 4: if (_controller.isBroken(i)) { _behavior_contact_4.push_back(0); nbCassee++; } else { _behavior_contact_4.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() ); } break; case 5: if (_controller.isBroken(i)) { _behavior_contact_5.push_back(0); nbCassee++; } else { _behavior_contact_5.push_back( _robot->bodies()[(i-nbCassee) * 3 + 3]->get_in_contact() ); } break; } } _behavior_traj.push_back(rob->pos()); t += step; next_step(); ++index; } Eigen::Vector3d end_mov_pos = rob->pos(); //std::cout<<"fin mov:"<<end_mov_pos<<std::endl; stabilize_robot(); Eigen::Vector3d end_stab_pos = rob->pos(); //std::cout<<"fin stab:"<<end_stab_pos<<std::endl; if((end_stab_pos-end_mov_pos).norm()>0.2) { _covered_distance=-10002; return; } // _covered_distance=sqrt((next_pos[0] - target_pos[0])*(next_pos[0] - target_pos[0])+(next_pos[1] - target_pos[1])*(next_pos[1] - target_pos[1])+(next_pos[2] - target_pos[2])*(next_pos[2] - target_pos[2])); // _covered_distance=2-_covered_distance; // _covered_distance = round(next_pos[1]*100) / 100.0f; _final_pos.resize(2); _final_pos[0]=end_stab_pos[0]-prev_pos[0]; _final_pos[1]=end_stab_pos[1]-prev_pos[1]; // std::cout<<"final pos:"<<_final_pos[0]<<" "<< _final_pos[1]<<std::endl; // _covered_distance = round(sqrt(next_pos[0]*next_pos[0]+next_pos[1]*next_pos[1]+next_pos[2]*next_pos[2])*100) / 100.0f; _covered_distance = round(_final_pos[1]*100) / 100.0f; }
//----------------------------------------------------------------------------- // render() //----------------------------------------------------------------------------- BOOL LLImagePreviewSculpted::render() { mNeedsUpdate = FALSE; LLGLSUIDefault def; LLGLDisable no_blend(GL_BLEND); LLGLEnable cull(GL_CULL_FACE); LLGLDepthTest depth(GL_TRUE); glMatrixMode(GL_PROJECTION); gGL.pushMatrix(); glLoadIdentity(); glOrtho(0.0f, mWidth, 0.0f, mHeight, -1.0f, 1.0f); glMatrixMode(GL_MODELVIEW); gGL.pushMatrix(); glLoadIdentity(); gGL.color4f(0.15f, 0.2f, 0.3f, 1.f); gl_rect_2d_simple( mWidth, mHeight ); glMatrixMode(GL_PROJECTION); gGL.popMatrix(); glMatrixMode(GL_MODELVIEW); gGL.popMatrix(); glClear(GL_DEPTH_BUFFER_BIT); LLVector3 target_pos(0, 0, 0); LLQuaternion camera_rot = LLQuaternion(mCameraPitch, LLVector3::y_axis) * LLQuaternion(mCameraYaw, LLVector3::z_axis); LLQuaternion av_rot = camera_rot; LLViewerCamera::getInstance()->setOriginAndLookAt( target_pos + ((LLVector3(mCameraDistance, 0.f, 0.f) + mCameraOffset) * av_rot), // camera LLVector3::z_axis, // up target_pos + (mCameraOffset * av_rot) ); // point of interest stop_glerror(); LLViewerCamera::getInstance()->setAspect((F32) mWidth / mHeight); LLViewerCamera::getInstance()->setView(LLViewerCamera::getInstance()->getDefaultFOV() / mCameraZoom); LLViewerCamera::getInstance()->setPerspective(FALSE, mOrigin.mX, mOrigin.mY, mWidth, mHeight, FALSE); const LLVolumeFace &vf = mVolume->getVolumeFace(0); U32 num_indices = vf.mIndices.size(); mVertexBuffer->setBuffer(LLVertexBuffer::MAP_VERTEX | LLVertexBuffer::MAP_NORMAL); gPipeline.enableLightsAvatar(); gGL.pushMatrix(); const F32 SCALE = 1.25f; gGL.scalef(SCALE, SCALE, SCALE); const F32 BRIGHTNESS = 0.9f; gGL.color3f(BRIGHTNESS, BRIGHTNESS, BRIGHTNESS); mVertexBuffer->draw(LLRender::TRIANGLES, num_indices, 0); gGL.popMatrix(); return TRUE; }
void Simu::_make_robot_init(float duration) { robot_t rob = this->robot(); Eigen::Vector3d rot = rob->rot(); _arrival_angle = atan2(cos(rot[2]) * sin(rot[1]) * sin(rot[0]) + sin(rot[2]) * cos(rot[0]), cos(rot[2]) * cos(rot[1])) * 180 / M_PI; //std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl; // std::cout<<"initial angle "<<_arrival_angle<<std::endl; Eigen::Vector3d target_pos(0, 2, 0.2); _controller.moveRobot(rob, 0); /* float t = 0.0f; while (t < 1) { t += step; next_step(); }*/ /*if(!stabilize_robot()) { _energy=1e20; _covered_distance=1e10; std::cout<<"non stab"<<std::endl; return; }*/ Eigen::VectorXd act_state = _get_state(rob); float t = 0; int index = 0; #ifdef GRAPHIC while (t < duration && !_visitor.done()) #else while (t < duration) #endif { _controller.moveRobot(rob, t); Eigen::VectorXd new_state = _get_state(rob); _energy += (act_state - new_state).array().abs().sum(); act_state = new_state; if (_robot->bodies()[0]->get_in_contact() || _env->get_colision_between_legs()) { #ifdef GRAPHIC std::cout << "mort subite" << std::endl; #endif std::cout << "mort subite" << std::endl; //Eigen::Vector3d next_pos = rob->pos(); //_covered_distance = round(next_pos[1]*100) / 100.0f; _covered_distance = 00.0f; //_covered_distance=-10002; return; } int nbCassee = 0; if (index % 2 == 0) for (unsigned i = 0; i < 6; ++i) { switch (i) { case 0: if (_controller.isBroken(i)) { _behavior_contact_0.push_back(0); nbCassee++; } else { _behavior_contact_0.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact()); } break; case 1: if (_controller.isBroken(i)) { _behavior_contact_1.push_back(0); nbCassee++; } else { _behavior_contact_1.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact()); } break; case 2: if (_controller.isBroken(i)) { _behavior_contact_2.push_back(0); nbCassee++; } else { _behavior_contact_2.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact()); } break; case 3: if (_controller.isBroken(i)) { _behavior_contact_3.push_back(0); nbCassee++; } else { _behavior_contact_3.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact()); } break; case 4: if (_controller.isBroken(i)) { _behavior_contact_4.push_back(0); nbCassee++; } else { _behavior_contact_4.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact()); } break; case 5: if (_controller.isBroken(i)) { _behavior_contact_5.push_back(0); nbCassee++; } else { _behavior_contact_5.push_back(_robot->bodies()[(i - nbCassee) * 3 + 3]->get_in_contact()); } break; } } _behavior_traj.push_back(rob->pos()); t += step; next_step(); ++index; } if (fabs(_angle) < 0.01) { stabilize_robot(); } Eigen::Vector3d next_pos = rob->pos(); // _covered_distance=sqrt((next_pos[0] - target_pos[0])*(next_pos[0] - target_pos[0])+(next_pos[1] - target_pos[1])*(next_pos[1] - target_pos[1])+(next_pos[2] - target_pos[2])*(next_pos[2] - target_pos[2])); // _covered_distance=2-_covered_distance; // _covered_distance = round(next_pos[1]*100) / 100.0f; _final_pos.resize(2); _final_pos[0] = next_pos[0]; _final_pos[1] = next_pos[1]; // _covered_distance = round(sqrt(next_pos[0]*next_pos[0]+next_pos[1]*next_pos[1]+next_pos[2]*next_pos[2])*100) / 100.0f; _covered_distance = round(next_pos[1] * 100) / 100.0f; if (fabs(_covered_distance) > 10) { _covered_distance = 00.0f; } _direction = atan2(-next_pos[0], next_pos[1]) * 180 / M_PI; rot = rob->rot(); _arrival_angle = atan2(cos(rot[2]) * sin(rot[1]) * sin(rot[0]) + sin(rot[2]) * cos(rot[0]), cos(rot[2]) * cos(rot[1])) * 180 / M_PI; while (_arrival_angle < -180) _arrival_angle += 360; while (_arrival_angle > 180) _arrival_angle -= 360; //std::cout<<"final angle "<<_arrival_angle<<" déviation: "<<_direction-_arrival_angle<<" fit: "<<_covered_distance<<std::endl; //std::cout<<rot[0]<< " " << rot[1] <<" "<<rot[2]<<std::endl; if (_transf) { std::vector<std::vector<float>> contacts; contacts.push_back(_behavior_contact_0); contacts.push_back(_behavior_contact_1); contacts.push_back(_behavior_contact_2); contacts.push_back(_behavior_contact_3); contacts.push_back(_behavior_contact_4); contacts.push_back(_behavior_contact_5); write_data(contacts, "contacts"); std::vector<float> angles; angles.push_back(rob->rot()[0]); angles.push_back(rob->rot()[1]); angles.push_back(rob->rot()[2]); write_data(angles, "angles"); write_data(_covered_distance, "fit"); } }
void DelaunayPulseIn::update( float elapsed ) { timer += elapsed; // update animation //printf("queue contains: "); vector<MovingPulse> dequeued; while ( !queued_pulses.empty() && queued_pulses.top().timer < timer ) { // dequeue dequeued.push_back( queued_pulses.top() ); // remove from queue queued_pulses.pop(); } // pings vector<pair< int, float > > pings; // coalesce pulses for the same node map<int,int> coalesced; for ( int i=0; i<dequeued.size(); i++ ) { int id = dequeued[i].id; // add ping here (because we might drop it later) pings.push_back( make_pair( id, dequeued[i].energy ) ); // coalesce if ( coalesced.find( id ) == coalesced.end() ) { coalesced[id] = i; } else { // merge dequeued[coalesced[id]].energy += dequeued[i].energy; // remove dequeued.erase( dequeued.begin()+i ); i--; } } // no go through coalesced for ( int i=0; i<dequeued.size(); i++ ) { // fetch id int curr = dequeued[i].id; float energy = dequeued[i].energy; //printf("%2i:%7.5f ", curr, energy ); // pulse the led if ( dequeued[i].timer > 0 ) lights->pulse( curr, energy ); // finished? if ( curr == target_index || energy < 0.001f ) continue; // add the closest neighbours to queue set<int> adj = delaunay->getNeighbours( curr ); ofxVec2f curr_pos( lights->getLight( curr ).getX(), lights->getLight( curr ).getY() ); ofxVec2f target_pos( lights->getLight( target_index ).getX(), lights->getLight( target_index ).getY() ); // use for calculating dot product (-> angle ) ofxVec2f target_delta = target_pos - curr_pos; float distance_to_target = target_delta.length(); target_delta /= distance_to_target; //printf("delta to target is %7.5f %7.5f\n", target_delta.x, target_delta.y ); for ( set<int>::iterator jt = adj.begin(); jt != adj.end(); ++jt ) { // add neighbours, distributing energy ofxVec2f adj_pos( lights->getLight( *jt ).getX(), lights->getLight( *jt ).getY() ); ofxVec2f adj_delta = adj_pos - curr_pos; float distance = adj_delta.length(); adj_delta /= distance; // dot product float direction_accuracy = target_delta.dot( adj_delta ); // would this path take us towards the target? //printf(" adj delta to next is %7.5f %f7.5 -> accuracy %7.5f\n", adj_delta.x, adj_delta.y, direction_accuracy ); if ( direction_accuracy > 0.5f ) { direction_accuracy*=direction_accuracy; // yes //float new_energy = direction_accuracy*0.5f*(max_brightness*(1.0f-(distance_to_target/initial_radius))); float falloff = 0.25f; float new_energy = energy*direction_accuracy*0.5f - energy*distance*falloff; queued_pulses.push( MovingPulse(*jt, new_energy, ofRandom(0.8f,1.2f)*(timer+(distance/speed)) ) ); } } } //printf("\n"); // send pings for ( int i=0; i<pings.size(); i++ ) { ofxOscMessage m; m.setAddress( "/delaunay/ping" ); m.addFloatArg( pings[i].second ); const Light& light = lights->getLight( pings[i].first ); m.addFloatArg( light.getX() ); m.addFloatArg( light.getY() ); m.addFloatArg( delaunay->getId() ); Osc::getInstance()->sendMessage( m ); } }