Merge branch 'marc' of github.com:MarcToussaint/rai into marc

Makefile

@@ -2,6 +2,8 @@ BASE = .
 
 target: src
 
+DEPEND = $(shell find rai -mindepth 1 -maxdepth 1 -printf "%f ")
+
 ################################################################################
 
 src_paths =  $(shell find rai -mindepth 1 -maxdepth 1 -type d -not -name 'retired' -printf "%f ")
@@ -16,6 +18,8 @@ initUbuntuPackages: force
 	@echo "##### calling make installUbuntu in each lib"
 	@find rai -mindepth 1 -maxdepth 1 -type d -exec make installUbuntu -C {} \;
 
+printUbuntu: $(DEPEND:%=inPath_printUbuntuPackages/%) printUbuntuPackages
+
 tests: $(test_paths:%=inPath_make/%)
 
 bin: $(bin_paths:%=inPath_make/%)

README.md

@@ -14,3 +14,23 @@ Parts of the code have there origin at around 2004 (Edinburgh). The code grew ov
 many projects from all lab members, but a somewhat consistent scope of code shared between projects. This repo exports a
 selection of the code shared between projects and contains a set of representations and methods for Robotics, ML and AI.
 As the functionality is diverse I don't even try to explain. The example projects will hopefully demonstrate it.
+
+## Quick Start
+
+```
+git clone [email protected]:MarcToussaint/rai.git
+# OR, if you don't have a github account:
+git clone https://github.com/MarcToussaint/rai.git
+cd rai
+
+# OPTIONAL! Often you will only need to compile some components and don't need all Ubuntu packages
+make -j1 printUbuntu  # to just print Ubuntu package dependencies per component
+make -j1 initUbuntuPackages  # calls sudo apt-get install; you can always interrupt
+
+cp build/config.mk.default build/config.mk    # edit this to modify dependencies or debug mode
+make runTests      # compile and run the essential tests
+make bin           # compile rai/bin/kinEdit and similar
+make               # to compile all components (rarely necessary)
+```
+
+If interested in the kinematics part, check `rai/Kin/kin.h` and `test/Kin/*` to get started.

build/generic.mk

@@ -337,6 +337,9 @@ inPath_makeLib/Hardware_%: $(BASE)/rai/Hardware/% $(PREOBJS)
 inPath_makeLib/%: $(BASE)/rai/% $(PREOBJS)
 	+@-$(BASE)/build/make-path.sh $< lib$*.so
 
+inPath_makeLib/%: $(BASE)/../src/% $(PREOBJS)
+	+@-$(BASE)/build/make-path.sh $< lib$*.so
+
 inPath_make/%: % $(PREOBJS)
 	+@-$(BASE)/build/make-path.sh $< x.exe
 

rai/Core/array.h

@@ -158,6 +158,7 @@ template<class T> struct Array : std::vector<T> {
   void referTo(const Array<T>& a);
   void referToRange(const Array<T>& a, int i, int I); // -> referTo(a,{i,I})
   void referToRange(const Array<T>& a, int i, int j, int J); // -> referTo(a,{i,I})
+  void referToRange(const Array<T>& a, int i, int j, int k, int K); // -> referTo(a,{i,I})
   void referToDim(const Array<T>& a, int i); // -> referTo
   void referToDim(const Array<T>& a, uint i, uint j);
   void referToDim(const Array<T>& a, uint i, uint j, uint k);
@@ -239,7 +240,7 @@ template<class T> struct Array : std::vector<T> {
   void delRows(uint i, uint k=1);
   void delColumns(int i, uint k=1);
   void insRows(int i, uint k=1);
-  void insColumns(uint i, uint k=1);
+  void insColumns(int i, uint k=1);
   void resizeDim(uint k, uint dk);
   void setAppend(const T& x); //? same as if(findValue(x)==-1) append(x)
   void setAppend(const Array<T>& x);

rai/Core/array.tpp

@@ -696,11 +696,12 @@ template<class T> void rai::Array<T>::delColumns(int i, uint k) {
 }
 
 /// inserts k columns at the i-th column [must be 2D]
-template<class T> void rai::Array<T>::insColumns(uint i, uint k) {
+template<class T> void rai::Array<T>::insColumns(int i, uint k) {
   CHECK(memMove, "only with memMove");
-  CHECK(k>0, "");
   CHECK_EQ(nd,2, "only for matricies");
-  CHECK(i<=d1, "range check error");
+  if(!k) return;
+  if(i<0) i+=d1;
+  CHECK(i<=(int)d1, "range check error");
   uint n=d1;
   resizeCopy(d0, n+k);
   for(uint j=d0; j--;) {
@@ -859,7 +860,7 @@ template<class T> rai::Array<T> rai::Array<T>::operator()(int i, std::pair<int,
 /// range reference access
 template<class T> rai::Array<T> rai::Array<T>::operator()(int i, int j, std::initializer_list<int> K) const {
   rai::Array<T> z;
-  if(K.size()==2){ NIY; }
+  if(K.size()==2) z.referToRange(*this, i, j, K.begin()[0], K.begin()[1]);
   else if(K.size()==0) z.referToDim(*this, i, j);
   else if(K.size()==1) z.referToDim(*this, i, j, K.begin()[0]);
   else HALT("range list needs 0,1, or 2 entries exactly");
@@ -1506,6 +1507,30 @@ template<class T> void rai::Array<T>::referToRange(const Array<T>& a, int i, int
   vec_type::_M_impl._M_end_of_storage = p+N;
 }
 
+/// make this array a subarray reference to \c a
+template<class T> void rai::Array<T>::referToRange(const Array<T>& a, int i, int j, int k, int K) {
+  CHECK(a.nd>2, "does not make sense");
+  CHECK(a.nd<=3, "not implemented yet");
+  freeMEM();
+  resetD();
+  reference=true; memMove=a.memMove;
+  if(i<0) i+=a.d0;
+  if(j<0) j+=a.d1;
+  if(k<0) k+=a.d2;
+  if(K<0) K+=a.d2;
+  if(k>K) return;
+  CHECK((uint)i<a.d0, "SubRange range error (" <<i <<"<" <<a.d0 <<")");
+  CHECK((uint)j<a.d1, "SubRange range error (" <<j <<"<" <<a.d1 <<")");
+  CHECK((uint)k<a.d2 && (uint)K<a.d2, "SubRange range error (" <<k <<"<" <<a.d2 <<", " <<K <<"<" <<a.d2 <<")");
+  if(a.nd==3) {
+    nd=1;  d0=K+1-k; d1=0; d2=0;  N=d0;
+    p = &a(i,j,k);
+  }
+  vec_type::_M_impl._M_start = p;
+  vec_type::_M_impl._M_finish = p+N;
+  vec_type::_M_impl._M_end_of_storage = p+N;
+}
+
 /// make this array a subarray reference to \c a
 template<class T> void rai::Array<T>::referToDim(const rai::Array<T>& a, int i) {
   CHECK(a.nd>1, "can't create subarray of array less than 2 dimensions");

rai/Geo/geo.cpp

@@ -12,7 +12,7 @@
 
 #ifndef RAI_NO_REGISTRY
 #include <Core/graph.h>
-REGISTER_TYPE(T, rai::Transformation);
+REGISTER_TYPE(T, rai::Transformation)
 #endif
 
 #ifdef RAI_GL
@@ -94,7 +94,8 @@ double Vector::diffZero() const { return fabs(x)+fabs(y)+fabs(z); }
 /// check whether isZero is true
 void Vector::checkZero() const {
   bool iszero = (x==0. && y==0. && z==0.);
-  CHECK_EQ(iszero, isZero, "you must have set this by hand!");
+  if(isZero) CHECK(iszero, "you must have set this by hand!");
+//  CHECK_EQ(iszero, isZero, "you must have set this by hand!");
 }
 
 /// is it normalized?
@@ -745,15 +746,15 @@ double Quaternion::sqrDiffZero() const { return (w>0.?rai::sqr(w-1.):rai::sqr(w+
 /// check whether isZero is true
 void Quaternion::checkZero() const {
   bool iszero = ((w==1. || w==-1.) && x==0. && y==0. && z==0.);
-  CHECK_EQ(iszero, isZero, "you must have set this by hand!");
+  if(isZero) CHECK(iszero, "you must have set this by hand!");
 }
 
 /// return the squared-error between two quads, modulo flipping
 double Quaternion::sqrDiff(const Quaternion& _q2) const{
   arr q1(&w, 4, true);
   arr q2(&_q2.w, 4, true);
-  if(quatScalarProduct(q1,q2)>=0) return sqrDistance(q1, q2);
-  return sqrDistance(-q1,q2);
+  if(scalarProduct(q1,q2)>=0) return sumOfSqr(q1-q2);
+  return sumOfSqr(q1+q2);
 }
 
 /// gets rotation angle (in rad [0, 2pi])
@@ -1556,23 +1557,23 @@ void Camera::setZero() {
   X.setZero();
   foc.setZero();
   heightAngle=90.;
-  heightAbs=10.;
-  focalLength=1.;
+  heightAbs=0.;
+  focalLength=0.;
   whRatio=1.;
   zNear=.1;
   zFar=1000.;
 }
 
 /// the height angle (in degrees) of the camera perspective; set it 0 for orthogonal projection
-void Camera::setHeightAngle(float a) { heightAngle=a; heightAbs=0.;}
+void Camera::setHeightAngle(float a) { focalLength=heightAbs=0.; heightAngle=a; }
 /// the absolute height of the camera perspective (automatically also sets heightAngle=0)
-void Camera::setHeightAbs(float h) { heightAngle=0.; heightAbs=h; }
+void Camera::setHeightAbs(float h) { focalLength=heightAngle=0.; heightAbs=h; }
 /// the z-range (depth range) visible for the camera
 void Camera::setZRange(float znear, float zfar) { zNear=znear; zFar=zfar; }
 /// set the width/height ratio of your viewport to see a non-distorted picture
 void Camera::setWHRatio(float ratio) { whRatio=ratio; }
 /// set the width/height ratio of your viewport to see a non-distorted picture
-void Camera::setFocalLength(float f) { heightAbs=heightAngle = 0;  focalLength = f; }
+void Camera::setFocalLength(float f) { heightAbs=heightAngle=0;  focalLength = f; }
 /// the frame's position
 void Camera::setPosition(float x, float y, float z) { X.pos.set(x, y, z); }
 /// rotate the frame to focus the absolute coordinate origin (0, 0, 0)
@@ -1640,34 +1641,43 @@ void Camera::glSetProjectionMatrix() {
 //  if(fixedProjectionMatrix.N) {
 //    glLoadMatrixd(fixedProjectionMatrix.p);
 //  } else {
-  if(heightAngle==0) {
-    if(heightAbs==0) {
-      arr P(4,4);
-      P.setZero();
-      P(0,0) = 2.*focalLength/whRatio;
-      P(1,1) = 2.*focalLength;
-      P(2,2) = (zFar + zNear)/(zNear-zFar);
-      P(2,3) = -1.;
-      P(3,2) = 2. * zFar * zNear / (zNear-zFar);
-      glLoadMatrixd(P.p);
-    }else{
-      glOrtho(-whRatio*heightAbs/2, whRatio*heightAbs/2,
-              -heightAbs/2, heightAbs/2, zNear, zFar);
-    }
-  } else
+  if(focalLength > 0.) { //focal lengh mode
+    CHECK(!heightAngle, "");
+    CHECK(!heightAbs, "");
+    arr P(4,4);
+    P.setZero();
+    P(0,0) = 2.*focalLength/whRatio;
+    P(1,1) = 2.*focalLength;
+    P(2,2) = (zFar + zNear)/(zNear-zFar);
+    P(2,3) = -1.;
+    P(3,2) = 2. * zFar * zNear / (zNear-zFar);
+    glLoadMatrixd(P.p);
+  }
+  if(heightAbs > 0.) { //ortho mode
+    CHECK(!heightAngle, "");
+    CHECK(!focalLength, "");
+    glOrtho(-whRatio*heightAbs/2., whRatio*heightAbs/2.,
+            -heightAbs/2., heightAbs/2., zNear, zFar);
+  }
+  if(heightAngle > 0.) { //normal perspective mode
+    CHECK(!focalLength, "");
+    CHECK(!heightAbs, "");
     gluPerspective(heightAngle, whRatio, zNear, zFar);
+  }
   double m[16];
   glMultMatrixd(X.getInverseAffineMatrixGL(m));
 #endif
 }
 
 /// convert from gluPerspective's non-linear [0, 1] depth to the true [zNear, zFar] depth
 double Camera::glConvertToTrueDepth(double d) {
+  CHECK(!heightAbs, "I think this is wrong for ortho view");
   return zNear + (zFar-zNear)*d/(zFar/zNear*(1.-d)+1.);
 }
 
 /// convert from gluPerspective's non-linear [0, 1] depth to the linear [0, 1] depth
 double Camera::glConvertToLinearDepth(double d) {
+  CHECK(!heightAbs, "I think this is wrong for ortho view");
   return d/(zFar/zNear*(1.-d)+1.);
 }
 
@@ -1677,8 +1687,7 @@ void Camera::setKinect(){
   focus(0., 0., 5.);
   setZRange(.1, 50.);
 #if 1
-  heightAbs=heightAngle = 0;
-  focalLength = 580./480.;
+  setFocalLength(580./480.);
 #else
   heightAngle=45;
 #endif

rai/Gui/opengl.cpp

@@ -196,8 +196,10 @@ void OpenGL::postRedrawEvent(bool fromWithinCallback) {
 }
 
 void OpenGL::resize(int w,int h) {
-  openWindow();
-  {
+//  openWindow();
+  if(s->windowID==-1){
+    Reshape(w, h);
+  }else{
     auto fg=singleFreeglut();
     s->accessWindow();
     glutReshapeWindow(w,h);
@@ -641,6 +643,70 @@ void glDrawAxes(double scale) {
 }
 
 
+void glDrawCamera(const rai::Camera &cam){
+  glTransform(cam.X);
+
+  glDrawAxes(.1);
+
+  double dxFar,dyFar,zFar;
+  double dxNear,dyNear,zNear;
+  zNear = cam.zNear;
+  zFar = cam.zFar;
+  if(zFar-zNear > 10.) zFar = zNear + .1;
+  if(cam.focalLength){
+    dyNear = zNear * .5/cam.focalLength;
+    dyFar = zFar * .5/cam.focalLength;
+    dxNear = cam.whRatio * dyNear;
+    dxFar = cam.whRatio * dyFar;
+  }
+  if(cam.heightAngle){
+//    zFar = zNear + .1*(zFar-zNear);
+    dyNear = zNear * ::sin(.5*cam.heightAngle/180.*RAI_PI);
+    dyFar = zFar * ::sin(.5*cam.heightAngle/180.*RAI_PI);
+    dxNear = cam.whRatio * dyNear;
+    dxFar = cam.whRatio * dyFar;
+  }
+  if(cam.heightAbs){
+    dyFar = dyNear = cam.heightAbs/2.;
+    dxFar = dxNear = cam.whRatio * dyNear;
+  }
+  glColor(.5,.5,.5);
+  glBegin(GL_LINE_STRIP);
+  glVertex3f(-dxNear, -dyNear, -zNear);
+  glVertex3f(-dxNear, dyNear, -zNear);
+  glVertex3f(dxNear, dyNear, -zNear);
+  glVertex3f(dxNear, -dyNear, -zNear);
+  glVertex3f(-dxNear, -dyNear, -zNear);
+  glEnd();
+  glBegin(GL_LINE_STRIP);
+  glVertex3f(-dxFar, -dyFar, -zFar);
+  glVertex3f(-dxFar, dyFar, -zFar);
+  glVertex3f(dxFar, dyFar, -zFar);
+  glVertex3f(dxFar, -dyFar, -zFar);
+  glVertex3f(-dxFar, -dyFar, -zFar);
+  glEnd();
+  glBegin(GL_LINES);
+  glVertex3f(0,0,0);
+  glVertex3f(-dxNear, -dyNear, -zNear);
+  glVertex3f(0,0,0);
+  glVertex3f(-dxNear, dyNear, -zNear);
+  glVertex3f(0,0,0);
+  glVertex3f(dxNear, -dyNear, -zNear);
+  glVertex3f(0,0,0);
+  glVertex3f(dxNear, dyNear, -zNear);
+  glEnd();
+  glBegin(GL_LINES);
+  glVertex3f(-dxNear, -dyNear, -zNear);
+  glVertex3f(-dxFar,  -dyFar,  -zFar);
+  glVertex3f(-dxNear, dyNear, -zNear);
+  glVertex3f(-dxFar,  dyFar,  -zFar);
+  glVertex3f(dxNear, -dyNear, -zNear);
+  glVertex3f(dxFar,  -dyFar,  -zFar);
+  glVertex3f(dxNear, dyNear, -zNear);
+  glVertex3f(dxFar,  dyFar,  -zFar);
+  glEnd();
+}
+
 void glDrawDisk(float radius) {
   GLUquadric *style=gluNewQuadric();
   gluDisk(style, 0, radius, 10, 1);
@@ -2045,6 +2111,8 @@ void OpenGL::renderInBack(bool _captureImg, bool _captureDep, int w, int h){
   if(w<0) w=width;
   if(h<0) h=height;
 
+  singleFreeglut(); //ensure that glut is initialized (if the drawer called glut)
+
   auto mut=singleGLAccess();
   dataLock.readLock();
   xBackgroundContext()->makeCurrent();

rai/Gui/opengl.h

@@ -64,7 +64,7 @@ void glColor(int col);
 void glColor(const arr& col);
 void id2color(byte rgb[3], uint id);
 uint color2id(byte rgb[3]);
-void glDrawText(const char* txt, float x, float y, float z, bool largeFont=false);
+void glDrawText(const char* txt, float x=0., float y=0., float z=0., bool largeFont=false);
 //void glShadowTransform();
 void glTransform(const rai::Transformation& t);
 void glTransform(const double pos[3], const double R[12]);
@@ -87,6 +87,7 @@ void glDrawCylinder(float radius, float length, bool closed=true);
 void glDrawCappedCylinder(float radius, float length);
 void glDrawAxis(double scale=-1.);
 void glDrawAxes(double scale);
+void glDrawCamera(const rai::Camera& cam);
 void glDrawGridBox(float x);
 void glDrawGridBox(float x1, float y1, float z1, float x2, float y2, float z2);
 void glDrawKhepera();

rai/KOMO/komo.cpp

@@ -966,7 +966,12 @@ void KOMO::setConfigFromFile(){
 }
 
 void KOMO::setIKOpt(){
-  setTiming(1, 1);
+  maxPhase = 1.;
+  stepsPerPhase = 1;
+  T = 1;
+  tau = 1.;
+  k_order = 1;
+//  setTiming(1, 1);
   setFixEffectiveJoints();
   setFixSwitchedObjects();
   setSquaredQVelocities();