This tutorial aims to introduce you to SCL's control architecture. The tutorial has two parts. First, joint space control. And second, end-effector control. The two happen in sequence so make sure you watch the graphics screen carefully to see the moment when one switches to the other.
Follow the README.txt file. If you look inside the xml file, you will find that this robot (an R6) is much more complex than the earlier ones.
The code starts off by controlling the robot in joint space. The associated generalized coordinate controller is given by:
// 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_ =
100 * (sin(tcurr-tstart) - rio.sensors_.q_.array()) - 20 * rio.sensors_.dq_.array();
The code then pauses the simulation and switches to an operational space controller. This controller is more sophisticated and projects the control potential field into task coordinates. It thus controls a subspace of the actual robot's dynamics. This type of control is commonly called {cartesian, task, operational} space control. The extra step compared to the previous step is to compute a hand Jacobian. The associated control equation is given by:
// Compute kinematic quantities
dyn_scl.computeTransformsForAllLinks(rgcm.rbdyn_tree_,rio.sensors_.q_);
dyn_scl.computeJacobianWithTransforms(Jx,*rhand,rio.sensors_.q_,hpos);
if(false == flag) { x_init = rhand->T_o_lnk_ * hpos; flag = true; }
x = rhand->T_o_lnk_ * hpos; //We'll make the hand draw a sine trajectory
dx = Jx.block(0,0,3,rio.dof_) * rio.sensors_.dq_;
// Controller : fgc = Jx' (kp * (sin(t)*.1 - (x-xinit)) + kv(0 - dx)) - kqv * dq
// Set the desired positions so that the hand draws a circle
double sin_ampl = 0.15;
x_des(0) = x_init(0)+sin(tcurr-tstart)*sin_ampl;
x_des(1) = x_init(1)+cos(tcurr-tstart)*sin_ampl; x_des(2) = 0;
rio.actuators_.force_gc_commanded_ =
Jx.block(0,0,3,rio.dof_).transpose() * (100*(x_des-x) - 20 * dx) - 20*rio.sensors_.dq_;
Of course, in case you forgot, the code snippets are drawn from the cpp file. Please do take a look at the full code to get a better understanding of what is going on. To learn the basic math behind such control strategies (and more advanced strategies) please read through the control tutorial provided in the references.
Operational Space Control Math Tutorial: 3-dof and 6-dof chain robots.
The provided controllers have no limits on the torque they can apply. This is clearly unrealistic. However, the joint forces they apply might underutilize (or exceed) the torque limits of real motors that actuate a similar robot. Your goal for this assignment is to understand how to select appropriate motors.
Go to the Maxon motors website and select a RE 40 DC brushed motor (e.g., id 148877) along with a suitable gear (e.g., id 223089). Look at the spec sheet for the motors. You will get their torque limits (remember to multiply the steady state motor torque output by the gear ratio).
Do the joint torques applied by the joint space controller exceed their limits? If so, reduce the control gains till they don't. Else, increase the control gains till joint torques approach their limits. Specify your final gain values. Also plot tracking error at each joint as a function of time for atleast 20 seconds of motion. Can you minimize it further?
NOTE : The provided controller already has some proportional and derivative gains set. Please review the control tutorial if you don't remember why gains are set the way they are.
Similar to the joint space example provided above, find the task space control gains that make the robot operate near its joint torque limits. Plot tracking error along each task axis as a function of time for atleast 20 seconds of motion. Can you minimize it further?