Plugins

Writing a simple plugin

• Plugins allow you to specify how the robots in your simulation are controlled. This will be demonstrated with a simple example using a pendulum
• A plugin requires two functions: An init() and a callback for the controller
• The init function should locate the reference to the simulator and to the body you are trying to control
• It should also set the controller of that body to your callback function

//need extern "C"
extern "C" {
void init(void* separator, const std::map<std::string, Moby::BasePtr>& read_map,
          double time) {
  for (std::map<std::string, Moby::BasePtr>::const_iterator i = read_map.begin();
       i !=read_map.end(); i++) {
    // Find the simulator reference                                                  
    if (!sim)
      sim = boost::dynamic_pointer_cast<Moby::EventDrivenSimulator>(i->second);

    // find the robot/body reference                                                      
    if (!pen)
      pen = boost::dynamic_pointer_cast<Moby::RCArticulatedBody>(i->second);

    //set your body controller to your callback
    pen->controller = &controller_callback;
  }
}
}

• The callback function controls the movement of your robot

void controller_callback(Moby::DynamicBodyPtr dbp, double t, void*) {
  double p_des = 0.0, v_des = 0.0;
  double kp = 10000.0, kv = 100.0;

  double perr = 0.0, derr = 0.0;

  for(int i=0; i<pen->get_joints().size(); i++){
    if(pen->get_joints()[i]->num_dof() == 0) continue;
    perr = p_des - pen->get_joints()[i]->q[0];
    derr = v_des - pen->get_joints()[i]->qd[0];

    static Ravelin::VectorNd U(1);
    U[0] = (kp * perr) + (kv * derr);

    pen->get_joints()[i]->add_force(U);
  }
}

• Here is what your plugin file should look like

#include <Moby/EventDrivenSimulator.h>
#include <Moby/RCArticulatedBody.h>

boost::shared_ptr<Moby::EventDrivenSimulator> sim;
Moby::RCArticulatedBodyPtr pen;

void controller_callback(Moby::DynamicBodyPtr dbp, double t, void*) {
  double p_des = 0.0, v_des = 0.0;
  double kp = 10000.0, kv = 100.0;

  double perr = 0.0, derr = 0.0;

  for(int i=0; i<pen->get_joints().size(); i++){
    if(pen->get_joints()[i]->num_dof() == 0) continue;
    perr = p_des - pen->get_joints()[i]->q[0];
    derr = v_des - pen->get_joints()[i]->qd[0];

    static Ravelin::VectorNd U(1);
    U[0] = (kp * perr) + (kv * derr);

    pen->get_joints()[i]->add_force(U);
  }
}

extern "C" {
void init(void* separator, const std::map<std::string, Moby::BasePtr>& read_map,
          double time) {
  for (std::map<std::string, Moby::BasePtr>::const_iterator i = read_map.begin();
       i !=read_map.end(); i++) {
    // Find the simulator reference                                                  
    if (!sim)
      sim = boost::dynamic_pointer_cast<Moby::EventDrivenSimulator>(i->second);

    // find the robot reference                                                      
    if (!pen)
      pen = boost::dynamic_pointer_cast<Moby::RCArticulatedBody>(i->second);
  }
  pen->controller = &controller_callback;
}
}

Building the plugin

• The plugin can be built by simply compiling with

$ g++ -shared <plugin-name>.cpp -o <plugin-name>.so

Having Moby load and use the plugin

• The plugin can be used with

$ moby-driver -r -p=<plugin-name>.so test.xml

Wiki home