added some docs

docs/source/index.rst

@@ -112,7 +112,15 @@ one may want to define
     :maxdepth: 2
 
     tools/*
-
+
+.. toctree:: 
+    :name: tutos
+    :caption: Tutorials
+    :glob:
+    :maxdepth: 2
+
+    tutos/*
+
 .. image:: _static/isir.png
     :width: 100px
     :align: left

docs/source/tutos/Cmakelist.rst

@@ -0,0 +1,29 @@
+CmakeList for using Orca and Traxxs
+--------------------------------------------------
+
+.. code-block:: bash
+
+  cmake_minimum_required(VERSION 3.1.0)
+  project("your_project")
+
+  ########### orocos ############
+  find_package(orocos_kdl REQUIRED)
+  ########### orca  #############
+  find_package(orca REQUIRED)
+  find_package(gazebo REQUIRED)
+  find_package(traxxs REQUIRED)
+  link_directories(${GAZEBO_LIBRARY_DIRS})
+
+
+  include_directories(
+      ${orocos_kdl_INCLUDE_DIRS}
+      ${GAZEBO_INCLUDE_DIRS}
+  )
+
+
+  # Warning pour gazebo 8
+  list(APPEND CMAKE_CXX_FLAGS "${GAZEBO_CXX_FLAGS} -fext-numeric-literals")
+
+  add_executable(demo0 demo0.cpp)
+  target_link_libraries(demo0 ${orocos_kdl_LIBRARIES} orca::orca ${GAZEBO_LIBRARIES} pthread tinyxml traxxs::traxxs traxxs::traxxs_softmotion)
+  install(TARGETS demo0 DESTINATION devel/lib)

docs/source/tutos/Energie_constraint.rst

@@ -0,0 +1,188 @@
+Energy constraint
+-----------------
+
+
+This tutorial provides an example of addition of a new constraint. The added constraint is related to the robot provisional kinetic energy (which has been recently introduced in https://hal.archives-ouvertes.fr/hal-01614508/en).
+
+This class inherits from GenericConstraint, and the associated **ControleVariable** is **JointSpaceTorque**. Remember the problem is written as a **quadratic problem**:
+
+.. math::
+
+    lb_C \leq Cx \leq ub_C
+
+for which we have to set the bounds and the constraint matrix. Those bounds and constraint are expressed explicitly as a function of joint torque. For this problem, we will create a new class, called **EnergyConstraint**.
+
+.. code-block:: cpp
+
+  class EnergyConstraint : public GenericConstraint
+  {
+    public:
+
+
+    void stateInitialize()
+     {
+         // This function initialize and resize the attributes
+            ...
+      }
+
+    void updateState()
+    {
+        // This function update all the attributes
+        ...
+     }   
+
+    void updateConstraintFunction()
+    {
+     ...
+
+     constraintFunction().lowerBound() = ec_min;
+     constraintFunction().upperBound() = ec_max;
+
+    }
+    void resize()
+    {
+     ...
+
+     constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf
+     constraintFunction().constraintMatrix()= C;
+    }
+
+    protected:
+     // Declare the attributes of this class 
+     Eigen::VectorXd  AccelerationDes_;
+     bool indice;
+     string name_of_cstr ;
+
+     Eigen::MatrixXd Masse_, J_, J_transp_, Lambda_ ;
+     Eigen::VectorXd Xd_curr_, gravity_Torque_, jdotqdot_;
+  };
+
+Now, let's see an complete example :
+
+.. code-block:: cpp
+
+  class EnergyConstraint : public GenericConstraint
+  {
+    public:
+    EnergyConstraint() : GenericConstraint( ControlVariable::JointSpaceTorque )
+    {
+    }
+
+
+
+    void setAccelerationDes(Eigen::VectorXd Acc)
+    {
+      AccelerationDes_ = Acc ;
+    }
+
+     void stateInitialize()
+    {
+       const int ndof_ = robot().getNrOfDegreesOfFreedom();
+       Masse_.resize(ndof_,ndof_);
+       J_.resize(6,ndof_);
+       jdotqdot_.resize(6);
+       Lambda_.resize(6,6);
+       gravity_Torque_.resize(ndof_);
+       Xd_curr_.resize(6);
+       AccelerationDes_.resize(6);
+     }
+
+     void updateState()
+     {
+       const int ndof_ = robot().getNrOfDegreesOfFreedom();
+
+       Masse_ = (robot().getFreeFloatingMassMatrix()).block(6,6,ndof_,ndof_);
+       J_ = (robot().getFrameFreeFloatingJacobian("link_7")).block(0,6,6,ndof_);
+       Lambda_ = (J_*(Masse_.inverse())*J_.transpose()).inverse();
+       Xd_curr_ = robot().getFrameVel("link_7");
+       gravity_Torque_ = (robot().generalizedBiasForces()).segment(6,ndof_);
+       jdotqdot_ = robot().getFrameBiasAcc("link_7");
+
+     }   
+    void updateConstraintFunction()
+    {
+       MutexLock lock(mutex);
+       auto world = gazebo::physics::get_world();
+       double sim_step_dt = world->GetPhysicsEngine()->GetRealTimeUpdateRate()*1e-6;
+       const double n_horizon_steps(1);                          
+       double horizon_temps = n_horizon_steps * sim_step_dt ;
+
+       ...
+       // Compute the current energy of end-effector 
+       double ec_curr = 0.5 * Xd_curr.transpose() * Lambda_ * Xd_curr;
+       double ec_next(0.);
+       ec_next = ec_curr + delta_X.transpose()* Lambda_*(jdotqdot - J*Masse.inverse()*Gravity_torque)[0];
+       ...
+       Eigen::VectorXd ec_max(1),ec_min(1);
+       ec_min << -100 - ec_next ;
+
+       ec_max << ec_lim - ec_next ;
+
+       constraintFunction().lowerBound() = ec_min;
+       constraintFunction().upperBound() = ec_max;
+
+     }
+
+    void resize()
+     {
+       MutexLock lock(mutex);
+       const int dim = OptimisationVector().getSize(getControlVariable());
+
+       ...
+       ...	
+       C = delta_X.transpose() * Lambda_ * J * Masse.inverse();
+
+       constraintFunction().resize(1,dim); // this resize lb,C and ub, C to zero, lb to -inf, ub to +inf
+       constraintFunction().constraintMatrix()= C;
+       }
+
+
+    protected:
+
+      Eigen::Matrix<double,6,1> AccelerationDes_;
+   };
+
+The function ``resize ()``  set the new dimension of constraint matrix and bounds with respect to **JointTorque**'s dimension, using ``dim = OptimisationVector().getSize(getControlVariable());``, also, it set the value of constraint matrix.
+The function ``setAccelerationDes(Eigen::VectorXd Acc)`` return acceleration desired who is used in the function ``updateConstraintFunction()`` for compute the bounds lb_C et ub_C ,   
+In the main() code:
+
+.. code-block:: cpp
+
+  int main(int argc, char** argv)
+  {
+    ...
+
+
+    EnergyConstraint energycstr;
+    energycstr.setRobotModel(robot);
+    Eigen::VectorXd Accelerationdes;
+    Accelerationdes.resize(6);
+    Accelerationdes = cart_acc_pid.getCommand();
+    energycstr.stateInitialize();
+    energycstr.updateState();
+    energycstr.setAccelerationDes(Accelerationdes);
+    energycstr.resize();
+    energycstr.updateConstraintFunction();
+    ...
+
+    energycstr.insertInProblem();
+    energycstr.activate();
+
+    ...
+
+     while(i++ <= 3000000)
+    {
+      ...
+      energycstr.insertInProblem();
+      energycstr.updateState();
+
+      energycstr.update();
+
+      ...}
+
+      ...
+
+      return 0
+    }
+
+You can easily change the type of constraint by changing the dimension of matrix constraint and the constraints.  See the complete code in "orca-pu-constraint.hpp". (https://github.com/ZzhengP/Stage)

docs/source/tutos/Trajectory_generator_using_TRAXXS.rst

@@ -0,0 +1,89 @@
+Trajectory generator: Traxxs 
+--------------------------------------
+
+
+TRAXXS is a framework to create and manage trajectories. See the complete information in "https://github.com/akeolab/traxxs/tree/4a2baff9064df5b3b7f288a44c2189c909ac1fed".
+
+
+This tutorials not show you how to use TRAXXS for generate a trajectory, because there is many example in the web of TRAXXS. Let's see how to use TRAXXS in ORCA.
+
+
+.. code-block:: cpp
+
+   #include <orca/orca.h>
+   #include <gazebo/gazebo.hh>
+   #include <gazebo/common/common.hh>
+   #include <gazebo/physics/physics.hh>
+   #include <gazebo/gazebo_client.hh>
+   #include <sdf/parser_urdf.hh>
+   #include <fstream>
+   #include <thread>
+   #include <iostream>
+   #include <fstream>
+   #include <vector>
+
+   #include <traxxs/tracker/tracker.hpp>
+   #include <traxxs/impl/traxxs_softmotion/traxxs_softmotion.hpp> 
+   #include "samples_helpers.hpp"
+   #include "samples_helpers_cart.hpp"
+
+
+
+   template< class T >
+   using sptr = std::shared_ptr<T>;
+   using namespace traxxs;
+
+   int main(int argc, char** argv)
+   { 
+      ...
+
+      // Velocity, acceleration, and jeck bounds
+      path::PathBounds4d path_bounds;              
+      path_bounds.dx << 0.4, 0.4, 0.4, 1.0;
+      path_bounds.ddx = 100.0 * path_bounds.dx;
+      path_bounds.j = 200.0 * path_bounds.ddx;
+
+      std::vector< std::shared_ptr < path::PathSegment > > segments;   // Declare the  complete segment
+      segments =  createMonSegments(path_bounds);                             // Create the segment via "createMonSegments"      function and apply the bounds  
+
+      //Transform the path to a trajectory
+      auto trajectory = std::make_shared< trajectory::Trajectory >();       
+      if ( !trajectory->set< ArcTrajGenSoftMotion >( segments ) )
+      return 1;
+
+      trajectory::TrajectoryState state, state_new;
+
+      // the status of the tracker, the tracker is used to send the next position 
+      tracker::TrackerStatus status;
+      auto validator = std::make_shared< tracker::TrackerSpaceValidatorPose4d >( 1e-2, 10 );
+      tracker::TrackerSpacePursuit tracker( validator );
+      //tracker::TrackerTimePursuit tracker;
+      tracker.reset( trajectory );
+      if ( !trajectory->getState( 0.0, state) ) {
+      cerr << "Could not get state !\n";
+      return 1;
+      }
+
+    for ( unsigned int i = 0; i < 20000; ++i )
+    {       
+
+      ...
+      status = tracker.next( dt, state, state_new );
+      // if the tracker fails
+      if ( status == tracker::TrackerStatus::Error ) {
+      std::cerr << "Tracker error.\n";
+      break;
+      }
+      // The next position is used in cartesian acceleration task.
+      ...
+      Pos_des=state_new.x.segment(0,3);
+      trajectoire(cart_pos_ref, 0,T,bottom,Pos_des);
+      ...	
+      // if the trajectory is finished  
+      if ( status == tracker::TrackerStatus::Finished )
+      break;
+      } 
+      ...
+      return 0
+      }
+

docs/source/tutos/change_robot's_configuration.rst

@@ -0,0 +1,69 @@
+Reconfigure your robot's initial state
+--------------------------------------
+
+
+
+.. code-block:: cpp
+
+
+  #include <orca/orca.h>
+  #include <gazebo/gazebo.hh>
+  #include <gazebo/common/common.hh>
+  #include <gazebo/physics/physics.hh>
+  #include <gazebo/gazebo_client.hh>
+  #include <sdf/parser_urdf.hh>
+  #include <fstream>
+  #include <thread>
+  #include <iostream>
+  #include <fstream>
+
+  ...
+
+  bool setModelConfiguration(gazebo::physics::ModelPtr gazebo_model , std::vector<std::string> joint_names,std::vector<double> joint_positions)
+  {
+      if (!gazebo_model)
+      {
+	  std::cerr  << "Model is not loaded" << std::endl;
+	  return false;
+      }
+
+      if (joint_names.size() != joint_positions.size())
+      {
+	  std::cerr <<  "joint_names lenght should be the same as joint_positions : " << joint_names.size() << " vs " << joint_positions.size() << std::endl;
+	  return false;
+      }
+
+      auto world = gazebo::physics::get_world();
+      // make the service call to pause gazebo
+      bool is_paused = world->IsPaused();
+      if (!is_paused) world->SetPaused(true);
+
+      std::map<std::string, double> joint_position_map;
+      for (unsigned int i = 0; i < joint_names.size(); i++)
+      {
+	joint_position_map[joint_names[i]] = joint_positions[i];
+      }
+
+      gazebo_model->SetJointPositions(joint_position_map);
+
+      // resume paused state before this call
+      world->SetPaused(is_paused);
+
+      return true;
+  }
+
+  int main(int argc, char** argv)
+  {
+    ...
+    setModelConfiguration(  gz_model , { "joint_0","joint_1" ,"joint_2", "joint_3","joint_4","joint_5","joint_6"} , {1.0,0.,0.0,-1.57,0.0,1.57,0.});
+    ...
+    return 0
+
+    }
+
+The function ``SetModelConfiguration`` change robot's initial state. In main program, you specify the joint's name and the value of joint's initial position.
+Here is a example who use Kuka lwr4+ with 7 joints.
+
+.. code-block :: cpp
+
+    setModelConfiguration(  gz_model , { "joint_0","joint_1" ,"joint_2", "joint_3","joint_4","joint_5","joint_6"} , {1.0,0.,0.0,-1.57,0.0,1.57,0.});