This tutorial aims to introduce you to SCL's more advanced control features including multi-task control. It also shows you how to specify a controller in the xml file (instead of manually adding your own control equation in the main runtime loop).
Follow the README.txt file. Notice that the xml file now has a controller specification that includes two tasks.
< controller name="opc">
< type>task< /type>
< must_use_robot>r6bot< /must_use_robot>
< task name="hand">
< type>TaskOpPos< /type>
< parent_link>hand< /parent_link>
< pos_in_parent>0 0 -0.4< /pos_in_parent>
< priority>0< /priority>
< task_dof>3< /task_dof>
< kp>200< /kp>
< kv>25< /kv>
< ka>0< /ka>
< force_max>100< /force_max>
< force_min>-100< /force_min>
< /task>
< !--0 task_dof means a gc task, ie full dofs -->
< task name="NullSpaceDampingTask">
< type>TaskNullSpaceDamping< /type>
< priority>1< /priority>
< task_dof>0< /task_dof>
< kp>0< /kp>
< kv>40< /kv>
< ka>0< /ka>
< force_max>300< /force_max>
< force_min>-300< /force_min>
< /task>
< /controller>
Since we are now using scl's parser, we have to obtain pointers to the control tasks in order to set trajectories (goal position etc.).
/******************************Set up Controller Specification************************************/
// Read xml file info into task specifications.
flag = p.readTaskControllerFromFile("./R6Cfg.xml","opc",must_use_robot,rtasks,rtasks_nc);
if(must_use_robot != "r6bot") {flag = false;} //Error check for file consistency
flag = flag && rctr_ds.init("opc",&rds,&rio,&rgcm); //Set up the control data structure..
// Tasks are initialized after find their type with dynamic typing.
flag = flag && scl_registry::registerNativeDynamicTypes();
flag = flag && scl_util::initMultiTaskCtrlDsFromParsedTasks(rtasks,rtasks_nc,rctr_ds);
flag = flag && rctr.init(&rctr_ds,&dyn_scl); //Set up the controller (needs parsed data and a dyn object)
if(false == flag){ return 1; } //Error check.
rtask_hand = dynamic_cast< scl::STaskOpPos*>( *(rctr_ds.tasks_.at("hand")) );
if(NULL == rtask_hand) {return 1;} //Error check
Since we stored a pointer to the task's control object, we can set trajectories on the fly.
// Move the hand in a sine wave rtask_hand->x_goal_(0) = 0.15*sin(tcurr-tstart)+0.6; rtask_hand->x_goal_(1) = 0.15*cos(tcurr-tstart);The control gains etc. have already been set in the xml file. As such, we don't have to explicitly specify them. Note, you should take a look at the CTaskOpPos class to see how this control task is implemented. In the future, you will probably copy/paste and customize it to your own control applications.
By now, you have been exposed to most of SCL's basic features. You can set up a robot as a kinematic chain, and can create a simulation with physics, graphics, and control. Note that SCL's parser and data structures substantially simplify the code you have to write.
To review what you learned, look at the following video and try to replicate it by playing around with the robot gains. This should give you an idea of how control performance changes with the usual control parameters.
If you familiarize yourself with control gains and multi-task control, you will be able to hack up controllers for complex humanoid robots in a matter of a few hours (minutes? ;-) ). This is usually enough for starters. However, feel free to peruse the more advanced tutorials.
Operational Space Control Math Tutorial: 3-dof and 6-dof chain robots.
You will now understand how a robot's inertial model influences the controller. The task is to execute an end-effector circle trajectory at 0.5Hz, and 3Hz. Note that the DARPA Cheetah Robot's feet move at about 3-4Hz at top speed, so this is a really fast motion.
First, instead of loading the provided R6 xml configuration file, set up your code to load the specs/Puma/PumaCfg.xml file's Puma robot.
Use this to initialize the dynamics object for physics integration.
(You may find this easier to do if you look at code in the scl_advanced_creating_new_tasks tutorial
or in scl_tutorial6_control_humanoid, both of which use standard robot specifications from the specs dir. Install the program "meld"
by typing "sudo apt-get install meld" on the terminal. Then use meld to compare your cpp file with the file for tute6 or adv_tute
and copy over the desired code..)
Next, select a suitable motor and gear for each joint and compute appropriate joint torque limits. You must apply these for your simulations by limiting f_gc_ in your code.
Next, create a copy of the Puma cfg and xml files and corrupt the inertias for all links. Corrupt them by a fixed percentage change (say 30%), randomly increasing or decreasing them for different links.
Use the parsed robot specification with corrupted inertial parameters to initialize the controller.
Note that you may corrupt the inertias in code as well if you find that easier.
Run the control simulation and plot the sine wave tracking error as a function of time for ten full circles. i.e., 20s long simulation for 0.5Hz, and ~3.5s long simulation for 3Hz. You may use 10 In case your simulation goes unstable, consider reducing your control gains. Discuss your results.
As a thought exercise, what inertial errors would you consider to be acceptable. Use your results above to motivate your answer.
Extra credit : Corrupt inertias one link at a time. Is your controller more sensitive to inertial errors for some links compared to others? Why?
Propose a controller test pipeline that will minimize the effect of modeling errors on proposed control specifications. We will expect you to adopt this for your project.
Will your pipeline account for Physics discrepances, real-world friction, inertial estimates, motor torque generation errors, and/or control singularities?