Skip to content
Browse files

Merge pull request #72 from alexlee-gk/mp

update with new color scheme
  • Loading branch information...
2 parents ae5887a + e97691b commit 80a3f24c8e6fe78dd2e8450fd4b8e46a8e43a16d alexlee-gk committed Sep 23, 2011
View
5 DiscreteRods/EnvObjects/EndEffector.cpp
@@ -360,13 +360,16 @@ void EndEffector::draw()
if (i == 2 || i == 5) { continue; }
drawCapsule(col_objs[i]);
}
- glColor3f(0.3, 0.3, 0.0);
+ glColor3f(0.0, 0.5, 0.0);
drawCapsule(col_objs[2]);
drawCapsule(col_objs[5]);
#else
for (int i = 1; i < 8; i++) {
drawCapsule(col_objs[i]);
}
+ glColor3f(0.0, 0.5, 0.0);
+ drawCapsule(col_objs[2]);
+ drawCapsule(col_objs[5]);
#endif
}
View
2 DiscreteRods/EnvObjects/EndEffector.h
@@ -86,7 +86,7 @@ class EndEffector : public EnvObject
void restore();
//static const double default_color0 = 0.7, default_color1 = 0.7, default_color2 = 0.7;
- static const double default_color0 = 120.0/255.0, default_color1 = 150.0/255.0, default_color2 = 120.0/255.0;
+ static const double default_color0 = 184.0/255.0, default_color1 = 134.0/255.0, default_color2 = 11.0/255.0;
static const double pieces = 3.0; //4.0;
static const double h = 9.0/3.0; //9.0/4.0; // (end-start)/pieces
static const double start = -3.0;
View
1 DiscreteRods/EnvObjects/InfinitePlane.cpp
@@ -88,6 +88,7 @@ void InfinitePlane::draw()
glPushMatrix();
glEnable(GL_COLOR_MATERIAL);
glColor3f(color0, color1, color2);
+
glDisable(GL_CULL_FACE);
bool tex = (file_name != "notexture");
View
74 DiscreteRods/EnvObjects/World.cpp
@@ -273,10 +273,19 @@ void World::draw(RenderMode render_mode)
getObjects<EndEffector>(end_effs);
if (end_effs.size() > 0)
end_effs[0]->draw();
+ if (end_effs.size() > 1) {
+ glColor3f(0.3, 0.3, 0.3);
+ drawSphere(end_effs[1]->getPosition(), 4.0);
+ }
if (end_effs.size() > 2)
end_effs[2]->draw();
+ if (end_effs.size() > 3) {
+ glColor3f(0.3, 0.3, 0.3);
+ drawSphere(end_effs[3]->getPosition(), 4.0);
+ }
if (end_effs.size() > 4)
end_effs[4]->draw();
+
vector<InfinitePlane*> planes;
getObjects<InfinitePlane>(planes);
if (planes.size() > 0)
@@ -391,6 +400,71 @@ void World::applyRelativeControl(const vector<Control*>& controls, double thresh
}
}
+void World::applyRelativeControl(const vector<Control*>& controls, vector<double>& displacements, double thresh, bool limit_displacement)
+{
+ assert(cursors.size() == controls.size());
+
+ vector<Vector3d> pre_positions;
+
+ if ((cursors.size() == 2) || (displacements.size() == 2)) {
+ pre_positions.resize(2);
+ for (int i = 0; i < cursors.size(); i++) {
+ assert(cursors[i]->isAttached());
+ if (cursors[i]->isAttached())
+ pre_positions[i] = cursors[i]->end_eff->getPosition();
+ }
+ } else {
+ for (int i = 0; i < displacements.size(); i++) {
+ displacements[i] = -1;
+ }
+ }
+
+ for (int i = 0; i < cursors.size(); i++) {
+ Cursor* cursor = cursors[i];
+ Matrix3d rotate(controls[i]->getRotate());
+
+ AngleAxisd rotate_aa(rotate);
+ AngleAxisd noise_rot = AngleAxisd(normRand(0, thresh*rotate_aa.angle()) * M_PI/180.0, Vector3d(normRand(0, 1.0), normRand(0, 1.0), normRand(0, 1.0)).normalized());
+ const Matrix3d cursor_rot = cursor->rotation * rotate * noise_rot;
+
+ double trans_norm = controls[i]->getTranslate().norm();
+
+ const Vector3d noise_vec = Vector3d(normRand(0, thresh*trans_norm),
+ normRand(0, thresh*trans_norm),
+ normRand(0, thresh*trans_norm));
+ const Vector3d cursor_pos = cursor->position + controls[i]->getTranslate() + EndEffector::grab_offset * cursor_rot.col(0) + noise_vec;
+
+ cursor->setTransform(cursor_pos, cursor_rot, limit_displacement);
+
+ if (controls[i]->getButton(UP))
+ cursor->openClose(limit_displacement);
+ if (controls[i]->getButton(DOWN))
+ cursor->attachDettach(limit_displacement);
+ }
+
+ for (int thread_ind = 0; thread_ind < threads.size(); thread_ind++) {
+ threads[thread_ind]->minimize_energy();
+ }
+ vector<EndEffector*> end_effs;
+ getObjects<EndEffector>(end_effs);
+ for (int ee_ind = 0; ee_ind < end_effs.size(); ee_ind++) {
+ if (!(controls[0]->getButton(UP)) && !(controls[1]->getButton(UP)))
+ end_effs[ee_ind]->updateTransformFromAttachment();
+ }
+
+ if ((cursors.size() == 2) || (displacements.size() == 3)) {
+ Matrix<double,6,1> u_tran;
+ for (int i = 0; i < cursors.size(); i++) {
+ assert(cursors[i]->isAttached());
+ if (cursors[i]->isAttached())
+ //cout << "norm of end effector " << i << ": " << (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm() << endl;
+ displacements[i] = (cursors[i]->end_eff->getPosition() - pre_positions[i]).norm();
+ u_tran.segment(i*3,3) = (cursors[i]->end_eff->getPosition() - pre_positions[i]);
+ }
+ displacements[2] = u_tran.norm();
+ }
+}
+
//The control is effectively applied to the tip of the end effector
void World::applyRelativeControl(const VectorXd& relative_control, double thresh, bool limit_displacement)
{
View
1 DiscreteRods/EnvObjects/World.h
@@ -120,6 +120,7 @@ class World
// for each control, there is 3 dof for translation, 3 for rotation, 2 for event
void setTransformFromController(const vector<ControllerBase*>& controls, bool limit_displacement = false); //applies controli to handle
void applyRelativeControl(const vector<Control*>& controls, double thresh=0.0, bool limit_displacement = false);
+ void applyRelativeControl(const vector<Control*>& controls, vector<double>& displacements, double thresh=0.0, bool limit_displacement = false);
void applyRelativeControl(const VectorXd& relative_control, double thresh=0.0, bool limit_displacement = false);
void applyRelativeControlJacobian(const VectorXd& relative_control, double thresh=0.0);
View
4 DiscreteRods/Makefile
@@ -19,7 +19,7 @@ else
LDFLAG=-lrt
endif
-VISION_LIBS = -L/usr/local/lib/ -lcxcore -lcv -lhighgui -lcvaux -lml -lm -L/usr/lib/ -lPvAPI -lrt
+VISION_LIBS = -L/usr/local/lib/ -lcxcore -lcv -lhighgui -lcvaux -lml -lm -L/usr/lib/ -lrt #-lPvAPI
VISION_INCLUDES = -I/usr/local/include/opencv/
SOCKETS_DIR = ../utils/sockets/
# OPT_COMP =
@@ -45,7 +45,7 @@ runExperiment: runExperiment.cpp threadutils_discrete.o threadpiece_discrete.o t
testThreadDiscrete_pickUp: testThreadDiscrete_pickUp.cpp threadutils_discrete.o threadpiece_discrete.o thread_discrete.o ThreadConstrained.o drawUtils.o imageloader.o collisionUtils.o Mouse.o Haptic.o World.o CollisionWorld.o WorldManager.o Capsule.o Control.o Cursor.o EndEffector.o InfinitePlane.o TexturedSphere.o Box.o Needle.o thread_socket_interface.o UDPSocket.o Socket.o StateRecorder.o StateReader.o TrajectoryRecorder.o TrajectoryReader.o worldSQP.o clock.o
- $(LD) $(INCLUDES) $(ISOTROPIC) $(DEBUG) $(NDEBUG) -o testThreadDiscrete_pickUp testThreadDiscrete_pickUp.cpp threadutils_discrete.o threadpiece_discrete.o thread_discrete.o ThreadConstrained.o drawUtils.o imageloader.o collisionUtils.o Mouse.o Haptic.o World.o CollisionWorld.o WorldManager.o Capsule.o Control.o Cursor.o EndEffector.o InfinitePlane.o TexturedSphere.o Box.o Needle.o thread_socket_interface.o UDPSocket.o Socket.o StateRecorder.o StateReader.o TrajectoryRecorder.o TrajectoryReader.o worldSQP.o clock.o $(LIBS) $(PROFILER)
+ $(LD) $(INCLUDES) $(VISION_INCLUDES) $(ISOTROPIC) $(DEBUG) $(NDEBUG) -o testThreadDiscrete_pickUp testThreadDiscrete_pickUp.cpp threadutils_discrete.o threadpiece_discrete.o thread_discrete.o ThreadConstrained.o drawUtils.o imageloader.o collisionUtils.o Mouse.o Haptic.o World.o CollisionWorld.o WorldManager.o Capsule.o Control.o Cursor.o EndEffector.o InfinitePlane.o TexturedSphere.o Box.o Needle.o thread_socket_interface.o UDPSocket.o Socket.o StateRecorder.o StateReader.o TrajectoryRecorder.o TrajectoryReader.o worldSQP.o clock.o $(LIBS) $(VISION_LIBS) $(PROFILER)
recordThreadVideo: recordThreadVideo.cpp threadutils_discrete.o threadpiece_discrete.o thread_discrete.o trajectory_reader.o
$(LD) $(INCLUDES) $(VISION_INCLUDES) $(DEBUG) $(NDEBUG) -o recordThreadVideo recordThreadVideo.cpp threadutils_discrete.o threadpiece_discrete.o thread_discrete.o trajectory_reader.o $(LIBS) $(VISION_LIBS)
View
11 DiscreteRods/ThreadConstrained.cpp
@@ -1021,8 +1021,7 @@ void ThreadConstrained::draw(bool mode) {
// cout << endl;
glPushMatrix();
- //glColor3f (0.5, 0.5, 0.2);
- glColor3f(184.0/255.0, 134.0/255.0, 11.0/255.0);
+ glColor3f (0.8, 0.2, 0.2);
double pts_cpy[points.size()+2][3];
double twist_cpy[points.size()+2];
for (int i=0; i < points.size(); i++)
@@ -1057,10 +1056,10 @@ void ThreadConstrained::draw(bool mode) {
0x0,
twist_cpy);
-#ifndef PICTURE
- glColor3f (1.0, 0.0, 0.0);
- drawSphere(points[points.size()/2], 1.5);
-#endif
+//#ifndef PICTURE
+// glColor3f (1.0, 0.0, 0.0);
+// drawSphere(points[points.size()/2], 1.5);
+//#endif
if (examine_mode) {
glColor3f (0.0, 0.5, 0.5);
View
35 DiscreteRods/plot_script.m
@@ -1,23 +1,38 @@
-D = load('~/rll/surgical/DiscreteRods/environmentFiles/w2/w2_data_plot.txt')
+prob = 'w4'
+
+D = load(strcat('~/rll/surgical/DiscreteRods/environmentFiles/',prob,'/',prob,'_data_plot.txt'))
+Ds = load(strcat('~/rll/surgical/DiscreteRods/environmentFiles/',prob,'s/',prob,'s_data_plot.txt'))
+
+s = size(D,1)
+
+h0 = 1:size(D,1)/2;
+h5 = size(D,1)/2+1:size(D,1);
+
h=figure
hold on
-p_open = plot(D(1:7,2),D(1:7,3))
+
+
+
+p_open = plot(D(h0,2),D(h0,3))
set(p_open, 'Color', [.2,.8,.2], 'LineWidth', 1)
-p_close5 = plot(D(8:14,2),D(8:14,3))
+p_close5 = plot(D(h5,2),D(h5,3))
set(p_close5, 'Color', [.2,.2,.8], 'LineWidth', 1)
-p_close10 = plot(D(15:21,2),D(15:21,3))
-set(p_close10, 'Color', [.8,.2,.2], 'LineWidth', 1)
+p_opens = plot(Ds(h0,2),Ds(h0,3))
+set(p_opens, 'Color', [.8,.2,.2], 'LineWidth', 1)
+
+p_close5s = plot(Ds(h5,2),Ds(h5,3))
+set(p_close5s, 'Color', [.2,.8,.8], 'LineWidth', 1)
-hleg = legend('Open Loop', 'Closed Loop, Horizon 5', 'Closed Loop, Horizon 10')
-set(hleg,'FontSize', 10,'TextColor',[0,0,0])
+hleg = legend('Open Loop', 'Closed Loop', 'Smoothed Open Loop', 'Smoothed Closed Loop')
+set(hleg,'FontSize', 10,'TextColor',[0,0,0], 'Location', 'SouthEast')
ylabel('Success Rate')
-xlabel('Noise')
+xlabel('\nu')
-set(findobj('type','axis'), 'FontSize', 16)
+set(findobj('type','axis'), 'FontSize', 24)
-print(h, '-dpdf', '~/rll/surgical/DiscreteRods/environmentFiles/w2/w2_open_closed_loop_success_noise.pdf', '-r125')
+print(h, '-dpdf', strcat('~/rll/surgical/DiscreteRods/environmentFiles/',prob,'/',prob,'_open_closed_loop_success_noise.pdf'), '-r125')
View
4 DiscreteRods/runExperiments0.py
@@ -1,10 +1,10 @@
import os, sys
-control_in_filename = ["x10s", "x10"]
+control_in_filename = ["w2", "w2s", "w3", "w3s", "w4", "w4s", "x10", "x10s"]
start_ind = 0
end_ind = -1
single_horizon = [0]
-noise_thresh = [0.8, 0.9, 1.0, 1.1, 1.2, 1.3]
+noise_thresh = [0.0]
for control_in in control_in_filename:
for horizon in single_horizon:
View
66 DiscreteRods/testThreadDiscrete_pickUp.cpp
@@ -43,6 +43,11 @@
#include "TrajectoryRecorder.h"
#include "TrajectoryReader.h"
+#include <opencv/cv.h>
+#include <opencv/highgui.h>
+
+using namespace cv;
+
// import most common Eigen types
USING_PART_OF_NAMESPACE_EIGEN
@@ -52,11 +57,11 @@ void displayTextInScreen(const char* textline, ...);
void bitmap_output(int x, int y, char *string, void *font);
void glutMenu(int ID);
void initGL();
+void save_opengl_image(char* filename);
void interruptHandler(int sig);
//void sqpSmoother(vector<World*>& trajectory_to_smooth, vector<World*>& smooth_trajectory);
//void sqpPlanner(World* start, World* goal, vector<World*>& trajectory);
-void closedLoopSQPController(World* start, vector<World*>& target,
- vector<vector<Control*> >& ctrls);
+void closedLoopSQPController(World* start, vector<World*>& target, vector<vector<Control*> >& ctrls);
VectorXd closedLoopSQPStepper(World* start, World* goal, WorldSQP* solver);
void chunkSmoother(vector<World*>& dirty, vector<vector<Control*> >& ctrl_in, vector<World*>& smooth, vector<vector<Control*> >& smooth_controls);
@@ -134,6 +139,9 @@ TrajectoryRecorder trajectory_recorder_world;
vector<World*> worlds;
int world_ind = 0;
+#define IMAGE_BASE_NAME "./environmentFiles/images/"
+int im_save_ind = 1;
+
//for collecting data
DIR *dir = NULL;
struct dirent *ent;
@@ -257,7 +265,8 @@ void processNormalKeys(unsigned char key, int x, int y)
sprintf(filename, "%s", ent->d_name);
vector<string> parameters;
boost::split(parameters, filename, boost::is_any_of("_"));
- if (parameters.size() != 7 || parameters[0]!=string(data_raw_filename)) {
+ if (parameters.size() != 7 || parameters[0]!=string(data_raw_filename) || parameters[4]!="2.0") {
+ //if (parameters.size() != 7 || parameters[0]!=string(data_raw_filename) || parameters[3] == "5") {
cout << "Ignoring file " << filename << endl;
processNormalKeys('b', x, y);
return;
@@ -301,6 +310,11 @@ void processNormalKeys(unsigned char key, int x, int y)
cout << "You answered no" << endl;
processNormalKeys('b', x, y);
return;
+ } else if (key == 'i') {
+ cout << "Please enter image destination file name (without extension): ";
+ char *dstFileName = new char[256];
+ cin >> dstFileName;
+ save_opengl_image(dstFileName);
}
else if (key == 't')
@@ -887,12 +901,13 @@ void drawStuff()
#endif
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
- glClearColor(1.0, 1.0, 1.0, 0.0);
+ glClearColor(0.4, 0.4, 0.4, 0.0);
glColor3f(1.0, 1.0, 1.0);
glPushMatrix ();
/* set up some matrices so that the object spins with the mouse */
glTranslatef (translate_frame[0], translate_frame[1], translate_frame[2]);
+#ifndef PICTURE
glDisable(GL_LIGHTING);
if (drawWorldInd < drawWorlds.size() &&
drawInd < drawWorlds[drawWorldInd].size() &&
@@ -910,6 +925,7 @@ void drawStuff()
bitmap_output(50, 40, "Goal (.)", GLUT_BITMAP_TIMES_ROMAN_24);
}
glEnable(GL_LIGHTING);
+#endif
#ifdef VIEW3D
glTranslatef(0.0, 0.0, +eye_focus_depth);
@@ -1445,6 +1461,48 @@ void interruptHandler(int sig) {
//setVisualizationData(openLoopWorlds);
}*/
+void save_opengl_image(char* filename)
+{
+ const int IMG_COLS_TOTAL = 900;
+ const int IMG_ROWS_TOTAL = 900;
+ //playback and save images
+ Mat img(900, 900, CV_8UC3);
+ vector<Mat> img_planes;
+ split(img, img_planes);
+
+ uchar tmp_data[3][900*900];
+
+ GLenum read_formats[3];
+ read_formats[0] = GL_BLUE;
+ read_formats[1] = GL_GREEN;
+ read_formats[2] = GL_RED;
+
+ for (int i=0; i < 3; i++)
+ {
+ glReadPixels(0, 0, IMG_COLS_TOTAL, IMG_ROWS_TOTAL, read_formats[i], GL_UNSIGNED_BYTE, tmp_data[i]);
+ img_planes[i].data = tmp_data[i];
+ }
+
+
+ vector<int> p(3);
+
+ p[0] = CV_IMWRITE_PNG_COMPRESSION;
+ p[1] = 20;
+ p[2] = 0;
+
+
+ merge(img_planes, img);
+ flip(img, img, 0);
+
+ char im_name[256];
+ sprintf(im_name, "%s%s.png", IMAGE_BASE_NAME, filename);
+ cout << "Image written to " << im_name << endl;
+ im_save_ind++;
+ imwrite(im_name, img, p);
+ waitKey(1);
+ //sleep(0);
+}
+
VectorXd closedLoopSQPStepper(World* start, World* goal, WorldSQP* solver) {
if (solver == NULL) assert(false); //initialize your shit
View
36 DiscreteRods/z.py
@@ -1,25 +1,41 @@
import os, sys
control_in_filename = [sys.argv[1]]
+control_in_filename = ["w4"]
set = int(sys.argv[2])
+ran = int(sys.argv[3])
print control_in_filename, set
start_ind = 0
end_ind = -1
-single_horizon = [5]
+single_horizon = [0]
-if set == 1:
- noise_thresh = [0.0, 0.2]
+if set == 0:
+ noise_thresh = [0.0]
+elif set == 1:
+ noise_thresh = [0.2]
elif set == 2:
- noise_thresh = [0.4, 0.6]
-else
- noise_thresh = [0.8, 1.0]
+ noise_thresh = [0.4]
+elif set == 3:
+ noise_thresh = [0.6]
+elif set == 4:
+ noise_thresh = [0.8]
+else:
+ noise_thresh = [1.0]
+
+noise_thresh = [1.0, 1.2, 1.4, 1.6, 1.8, 2.0, 2.2, 2.4, 2.6, 2.8, 3.0]
for control_in in control_in_filename:
for horizon in single_horizon:
for noise in noise_thresh:
- for i in range(0, 10):
- run_command = "%s %s %s %s %d %d %d %f" % ("./runExperiment", control_in + "_" + str(start_ind) + "_" + str(end_ind) + "_" + str(horizon) + "_" + str(noise) + "_exp" + str(i), control_in, control_in + "_world", start_ind, end_ind, horizon, noise)
- print run_command
- os.system(run_command)
+ if (ran == 0):
+ for i in range(0, 10):
+ run_command = "%s %s %s %s %d %d %d %f" % ("./runExperiment", control_in + "_" + str(start_ind) + "_" + str(end_ind) + "_" + str(horizon) + "_" + str(noise) + "_exp" + str(i), control_in, control_in + "_world", start_ind, end_ind, horizon, noise)
+ print run_command
+ os.system(run_command)
+ else:
+ for i in range(0, 10):
+ run_command = "%s %s %s %s %d %d %d %f" % ("./runExperiment", control_in + "_" + str(start_ind) + "_" + str(end_ind) + "_" + str(horizon) + "_" + str(noise) + "_exp" + str(i), control_in, control_in + "_world", start_ind, end_ind, horizon, noise)
+ print run_command
+ os.system(run_command)
View
3 utils/drawUtils.cpp
@@ -29,6 +29,9 @@ void drawCapsule(btCollisionObject* col_obj, bool include_repulsion_dist)
float radius = capsule_shape->getRadius() - ((include_repulsion_dist) ? 0.0 : REPULSION_DIST);
double cylinder[4][3] = { {-half_height-1.0, 0.0, 0.0} , {-half_height, 0.0, 0.0} , {half_height, 0.0, 0.0} ,
{half_height+1.0, 0.0, 0.0} };
+#ifdef PICTURE
+ gleSetNumSides(60);
+#endif
glePolyCylinder(4, cylinder, NULL, radius);
glTranslated(-half_height, 0.0, 0.0);
glutSolidSphere(radius, 20, 16);
View
2 utils/drawUtils.h
@@ -22,6 +22,8 @@
#include <btBulletDynamicsCommon.h>
+//#define PICTURE
+
#include "../DiscreteRods/threadutils_discrete.h"
#include "../DiscreteRods/Collisions/collisionUtils.h"

0 comments on commit 80a3f24

Please sign in to comment.
Something went wrong with that request. Please try again.