usc_clmc avatar usc_clmc committed 70ae9d6

Plot 2D STOMP test using python and matplotlib

Comments (0)

Files changed (4)

planning/stomp/include/stomp/covariant_movement_primitive.h

     virtual ~CovariantMovementPrimitive();
 
 
-    // Functions that are specific to CovariantTrajectoryPolicy:
     bool initialize(ros::NodeHandle& node_handle);
     bool initialize(ros::NodeHandle& node_handle,
                                                const int num_time_steps,
                                                const std::vector<double>& derivative_costs);
     bool setToMinControlCost(Eigen::VectorXd& start, Eigen::VectorXd& goal);
     bool getParametersAll(std::vector<Eigen::VectorXd>& parameters);
-    void setFileNameBase(const std::string& file_name_base);
-
-    // Functions inherited from Policy:
 
     /**
      * Sets the number of time steps used in reinforcement learning
     bool computeControlCosts(const std::vector<Eigen::MatrixXd>& control_cost_matrices, const std::vector<Eigen::VectorXd>& parameters,
                              const std::vector<Eigen::VectorXd>& noise, const double weight, std::vector<Eigen::VectorXd>& control_costs);
 
-    // Functions inherited from LibraryItem:
-
-    std::string getInfoString();
-    std::string getClassName();
-    bool readFromDisc(const std::string directory_name, const int item_id, const int trial_id = 0);
-    bool writeToDisc(const int trial_id = 0);
-    bool readFromDisc(const std::string abs_file_name);
-    bool writeToDisc(const std::string abs_file_name);
-    std::string getFileName(const int trial_id);
+    bool writeToFile(const std::string abs_file_name);
 
 
 private:
     return true;
 }
 
-inline std::string CovariantMovementPrimitive::getInfoString()
-{
-    return "";
-}
-
-inline std::string CovariantMovementPrimitive::getClassName()
-{
-    return "CovariantTrajectoryPolicy";
-}
-
-inline void CovariantMovementPrimitive::setFileNameBase(const std::string& file_name_base)
-{
-    file_name_base_ = file_name_base;
-}
-
 
 }
 

planning/stomp/src/covariant_movement_primitive.cpp

     return true;
 }
 
-bool CovariantMovementPrimitive::readFromDisc(const std::string directory_name, const int item_id, const int trial_id)
-{
-    // TODO: implement this
-    return true;
-}
+//bool CovariantMovementPrimitive::readFromDisc(const std::string abs_file_name)
+//{
+//    // TODO: implement this
+//    return true;
+//}
 
-bool CovariantMovementPrimitive::writeToDisc(const int trial_id)
-{
-    writeToDisc(getFileName(trial_id));
-    return true;
-}
-
-bool CovariantMovementPrimitive::readFromDisc(const std::string abs_file_name)
-{
-    // TODO: implement this
-    return true;
-}
-
-bool CovariantMovementPrimitive::writeToDisc(const std::string abs_file_name)
+bool CovariantMovementPrimitive::writeToFile(const std::string abs_file_name)
 {
     FILE *f;
     f = fopen(abs_file_name.c_str(), "w");
     return true;
 }
 
-std::string CovariantMovementPrimitive::getFileName(const int trial_id)
-{
-    std::ostringstream ss;
-    ss << file_name_base_ << trial_id << ".txt";
-    return ss.str();
 }
-
-}

planning/stomp/test/stomp_2d_test.cpp

 
 #include "stomp_2d_test.h"
 #include <ros/ros.h>
+#include <sstream>
 
 namespace stomp
 {
 
   stomp_.initialize(node_handle_, shared_from_this());
 
-  for (int i=0; i<100; ++i)
+  policy_->writeToFile("noiseless_0.txt");
+
+  for (int i=1; i<100; ++i)
   {
     stomp_.runSingleIteration(i);
+    std::stringstream ss;
+    ss << "noiseless_" << i << ".txt";
+    policy_->writeToFile(ss.str());
   }
 
   return 0;

planning/stomp/test/stomp_2d_test_plot.py

+#!/usr/bin/env python
+
+import matplotlib
+matplotlib.use('GtkAgg')
+import matplotlib.pyplot as plt
+import numpy
+import time
+
+# ugly function required for old version of matplotlib
+def pause(plt, interval):
+    figManager = plt._pylab_helpers.Gcf.get_active()
+    if figManager is not None:
+        canvas = figManager.canvas
+        canvas.draw()
+        was_interactive = plt.isinteractive()
+        if not was_interactive:
+            plt.ion()
+            plt.show(False)
+        canvas.start_event_loop(interval)
+        if not was_interactive:
+            plt.ioff()
+        return
+
+plt.figure(1);
+#line, = plt.plot(0,0);
+plt.draw()
+plt.hold(False)
+
+for i in range(0,100):
+    file_name = '/Network/Servers/titian/Volumes/titian/kalakris/.ros/noiseless_%d.txt'%(i)
+    data = numpy.genfromtxt(file_name)
+    plt.plot(data[:,0], data[:,1])
+    #print i
+    #line.set_xdata(data[:,0]);
+    #line.set_ydata(data[:,1]);
+    #plt.draw()
+    pause(plt, 0.01)
+    
+plt.show()
Tip: Filter by directory path e.g. /media app.js to search for public/media/app.js.
Tip: Use camelCasing e.g. ProjME to search for ProjectModifiedEvent.java.
Tip: Filter by extension type e.g. /repo .js to search for all .js files in the /repo directory.
Tip: Separate your search with spaces e.g. /ssh pom.xml to search for src/ssh/pom.xml.
Tip: Use ↑ and ↓ arrow keys to navigate and return to view the file.
Tip: You can also navigate files with Ctrl+j (next) and Ctrl+k (previous) and view the file with Ctrl+o.
Tip: You can also navigate files with Alt+j (next) and Alt+k (previous) and view the file with Alt+o.