Sebastian Gomez-Gonzalez
robcpp

Repository

#include <iostream>
#include <armadillo>
#include <memory>
#include <robotics.hpp>

using namespace std;
using namespace arma;
using namespace robotics;

int main() {
  // Creating a basic ProMP with the desired parameters
  vec mu_w {0, 0, 0, 0};
  mat Sigma_w = 100*eye<mat>(4,4);
  mat Sigma_y = 0.0001*eye<mat>(2,2);
  ProMP promp(mu_w, Sigma_w, Sigma_y);

  //Setting a third order polynomial basis function for the ProMP
  shared_ptr<ScalarBasisFun> kernel{ new ScalarPolyBasis(3) };
  //And create a new ProMP for a robot with one degree of freedom
  FullProMP poly(kernel, promp, 1);

  //If we want the ProMP to take us from 0 to 1 in 0.5 seconds, we use conditioning
  double T=0.5;
  vec init_pos{0}, init_vel{0}, final_pos{1}, final_vel{0};
  poly = poly.condition_current_state(0, T, init_pos, init_vel);
  poly = poly.condition_current_state(T, T, final_pos, final_vel);

  //Now print the mean trajectory of the conditioned ProMP with time intervals of 0.1 seconds
  for (double t=0.0; t<=T; t+=0.1) {
    TrajectoryStep step = poly.mean_traj_step(t, T);
    cout << "Time: " << t <<  " Position: " << step.q[0] << " Velocity: " << step.qd[0] << endl;
  }

}
Time: 0 Position: 0.000013 Velocity: 0.000004
Time: 0.1 Position: 0.104011 Velocity: 1.91995
Time: 0.2 Position: 0.352004 Velocity: 2.87992
Time: 0.3 Position: 0.647996 Velocity: 2.87992
Time: 0.4 Position: 0.89599 Velocity: 1.91995
Time: 0.5 Position: 0.999987 Velocity: 0.000002
#include <stdio.h>
#include <robotics/table_tennis/table_tennis.h>

int main() {
  char opt[10];
  /* Allocate the resources and load configurations */
  void *table = rob_tt_load_table("models/table.txt");
  void *ball_model = rob_tt_load_ball_model(table, "models/trained_kf.txt");
  double deltaT = 0.016; /* The model in the file trained_kf.txt was trained with this deltaT */
  double obs[3], position[3];
  void *ball_state;
  double time = 0.0;

  printf("Now the models are loaded. Suppose we see a new ball trajectory (Initial time = %.2lf s)\n", time);
  ball_state = rob_tt_new_ball_traj(ball_model);
  while(1) {
    printf("Insert the 3D location of the ball observation at t = %.3lf s \
(Each coordinate separated by space)\n", time);
    scanf("%lf %lf %lf", &obs[0], &obs[1], &obs[2]);

    printf("The Mahalanobis distance of the given observation to the current estimate is %.4lf\n",
        rob_tt_ball_mah_dist(ball_state, ball_model, obs));
    printf("Do you accept this ball as correct (No means outlier)? (Y/N) ");
    scanf("%s", opt);
    /* Take anything different from No as yes. In case Yes add ball observation to the state */
    if (opt[0] != 'N' && opt[0]!='n')
      rob_tt_ball_observe(ball_state, ball_model, obs);
    /* Now compute the ball estimated (filtered) position */
    rob_tt_ball_position(position, ball_state, ball_model);
    printf("The current ball filtered position (Position estimate) is (%.3lf, %.3lf, %.3lf)\n", position[0],
        position[1], position[2]);

    printf("Do you want to exit the demo? (Y/N) ");
    scanf("%s", opt);
    /* Take anything different from yes as no */
    if (opt[0] == 'Y' || opt[0]=='y')
      break;

    /* Advancing time by deltaT */
    rob_tt_ball_advance_traj(ball_state, ball_model, deltaT);
    time += deltaT;
  }
  /* Now, make sure you release all the resources you allocated */
  rob_tt_free_ball_model(ball_model);
  rob_tt_free_table(table);
}
sudo apt-get install libarmadillo-dev libboost-dev libboost-test-dev cmake libnlopt-dev libzmq3-dev \
  python-dev swig libboost-log-dev libboost-program-options-dev
mkdir build
cd build
cmake ..
make
make test #optional (Tests that the compiled library passes the test cases)
make install
cmake .. -DPYLIB=Off