Skip to content

Commit

Permalink
TM_gravity more robust
Browse files Browse the repository at this point in the history
  • Loading branch information
Marc Toussaint committed Mar 26, 2018
1 parent b496c82 commit db13277
Show file tree
Hide file tree
Showing 5 changed files with 106 additions and 22 deletions.
2 changes: 2 additions & 0 deletions rai/KOMO/Makefile
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,6 +4,8 @@ OUTPUT = lib$(NAME).so

DEPEND = Core Geo Kin Optim Algo

GL = 1

SRCS = $(shell find . -maxdepth 1 -name '*.cpp' )
OBJS = $(SRCS:%.cpp=%.o)

Expand Down
61 changes: 60 additions & 1 deletion rai/KOMO/komo.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,6 +29,10 @@
#include <Optim/convert.h>
#include <Kin/kin_physx.h>

#ifdef RAI_GL
# include <GL/gl.h>
#endif

using namespace rai;

//===========================================================================
Expand Down Expand Up @@ -1251,18 +1255,43 @@ void KOMO::plotTrajectory(){
gnuplot("load 'z.trajectories.plt'");
}

struct DrawPaths : GLDrawer{
arr& X;
DrawPaths(arr& X): X(X){}
void glDraw(OpenGL& gl){
glColor(0.,0.,0.);
for(uint i=0;i<X.d0;i++){
glBegin(GL_LINES);
for(uint t=0;t<X.d1;t++){
rai::Transformation pose;
pose.set(&X(i,t,0));
// glTransform(pose);
glVertex3d(pose.pos.x, pose.pos.y, pose.pos.z);
}
glEnd();
}
}
};

bool KOMO::displayTrajectory(double delay, bool watch, const char* saveVideoPrefix){
const char* tag = "KOMO planned trajectory";
if(!gl){
gl = new OpenGL ("KOMO display");
gl->camera.setDefault();
}

uintA allFrames;
allFrames.setStraightPerm(configurations.last()->frames.N);
arr X = getPath_frames(allFrames);
DrawPaths drawX(X);


for(int t=-(int)k_order; t<(int)T; t++) {
if(saveVideoPrefix) gl->captureImg=true;
gl->clear();
gl->add(glStandardScene, 0);
gl->addDrawer(configurations(t+k_order));
gl->add(drawX);
if(delay<0.){
if(delay<-10.) FILE("z.graph") <<*configurations(t+k_order);
gl->watch(STRING(tag <<" (time " <<std::setw(3) <<t <<'/' <<T <<')').p);
Expand All @@ -1276,6 +1305,33 @@ bool KOMO::displayTrajectory(double delay, bool watch, const char* saveVideoPref
int key = gl->watch(STRING(tag <<" (time " <<std::setw(3) <<T-1 <<'/' <<T <<')').p);
return !(key==27 || key=='q');
}
gl->clear();
return false;
}

bool KOMO::displayPath(bool watch){
uintA allFrames;
allFrames.setStraightPerm(configurations.last()->frames.N);
arr X = getPath_frames(allFrames);
CHECK_EQ(X.nd, 3, "");
CHECK_EQ(X.d2, 7, "");

DrawPaths drawX(X);

if(!gl){
gl = new OpenGL ("KOMO display");
gl->camera.setDefault();
}
gl->clear();
gl->add(glStandardScene, 0);
gl->addDrawer(configurations.last());
gl->add(drawX);
if(watch){
int key = gl->watch();
return !(key==27 || key=='q');
}
gl->update(NULL, false, false, true);
gl->clear();
return false;
}

Expand Down Expand Up @@ -1356,7 +1412,7 @@ void KOMO::set_x(const arr& x){
else configurations(s)->setJointState(x[t]);
if(useSwift){
configurations(s)->stepSwift();
// configurations(s)->proxiesToContacts(1.1);
configurations(s)->proxiesToContacts(1.1);
}
x_count += x_dim;
}
Expand Down Expand Up @@ -1646,6 +1702,9 @@ void KOMO::Conv_MotionProblem_KOMO_Problem::phi(arr& phi, arrA& J, arrA& H, uint
//-- set the trajectory
komo.set_x(x);

if(komo.animateOptimization)
komo.displayPath(false);

CHECK(dimPhi,"getStructure must be called first");
// getStructure(NoUintA, featureTimes, tt);
// if(WARN_FIRST_TIME){ LOG(-1)<<"calling inefficient getStructure"; WARN_FIRST_TIME=false; }
Expand Down
4 changes: 3 additions & 1 deletion rai/KOMO/komo.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,9 @@ struct KOMO{
arrA featureValues; ///< storage of all features in all time slices
rai::Array<ObjectiveTypeA> featureTypes; ///< storage of all feature-types in all time slices
arr dualSolution; ///< the dual solution computed during constrained optimization
struct OpenGL *gl; ///< internal only: used in 'displayTrajectory'
struct OpenGL *gl=NULL; ///< internal only: used in 'displayTrajectory'
int verbose; ///< verbosity level
bool animateOptimization=false; ///< display the current path for each evaluation during optimization
double runTime=0.; ///< just measure run time
ofstream *fil=NULL;

Expand Down Expand Up @@ -183,6 +184,7 @@ struct KOMO{
void checkGradients(); ///< checks all gradients numerically
void plotTrajectory();
bool displayTrajectory(double delay=0.01, bool watch=true, const char* saveVideoPrefix=NULL); ///< display the trajectory; use "vid/z." as vid prefix
bool displayPath(bool watch=true); ///< display the trajectory; use "vid/z." as vid prefix
rai::Camera& displayCamera(); ///< access to the display camera to change the view
PhysXInterface& physx(){ return world.physx(); }

Expand Down
58 changes: 39 additions & 19 deletions rai/Kin/TM_gravity.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,13 +6,18 @@
#include <Kin/TM_PairCollision.h>

void shapeFunction(double &x, double &dx){
if(x>0.){ x=0.; dx=0.; return; }
if(x>1. || x<-1.){ x=1.; dx=0.; return; }
double x2=x*x;
dx = 1.5*(1.-x2);
x = 0.5*(3.*x-x2*x);
}


TM_Gravity::TM_Gravity(){
gravity = rai::getParameter<double>("TM_Gravity/gravity", 9.81);
}

void TM_Gravity::phi(arr &y, arr &J, const WorldL &Ktuple){

y.clear();
Expand All @@ -37,7 +42,7 @@ void TM_Gravity::phi(arr &y, arr &J, const WorldL &Ktuple){
pos.order=1;
pos.TaskMap::phi(p0, (&J?J0:NoArr), Ktuple);

arr v_ref = {0.,0.,-.1};
arr v_ref = {0.,0.,-gravity};
arr Jv_ref = zeros(3, K.q.N);
#if 0
if(false && a->contacts.N){
Expand Down Expand Up @@ -96,10 +101,10 @@ void TM_Gravity::phi(arr &y, arr &J, const WorldL &Ktuple){
}

if(order==2){
rai::KinematicWorld& K = *Ktuple(-1);
rai::KinematicWorld& K = *Ktuple(-2);

arr acc, Jacc;
arr acc_ref = {0.,0.,-9.81};
arr acc_ref = {0.,0.,-gravity};
arr Jacc_ref = zeros(3, K.q.N);
for(rai::Frame *a:K.frames){
if(a->flags & (1<<FL_gravityAcc)){
Expand All @@ -124,10 +129,18 @@ void TM_Gravity::phi(arr &y, arr &J, const WorldL &Ktuple){

arr d, Jd;
TM_PairCollision dist(con->a.ID, con->b.ID, true, false);
dist.phi(d, (&J?Jd:NoArr), K);
if(&J) expandJacobian(Jd, Ktuple, -1);
d *= 1e0;
if(&J) Jd *= 1e0;
dist.phi(d, (&J?Jd:NoArr), *Ktuple(-2));
if(&J) expandJacobian(Jd, Ktuple, -2);
d *= 1.;
if(&J) Jd *= 1.;

// arr d2, Jd2;
// TM_PairCollision dist2(con->a.ID, con->b.ID, true, false);
// dist2.phi(d2, (&J?Jd2:NoArr), *Ktuple(-1));
// if(&J) expandJacobian(Jd2, Ktuple, -1);
// d += d2;
// if(&J) Jd += Jd2;

// d.scalar() = tanh(d.scalar());
// if(&J) Jd *= (1.-d.scalar()*d.scalar());
// double dd;
Expand All @@ -137,21 +150,28 @@ void TM_Gravity::phi(arr &y, arr &J, const WorldL &Ktuple){
arr c, Jc;
TM_PairCollision coll(con->a.ID, con->b.ID, false, true);
coll.phi(c, (&J?Jc:NoArr), K);
if(length(c)<1e-6) continue;
normalizeWithJac(c, Jc);
if(&J) expandJacobian(Jc, Ktuple, -1);
if(&J) expandJacobian(Jc, Ktuple, -2);

double sign = scalarProduct(c,err);

// cout <<"time " <<t <<" frame " <<a->name <<" norm=" <<c <<" dist=" <<d <<endl;
#if 0
if(&J) J -= ( c*~c*J + c*~y*Jc + scalarProduct(c,y)*Jc );
y -= c*scalarProduct(c,y);
#elif 1
if(&J) J -= (1.-d.scalar())*( c*~c*J + c*~y*Jc + scalarProduct(c,y)*Jc ) - c*scalarProduct(c,y)*Jd;
y -= (1.-d.scalar())*c*scalarProduct(c,y);
if(sign<0.){
if(&J) J -= (1.-d.scalar())*( c*~c*J + c*~y*Jc + scalarProduct(c,y)*Jc ) - c*scalarProduct(c,y)*Jd;
y -= (1.-d.scalar())*c*scalarProduct(c,y);
}
#else
double dfactor=exp(-0.5*d.scalar()*d.scalar()/.01);
double ddfactor = dfactor * (-d.scalar()/.01);
if(&J) J -= dfactor*( c*~c*J + c*~y*Jc + scalarProduct(c,y)*Jc ) + ddfactor*c*scalarProduct(c,y)*Jd;
y -= dfactor*c*scalarProduct(c,y);
if(sign<0.){
double dfactor=exp(-0.5*d.scalar()*d.scalar()/.01);
double ddfactor = dfactor * (-d.scalar()/.01);
if(&J) J -= dfactor*( c*~c*J + c*~y*Jc + scalarProduct(c,y)*Jc ) + ddfactor*c*scalarProduct(c,y)*Jd;
y -= dfactor*c*scalarProduct(c,y);
}
#endif

#if 0
Expand All @@ -167,13 +187,13 @@ void TM_Gravity::phi(arr &y, arr &J, const WorldL &Ktuple){
// }
// y += c*d.scalar()*scalarProduct(c,err);

y.append(0.);
if(&J) J.append(zeros(1, J.d1));
// y.append(0.);
// if(&J) J.append(zeros(1, J.d1));
#endif
}
}else{
y.append(0.);
if(&J) J.append(zeros(1, J.d1));
// y.append(0.);
// if(&J) J.append(zeros(1, J.d1));
}
}
}
Expand All @@ -187,7 +207,7 @@ uint TM_Gravity::dim_phi(const WorldL &Ktuple){
rai::KinematicWorld& K = *Ktuple(-1);
uint d = 0;
for(rai::Frame *a: K.frames) if(a->flags & (1<<FL_gravityAcc)){
d+=4;
d+=3;
}
return d;
}
3 changes: 2 additions & 1 deletion rai/Kin/TM_gravity.h
View file
Original file line number Diff line number Diff line change
@@ -1,8 +1,9 @@
#include "taskMap.h"

struct TM_Gravity : TaskMap {
double gravity=9.81;

TM_Gravity(){}
TM_Gravity();

virtual void phi(arr& y, arr& J, const rai::KinematicWorld& K){ HALT("can only be of higher order"); }
virtual uint dim_phi(const rai::KinematicWorld& K){ HALT("can only be of higher order"); }
Expand Down

0 comments on commit db13277

Please sign in to comment.