コード例 #1
0
/** A sample application to demonstrate a physics simulation in scl.
 *
 * Moving forward from tutorial 3, we will now control the 6 DOF
 * demo robot (r6bot) with the physics engine running.
 *
 * SCL Modules used:
 * 1. data_structs
 * 2. dynamics (physics)
 * 4. dynamics (control matrices)
 * 4. graphics (chai)
 * */
int main(int argc, char** argv)
{
  std::cout<<"\n***************************************\n";
  std::cout<<"Standard Control Library Tutorial #4";
  std::cout<<"\n***************************************\n";

  scl::SRobotParsed rds;     //Robot data structure....
  scl::SGraphicsParsed rgr;  //Robot graphics data structure...
  scl::SGcModel rgcm;        //Robot data structure with dynamic quantities...
  scl::SRobotIO rio;         //I/O data structure
  scl::CGraphicsChai rchai;  //Chai interface (updates graphics rendering tree etc.)
  scl::CDynamicsScl dyn_scl; //Robot kinematics and dynamics computation object...
  scl::CDynamicsTao dyn_tao; //Robot physics integrator
  scl::CParserScl p;         //This time, we'll parse the tree from a file...
  sutil::CSystemClock::start();

  /******************************Load Robot Specification************************************/
  //We will use a slightly more complex xml spec than the first few tutorials
  bool flag = p.readRobotFromFile("../../specs/Kuka/KukaCfg.xml","../../specs/","KukaBot",rds);
  flag = flag && rgcm.init(rds);            //Simple way to set up dynamic tree...
  flag = flag && dyn_tao.init(rds);         //Set up integrator object
  flag = flag && dyn_scl.init(rds);         //Set up kinematics and dynamics object
  flag = flag && rio.init(rds.name_,rds.dof_);
  for(unsigned int i=0;i<rds.dof_;++i){ rio.sensors_.q_(i) = rds.rb_tree_.at(i)->joint_default_pos_; }
  if(false == flag){ return 1; }            //Error check.

  /******************************ChaiGlut Graphics************************************/
  glutInit(&argc, argv); // We will use glut for the window pane (not the graphics).

  flag = p.readGraphicsFromFile("../../specs/Kuka/KukaCfg.xml","KukaBotStdView",rgr);
  flag = flag && rchai.initGraphics(&rgr);
  flag = flag && rchai.addRobotToRender(&rds,&rio);
  flag = flag && scl_chai_glut_interface::initializeGlutForChai(&rgr, &rchai);
  if(false==flag) { std::cout<<"\nCouldn't initialize chai graphics\n"; return 1; }

  /******************************Simulation************************************/
  // Now let us integrate the model for a variety of timesteps and see energy stability
  std::cout<<"\nIntegrating the r6bot's physics. \nWill test two different controllers.\n Press (x) to exit at anytime.";
  long iter = 0, n_iters=200; double dt=0.0001;

  omp_set_num_threads(2);
  int thread_id; double tstart, tcurr; flag = false;
  const Eigen::Vector3d hpos0(0,1,0); //control position of op-point wrt. hand
  const Eigen::Vector3d hpos1(0,1,1); //control position of op-point wrt. hand
  Eigen::MatrixXd Jx0, Jx1;
  Eigen::Vector3d x0, x0_des=(0,0,0), x_init0, dx0;
  Eigen::Vector3d x1, x1_des1=x0_des+(hpos1-hpos0), x_init1, dx1;
  scl::SRigidBodyDyn *rhand = rgcm.rbdyn_tree_.at("hand");

#pragma omp parallel private(thread_id)
  {
    thread_id = omp_get_thread_num();
    if(thread_id==1) //Simulate physics and update the rio data structure..
    {
      // Controller 1 : gc controller
      std::cout<<"\n\n***************************************************************"
          <<"\n Starting joint space (generalized coordinate) controller..."
          <<"\n This will move the joints in a sine wave"
          <<"\n***************************************************************";
      tstart = sutil::CSystemClock::getSimTime();
      sutil::CSystemClock::tick(dt);
      while(iter < n_iters && true == scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running)
      {
        tcurr = sutil::CSystemClock::getSysTime();
        // Controller : fgc = kp * (sin(t) - q) + kv(0 - dq)
        // Set the desired positions so that the joints move in a sine wave
        rio.actuators_.force_gc_commanded_ = 50* (sin(tcurr-tstart) - rio.sensors_.q_.array()) - 20 *       rio.sensors_.dq_.array();
        dyn_tao.integrate(rio,dt);
        iter++;
        const timespec ts = {0, 5000};/*.05ms*/
        nanosleep(&ts,NULL);

        if(iter % 1000 == 0){
           // std::cout<<"\n"<<(sin(tcurr-tstart) - rio.sensors_.q_.array()).transpose();
        //std::cout<<"\n"<<rio.actuators_.force_gc_commanded_.transpose();
            }
      }
     sleep(1);

      // Controller 2 : Operational space controller
      std::cout<<"\n\n***************************************************************"
          <<"\n Starting op space (task coordinate) controller..."
          <<"\n This will move the hand in a circle. x =sin(t), y=cos(t)."
          <<"\n***************************************************************";

      tstart = sutil::CSystemClock::getSimTime(); iter = 0;
	  Eigen::Vector3d ball_forces,ball_pos,ball_vel,ball_accel,paddle_orientation;
      Eigen::Vector3d ball_pos_int = (1, 1, 10);
      Eigen::Vector3d ball_vel_int =(0, 0, 0);
      Eigen::Vector3d gravity = (0,0,-9.8),ez=(0,0,1);
      Eigen::Vector3d ball_force_est,ball_pos_est, ball_vel_est,ball_accel_est,orientation;
      bool hitting = false;
      float ball_pos_threshold = 2,radius=.025,rho=1,Cd=.1,mass=1,impact_time=(10000*dt),Delta_T=.01;
      int est_iter = 0;
      ball_pos=ball_pos_int;
      ball_vel=ball_vel_int;

      while( iter<200000 && true == scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running)
      {
        tcurr = sutil::CSystemClock::getSimTime();


		paddle_orientation=x1-x0;


		if(ball_pos(2)<x0(2)){ //checks for impact
			ball_vel=1.2*ball_vel-2(ball_vel.transpose()*paddle_orientation.normalized())*paddel_orientation.normalized();
			rtask_hand->x0_des(2)=0;
			hitting=false;
			impact_time=(iter+10000)*dt;

		}else{ //if no impact
			if(hitting){       //check to see if hitting          
				rtask_hand->x0_des(2)=100;

		}else if(iter*dt>imapct_time-Delta_T){ //check to see it should be hitting
			hitting=true;  

		}else{ //if no impact and not hitting esitmate trejectory
			ball_pos_est=ball_pos; //intialize for estimation
			ball_vel_est=ball_vel;
			est_iter = 0;

			while(ball_pos_est(2) > ball_pos_threshold){//estimated trajectory
				ball_force_est=gravity-.5*rho*3.14159*radius*radius*Cd*ball_vel_est.squaredNorm()*ball_vel_est.Norm();
				ball_accel_est=ball_force_est/mass;
				ball_vel_est=ball_vel_est+ball_accel_est*dt;
				ball_pos_est=ball_pos_est+ball_vel_est*dt;
				est_iter++;
									     							  
			}							  
			impact_time=(iter+est_iter)*dt;
			x0_des(0) = ball_pos_est(0); //crossing location in z plane
			x0_des(1) = ball_pos_est(1);
	      
		}
	    //computing ball forces since no impact
			ball_force=gravity-.5*rho*3.14159*radius*radius*Cd*ball_vel.squaredNorm()*ball_vel.Norm();
			ball_accel=ball_force/mass;
			ball_vel=ball_vel+ball_accel*dt;
				
		}
		

		

		orientation=(ez-ball_vel)/sqrt(2*ball_vel.norm()-ball_vel.transpose()*(0,0,1));

		x1_des=x0_des+orientation.normalized();

        // Compute control forces (note that these directly have access to the io data ds).
        rctr.computeDynamics();
        rctr.computeControlForces();

        // Integrate the dynamics
        dyn_tao.integrate(rio,dt); iter++;
		ball_pos=ball_pos+ball_vel*dt;

        // Compute kinematic quantities
        dyn_scl.computeTransformsForAllLinks(rgcm.rbdyn_tree_,rio.sensors_.q_);

        dyn_scl.computeJacobianWithTransforms(Jx0,*rhand,rio.sensors_.q_,hpos0);
        dyn_scl.computeJacobianWithTransforms(Jx1,*rhand,rio.sensors_.q_,hpos1);

        
		if(false == flag) { x_init = rhand->T_o_lnk_ * hpos0; flag = true; }

        x0 = rhand->T_o_lnk_ * hpos0; 
        x1 = rhand->T_o_lnk_ * hpos1; 

        dx0 = Jx0.block(0,0,3,rio.dof_) * rio.sensors_.dq_;
		dx1 = Jx1.block(0,0,3,rio.dof_) * rio.sensors_.dq_;

        rio.actuators_.force_gc_commanded_ = Jx0.block(0,0,3,rio.dof_).transpose() * (1000*(x0_des-x0) - 10 * dx0) - 200*rio.sensors_.dq_;
        rio.actuators_.force_gc_commanded_ += Jx1.block(0,0,3,rio.dof_).transpose() * (1000*(x1_des-x1) - 10 * dx1) - 200*rio.sensors_.dq_;

        // Integrate the dynamics
        dyn_tao.integrate(rio,dt); iter++; const timespec ts = {0, 5000};/*.05ms*/
        nanosleep(&ts,NULL);
        sutil::CSystemClock::tick(dt);
        if(iter % 1000 == 0)
        { std::cout<<"\n"<<(x0_des-x0).transpose()<<" "<<(x0_des-x0).norm();
          //std::cout<<"\n"<<rio.actuators_.force_gc_commanded_.transpose();
        }
      }
      //Then terminate
      scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running = false;
    }
    else  //Read the rio data structure and updated rendererd robot..
      while(true == scl_chai_glut_interface::CChaiGlobals::getData()->chai_glut_running)
      { glutMainLoopEvent(); const timespec ts = {0, 15000000};/*15ms*/
        nanosleep(&ts,NULL); }
  }


  /******************************Exit Gracefully************************************/
  std::cout<<"\n\nExecuted Successfully";
  std::cout<<"\n**********************************\n"<<std::flush;

  return 0;
}
コード例 #2
0
ファイル: ai_logplayer.cpp プロジェクト: kn2cssl/kn2cPrograms
void AI_logPlayer::recordLog(WorldModel *wm, QList<RobotCommand> RCs)
{
    log_chunk data_chunk;

    data_chunk.set_chunk_number(this->counter++);

    Ball_message* ball(data_chunk.mutable_ball());
    ball->set_isvalid(wm->ball.isValid);
    position_message* ball_pos(ball->mutable_position());
    ball_pos->set_x(wm->ball.pos.loc.x);
    ball_pos->set_y(wm->ball.pos.loc.y);
    ball_pos->set_dir(wm->ball.pos.dir);
    position_message* ball_vel(ball->mutable_velocity());
    ball_vel->set_x(wm->ball.vel.loc.x);
    ball_vel->set_y(wm->ball.vel.loc.y);
    ball_vel->set_dir(wm->ball.vel.dir);

    for(int i = 0; i < PLAYERS_MAX_NUM; i++ )
    {
        Robot_message *our = data_chunk.add_ours();
        our->set_isvalid(wm->ourRobot[i].isValid);

        Robot_message_AgentRole role = returnProtoRole(wm->ourRobot[i].Role);
        our->set_role(role);

        Robot_message_AgentStatus status = returnProtoStatus(wm->ourRobot[i].Status);
        our->set_status(status);

        Robot_message_AgentRegion region = returnProtoRegion(wm->ourRobot[i].Region);
        our->set_region(region);

        position_message* robo_pos(our->mutable_position());
        robo_pos->set_x(wm->ourRobot[i].pos.loc.x);
        robo_pos->set_y(wm->ourRobot[i].pos.loc.y);
        robo_pos->set_dir(wm->ourRobot[i].pos.dir);

        position_message* robo_vel(our->mutable_velocity());
        robo_vel->set_x(wm->ourRobot[i].vel.loc.x);
        robo_vel->set_y(wm->ourRobot[i].vel.loc.y);
        robo_vel->set_dir(wm->ourRobot[i].vel.dir);

        //        RobotCommand_message rc = our->rc();
        //        position_message fin_pos = rc.fin_pos();
        //        fin_pos.set_x(wm->ourRobot[i].);
    }

    for(int i = 0; i < PLAYERS_MAX_NUM; i++ )
    {
        Robot_message *opp = data_chunk.add_opps();
        opp->set_isvalid(wm->oppRobot[i].isValid);

        position_message* robo_pos(opp->mutable_position());
        robo_pos->set_x(wm->oppRobot[i].pos.loc.x);
        robo_pos->set_y(wm->oppRobot[i].pos.loc.y);
        robo_pos->set_dir(wm->oppRobot[i].pos.dir);

        position_message* robo_vel(opp->mutable_velocity());
        robo_vel->set_x(wm->oppRobot[i].vel.loc.x);
        robo_vel->set_y(wm->oppRobot[i].vel.loc.y);
        robo_vel->set_dir(wm->oppRobot[i].vel.dir);
    }

    chunks.append(data_chunk);
}
コード例 #3
0
ファイル: goalkeeper.cpp プロジェクト: unball/ieee-very-small
void Goalkeeper::updateBallPos()
{
	Vector ball_pos(Ball::getInstance().getX(),Ball::getInstance().getY());
	ball_pos_ = ball_pos;
}