#include "tutsim.hpp"
#include <opspace/Task.hpp>
#include <jspace/test/sai_util.hpp>
#include <boost/shared_ptr.hpp>
#include <err.h>
Go to the source code of this file.
Namespaces | |
namespace | tut03 |
Classes | |
class | tut03::JTask |
Functions | |
static std::string | model_filename (TUTROB_XML_PATH_STR) |
static size_t | mode (0) |
static bool | servo_cb (size_t toggle_count, double wall_time_ms, double sim_time_ms, jspace::State &state, jspace::Vector &command) |
static void | draw_cb (double x0, double y0, double scale) |
int | main (int argc, char **argv) |
Variables | |
static boost::shared_ptr < jspace::Model > | model |
static boost::shared_ptr < tut03::JTask > | jtask |
This tutorial demonstrates the effect of gravity compensation. It is very similar to the tutorial number 2 in that it uses a custom opspace::Task subclass that is just a simple joint-position PD servo. However, the tut03::JTask class contains a flag to switch gravity compensation on or off.
When the tutorial starts, it is in a mode where the robot position and velocity gets constantly re-initialized to a swaying motion. When you press Toggle, it switches to joint-space position control with gravity compensation, using the current joint position as goal. The robot will thus overshoot due to its current velocity, and then converge back to the position it had when you clicked Toggle. Then, when you press Toggle again, it will keep servoing but switch off gravity compensation. This will make the robot "sag" with respect to its desired position, illustrating the effect of gravity. Clicking Toggle again goes back to the swaying motion and the cycle repeats.
Definition in file tut03_gravity_compensation.cpp.
static void draw_cb | ( | double | x0, | |
double | y0, | |||
double | scale | |||
) | [static] |
Definition at line 210 of file tut03_gravity_compensation.cpp.
References tutsim::draw_delta_jpos(), tutsim::draw_robot(), jtask, and mode.
int main | ( | int | argc, | |
char ** | argv | |||
) |
Definition at line 219 of file tut03_gravity_compensation.cpp.
References draw_cb, jtask, model, model_filename(), tutsim::run(), servo_cb, and tutsim::set_draw_cb().
static size_t mode | ( | 0 | ) | [static] |
static std::string model_filename | ( | TUTROB_XML_PATH_STR | ) | [static] |
static bool servo_cb | ( | size_t | toggle_count, | |
double | wall_time_ms, | |||
double | sim_time_ms, | |||
jspace::State & | state, | |||
jspace::Vector & | command | |||
) | [static] |
Definition at line 129 of file tut03_gravity_compensation.cpp.
References jtask, mode, model, jspace::State::position_, jspace::pretty_print(), and jspace::State::velocity_.
boost::shared_ptr<tut03::JTask> jtask [static] |
Definition at line 125 of file tut03_gravity_compensation.cpp.
boost::shared_ptr<jspace::Model> model [static] |
Definition at line 124 of file tut03_gravity_compensation.cpp.