Libbarrett Programming Manual
Libbarrett Programming Manual
Table of Contents
Section 1 The Physical Stuff
1.1 - Understanding your WAM Arm 1.2 Understanding your WAMs Safety System 1.3 Joint Torques, Positions, and Velocities, and Cartesian Values
Joint Six Joint Two (Rotates inner link about its end) Joint One (Rotates entire arm about the base) (Rotates wrist about its end) Joint Seven (Rotates tool-plate (or hand) about its center)
The WAMs motors are driven by Pucks, Barretts patented motor controllers. Pucks are exceptionally small and they are placed throughout the WAM Arm, rather than stored in a motor controller box outside of the robot. Pucks communicate with the control PC through the CAN Bus (Controller Area Network), and all communication is monitored by the Safety Board. The Safety Board is a large PCB in the base of the WAM that monitors the velocity of the arm, the magnitude of the torque commands being sent to the motors, the communication rate between the control PC and the Pucks, and the status of the buttons on the Pendants (see the next page). The CAN bus is a 2-wire differential serial bus that provides digital communication at 1 Mbps with high noise immunity. The protocol used is proprietary for the WAM (not CANopen). For more information on CAN, visit http://en.wikipedia.org/wiki/CAN_bus. Pucks are the lowest level of control that the programmer needs to understand. They send motor positions to the WAM PC and receive motor torques back from the control PC in a single control loop. The control loop may run at any rate up to 1 kHz, but the default rate is 500 Hz. When you send a command, it is translated into CAN signals and addressed to the appropriate Pucks. The image below shows a Puck and its leads.
CAN Bus
The WAM must be activated manually before any commands can be executed. If there is a problem, it will idle or fault automatically. In addition, it can be manually idled or faulted by hitting the emergency stop button. The emergency stop will stop the robot instantly, and must be reset by turning the button a quarter turn clockwise before reactivating the WAM. When power is not being applied to the Pucks, the WAM resistively brakes, meaning that it does not actively exert any force, but instead lightly resists any forces applied to it. This results in the WAM slowly falling until its hits a physical impediment. It also allows the WAM to be easily moved into a resting position by the operator. There are two Pendants that attach to the WAM. The Control and Display pendants both show the state of the WAM and have an emergency stop button, but only the Control pendant can idle or activate the WAM. The diagram below shows the control pendant and describes its functions.
Pushing the idle or activate button at the same time as the shift button changes the WAM to that state, as long as the safety system determines it is safe to do so. The WAM must be idled before it can be activated. These lights show the status of different factors that impact the state of the WAM. If they are all green, the WAM is either active or able to be activated. If one or more are red, the WAM is faulted and will stop. If there is a warning while the WAM is active, it will not stop; however, it should Libbarrett Quick Start Guide Modified be noted that the program may be July 29, 2012 Page 6 approaching the limits of safe operation if that is the case.
X Y
The user inputs a position into his or her computer. The computer translates this position into a packet and sends it to the WAM PC.
The WAM PC receives the packet in a blocking function that is not part of the realtime code, calculates the WAMs path, and sends ONLY the spline to the realtime functions. These functions send commands to the WAM to move it to the desired position.
P
Setpoin t
Error e(t)
I D
Feedback
Proce ss Outpu t
First Order Filter This is a filter that attenuates the high-frequency components of a signal going through it, such that it is smoothed out, which prevents sudden changes. The following graphs demonstrate the difference between a filtered signal and unfiltered signal.
Signa l
Si g n al
Trapezoidal Velocity Profile This is a function of velocity over time, where there is a period of constant acceleration, a period of constant velocity, then a period of constant deceleration, forming a trapezoid. This is commonly used by the WAM to smoothly move its joints to a given position. An example of such a relationship, in graph form, is below.
Max Velocity
Velocit y
Time Libbarrett Quick Start Guide Modified July 29, 2012 Page 11
Wam<DOF> - This is a class that contains all the Systems and other objects necessary to operate the real-life WAM with the given DOF. This object is not a System, but it is comprised mainly of various systems. The following are classes contained in every WAM object: LowLevelWam This is the core of a WAM Object, it is representative of the actual hardware WAM, and handles the communications between the software and the arm. It is a System which takes joint torques as an input, converts them to motor torques which are sent out to the Pucks, and receives motor torques back from the Pucks, which it converts to joint positions and joint velocities and places in its output. jtSummer An object that adds all of its inputs together and feeds the result to its output. In this case, in the virtual WAM object, it is adding together joint torques. jpOutput and jvOutput These are virtual spigots for joint positions and velocities, respectively, to which you can attach the input/output hoses that connect WAM systems. toolOrientation and toolPosition These are Systems which compute the position and orientation of the end point of the WAM. supervisoryController This object accepts any input which the PID Controllers in the WAM object can accept. It is not a System, however. It decides which controller should deal with the input, and passes it to that controller. It then handles the output of that controller and passes it to the jtSummer. It is told which signal to follow with the wam.trackReferenceSignal() call, and can be reset with the wam.idle() call. gravityCompensator This object, when connected to the jtSummer, calculates the force of gravity on the WAM and compensates for it by sending the appropriate joint torques to the arm.
To write a program for the WAM, create a virtual WAM object. A WAM Object is a collection of Systems that makes it easier to write the rest of the code. LowLevelWam is the object which deals directly with the real WAM arm at the core of the system. While it is possible to write code that deals directly with the LowLevelWam, it is generally easier to work with the tools built around it, because they will yield values that are easier to understand than joint torques. The tools built around the LowLevelWam are mostly Systems, which are created and connected together when you create a new WAM object in your code. The diagram below shows this framework, and the flow of information between the Systems, in a brand new WAM object. Notice the clear control loop, where a series of controllers are already receiving feedback from the WAM, and that they are all contained within the supervisoryController. The supervisoryController attaches the controller systems to the Summer based on the reference signal it receives (none are connected by default), and the Summer adds all the control torques together before passing them to the LowLevelWam. This loop is what all WAM programs interact with or modify.
namespace math = barrett::math; namespace systems = barrett::systems; namespace units = barrett::units; using using using using barrett::Wam; systems::connect; systems::reconnect; systems::disconnect;
const int DOF = 4; const double T_s = 0.002; typedef typedef typedef typedef units::JointTorques<DOF>::type jt_type; units::JointPositions<DOF>::type jp_type; units::JointVelocities<DOF>::type jv_type; units::CartesianPosition::type cp_type;
inline void waitForEnter() { static std::string line; std::getline(std::cin, line); } #endif /* STDHDR_H_ */
This code includes all the necessary libbarrett components and then renames several namespaces for convenience. Following that, it creates two constants that govern the coding environment DOF indicates the type of WAM (typically 4 or 7) and T_s is the Libbarrett Quick Start Guide Modified July 29, 2012 Page 15
period of the realtime code operations, meaning that the systems all operate every 0.002 seconds. The typedefs and waitForEnter() method at the bottom have been included for convenience.
The first part reads an interchangeable config file. This file contains calibration settings for gravity compensation and other internal WAM components. However, the config file is only good for gravity calibration (Section 4). The second part of the code calls upon the Execution Manager. The manager is necessary for the program to run correctly, as it tracks all of the connected Systems and tells them to update every 0.002 seconds. Without it, no realtime code executes. The WAM must be then idled and moved to the home position before the WAM virtual object is created, because the code must be able to communicate with the physical WAM. If the WAM is not ready to communicate, faulting or errors will occur.
When gravityCompensate() is called, one of the virtual hoses that connects systems together is connected or disconnected. The following is the gravityCompensate() code found in libbarrett:
gravityCompensate from wam-inl.h void Wam<DOF>::gravityCompensate(bool compensate = true) { if (compensate) { systems::forceConnect(gravity.output, jtSum.getInput(GRAVITY_INPUT)); } else { systems::disconnect(jtSum.getInput(GRAVITY_INPUT)); } }
This is the code that connects the gravity compensator to the summer. Compare this code back to the block diagram of the WAM object for a better perspective. We achieve the desired effect of the systems; they connect, disconnect, and mix at will, making the code both flexible and stable and link using short code like above.
Use loops to recurse through arrays instead of referencing them through hard numbers. This makes the code easier to extend.
Give your new code AS LITTLE information as possible; the library is written to isolate data and thereby prevent accidental modifications.
Rely on libbarrett as much as possible. The code you are hoping to write (at least at first) is probably a lot simpler than you think it is if you use the built-in functionality of the library.
Isolate thinking from realtime code. Realtime is for passing information around and performing concise calculations. It is NOT for waiting for input, writing output, or crunching large amounts of data. If you try to do too much within realtime, your code will take too long to execute, and interfere with controlling the WAM.
Notice how the code up until this point is the same as the code in the previous program. A WAM object was created and was told to compensate for gravity, leaving it in a stable, equilibrium state. This is a perfect springboard into the code that follows, which gives the user several modes to place the WAM in. Notice how the code that follows is supervisory it does not do anything, it just passes on actions for Execution Manager to do. This is an example of separating realtime and nonrealtime code.
bool going = true; std::string line; jp_type setPoint; cp_type cartSetPoint; std::cout std::cout std::cout std::cout std::cout << << << << << Please enter a command below:\n; i = Idle (Gravity Compensation ONLY)\n; h = Hold Position\n; p = Go to Position\n; x = Exit\n\n;
while (going) {
std::cout << >>> ; std::getline(std::cin, line); switch(line[0]) { case x: going = false; break; case i: wam.idle(); break; case h: holder.setOutput(wam.getJointPositions()); wam.trackReferenceSignal(holder.output); break; case p: std::cout << x = ; std::cin >> cartSetPoint[0]; std::cout << y = ; std::cin >> cartSetPoint[1]; std::cout >> z = ; std::cin >> cartSetPoint[2]; wam.moveTo(cartSetPoint); break; } } rtem.stop(); }
This code gives the user four options: hit i, h, or p. The first, i, idling, leaves gravity compensation on, but cancels all other commands the WAM might be following by disconnecting the PID controllers with the wam.idle() method. The second, h causes the WAM to try to hold its position, by taking a snapshot of the WAMs joint positions and telling it to maintain them. TrackReferenceSignal() is very useful, because it can accept joint positions, torques, velocities, and Cartesian positions and effectively deal with them (it is a feature of the supervisory controller). While it can accept all these types, they must come from an output of another system in order for the controller to accept them. The third, p takes a Cartesian position as an input and commands the WAM to move the endpoint of the arm to that position. This uses the moveTo() call, one of several included WAM methods. moveTo can handle any types that supervisoryController can. Finally, x is the exit choice; it will cause the loop to break and the program to exit. Libbarrett Quick Start Guide Modified July 29, 2012 Page 20
The key piece of this class is the operate() function. This is the function called by the ExecutionManager every .002 seconds, along with all the other operate() functions in all the other systems. This system receives values from the systems::SingleIO template, which gives it a single input and a single output. operate() gets the input, does its manipulation, then sends it out via the output. This is the basis upon which all systems operate this system will be connected to others through its inputs and
outputs, and data will move through the network of systems through those connections.
After understanding realtime code, it is also important to understand the role of the supervisory code that compliments the realtime. Below is the code for this program:
lock_joints.cpp #include stdhdr.h #include lock_joints.h int main(int argc, char** argv) { std::cout << "Idle the WAM then press [Enter].\n"; waitForEnter(); libconfig::Config config; config.readFile("/etc/barrett/wam4.conf"); systems::RealTimeExecutionManager rtem(T_s, false); systems::System::defaultExecutionManager = &rtem; Wam<DOF> wam(config.lookup("wam")); systems::LockJoints lockingSystem; connect(wam.jpOutput, lockingSystem.input); rtem.start(); std::cout << "Activate the WAM now.\n"; std::cout << "Press [Enter] to compensate for gravity.\n"; waitForEnter(); wam.gravityCompensate(); bool locked = false; bool going = true; std::string line; while (going) { std::cout << ">>> "; std::getline(std::cin, line); switch (line[0]) { case 'x': going = false; break; case 'l': locked = !locked; if (locked){ wam.moveTo(jp_type(0.0)); wam.trackReferenceSignal(lockingSystem.output); } else { wam.idle(); } break; } } rtem.stop(); }
The switch/case statement waits for input; x will exit and l will toggle between normal gravity compensation and the joints being tied together. Notice how the WAM is moved to its zero position before the joints lock. This prevents the WAM from moving suddenly and somewhat unpredictably while trying to make its joints match up Libbarrett Quick Start Guide Modified July 29, 2012 Page 23
when they are bound. It does this by assuring that they are already in a safe position to begin with.