#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 | tut02 |
Classes | |
class | tut02::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 < tut02::JTask > | jtask |
Example of how to create and use a custom subclass of opspace::Task. The tut02::JTask class performs joint-space PD control without compensating for either gravity or inertial coupling. It is a straightforward simplsitic implementation which serves to show the minimal implementation work required to create an opspace::Task, and it also demonstrates that without gravity and mass-inertia compensation you will get rather low performance.
When you start the simulation, it is initially in a mode where it bypasses the tut02::JTask by simply sending a sinusoidal torque to the first joint and adding damping to all joints. This makes the arm sway around somewhat erratically. When you press Toggle, it sets the tut02::JTask goal to a position that is a zig-zag shape of +/- 45deg added to the current joint state, and it starts servoing to it. You'll see the arm converge more or less to the desired position. Pressing Toggle again goes back to the swaying mode.
Definition in file tut02_jtask.cpp.
static void draw_cb | ( | double | x0, | |
double | y0, | |||
double | scale | |||
) | [static] |
Definition at line 182 of file tut02_jtask.cpp.
References tutsim::draw_delta_jpos(), tutsim::draw_robot(), jtask, and mode.
int main | ( | int | argc, | |
char ** | argv | |||
) |
Definition at line 191 of file tut02_jtask.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 126 of file tut02_jtask.cpp.
References jtask, mode, model, jspace::State::position_, jspace::pretty_print(), and jspace::State::velocity_.
boost::shared_ptr<tut02::JTask> jtask [static] |
boost::shared_ptr<jspace::Model> model [static] |
Definition at line 121 of file tut02_jtask.cpp.