Ressurect from cito grad approx

CMakeLists.txt

@@ -3,7 +3,7 @@ project(cito)
 
 add_compile_options(-std=c++11)
 
-find_package(catkin REQUIRED)
+# find_package(catkin REQUIRED)
 find_package(Eigen3 REQUIRED)
 find_package(yaml-cpp 0.6 REQUIRED)
 find_package(fcl REQUIRED)
@@ -21,13 +21,13 @@ endif()
 set(CMAKE_BUILD_TYPE Release)
 # set(CMAKE_CXX_FLAGS "-Wall -Wextra -Wpedantic")
 
-catkin_package()
+# catkin_package()
 
 include_directories(
  include
  ${EIGEN3_INCLUDE_DIR}
  ${MUJOCO_DIR}/include
- ${SNOPT_DIR}/include
+ ${SNOPT_DIR}/interfaces/include
 )
 
 # Create the CITO library
@@ -42,8 +42,8 @@ add_library(${PROJECT_NAME}
 )
 if (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
 	target_link_libraries(${PROJECT_NAME}
-                          ${MUJOCO_DIR}/bin/libmujoco200nogl.so
-                          ${SNOPT_DIR}/lib/libsnopt7_cpp.so
+                          ${MUJOCO_DIR}/lib/libmujoco.so
+                          ${SNOPT_DIR}/lib/.libs/libsnopt7_cpp.so
                           pthread
                           yaml-cpp
                           fcl
@@ -81,14 +81,16 @@ endif()
 add_executable(${NODE_NAME2} src/playlog.cpp)
 if (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
 	target_link_libraries(${NODE_NAME2}
-                          ${MUJOCO_DIR}/bin/libmujoco200.so
-                          ${MUJOCO_DIR}/bin/libglew.so
-                          ${MUJOCO_DIR}/bin/libglfw.so.3
-                          GL
-                          yaml-cpp
+						  ${PROJECT_NAME}
+                        #   ${MUJOCO_DIR}/bin/libmujoco.so
+                        #   ${MUJOCO_DIR}/bin/libglew.so
+                        #   ${MUJOCO_DIR}/bin/libglfw.so.3
+                          glfw
+                        #   yaml-cpp
 	)
 elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
 	target_link_libraries(${NODE_NAME2}
+						  ${PROJECT_NAME}
                           ${MUJOCO_DIR}/bin/libmujoco200.dylib
                           ${MUJOCO_DIR}/bin/libglfw.3.dylib
                           yaml-cpp
@@ -107,8 +109,8 @@ elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
 	)
 endif()
 
-# Unit tests
-catkin_add_gtest(${PROJECT_NAME}-test test/test_cito.cpp)
-if(TARGET ${PROJECT_NAME}-test)
-    target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
-endif()
+# # Unit tests
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_cito.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#     target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()

config/params.yaml

@@ -2,20 +2,20 @@
 model: "iiwa_workcell.xml"
 
 # Penalty loop
-penaltyLoop: true
+penaltyLoop: false
 taskType: 0 # 0: manipulation, 1: locomotion
 
 # Simulation
 tf: 1.00	# [s] final time
-tc: 5e-2 	# [s] control sampling period
+tc: 1e-1 	# [s] control sampling period
 
 # Contact pairs
 ## number of contact pairs
-npair: 4
+npair: 1
 ## indices of the sites on the robot
-spair1: [4,4,4,4]
+spair1: [4]
 ## indices of the corresponding sites in the environment
-spair2: [0,1,2,3]
+spair2: [0]
 
 # Task
 ## desired final pose of the control joint
@@ -26,5 +26,5 @@ controlJointDOF0: 0
 # Weights
 w1: 1e4  	  # weight on deviations in x and y directions
 w2: 1e0  	  # weight on deviations in z and orientation
-w3: 0e-2  	# weight on final velocities
-w4: 5e-1  	# weight on virtual stiffness
+w3: 2e-3  	# weight on final velocities
+w4: 2e-0  	# weight on virtual stiffness

include/cito/numdiff.h

@@ -13,6 +13,15 @@
 
 #include "cito/control.h"
 
+struct derivative_interpolator{
+    std::string keyPoint_method;
+    int min_n;
+    int max_n;
+    std::vector<double> jerk_thresholds;
+    std::vector<double> velChange_thresholds;
+    double error_threshold;
+};
+
 class NumDiff
 {
 public:
@@ -23,6 +32,12 @@ class NumDiff
     /// This function calculates derivatives of the state and control trajectories
     void linDyn(const mjData *dMain, const eigVd &uMain, double *Fxd, double *Fud, double compensateBias);
 
+    void saveLinearisation(const std::string file_name, eigTd Fxd, eigTd Fud, int horizon);
+
+    std::vector<int> generateKeypoints(derivative_interpolator di, const eigMd X, int horizon);
+
+    void interpolateDerivs(std::vector<int> keypoints, eigTd &Fxd, eigTd &Fud, int horizon);
+
 private:
     /// This function sets xNew to the integration of data given a control input
     void copyTakeStep(const mjData *dMain, const eigVd &u, double *xNew, double compensateBias);

include/cito/params.h

@@ -19,7 +19,7 @@
 
 #include <Eigen/Dense>
 #include <yaml-cpp/yaml.h>
-#include "mujoco.h"
+#include "mujoco/mujoco.h"
 
 /// Types
 typedef Eigen::VectorXd eigVd;
@@ -46,7 +46,7 @@ class Params
 {
 public:
     /// Constructor
-    Params(const mjModel *model_);
+    Params(const mjModel *model_, int testNumber);
     /// Destructor
     ~Params();
     /// Simulation and model parameters

include/cito/scvx.h

@@ -30,6 +30,8 @@ class SCVX
     // This function refreshes SCVX variables for a new run
     void refresh();
 
+    double optTime, costReduction, derivsTime, qpTime;
+
 private:
     /// MuJoCo model
     const mjModel *m;
@@ -42,6 +44,10 @@ class SCVX
         beta_expand, beta_shrink, // trust-region expand and shrink factors
         r0, rMin, rMax;           // initial, min, and max trust-region radius
     bool *accept, dLTolMet = false, stop = false;
+    double *time_derivs;
+    double *time_qp;
+
+
     /// Trajectories
     eigMd XSucc, dX, XTilde;
     eigMd USucc, dU, UTemp;