Note that this is an involved tutorial. Take your time and carefully read the code. This tutorial aims to introduce you to multi-task control for humanoids in SCL. You will learn that controlling a complex humanoid is not all that different from controlling a simpler kinematic chain robot. The tutorial will show you how to use the generic robot models in the "specs/" folder. It will also show you how to use the glut keyboard handlers to specify robot trajectories. Finally, the tutorial will introduce you to the singleton data structures that simplify data access (note that these are optional, and might be complex for people who do not have a lot of experience with programming).
Follow the README.txt file in the directory. Note that the xml file is not in this folder anymore. Instead we will pick a generic humanoid model called Stanbot from the specs folder.
SCL's focus has been to make its different modules independent by design. As such, SCL requires some form of data sharing between the different modules. One might manually do this by passing around data structures (as was done in earlier tutorials). However, doing so becomes cumbersome in the long run. Instead, SCL provides a global "singleton" data structure called the databse, which stores such data. Modules may access the data by getting a pointer to the memory location in the database. The following code sets up the database:
/******************************Set up Shared Memory (Database)************************************/
// This will help scl find the graphics files. Usually they are specified wrt. some
// specs dir. An alternate way is to set this dir as an environment variable (but we won't
// do that for now). Instead we'll set the dir in a global shared memory "database".
if(NULL == scl::CDatabase::getData())
{ std::cout<<"\n ERROR : Could not initialize global data storage"; return 1; }
// This helps locate the specs folder. Note that the application assumes the specs folder is two dirs up.
scl_util::getCurrentDir(scl::CDatabase::getData()->cwd_);
scl::CDatabase::getData()->dir_specs_ = scl::CDatabase::getData()->cwd_ + std::string("../../specs/");
Note that the database is a fast, but fairly involved solution. Using pointers is somewhat error-prone for less experienced programmers. Moreover, it is possible to create race conditions while multi-threading different SCL modules. As such, there are efforts underway to create a transactional database, which different modules keep a copy of the data and periodically sync with the database. All operations are atomic and so there are no multi-threading issues. However, there is a price to be paid in loss of performance. Check this spot in the near future for more updates (or get in touch with Samir if you'd like to contribute).
If you are not comfortable with the programming level required in this tutorial, feel free to use the earlier tutorial(s) as a template. Note that there is no loss of generality in doing so. Merely that your code might become less scalable and more tedious. But you will NOT lose any functionality.
In the same way as the earlier tutorial(s). The only difference being that we will extract two task controllers this time, one for each hand. This will allow us to specify trajectories for both hands.
Next the code starts the simulation with sine wave motions at both hands. You can also switch to moving the hands with the keyboard. The relevant code is here:
std::cout<<"\n\n***************************************************************"
<<"\n Starting op space (task coordinate) controller..."
<<"\n This will move the humanoid's hands in circles."
<<"\n NOTE: This controller works with the system clock. So it will"
<<"\n behave differently on different computers."
<<"\n\n Press '1' to flip control of the left hand \nto either the {sw,da,eq keys} or {a default sine wave}"
<<"\n***************************************************************";
// ....
// Move the right hand in a sine wave
rtask_hand->x_goal_(0) = 0.15*sin(tcurr-tstart)-0.15;
rtask_hand->x_goal_(1) = 0.25*cos(tcurr-tstart);
rtask_hand->x_goal_(2) = -0.25;
// Move the left hand in a different sine wave
if(!scl::CDatabase::getData()->s_gui_.ui_flag_[1]){
rtask_lhand->x_goal_(0) = 0.15*sin((tcurr-tstart)*4.0)-0.05;
rtask_lhand->x_goal_(1) = 0.15*cos((tcurr-tstart)*4.0)+0.45;
rtask_lhand->x_goal_(2) = -0.1;
}
else
{ rtask_lhand->x_goal_ = scl::CDatabase::getData()->s_gui_.ui_point_[0]; }
Note the use of the s_gui_.ui_flag_[1]. This is a pre-defined bool variable in the database that flips value when users press the "1" key. There is also the s_gui_.ui_point_[0], which is a vector of length 3 that can be controlled by ws da eq (inc/dec; x,y,z). There are other such variables. Look inside "src/scl/graphics/chai/ChaiGlutHandlers.hpp" in the keyboard handler function for more details.
Play around with Stanbot. Change the control gains in the config file. Change the inertia of some of the links in the robot spec. See if you can make the controller go crazy... Have fun! ;-)
Operational Space Control Math Tutorial: 3-dof and 6-dof chain robots.