From 2fddb5aec0667971fa4a72bd59d080d2966e3c28 Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Wed, 7 Nov 2018 14:40:54 +0000 Subject: [PATCH 01/10] Humanoid from `Simulations.zip` from `http://robot.informatik.uni-leipzig.de/research/supplementary/TiPI2013/` with `pimax.cpp` being replaced by the `lpzrobots` version. --- ode_robots/simulations/humanoid-TiPI/Makefile | 88 ++++ .../simulations/humanoid-TiPI/Makefile.conf | 6 + .../simulations/humanoid-TiPI/environment.h | 323 ++++++++++++ ode_robots/simulations/humanoid-TiPI/main.cpp | 496 ++++++++++++++++++ 4 files changed, 913 insertions(+) create mode 100644 ode_robots/simulations/humanoid-TiPI/Makefile create mode 100644 ode_robots/simulations/humanoid-TiPI/Makefile.conf create mode 100644 ode_robots/simulations/humanoid-TiPI/environment.h create mode 100644 ode_robots/simulations/humanoid-TiPI/main.cpp diff --git a/ode_robots/simulations/humanoid-TiPI/Makefile b/ode_robots/simulations/humanoid-TiPI/Makefile new file mode 100644 index 00000000..f575d101 --- /dev/null +++ b/ode_robots/simulations/humanoid-TiPI/Makefile @@ -0,0 +1,88 @@ +# -*- mode: makefile; -*- +# Do not edit, this is file generated by m4 from Makefile.4sim.m4 and +# copied to the simulations while installation! +# Make your changes in Makefile.conf. +# E.g. add there files to compile or custom libs and so on. + +# File: Makefile for lpzrobots simulations +# Author: Georg Martius +# Date: Oct 2009 + + + + + + + + + +EXEC = start +# add files to compile in the conf file +include Makefile.conf + +CFILES = $(addsuffix .cpp, $(FILES)) +OFILES = $(addsuffix .o, $(FILES)) + +# the CFGOPTS are set by the opt and dbg target +CFGOPTS= +INC += -I. +BASELIBS = $(shell ode_robots-config $(CFGOPTS) --static --libs) $(shell selforg-config $(CFGOPTS) --static --libs) +BASELIBSSHARED := $(shell ode_robots-config $(CFGOPTS) --libs) $(shell selforg-config $(CFGOPTS) --libs) + + + + + + + + + + + + + +LIBS += $(BASELIBS) $(ADDITIONAL_LIBS) + +INC += -I. + +CXX = g++ +CPPFLAGS = -Wall -pipe -Wno-deprecated $(INC) $(shell selforg-config $(CFGOPTS) --cflags) \ + $(shell ode_robots-config $(CFGOPTS) --intern --cflags) + +normal: + $(MAKE) $(EXEC) +opt: + $(MAKE) CFGOPTS=--opt EXEC=$(EXEC)_opt $(EXEC)_opt +dbg: + $(MAKE) CFGOPTS=--dbg EXEC=$(EXEC)_dbg $(EXEC)_dbg +shared: + $(MAKE) BASELIBS="$(BASELIBSSHARED)" LIBSELFORG="$(LIBSELFORGSHARED)" \ + LIBODEROBOTS="$(LIBODEROBOTSSHARED)" $(EXEC) + +$(EXEC): Makefile Makefile.depend $(OFILES) + $(CXX) $(CPPFLAGS) $(OFILES) $(LIBS) -o $(EXEC) + + + +Makefile.depend: + makedepend -- $(CPPFLAGS) -- $(CFILES) -f- > Makefile.depend 2>/dev/null + +depend: + makedepend -- $(CPPFLAGS) -- $(CFILES) -f- > Makefile.depend 2>/dev/null + +check-syntax: + $(CXX) $(CPPFLAGS) -Wextra -S -fsyntax-only $(CHK_SOURCES) + cppcheck --std=c++11 --std=posix --enable=performance,information,portability --suppress=incorrectStringBooleanError --suppress=invalidscanf --quiet --template={file}:{line}:{severity}:{message} $(CHK_SOURCES) + +todo: + find -name "*.[ch]*" -exec grep -Hni "TODO" {} \; + +tags: + etags $(find -name "*.[ch]") + + + +clean: + rm -f $(EXEC) $(EXEC)_dbg $(EXEC)_opt *.o Makefile.depend + +-include Makefile.depend diff --git a/ode_robots/simulations/humanoid-TiPI/Makefile.conf b/ode_robots/simulations/humanoid-TiPI/Makefile.conf new file mode 100644 index 00000000..a559ef03 --- /dev/null +++ b/ode_robots/simulations/humanoid-TiPI/Makefile.conf @@ -0,0 +1,6 @@ +# Configuration for simulation makefile +# Please add all cpp files you want to compile for this simulation +# to the FILES variable +# You can also tell where you haved lpzrobots installed + +FILES = main diff --git a/ode_robots/simulations/humanoid-TiPI/environment.h b/ode_robots/simulations/humanoid-TiPI/environment.h new file mode 100644 index 00000000..3a578cac --- /dev/null +++ b/ode_robots/simulations/humanoid-TiPI/environment.h @@ -0,0 +1,323 @@ +/*************************************************************************** + * Copyright (C) 2005 by Robot Group Leipzig * + * martius@informatik.uni-leipzig.de * + * fhesse@informatik.uni-leipzig.de * + * der@informatik.uni-leipzig.de * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + * * + ***************************************************************************/ + +#ifndef __ENVIRONMENT_H +#define __ENVIRONMENT_H + +#include +#include + +#include +#include +#include +#include +#include + +using namespace lpzrobots; +using namespace std; + + +class Env : public Configurable { +public: + enum EnvType { None, Normal, Octa, Pit, Pit2, OpenPit, Uterus, Stacked }; + + Env(EnvType t = None) : Configurable("Environment","1.0") { + type = t; + widthground = 25.85; + heightInner = 2; + pitsize = 1; + pitPosition = Pos(0,0,0); + height = 0.8; + thickness = 0.2; + uterussize = 1; + roughness = 2; + hardness = 10; + numgrounds = 2; + distance = 2; + useColorSchema = false; + + + numSpheres = 0; + numBoxes = 0; + numCapsules = 0; + numSeeSaws = 0; + numBoxPiles = 0; + + if(t==Octa || t==Pit || t==None){ + addParameter("pitheight", &heightInner, 0, 10, "height of circular pit"); + addParameter("pitsize", &pitsize, 0, 10, "size of the pit (diameter)"); + } + addParameter("height", &height, 0, 10, "height of square walls"); + addParameter("roughness", &roughness, 0, 10, + "roughness of ground (friction parameters) (and walls in pit)"); + addParameter("hardness", &hardness, 0, 50, + "hardness of ground (friction parameters) (and walls in pit)"); + if(t==Stacked){ + addParameter("numgrounds", &numgrounds, 0, 10, + "number of stacked rectangular playgrounds"); + addParameter("distance", &distance, 0.2, 10, + "distance between stacked rectangular playgrounds"); + addParameter("heightincrease", &heightincrease, 0., 1, + "height increase between subsequent stacked rectangular playgrounds"); + } + if(t==Normal || t==Octa || t==Stacked || t==None){ + addParameter("size", &widthground, 0.2, 20, + "size of rectangular playgrounds"); + } + } + + EnvType type; + std::list playgrounds; + + // playground parameter + double widthground; + double heightground; + double heightInner; + double pitsize; + double height; + double thickness; + double uterussize; + double roughness; + double hardness; + + Pos pitPosition; + Pos pit2Position; + + int numgrounds; + double distance; + double heightincrease; + + bool useColorSchema; // playgrounds are black and white and get color from schema + + // obstacles + int numSpheres; + int numBoxes; + int numCapsules; + int numSeeSaws; + int numBoxPiles; + + OdeHandle odeHandle; + OsgHandle osgHandle; + GlobalData* global ; + + + /** creates the Environment + */ + void create(const OdeHandle& odeHandle, const OsgHandle& osgHandle, + GlobalData& global, bool recreate=false){ + this->odeHandle=odeHandle; + this->osgHandle=osgHandle; + this->global=&global; + if(recreate && !playgrounds.empty()){ + FOREACH(std::list, playgrounds, p){ + removeElement(global.obstacles, *p); + delete (*p); + } + playgrounds.clear(); + } + AbstractGround* playground; + switch (type){ + case Octa: + case Normal: + { + playground = new Playground(odeHandle, osgHandle, + osg::Vec3(widthground, 0.208, height)); + // playground->setTexture("Images/really_white.rgb"); + // playground->setGroundTexture("Images/yellow_velour.rgb"); + if(useColorSchema) + playground->setTexture(0,0,TextureDescr("Images/wall_bw.jpg",-1.5,-3)); // was: wall.rgb + playground->setGroundThickness(0.2); + playground->setPosition(osg::Vec3(0,0,.0)); + Substance substance(roughness, 0.0, hardness, 0.95); + playground->setGroundSubstance(substance); + global.obstacles.push_back(playground); + playgrounds.push_back(playground); + } + if (type==Octa) { // this playground comes second for dropping obstacles + playground = new OctaPlayground(odeHandle, osgHandle, + osg::Vec3(pitsize, thickness, heightInner), 12, false); + playground->setTexture("Images/really_white.rgb"); + Color c = osgHandle.getColor("Monaco"); + c.alpha()=0.15; + playground->setColor(c); + playground->setPosition(pitPosition); // playground positionieren und generieren + global.obstacles.push_back(playground); + playgrounds.push_back(playground); + } + break; + case Pit2: + case Pit: + { + Substance soft = Substance::getPlastic(2); + soft.roughness = roughness; + soft.hardness = hardness; + OdeHandle myHandle = odeHandle; + myHandle.substance = soft; + playground = new Playground(myHandle, osgHandle, + osg::Vec3(pitsize, thickness, height), + 1, true); + playground->setTexture("Images/really_white.rgb"); + playground->setGroundSubstance(soft); + + Color c = osgHandle.getColor("Monaco"); + c.alpha()=0.15; + playground->setColor(c); + playground->setPosition(pitPosition); // playground positionieren und generieren + global.obstacles.push_back(playground); + playgrounds.push_back(playground); + } + if(type==Pit2){ + Substance soft = Substance::getPlastic(2); + soft.roughness = roughness; + soft.hardness = hardness; + OdeHandle myHandle = odeHandle; + myHandle.substance = soft; + playground = new Playground(myHandle, osgHandle, + osg::Vec3(pitsize, thickness, height), + 1, true); + playground->setTexture("Images/really_white.rgb"); + playground->setGroundSubstance(soft); + + Color c = osgHandle.getColor("Monaco"); + c.alpha()=0.15; + playground->setColor(c); + playground->setPosition(pit2Position); // playground positionieren und generieren + global.obstacles.push_back(playground); + playgrounds.push_back(playground); + } + break; + case OpenPit: + case Uterus: + { + // we stack two playgrounds in each other. + // The outer one is hard (and invisible) and the inner one is soft + int anzgrounds=2; + // this is the utterus imitation: high slip, medium roughness, high elasticity, soft + Substance uterus(roughness, 0.1 /*slip*/, + hardness, 0.95 /*elasticity*/); + double thickness = 0.4; + for (int i=0; i< anzgrounds; i++){ + OdeHandle myHandle = odeHandle; + if(i==0){ + myHandle.substance = uterus; + }else{ + myHandle.substance.toMetal(.2); + } + Playground* playground = new Playground(myHandle, osgHandle, + osg::Vec3(uterussize+2*thickness*i, + i==0 ? thickness : .5, height), + 1, i==0); + playground->setTexture("Images/dusty.rgb"); + if(i==0){ // set ground also to the soft substance + playground->setGroundSubstance(uterus); + } + playground->setColor(Color(0.5,0.1,0.1,i==0? .2 : 0)); // outer ground is not visible (alpha=0) + playground->setPosition(osg::Vec3(0,0,i==0? thickness : 0 )); // playground positionieren und generieren + global.obstacles.push_back(playground); + playgrounds.push_back(playground); + } + + break; + } + case Stacked: + { + for (int i=0; i< numgrounds; i++){ + playground = new Playground(odeHandle, osgHandle, + osg::Vec3(widthground+distance*i, .2, + 0.2+height+heightincrease*i), + 1, i==(numgrounds-1)); + + if(useColorSchema) + playground->setTexture(0,0,TextureDescr("Images/wall_bw.jpg",-1.5,-3)); + playground->setGroundThickness(0.2); + playground->setPosition(osg::Vec3(0,0,0)); // playground positionieren und generieren + Substance substance(roughness, 0.0, hardness, 0.95); + playground->setGroundSubstance(substance); + + global.obstacles.push_back(playground); + playgrounds.push_back(playground); + } + break; + } + default: + break; + } + } + + void placeObstacles(const OdeHandle& odeHandle, const OsgHandle& osgHandle, + GlobalData& global){ + + + for(int i=0; isetColor("wall"); + seesaw->setPose(ROTM(M_PI/2.0,0,0,1)*TRANSM(1, -i,.0)); + global.obstacles.push_back(seesaw); + } + + for(int i=0; isetColor("wall"); + boxpile->setPose(ROTM(M_PI/5.0,0,0,1)*TRANSM(-5, -5-5*i,0.2)); + global.obstacles.push_back(boxpile); + } + + for(int i=0; isetPosition(Pos(i*0.5-2, 3+i*4, 1.0)); + s->setTexture("Images/dusty.rgb"); + global.obstacles.push_back(s); + } + + for(int i=0; isetTexture("Images/light_chess.rgb"); + b->setPosition(Pos(i*0.5-5, i*0.5, 1.0)); + global.obstacles.push_back(b); + } + + for(int i=0; isetPosition(Pos(i-1, -i, 1.0)); + c->setColor(Color(0.2f,0.2f,1.0f,0.5f)); + c->setTexture("Images/light_chess.rgb"); + global.obstacles.push_back(c); + } + + } + + virtual void notifyOnChange(const paramkey& key){ + pitsize = max(pitsize,0.3); + create(odeHandle,osgHandle,*global,true); + } + +}; + + +#endif + diff --git a/ode_robots/simulations/humanoid-TiPI/main.cpp b/ode_robots/simulations/humanoid-TiPI/main.cpp new file mode 100644 index 00000000..7729b8fa --- /dev/null +++ b/ode_robots/simulations/humanoid-TiPI/main.cpp @@ -0,0 +1,496 @@ +/*************************************************************************** + * Copyright (C) 2012 by Robot Group Leipzig * + * martius@informatik.uni-leipzig.de * + * der@informatik.uni-leipzig.de * + * * + * This program is free software; you can redistribute it and/or modify * + * it under the terms of the GNU General Public License as published by * + * the Free Software Foundation; either version 2 of the License, or * + * (at your option) any later version. * + * * + * This program is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * + * GNU General Public License for more details. * + * * + * You should have received a copy of the GNU General Public License * + * along with this program; if not, write to the * + * Free Software Foundation, Inc., * + * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * + * * + ***************************************************************************/ +#include + +#include +#include +#include +#include +#include + +// used wiring +#include +#include + +#include + +#include +// for additional sensor +//#include +//#include + +#include + +// #include "pimax.h" + +#include "environment.h" + +// fetch all the stuff of lpzrobots into scope +using namespace lpzrobots; +using namespace std; + +#define ROBOTSTOREFILE "humanoid_initial.rob" + +enum SimType { Normal, Rescue, Fight, Reck, Bungee}; +string typeToString(SimType t){ + switch(t){ + case Normal: + return "Normal"; + case Reck: + return "Reck"; + case Rescue: + return "Rescue"; + case Bungee: + return "Bungee"; + case Fight: + return "Fight"; + } + return "unknown"; +} +double noise = 0.01; +double cInit = 1.0; +double tilt = 0.0; +double powerfactor = 1.0; +double eps = 0.02; +double epsA = 0.1; +string name = ""; +double noisecontrol= 0; +char* controllerfile[2]={0,0}; + +class ThisSim : public Simulation { +public: + SimType type; + Env env; + + Joint* fixator; + Joint* reckLeft; + Joint* reckRight; + PassiveCapsule* reck; + Playground* playground; + // AbstractObstacle* playground; + double hardness; + Substance s; + + double reckX; + double reckY; + double reckZ; + + ThisSim(SimType type) + : type(type){ + addPaletteFile("colors/UrbanExtraColors.gpl"); + addColorAliasFile("colors/UrbanColorSchema.txt"); + if(type==Rescue) + addColorAliasFile("overwriteGroundColor.txt"); + setGroundTexture("Images/whiteground.jpg"); + setTitle("pred. inf. maximization "); + } + + // starting function (executed once at the beginning of the simulation loop) + void start(const OdeHandle& odeHandle, const OsgHandle& osgHandle, GlobalData& global) + { + setCameraHomePos (Pos(3.96562, 9.00244, 2.74255), Pos(157.862, -12.7123, 0)); + + setCameraMode(Static); + + int humanoids = 1; + + bool fixedInAir = false; + env.type=Env::Normal; + global.odeConfig.setParam("noise",noise); //for more variety + // global.odeConfig.setParam("realtimefactor",1); + global.odeConfig.setParam("simstepsize",0.01); + global.odeConfig.setParam("controlinterval",2); + global.odeConfig.setParam("gravity", -6); + + switch(type){ + case Normal: + fixedInAir = false; + global.odeConfig.setParam("controlinterval",2); + env.numSeeSaws = 0; + env.roughness = 2.5; + break; + case Bungee: + break; + case Reck: + env.height=2.0; + break; + case Rescue: + global.odeConfig.setParam("controlinterval",2); + setCameraHomePos (Pos(1.97075, 4.99419, 2.03904), Pos(159.579, -13.598, 0)); + env.type = Env::Pit; + env.pitsize = 1.0;//.9; + env.thickness = .1; + env.height = 1.4; + env.roughness = 2.0; + env.hardness = 30; + env.numSeeSaws = 0; + break; + case Fight: + env.type = Env::Normal; + global.odeConfig.setParam("controlinterval",2); + global.odeConfig.setParam("gravity", -4); + env.widthground = 5; + env.height = 0.5; + env.roughness = 3; + humanoids = 2; + env.numSeeSaws = 0; + break; + } + + env.create(odeHandle, osgHandle, global); + + env.numSpheres = 0; + env.numBoxes = 0; + env.numCapsules = 0; + env.placeObstacles(odeHandle, osgHandle, global); + + global.configs.push_back(&env); + // global.configs.push_back(this); + + fixator=0; + reckLeft = reckRight = 0; + reck = 0; + reckX = reckY = reckZ = 0; + + for (int i=0; i< humanoids; i++){ //Several humans + bool reckturner = (type==Reck); + if (i>0) reckturner=false; + + // normal servos + // SkeletonConf conf = Skeleton::getDefaultConf(); + // velocity servos + SkeletonConf conf = Skeleton::getDefaultConfVelServos(); + + OsgHandle skelOsgHandle=osgHandle.changeColorSet(i); + double initHeight=0.8; + + conf.useBackJoint = true; + conf.powerFactor = powerfactor; + conf.dampingFactor = .0; + + switch(type){ + case Normal: + // conf.powerFactor = 1.5; + initHeight = 0.45; + break; + case Reck: + // conf.powerFactor = 0.3; + conf.relForce = 2; + conf.handsRotating = true; + initHeight = 0.45; + // conf.armPower = 30; + break; + case Rescue: + // conf.powerFactor = 1.25; + break; + case Bungee: + // conf.powerFactor = .2; + initHeight = 0.45; + break; + case Fight: + // conf.powerFactor = 1.0; + conf.useGripper=true; + conf.gripDuration = 10; + conf.releaseDuration = 5; + conf.handsRotating = false; + break; + } + + OdeHandle skelHandle=odeHandle; + Skeleton* human0 = new Skeleton(skelHandle, skelOsgHandle,conf, + "Humanoid" + itos(i) + "_" + typeToString(type) + "_" + ::name); + + // to add sensors additional sensors use these lines + //std::list sensors; + // additional sensor + // sensors.push_back(new AxisOrientationSensor(AxisOrientationSensor::OnlyZAxis)); + // AddSensors2RobotAdapter* human = + // new AddSensors2RobotAdapter(skelHandle, osgHandle, human0, sensors); + + OdeRobot* human = human0; + human->place( ROTM(M_PI_2,1,0,0)*ROTM( i%2==0 ? M_PI : 0,0,0,1) + //*TRANSM(.2*i,2*i,.841/*7*/ +2*i)); + * ROTM( tilt, 1,0,0) + * TRANSM(1*i, 0.10*i, initHeight)); + + if( fixedInAir){ + Primitive* trunk = human->getMainPrimitive(); + + fixator = new FixedJoint(trunk, global.environment); + // // fixator = new UniversalJoint(trunk, global.environment, Pos(0, 1.2516, 0.0552) , Axis(0,0,1), Axis(0,1,0)); + fixator->init(odeHandle, osgHandle); + }else if(reckturner){ + Primitive* leftHand = human->getAllPrimitives()[Skeleton::Left_Hand]; + Primitive* rightHand = human->getAllPrimitives()[Skeleton::Right_Hand]; + // reckX=leftHand->getPosition().x(); + reckY=leftHand->getPosition().y(); + createOrMoveReck(odeHandle, osgHandle.changeColor("wall"), global, + leftHand->getPosition().z()); + + reckLeft = new SliderJoint(leftHand, reck->getMainPrimitive(), leftHand->getPosition(), Axis(1,0,0)); + reckLeft->init(odeHandle, osgHandle,false); + reckRight = new SliderJoint(rightHand, reck->getMainPrimitive(), rightHand->getPosition(), Axis(1,0,0)); + reckRight->init(odeHandle, osgHandle,false); + } + + One2OneWiring* wiring; + if(noisecontrol>0) + wiring = new MotorNoiseWiring(new ColorUniformNoise(noisecontrol),1.0); + else + wiring = new One2OneWiring(new ColorUniformNoise(0.1)); + + AbstractController* controller; + if(noisecontrol>0){ + controller = new SineController(); + controller->setParam("amplitude",0); // constant 0 output + }else{ + PiMaxConf pc = PiMax::getDefaultConf(); + pc.onlyMainParameters = false; + pc.initFeedbackStrength=cInit; + controller = new PiMax(pc); + // pimax + controller->setParam("metrics",1); + controller->setParam("epsC",eps); + controller->setParam("epsA",epsA); + controller->setParam("sense",6); + controller->setParam("s4avg",1); + controller->setParam("s4delay",1); + controller->setParam("damping",0); // < new (added after bungee experiments!) + } + + OdeAgent* agent = new OdeAgent(global); + + agent->init(controller, human, wiring); + //agent->setTrackOptions(TrackRobot(true,true,false,true,"bodyheight",20)); // position and speed tracking every 20 steps + + switch(type){ + case Normal: + break; + case Reck: + break; + case Rescue: + break; + case Bungee: + agent->addOperator(new PullToPointOperator(Pos(0,0,3),30,true, + PullToPointOperator::Z, + 0, 0.1, true)); + break; + case Fight: + agent->addOperator(new BoxRingOperator(Pos(0,0,1.2), env.widthground/2.0, + 0.2, 200, false)); + break; + } + + if(controllerfile[i]){ + if(controller->restoreFromFile(controllerfile[i])){ + printf("restoring controller: %s\n", controllerfile[i]); + } else{ + fprintf(stderr,"restoring of controller failed!\n"); + exit(1); + } + } + + if(i==0) human->storeToFile(ROBOTSTOREFILE); + + global.configs.push_back(agent); + global.agents.push_back(agent); + + }// Several humans end + + // add Grippers + if(type==Fight){ + Skeleton* h1 = dynamic_cast(global.agents[0]->getRobot()); + Skeleton* h2 = dynamic_cast(global.agents[1]->getRobot()); + if(h1 && h2){ + FOREACH(GripperList, h1->getGrippers(), g){ + (*g)->addGrippables(h2->getAllPrimitives()); + } + FOREACH(GripperList, h2->getGrippers(), g){ + (*g)->addGrippables(h1->getAllPrimitives()); + } + }else{ + fprintf(stderr,"Cannot convert Humanoids!"); + } + } + }; + + + void createOrMoveReck(const OdeHandle& odeHandle, const OsgHandle& osgHandle, + GlobalData& global, double amount){ + if(type==Reck) { + if(fixator) delete fixator; + if(!reck){ + reck = new PassiveCapsule(odeHandle, osgHandle, + 0.02,env.widthground-0.2, 1.0); + reck->setPose(ROTM(M_PI_2,0,1,0)*TRANSM(reckX, reckY,0)); + global.obstacles.push_back(reck); + reckZ = amount; + }else{ + reckZ += amount; + } + + Primitive* r = reck->getMainPrimitive(); + r->setPose(ROTM(M_PI_2,0,1,0)*TRANSM(reckX,reckY,reckZ)); + fixator = new FixedJoint(r, global.environment); + fixator->init(odeHandle, osgHandle, false); + } + } + + virtual void bindingDescription(osg::ApplicationUsage & au) const { + if(reck){ + au.addKeyboardMouseBinding("Sim: b/B","move high bar down/up"); + au.addKeyboardMouseBinding("Sim: x/X","release Robot from fixation (if fixated)"); + } + } + + // add own key handling stuff here, just insert some case values + virtual bool command(const OdeHandle& odeHandle, const OsgHandle& osgHandle, + GlobalData& global, int key, bool down) + { + if (down) { // only when key is pressed, not when released + switch ( (char) key ) + { + case 'X': + case 'x': + if(fixator) delete fixator; + fixator=0; + return true; + break; + case 'b': + createOrMoveReck(odeHandle, osgHandle, globalData, -0.1); + return true; + break; + case 'B': + createOrMoveReck(odeHandle, osgHandle, globalData, 0.1); + return true; + break; + case 'l': + { + globalData.agents[0]->getRobot()->restoreFromFile(ROBOTSTOREFILE); + globalData.agents[0]->getWiring()->reset(); + } + return true; + break; + default: + return false; + break; + } + } + return false; + } + + virtual void usage() const { + printf("\t-rescue\thumanoid in a pit\n"); + printf("\t-reck\thumanoid at a reck bar\n"); + printf("\t-fight\ttwo humanoid in a fighting arena\n"); + printf("\t-bungee\ta weak humanoid attached to bungee\n"); + printf("\t-noise strength\tnoise strength (def: 0.01)\n"); + printf("\t-cInit strength\tinitial value for Controller diagonal (def: 1.0)\n"); + printf("\t-tilt angle\tangle in rad by which to tilt the robot initialially (def: 0.0)\n"); + printf("\t-powerfactor factor\tpowerfactor of robot (def: 1.0)\n"); + printf("\t-epsC learningrate\t learning rate for controller (def: 0.02)\n"); + printf("\t-epsA learningrate\t learning rate for model (def: 0.1)\n"); + printf("\t-name NAME\tname of experiment (def: )\n"); + printf("\t-loadcontroler file \tload the controller at startup\n"); + printf("\t-loadcontroler1 file \tload the controller for second humanoid at startup\n"); + printf("\t-noisecontrol tau \tuse noise as motor commands with correlation length 1/tau \n"); + }; + +}; + +int main (int argc, char **argv) +{ + SimType type=Normal; + if (Simulation::contains(argv, argc, "-rescue")) { + type= Rescue; + } + if (Simulation::contains(argv, argc, "-reck")) { + type= Reck; + } + if (Simulation::contains(argv, argc, "-fight")) { + type= Fight; + } + if (Simulation::contains(argv, argc, "-bungee")) { + type= Bungee; + } + int index = Simulation::contains(argv, argc, "-noise"); + if (index>0 && index0 && index0 && index0 && index0 && index0 && index0 && index0 && index0 && index0 && index Date: Wed, 7 Nov 2018 15:14:56 +0000 Subject: [PATCH 02/10] example scenarios from paper PLoS2013 --- .../simulations/humanoid-TiPI/scenarios.sh | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) create mode 100755 ode_robots/simulations/humanoid-TiPI/scenarios.sh diff --git a/ode_robots/simulations/humanoid-TiPI/scenarios.sh b/ode_robots/simulations/humanoid-TiPI/scenarios.sh new file mode 100755 index 00000000..1680f9a6 --- /dev/null +++ b/ode_robots/simulations/humanoid-TiPI/scenarios.sh @@ -0,0 +1,16 @@ +#!/bin/bash + +std="-r 1 -noise 0.01 -f 100 ntst -nographics -simtime 40 -power 0.5 -loadcontroller bungee_medium1.ctrl" +./start_opt -r 1 $std -tilt 0 -name "control" & +./start_opt -r 1 $std -tilt 0 -name "pos1" & +./start_opt -r 1 $std -tilt 0.01 -name "pos3" +./start_opt -r 1 $std -tilt 0.1 -name "pos2" +./start_opt -r 1 $std -tilt 0 -name "pos1" -rescue & +./start_opt -r 1 $std -tilt 0.01 -name "pos3" -rescue +./start_opt -r 1 $std -tilt 0.1 -name "pos2" -rescue +./start_opt -r 1 $std -tilt 0 -name "pos1" -bungee & +./start_opt -r 1 $std -tilt 0.01 -name "pos3" -bungee +./start_opt -r 1 $std -tilt 0.1 -name "pos2" -bungee +./start_opt -r 1 $std -tilt 0 -name "pos1" -reck & +./start_opt -r 1 $std -tilt 0.01 -name "pos3" -reck +./start_opt -r 1 $std -tilt 0.1 -name "pos2" -reck From d5e6df7481c3445a0908d3a1d33af1ec25daad07 Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Wed, 7 Nov 2018 15:37:39 +0000 Subject: [PATCH 03/10] Adding assert as sanity check. At that point, the actual `a_tm1` is in fact $a_{t-1} = K(s_t)$ and thus doesn't equal $a_{t-1} = C_{t-1} * s_{t-1} + h_{t-1}$ --- selforg/controller/pimax.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/selforg/controller/pimax.cpp b/selforg/controller/pimax.cpp index 3f50bd67..4a3ff866 100644 --- a/selforg/controller/pimax.cpp +++ b/selforg/controller/pimax.cpp @@ -240,6 +240,7 @@ void PiMax::learn(){ const Matrix& z = (C * (s) + h); const Matrix& a = z.map(g); // actually the same as a_tm1 + assert(a == a_tm1); const Matrix& g_prime = z.map(g_s); gs_buffer[(t-1) % buffersize] = g_prime; From 9293e651f21ad3f4e98358a179030400251b7c7d Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Wed, 7 Nov 2018 16:04:29 +0000 Subject: [PATCH 04/10] computing $a_t$ with updated parameters C,h --- selforg/controller/pimax.cpp | 29 ++++++++++++++++++++++++++--- 1 file changed, 26 insertions(+), 3 deletions(-) diff --git a/selforg/controller/pimax.cpp b/selforg/controller/pimax.cpp index 4a3ff866..76b9ef4d 100644 --- a/selforg/controller/pimax.cpp +++ b/selforg/controller/pimax.cpp @@ -150,14 +150,37 @@ void PiMax::seth(const matrix::Matrix& _h){ // performs one step (includes learning). Calculates motor commands from sensor inputs. void PiMax::step(const sensor* s_, int number_sensors, motor* a_, int number_motors){ - stepNoLearning(s_, number_sensors, a_, number_motors); - if(t<=buffersize) return; - t--; // stepNoLearning increases the time by one - undo here + + // fill buffers without learning first + if(t<=buffersize) { + stepNoLearning(s_, number_sensors, a_, number_motors); + return; + } + + s.set(number_sensors,1,s_); // store sensor values + + // averaging over the last s4avg values of s_buffer + conf.steps4Averaging = ::clip(conf.steps4Averaging,1,buffersize-1); + if(conf.steps4Averaging > 1) + s_smooth += (s - s_smooth)*(1.0/conf.steps4Averaging); + else + s_smooth = s; + + s_buffer[t%buffersize] = s_smooth; // we store the smoothed sensor value // learn controller and model if(epsC!=0 || epsA!=0) learn(); + // calculate controller values based on current input values (smoothed) + Matrix a = (C*(s_smooth) + h).map(g); + + // Put new output vector in ring buffer a_buffer + a_buffer[t%buffersize] = a; + + // convert a to motor* + a.convertToBuffer(a_, number_motors); + // update step counter t++; }; From c2b511007e674c400626d33151feb6e54db130d3 Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Wed, 7 Nov 2018 16:09:30 +0000 Subject: [PATCH 05/10] `epsC` and `epsA` can be changed online from 0 to !0, buffer entries then will be wrong for learning, hence always enter `learn()` --- selforg/controller/pimax.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/selforg/controller/pimax.cpp b/selforg/controller/pimax.cpp index 76b9ef4d..4a876b70 100644 --- a/selforg/controller/pimax.cpp +++ b/selforg/controller/pimax.cpp @@ -169,8 +169,7 @@ void PiMax::step(const sensor* s_, int number_sensors, s_buffer[t%buffersize] = s_smooth; // we store the smoothed sensor value // learn controller and model - if(epsC!=0 || epsA!=0) - learn(); + learn(); // calculate controller values based on current input values (smoothed) Matrix a = (C*(s_smooth) + h).map(g); From f80689945101a3fa6090cbfd884de95701078fd5 Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Wed, 7 Nov 2018 17:31:17 +0000 Subject: [PATCH 06/10] have a re-usable function for smoothing `s` --- selforg/controller/pimax.cpp | 36 ++++++++++++++++++------------------ selforg/controller/pimax.h | 2 ++ 2 files changed, 20 insertions(+), 18 deletions(-) diff --git a/selforg/controller/pimax.cpp b/selforg/controller/pimax.cpp index 4a876b70..4e281127 100644 --- a/selforg/controller/pimax.cpp +++ b/selforg/controller/pimax.cpp @@ -147,6 +147,14 @@ void PiMax::seth(const matrix::Matrix& _h){ h=_h; } +matrix::Matrix PiMax::smoothing_s(const matrix::Matrix &new_s, const matrix::Matrix &old_s, const int steps) { + + if(steps > 1) + return old_s + (new_s - old_s)*(1.0/steps); + else + return new_s; +} + // performs one step (includes learning). Calculates motor commands from sensor inputs. void PiMax::step(const sensor* s_, int number_sensors, motor* a_, int number_motors){ @@ -157,16 +165,12 @@ void PiMax::step(const sensor* s_, int number_sensors, return; } - s.set(number_sensors,1,s_); // store sensor values - + // store sensor values + s.set(number_sensors,1,s_); // averaging over the last s4avg values of s_buffer - conf.steps4Averaging = ::clip(conf.steps4Averaging,1,buffersize-1); - if(conf.steps4Averaging > 1) - s_smooth += (s - s_smooth)*(1.0/conf.steps4Averaging); - else - s_smooth = s; - - s_buffer[t%buffersize] = s_smooth; // we store the smoothed sensor value + s_smooth = smoothing_s(s, s_smooth, ::clip(conf.steps4Averaging,1,buffersize-1)); + // we store the smoothed sensor value + s_buffer[t%buffersize] = s_smooth; // learn controller and model learn(); @@ -191,16 +195,12 @@ void PiMax::stepNoLearning(const sensor* s_, int number_sensors, assert((unsigned)number_sensors <= this->number_sensors && (unsigned)number_motors <= this->number_motors); - s.set(number_sensors,1,s_); // store sensor values - + // store sensor values + s.set(number_sensors,1,s_); // averaging over the last s4avg values of s_buffer - conf.steps4Averaging = ::clip(conf.steps4Averaging,1,buffersize-1); - if(conf.steps4Averaging > 1) - s_smooth += (s - s_smooth)*(1.0/conf.steps4Averaging); - else - s_smooth = s; - - s_buffer[t%buffersize] = s_smooth; // we store the smoothed sensor value + s_smooth = smoothing_s(s, s_smooth, ::clip(conf.steps4Averaging,1,buffersize-1)); + // we store the smoothed sensor value + s_buffer[t%buffersize] = s_smooth; // calculate controller values based on current input values (smoothed) Matrix a = (C*(s_smooth) + h).map(g); diff --git a/selforg/controller/pimax.h b/selforg/controller/pimax.h index dc7aafa9..31b2db9e 100644 --- a/selforg/controller/pimax.h +++ b/selforg/controller/pimax.h @@ -169,6 +169,8 @@ class PiMax : public AbstractController, public Teachable, public Parametrizable paramint tau; // length of time window + virtual matrix::Matrix smoothing_s(const matrix::Matrix &new_s, const matrix::Matrix &old_s, const int steps); + /// learn values model and controller (A,b,C,h) virtual void learn(); From b148ed45fe5484e7abf0df67de92600746d3e5f1 Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Thu, 8 Nov 2018 14:08:44 +0000 Subject: [PATCH 07/10] include pimax from `selforg/controller` --- ode_robots/simulations/humanoid-TiPI/main.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/ode_robots/simulations/humanoid-TiPI/main.cpp b/ode_robots/simulations/humanoid-TiPI/main.cpp index 7729b8fa..6e8a002c 100644 --- a/ode_robots/simulations/humanoid-TiPI/main.cpp +++ b/ode_robots/simulations/humanoid-TiPI/main.cpp @@ -40,8 +40,6 @@ #include -// #include "pimax.h" - #include "environment.h" // fetch all the stuff of lpzrobots into scope From e8c954b09a2dbc9eddcd2e93a017f47e23e3718f Mon Sep 17 00:00:00 2001 From: Marcus Scheunemann Date: Thu, 8 Nov 2018 19:50:05 +0000 Subject: [PATCH 08/10] Delete generated file --- ode_robots/simulations/humanoid-TiPI/Makefile | 88 ------------------- 1 file changed, 88 deletions(-) delete mode 100644 ode_robots/simulations/humanoid-TiPI/Makefile diff --git a/ode_robots/simulations/humanoid-TiPI/Makefile b/ode_robots/simulations/humanoid-TiPI/Makefile deleted file mode 100644 index f575d101..00000000 --- a/ode_robots/simulations/humanoid-TiPI/Makefile +++ /dev/null @@ -1,88 +0,0 @@ -# -*- mode: makefile; -*- -# Do not edit, this is file generated by m4 from Makefile.4sim.m4 and -# copied to the simulations while installation! -# Make your changes in Makefile.conf. -# E.g. add there files to compile or custom libs and so on. - -# File: Makefile for lpzrobots simulations -# Author: Georg Martius -# Date: Oct 2009 - - - - - - - - - -EXEC = start -# add files to compile in the conf file -include Makefile.conf - -CFILES = $(addsuffix .cpp, $(FILES)) -OFILES = $(addsuffix .o, $(FILES)) - -# the CFGOPTS are set by the opt and dbg target -CFGOPTS= -INC += -I. -BASELIBS = $(shell ode_robots-config $(CFGOPTS) --static --libs) $(shell selforg-config $(CFGOPTS) --static --libs) -BASELIBSSHARED := $(shell ode_robots-config $(CFGOPTS) --libs) $(shell selforg-config $(CFGOPTS) --libs) - - - - - - - - - - - - - -LIBS += $(BASELIBS) $(ADDITIONAL_LIBS) - -INC += -I. - -CXX = g++ -CPPFLAGS = -Wall -pipe -Wno-deprecated $(INC) $(shell selforg-config $(CFGOPTS) --cflags) \ - $(shell ode_robots-config $(CFGOPTS) --intern --cflags) - -normal: - $(MAKE) $(EXEC) -opt: - $(MAKE) CFGOPTS=--opt EXEC=$(EXEC)_opt $(EXEC)_opt -dbg: - $(MAKE) CFGOPTS=--dbg EXEC=$(EXEC)_dbg $(EXEC)_dbg -shared: - $(MAKE) BASELIBS="$(BASELIBSSHARED)" LIBSELFORG="$(LIBSELFORGSHARED)" \ - LIBODEROBOTS="$(LIBODEROBOTSSHARED)" $(EXEC) - -$(EXEC): Makefile Makefile.depend $(OFILES) - $(CXX) $(CPPFLAGS) $(OFILES) $(LIBS) -o $(EXEC) - - - -Makefile.depend: - makedepend -- $(CPPFLAGS) -- $(CFILES) -f- > Makefile.depend 2>/dev/null - -depend: - makedepend -- $(CPPFLAGS) -- $(CFILES) -f- > Makefile.depend 2>/dev/null - -check-syntax: - $(CXX) $(CPPFLAGS) -Wextra -S -fsyntax-only $(CHK_SOURCES) - cppcheck --std=c++11 --std=posix --enable=performance,information,portability --suppress=incorrectStringBooleanError --suppress=invalidscanf --quiet --template={file}:{line}:{severity}:{message} $(CHK_SOURCES) - -todo: - find -name "*.[ch]*" -exec grep -Hni "TODO" {} \; - -tags: - etags $(find -name "*.[ch]") - - - -clean: - rm -f $(EXEC) $(EXEC)_dbg $(EXEC)_opt *.o Makefile.depend - --include Makefile.depend From 694ab54a9615de0a82f1748b107818347806a269 Mon Sep 17 00:00:00 2001 From: "Marcus M. Scheunemann" Date: Fri, 9 Nov 2018 11:47:52 +0000 Subject: [PATCH 09/10] Iteration over the sum (A20) computes $\Delta C$. However, so far the iteration seems to compute the weight update with adding $C_{t-1}$ to the change within the sum. Results are the same as before for $\tau=2$. --- selforg/controller/pimax.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/selforg/controller/pimax.cpp b/selforg/controller/pimax.cpp index 4e281127..b8231cb3 100644 --- a/selforg/controller/pimax.cpp +++ b/selforg/controller/pimax.cpp @@ -306,6 +306,11 @@ void PiMax::learn(){ double epsCN = epsC/(100.0*(tau-1)); // l means: time point t-l // x,y, L and g' are to be taken at t-l + + // $\Delta C$ and $\Delta h$ + matrix::Matrix dC(C.getM(), C.getN()); + matrix::Matrix dh(h.getM(), 1); + for(int l=1; l Date: Fri, 9 Nov 2018 15:57:52 +0000 Subject: [PATCH 10/10] A for $\delta u$, the Jacobian L(t-l) is calculated depended on the parameters C at time (t-l). $\partial \psi(s_{t-l}) / \partial s_{t-l}$ is depended on $a_{t-l}$ and therefore on $C_{t-l}$. Again, nothing changes for $\tau=2$. --- selforg/controller/pimax.cpp | 6 +++++- selforg/controller/pimax.h | 1 + 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/selforg/controller/pimax.cpp b/selforg/controller/pimax.cpp index b8231cb3..586c74a5 100644 --- a/selforg/controller/pimax.cpp +++ b/selforg/controller/pimax.cpp @@ -302,6 +302,9 @@ void PiMax::learn(){ for(int l=2; l 0){ double epsCN = epsC/(100.0*(tau-1)); // l means: time point t-l @@ -316,8 +319,9 @@ void PiMax::learn(){ const Matrix& al = a_buffer[(t-l)%buffersize]; const Matrix& sl = s_buffer[(t-l)%buffersize]; const Matrix& gs = gs_buffer[(t-l)%buffersize]; + const Matrix& Cl = C_buffer[(t-l)%buffersize]; const Matrix& dmu = ((A^T)*du[l]) & gs; - const Matrix& epsrel = (C*ds[l]) & dmu * 2 * sense; + const Matrix& epsrel = (Cl*ds[l]) & dmu * 2 * sense; const Matrix& metric = useMetric ? gs.map(one_over).map(sqr) : gs.mapP(1, constant); diff --git a/selforg/controller/pimax.h b/selforg/controller/pimax.h index 31b2db9e..bc0f30de 100644 --- a/selforg/controller/pimax.h +++ b/selforg/controller/pimax.h @@ -144,6 +144,7 @@ class PiMax : public AbstractController, public Teachable, public Parametrizable matrix::Matrix s_buffer[buffersize]; // buffer of sensor values matrix::Matrix xi_buffer[buffersize]; // buffer of pred errors matrix::Matrix gs_buffer[buffersize]; // buffer of g' + matrix::Matrix C_buffer[buffersize]; // buffer of C matrix::Matrix L_buffer[buffersize]; // buffer of Jacobians matrix::Matrix s; // current sensor value vector