void loop(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
static double start_time = t;
const std::vector<std::string>
eef_names_ = ctrl->get_data<std::vector<std::string> >("init.end-effector.id");
int NUM_FEET = eef_names_.size();
boost::shared_ptr<Ravelin::Pose3d> base_frame( new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_link_frame")));
Ravelin::VectorNd qd = ctrl->get_joint_generalized_value(Pacer::Controller::velocity);
Ravelin::VectorNd qdd = ctrl->get_joint_generalized_value(Pacer::Controller::acceleration);
int NUM_JOINT_DOFS = qd.rows();
// Initialize end effectors
Ravelin::VectorNd local_q = ctrl->get_generalized_value(Pacer::Controller::position);
#ifdef USE_OSG_DISPLAY
// 1 w/ base position
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Pose3d foot_pose(Ravelin::Matrix3d(link->get_pose()->q)*Ravelin::Matrix3d(0,0,-1, -1,0,0, 0,1,0),link->get_pose()->x,link->get_pose()->rpose);
foot_pose.update_relative_pose(Pacer::GLOBAL);
Utility::visualize.push_back( Pacer::VisualizablePtr( new Pacer::Pose(foot_pose,0.8)));
OUT_LOG(logERROR) << eef_names_[i] << "-orientation: " << t << " " << foot_pose.q;
}
#endif
// q w/o base position
local_q.set_sub_vec(NUM_JOINT_DOFS,Utility::pose_to_vec(Ravelin::Pose3d()));
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Origin3d xd,xdd;
//angular
Ravelin::Origin3d rpy,axd,axdd;
// Calc jacobian for AB at this EEF
Ravelin::MatrixNd J = ctrl->calc_link_jacobian(local_q,eef_names_[i]);
// Now that model state is set ffrom jacobian calculation
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
Ravelin::Pose3d foot_pose(Ravelin::Matrix3d(link->get_pose()->q)*Ravelin::Matrix3d(0,0,-1, -1,0,0, 0,1,0),link->get_pose()->x,link->get_pose()->rpose);
foot_pose.update_relative_pose(Pacer::GLOBAL);
// Ravelin::Origin3d x(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
bool new_var = ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.x",foot_pose.x);
ctrl->set_data<Ravelin::Quatd>(eef_names_[i]+".state.q",foot_pose.q);
J.block(0,3,0,NUM_JOINT_DOFS).mult(qd,xd);
J.block(0,3,0,NUM_JOINT_DOFS).mult(qdd,xdd);
J.block(3,6,0,NUM_JOINT_DOFS).mult(qd,axd);
J.block(3,6,0,NUM_JOINT_DOFS).mult(qdd,axdd);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.xd",xd);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".state.xdd",xdd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::position,foot_pose.x);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::velocity,xd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::acceleration,xdd);
ctrl->set_foot_value(eef_names_[i],Pacer::Controller::load,Ravelin::Origin3d(0,0,0));
if(new_var){
ctrl->set_data<Ravelin::Quatd>(eef_names_[i]+".init.q",foot_pose.q);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".init.x",foot_pose.x);
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".init.xd",xd);
}
ctrl->set_data<bool>(eef_names_[i]+".stance",false);
}
if(start_time == t){
local_q.segment(0,qd.rows()) = Ravelin::VectorNd::zero(qd.rows());
ctrl->set_model_state(local_q);
std::vector<Ravelin::Origin3d> x1(NUM_FEET) ,x2(NUM_FEET);
for(unsigned i=0;i<NUM_FEET;i++){
Ravelin::Origin3d x,base_x;
ctrl->get_data<Ravelin::Origin3d>(eef_names_[i]+".init.x",x);
base_x = x;
base_x[2] = 0;
ctrl->set_data<Ravelin::Origin3d>(eef_names_[i]+".base",base_x);
x2[i] = base_x;
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
x1[i] = Ravelin::Origin3d(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
}
std::fill(local_q.segment(0,qd.rows()).begin(),local_q.segment(0,qd.rows()).end(),M_PI);
ctrl->set_model_state(local_q);
for(unsigned i=0;i<NUM_FEET;i++){
const boost::shared_ptr<Ravelin::RigidBodyd> link = ctrl->get_link(eef_names_[i]);
x2[i] = Ravelin::Origin3d(Ravelin::Pose3d::transform_point(Pacer::GLOBAL,Ravelin::Vector3d(0,0,0,link->get_pose())).data());
// write in maximum reach from limb base
double reach = (x1[i]-x2[i]).norm();
ctrl->set_data<double>(eef_names_[i]+".reach",reach*0.95);
}
}
}
void loop(){
boost::shared_ptr<Pacer::Controller> ctrl(ctrl_weak_ptr);
const unsigned X=0,Y=1,Z=2,THETA=5;
// World frame
boost::shared_ptr<Ravelin::Pose3d>
environment_frame(new Ravelin::Pose3d());
Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Pose(*environment_frame.get())));
boost::shared_ptr<Ravelin::Pose3d>
base_horizontal_frame(new Ravelin::Pose3d(ctrl->get_data<Ravelin::Pose3d>("base_horizontal_frame")));
Ravelin::Vector3d com(base_horizontal_frame->x.data());
OUT_LOG(logERROR) << "x = " << base_horizontal_frame->x ;
Ravelin::VectorNd base_xd;
ctrl->get_base_value(Pacer::Robot::velocity,base_xd);
OUT_LOG(logERROR) << "xd = " << base_xd ;
/////////////////////
/// SET VARIABLES ///
// Command robot to walk in a direction
Ravelin::VectorNd command;
command.set_zero(6);
double max_forward_speed = ctrl->get_data<double>(plugin_namespace+".max-forward-speed");
double max_strafe_speed = ctrl->get_data<double>(plugin_namespace+".max-strafe-speed");
double max_turn_speed = ctrl->get_data<double>(plugin_namespace+".max-turn-speed");
// if FALSE, drive like a car (x and theta)
// Q: if TRUE, turn toward waypoint while stepping in direction of waypoint
bool HOLONOMIC = false;
/////////////////////////////////////
/// Assign WAYPOINTS in the plane ///
static std::vector<Point> waypoints;
std::vector<double> waypoints_vec = ctrl->get_data<std::vector<double> >(plugin_namespace+".waypoints");
// if (waypoints not inititalized) OR (waypoints changed) OR (only one waypoint)
// update waypoints
if(waypoints.empty() || waypoints_vec.size()/2 != waypoints.size() || waypoints_vec.size() == 2){
waypoints.clear();
for(int i=0;i<waypoints_vec.size();i+=2)
waypoints.push_back(Point(waypoints_vec[i],waypoints_vec[i+1]));
}
/////////////////////////////
/// CHOOSE NEXT WAYPOINT ///
int num_waypoints = waypoints.size();
// if waypoints are empty
// do not do anything (do not update SE2 command)
if(num_waypoints != 0){
Ravelin::Vector3d goto_point;
static int waypoint_index = 0;
static Ravelin::Vector3d
next_waypoint(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
if(waypoints.size() == 1){
waypoint_index = 0;
next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
}
double distance_to_wp = (Ravelin::Origin3d(next_waypoint.data()) - Ravelin::Origin3d(com[X],com[Y],0)).norm();
double max_dist = ctrl->get_data<double>(plugin_namespace+".tolerance");
if( distance_to_wp < max_dist){
OUT_LOG(logDEBUG1) << "waypoint reached, incrementing waypoint.";
OUTLOG(next_waypoint,"this_wp",logDEBUG1);
OUT_LOG(logDEBUG1) << "this waypoint: " << next_waypoint;
OUT_LOG(logDEBUG1) << "robot position: " << com;
waypoint_index = (waypoint_index+1)% num_waypoints;
if(waypoint_index == 0)
exit(0);
next_waypoint = Ravelin::Vector3d(waypoints[waypoint_index].first,waypoints[waypoint_index].second,0,environment_frame);
}
OUT_LOG(logDEBUG1) << "num_wps = " << num_waypoints;
OUT_LOG(logDEBUG1) << "distance_to_wp = " << distance_to_wp;
OUT_LOG(logDEBUG1) << "waypoint_index = " << waypoint_index;
Utility::visualize.push_back(Pacer::VisualizablePtr(new Pacer::Ray(next_waypoint,com,Ravelin::Vector3d(1,0.5,0))));
OUT_LOG(logDEBUG1) << "next_wp" << next_waypoint;
for(int i=0;i<num_waypoints;i++){
Ravelin::Vector3d wp(waypoints[i].first,waypoints[i].second,0,environment_frame);
OUT_LOG(logDEBUG1) << "\twp" << wp;
Utility::visualize.push_back(Pacer::VisualizablePtr( new Pacer::Point(wp,Ravelin::Vector3d(1,0.5,0),0.1)));
}
goto_point = next_waypoint;
///////////////////////
/// GO TO WAYPOINT ///
// Find direction to waypoint from robot position
Ravelin::Vector3d goto_direction =
Ravelin::Vector3d(goto_point[X],goto_point[Y],0,environment_frame)
- Ravelin::Vector3d(com[X],com[Y],0,environment_frame);
goto_direction = Ravelin::Pose3d::transform_vector(base_horizontal_frame,goto_direction);
goto_direction.normalize();
double angle_to_goal = atan2(goto_direction[Y],goto_direction[X]);
OUT_LOG(logDEBUG) << "angle_to_goal = " << angle_to_goal;
// If robot is facing toward goal already, walk in that direction
if(fabs(angle_to_goal) < M_PI_4){
if(HOLONOMIC){
command[Y] = goto_direction[Y]*max_strafe_speed;
}
command[X] = goto_direction[X]*max_forward_speed;
command[THETA] = angle_to_goal;
} else {
command[THETA] = Utility::sign(angle_to_goal)*max_turn_speed;
if(HOLONOMIC){
command[X] = goto_direction[X]*max_forward_speed;
command[Y] = goto_direction[Y]*max_strafe_speed;
} else {
command[X] = 0;
command[Y] = 0;
}
}
// double slow_down_tol = 0.1;
// if(distance_to_wp < slow_down_tol)
// command *= distance_to_wp*slow_down_tol+0.05;
Ravelin::Origin3d command_SE2(command[X],command[Y],command[THETA]);
ctrl->set_data<Ravelin::Origin3d>("SE2_command",command_SE2);
}
}
void ProcessLTX(LPCSTR tgt_name, LPCSTR params, BOOL bFast)
{
xr_string ltx_name;
LPCSTR ltx_nm = strstr(params,".ltx"); VERIFY(ltx_nm!=0);
string_path ltx_fn;
string_path tmp;
strncpy (tmp,params,ltx_nm-params); tmp[ltx_nm-params]=0;
_Trim (tmp);
strcat (tmp,".ltx");
strcpy (ltx_fn,tmp);
// append ltx path (if exist)
string_path fn,dr,di;
_splitpath (ltx_fn,dr,di,0,0);
strconcat (sizeof(fn),fn,dr,di);
if (0!=fn[0])
FS.append_path ("ltx_path",fn,0,false);
if (!FS.exist(ltx_fn)&&!FS.exist(ltx_fn,"$app_root$",tmp))
Debug.fatal (DEBUG_INFO,"ERROR: Can't find ltx file: '%s'",ltx_fn);
CInifile ltx (ltx_fn);
printf ("Processing LTX...\n");
BOOL make_pack = ltx.line_exist("options","make_pack") ? ltx.r_bool("options","make_pack") : true;
LPCSTR copy_path= ltx.line_exist("options","copy_path") ? ltx.r_string("options","copy_path") : 0;
if (ltx.line_exist("options","exclude_exts"))
_SequenceToList(exclude_exts,ltx.r_string("options","exclude_exts"));
xr_vector<char*> list;
xr_vector<char*> fl_list;
if(ltx.section_exist("include_folders"))
{
CInifile::Sect& if_sect = ltx.r_section("include_folders");
for (CInifile::SectCIt if_it=if_sect.Data.begin(); if_it!=if_sect.Data.end(); if_it++)
{
BOOL ifRecurse = CInifile::IsBOOL(if_it->second.c_str());
u32 folder_mask = FS_ListFolders | (ifRecurse?0:FS_RootOnly);
string_path path;
LPCSTR _path = 0==xr_strcmp(if_it->first.c_str(),".\\")?"":if_it->first.c_str();
strcpy (path,_path);
u32 path_len = xr_strlen(path);
if ((0!=path_len)&&(path[path_len-1]!='\\')) strcat(path,"\\");
Log ("");
OUT_LOG ("Processing folder: '%s'",path);
BOOL efRecurse;
BOOL val = IsFolderAccepted(ltx,path,efRecurse);
if (val || (!val&&!efRecurse))
{
if (val) ProcessFolder (list,path);
xr_vector<char*>* i_fl_list = FS.file_list_open ("$target_folder$",path,folder_mask);
if (!i_fl_list)
{
Log ("ERROR: Unable to open folder list:", path);
continue;
}
xr_vector<char*>::iterator it = i_fl_list->begin();
xr_vector<char*>::iterator itE = i_fl_list->end();
for (;it!=itE;++it){
xr_string tmp_path = xr_string(path)+xr_string(*it);
bool val = IsFolderAccepted(ltx,tmp_path.c_str(),efRecurse);
if (val)
{
fl_list.push_back(xr_strdup(tmp_path.c_str()));
Msg ("+F: %s",tmp_path.c_str());
// collect files
if (ifRecurse)
ProcessFolder (list,tmp_path.c_str());
}else
{
Msg ("-F: %s",tmp_path.c_str());
}
}
FS.file_list_close (i_fl_list);
}else
{
Msg ("-F: %s",path);
}
}
}//if(ltx.section_exist("include_folders"))
// compress
{
if(ltx.section_exist("include_files"))
{
CInifile::Sect& if_sect = ltx.r_section("include_files");
for (CInifile::SectCIt if_it=if_sect.Data.begin(); if_it!=if_sect.Data.end(); if_it++)
{
list.push_back (xr_strdup(if_it->first.c_str()));
}
}
}
CompressList (tgt_name,&list,&fl_list,bFast,make_pack,copy_path);
// free
xr_vector<char*>::iterator it = list.begin();
xr_vector<char*>::iterator itE = list.end();
for (;it!=itE;++it) xr_free(*it);
it = fl_list.begin();
itE = fl_list.end();
for (;it!=itE;++it) xr_free(*it);
exclude_exts.clear_and_free();
}
