/** 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;
}
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);
}
void Goalkeeper::updateBallPos()
{
Vector ball_pos(Ball::getInstance().getX(),Ball::getInstance().getY());
ball_pos_ = ball_pos;
}
