Commit 889b2269 authored by mnaveau's avatar mnaveau
Browse files

Introduce a new PG dealing with the automatic foot step with non linear constraints.

parent 7d6ccc02
......@@ -252,5 +252,15 @@ namespace PatternGeneratorJRL
return os;
}
/// Structure to model a circle (e.g : a stricly convex obstable)
struct Circle_t
{
double x_0 ;
double y_0 ;
double r ;
double margin ;
};
typedef struct Circle_t Circle ;
}
#endif
......@@ -78,8 +78,10 @@ SET(INCLUDES
ZMPRefTrajectoryGeneration/mpc-trajectory-generation.hh
ZMPRefTrajectoryGeneration/ZMPRefTrajectoryGeneration.hh
ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.hh
ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.hh
ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.hh
ZMPRefTrajectoryGeneration/generator-vel-ref.hh
ZMPRefTrajectoryGeneration/nmpc_generator.hh
ZMPRefTrajectoryGeneration/FilteringAnalyticalTrajectoryByPreviewControl.hh
ZMPRefTrajectoryGeneration/problem-vel-ref.hh
ZMPRefTrajectoryGeneration/ZMPQPWithConstraint.hh
......@@ -137,6 +139,7 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/ZMPQPWithConstraint.cpp
ZMPRefTrajectoryGeneration/ZMPConstrainedQPFastFormulation.cpp
ZMPRefTrajectoryGeneration/ZMPVelocityReferencedQP.cpp
ZMPRefTrajectoryGeneration/ZMPVelocityReferencedSQP.cpp
ZMPRefTrajectoryGeneration/OrientationsPreview.cpp
ZMPRefTrajectoryGeneration/AnalyticalMorisawaAbstract.cpp
ZMPRefTrajectoryGeneration/AnalyticalMorisawaCompact.cpp
......@@ -144,6 +147,7 @@ SET(SOURCES
ZMPRefTrajectoryGeneration/problem-vel-ref.cpp
ZMPRefTrajectoryGeneration/qp-problem.cpp
ZMPRefTrajectoryGeneration/generator-vel-ref.cpp
ZMPRefTrajectoryGeneration/nmpc_generator.cpp
ZMPRefTrajectoryGeneration/mpc-trajectory-generation.cpp
ZMPRefTrajectoryGeneration/DynamicFilter.cpp
MultiContactRefTrajectoryGeneration/MultiContactHirukawa.cc
......
......@@ -301,7 +301,7 @@ RelativeFeetInequalities::compute_linear_system ( convex_hull_t & ConvexHull,
y2 = ConvexHull.Y_vec[0];
x1 = ConvexHull.X_vec[i];
x2 = ConvexHull.X_vec[0];
cout << x1 << " " << x2 << " " << sign << " " << y1 << " " << y2 << endl;
dx = y1-y2;
dy = x2-x1;
dc = dx*x1+dy*y1;
......@@ -376,3 +376,10 @@ RelativeFeetInequalities::CallMethod( std::string &Method, std::istringstream &A
}
}
}
void
RelativeFeetInequalities::getFeetSize(FootHalfSize & leftFootSize, FootHalfSize & rightFootSize)
{
leftFootSize = LeftFootSize_ ;
rightFootSize = RightFootSize_ ;
}
......@@ -99,6 +99,13 @@ namespace PatternGeneratorJRL
/// \param[in] Args: Arguments of the methods.
virtual void CallMethod (std::string & Method, std::istringstream & Args);
/// \brief Reimplement the interface of SimplePluginManager
///
/// \param[in] Method: The method to be called.
/// \param[in] Args: Arguments of the methods.
void getFeetSize(FootHalfSize & leftFootSize, FootHalfSize & rightFootSize);
//
// Private member functions
//
......
......@@ -604,7 +604,7 @@ void ZMPVelocityReferencedQP::OnLine(double time,
RightFootTraj_deq_,
deltaCOMTraj_deq_);
#define DEBUG
//#define DEBUG
#ifdef DEBUG
dynamicFilter_->Debug(COMTraj_deq_ctrl_,
LeftFootTraj_deq_ctrl_,
......
This diff is collapsed.
......@@ -188,10 +188,8 @@ namespace PatternGeneratorJRL
convex_hull_t::convex_hull_t( unsigned nbVert, unsigned nbIneq ):
nbIneq_(0), nbVert_(0)
{
resize( nbVert, nbIneq );
clear();
resize( nbVert, nbIneq );
}
......
......@@ -443,6 +443,43 @@ IF(HRP2_DYNAMICS_FOUND)
ENDIF(HRP2_14_FOUND)
ENDIF(HRP2_DYNAMICS_FOUND)
################
# SQP test #
################
IF(HRP2_DYNAMICS_FOUND)
IF(HRP2_14_FOUND)
MESSAGE(STATUS "hrp2-dynamics taken into account")
ADD_EXECUTABLE(TestNaveau2015
../src/portability/gettimeofday.cc
TestNaveau2015.cpp
CommonTools.cpp
TestObject.cpp
ClockCPUTime.cpp
)
MESSAGE(STATUS "jrl data dir: " ${HRP2_14_PKGDATAROOTDIR})
SET(hrp2dynamicsmodelpath ${HRP2_14_PKGDATAROOTDIR}/)
SET(hrp2dynamicsspec
${HRP2_14_PKGDATAROOTDIR}/HRP2SpecificitiesSmall.xml
)
SET(hrp2dynamicsljr
${HRP2_14_PKGDATAROOTDIR}/HRP2LinkJointRankSmall.xml
)
SET(hrp2dynamicsinitconfig
${HRP2_14_PKGDATAROOTDIR}/HRP2JRLInitConfig.dat)
TARGET_LINK_LIBRARIES(TestNaveau2015 ${PROJECT_NAME})
PKG_CONFIG_USE_DEPENDENCY(TestNaveau2015 hrp2-dynamics)
IF(NOT ${WITH_HRP2_DYNAMICS} EQUAL "" )
TARGET_COMPILE_DEFINITIONS(TestNaveau2015 PRIVATE WITH_HRP2DYNAMICS=1)
ENDIF(NOT ${WITH_HRP2_DYNAMICS} EQUAL "" )
ADD_DEPENDENCIES(TestNaveau2015 ${PROJECT_NAME})
ENDIF(HRP2_14_FOUND)
ENDIF(HRP2_DYNAMICS_FOUND)
#####################
# Add user examples #
#####################
......
......@@ -776,12 +776,12 @@ protected:
#define localNbOfEvents 12
struct localEvent events [localNbOfEvents] =
{ { 5*200,&TestHerdt2010::walkForward1m_s},
{10*200,&TestHerdt2010::walkForward1m_s},
{15*200,&TestHerdt2010::walkForward1m_s},
{20*200,&TestHerdt2010::walkSidewards1m_s},
{25*200,&TestHerdt2010::walkSidewards1m_s},
{10*200,&TestHerdt2010::walkForward2m_s},
{15*200,&TestHerdt2010::walkForward3m_s},
{20*200,&TestHerdt2010::walkForward2m_s},
{25*200,&TestHerdt2010::walkForward2m_s},
{30*200,&TestHerdt2010::walkSidewards1m_s},
{35*200,&TestHerdt2010::startTurningRightOnSpot},
{35*200,&TestHerdt2010::walkSidewards2m_s},
{50*200,&TestHerdt2010::stop},
{55*200,&TestHerdt2010::stopOnLineWalking}};
//{5*200,&TestHerdt2010::startTurningRightOnSpot},
......
/*
* Copyright 2010,
*
* Andrei Herdt
* Maximilien naveau
* Olivier Stasse
*
* JRL, CNRS/AIST
......@@ -22,8 +22,7 @@
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/* \file This file tests A. Herdt's walking algorithm for
* automatic foot placement giving an instantaneous CoM velocity reference.
/* \file This file tests Kajita's dynamic filter on a simple case
*/
#include "Debug.hh"
#include "CommonTools.hh"
......
......@@ -136,7 +136,7 @@ public:
assert(comPos_.size() == lfoot_.size()
&& lfoot_.size() == comSpeed_.size()
&& comPos_.size() == comSpeed_.size()
&& 19.3/samplingPeriod_);
&& 19/samplingPeriod_);
ofstream aof;
string aFileName;
static int iteration = 0 ;
......@@ -154,13 +154,13 @@ public:
for(unsigned int i = 0 ; i < lfoot_.size() ; i++){
aof << filterprecision( data_time_[i] ) << " " ; // 1
aof << filterprecision( lfoot_[i](0) ) << " " ; // 2 x
aof << filterprecision( lfoot_[i](1) ) << " " ; // 3 y
aof << filterprecision( lfoot_[i](2) ) << " " ; // 4 z
aof << filterprecision( rfoot_[i](0) ) << " " ; // 2 x
aof << filterprecision( rfoot_[i](1) ) << " " ; // 3 y
aof << filterprecision( rfoot_[i](2) ) << " " ; // 4 z
aof << filterprecision( rfoot_[i](0) ) << " " ; // 5 x
aof << filterprecision( rfoot_[i](1) ) << " " ; // 6 y
aof << filterprecision( rfoot_[i](2) ) << " " ; // 7 z
aof << filterprecision( lfoot_[i](0) ) << " " ; // 5 x
aof << filterprecision( lfoot_[i](1) ) << " " ; // 6 y
aof << filterprecision( lfoot_[i](2) ) << " " ; // 7 z
aof << filterprecision( rhand_[i](0) ) << " " ; // 8 x
aof << filterprecision( rhand_[i](1) ) << " " ; // 9 y
......@@ -318,9 +318,9 @@ protected:
return 1 ;
}
double liftCoef = 0.9;
double liftCoef = 0.8;
double gripperCoef = 1.0;
double WayPoint_z = 0.05 ;
double WayPoint_z = 0.0 ;
double epsilon = 0.001 ;
vector<double> MP ;
vector<double> ToMP ;
......@@ -341,7 +341,7 @@ protected:
TimeInterval = timing_[it] ; iniPos = lfs_[it-1](2) ; finPos = lfs_[it](2) ;
if (finPos - iniPos > epsilon )
{
ToMP.push_back(0.4*TimeInterval);
ToMP.push_back(0.3*TimeInterval);
MP.push_back(finPos+WayPoint_z);
}
else if (finPos - iniPos <= epsilon && finPos - iniPos >= -epsilon )
......@@ -361,7 +361,7 @@ protected:
TimeInterval = timing_[it] ; iniPos = rfs_[it-1](2) ; finPos = rfs_[it](2) ;
if (finPos - iniPos > epsilon )
{
ToMP.push_back(0.4*TimeInterval);
ToMP.push_back(0.3*TimeInterval);
MP.push_back(finPos+WayPoint_z);
}
else if (finPos - iniPos <= epsilon && finPos - iniPos >= -epsilon )
......@@ -383,7 +383,7 @@ protected:
if (finPos - iniPos > epsilon )
{
ToMP.push_back(0.4*TimeInterval);
MP.push_back(finPos+0.01);
MP.push_back(iniPos+0.6*(finPos-iniPos));
}
else if (finPos - iniPos <= epsilon && finPos - iniPos >= -epsilon )
{}
......@@ -406,7 +406,7 @@ protected:
double UnlockedSwingPeriod = LFX_->FT() ;
double ss_time = LFZ_->FT() ;
double EndOfLiftOff = (ss_time-UnlockedSwingPeriod)*0.5;
double EndOfLiftOff = (ss_time-UnlockedSwingPeriod)*0.66;
double StartLanding = EndOfLiftOff + UnlockedSwingPeriod;
double timeOfInterpolation = 0.0 ;
......@@ -594,11 +594,11 @@ protected:
MAL_VECTOR_DIM(aSRH,double,6);
double init_lfx = 0.00949035 ;
double init_lfy = 0.095 ;
double init_lfy = -0.095 ;
double init_lfz = 0.0 ;
//
double init_rfx = 0.00949035 ;
double init_rfy = -0.095 ;
double init_rfy = +0.095 ;
double init_rfz = 0.0 ;
// 10cm handrill ////////////////////////// to pass to left hand be carefull!!!!!
......@@ -621,7 +621,7 @@ protected:
// 15cm handrill ///////////////////////////
double sth = 0.15 ; // stair height
double sta = -0.4537; // stair angle
double sta = -0.47123889803846897; // stair angle
double init_hx = 0.0418343 ;
double init_hy = -0.331008 ; // right hand
double init_hz = 0.704285 ;
......@@ -629,12 +629,13 @@ protected:
double init_hpitch = -0.25421091 ;
double init_hyaw = 0.08982785 ;
// first hand pose
double hx_1 = 0.3 ;
double deltax = 0.0 ;
double hx_1 = 0.3535 + deltax * cos(sqrt(sta*sta)) ;
double hy_1 = -0.35 ;
double hz_1 = 0.839 ;
double hz_1 = 0.8524 + deltax * sin(sqrt(sta*sta)) ;
// sec hand pose
double hx_2 = 0.6 ;
double hz_2 = 0.985 ;
double hx_2 = 0.65346 + deltax * cos(sqrt(sta*sta)) ;
double hz_2 = 1.005 + deltax * sin(sqrt(sta*sta)) ;
......@@ -646,7 +647,7 @@ protected:
aSLF(4)=0.0 ; aSRF(4)=0.0 ; aSRH(4)=init_hpitch ;
aSLF(5)=0.0 ; aSRF(5)=0.0 ; aSRH(5)=init_hyaw ;
lfs_.push_back(aSLF); rfs_.push_back(aSRF); rhs_.push_back(aSRH);
timing_.push_back(initialTime_+3*supportPeriod_/2);
timing_.push_back(initialTime_+supportPeriod_/2);
//double init_hy = -0.331008 ; // left hand
// 1 0.7 second
......@@ -694,7 +695,7 @@ protected:
init_rfx = 0.0;//////////////////////////////////////////////////////////////
aSLF(0)=init_lfx+0.3 ; aSRF(0)=init_rfx+0.3 ; aSRH(0)=hx_1 ;
aSLF(1)=init_lfy ; aSRF(1)=init_rfy ; aSRH(1)=hy_1 ;
aSLF(2)=sth ; aSRF(2)=sth ; aSRH(2)=hz_1 ;
aSLF(2)=sth ; aSRF(2)=sth ; aSRH(2)=hz_1 ;
aSLF(3)=0.0 ; aSRF(3)=0.0 ; aSRH(3)=0.0 ;
aSLF(4)=0.0 ; aSRF(4)=0.0 ; aSRH(4)=sta ;
aSLF(5)=0.0 ; aSRF(5)=0.0 ; aSRH(5)=0.0 ;
......@@ -704,7 +705,7 @@ protected:
// 6 0.1 s
aSLF(0)=init_lfx+0.3 ; aSRF(0)=init_rfx+0.3 ; aSRH(0)=hx_1 ;
aSLF(1)=init_lfy ; aSRF(1)=init_rfy ; aSRH(1)=hy_1 ;
aSLF(2)=sth ; aSRF(2)=sth ; aSRH(2)=hz_1 ;
aSLF(2)=sth ; aSRF(2)=sth ; aSRH(2)=hz_1 ;
aSLF(3)=0.0 ; aSRF(3)=0.0 ; aSRH(3)=0.0 ;
aSLF(4)=0.0 ; aSRF(4)=0.0 ; aSRH(4)=sta ;
aSLF(5)=0.0 ; aSRF(5)=0.0 ; aSRH(5)=0.0 ;
......@@ -770,14 +771,14 @@ protected:
aSLF(5)=0.0 ; aSRF(5)=0.0 ; aSRH(5)=0.0 ;
lfs_.push_back(aSLF); rfs_.push_back(aSRF); rhs_.push_back(aSRH);
//timing_.push_back(supportPeriod_);
timing_.push_back(finalTime_+3*supportPeriod_/2);
timing_.push_back(finalTime_+supportPeriod_/2);
return 1 ;
}
int readData()
{
std::string dataPath = "/home/mnaveau/devel/mkudruss_data/2015_06_29_16h47m/" ;
std::string dataPath = "/home/mnaveau/devel/mkudruss_data/2015_07_03_10h16m/" ;
std::string dataFile = dataPath + "conv_sd_walking_stair_climbing_2_steps_15cm_ds.csv" ;
std::ifstream dataStream ;
......
/*
* Copyright 2009, 2010, 2014
*
* Maximilien Naveau
* Olivier Stasse,
*
* JRL, CNRS/AIST
*
* This file is part of walkGenJrl.
* walkGenJrl is free software: you can redistribute it and/or modify
* it under the terms of the GNU Lesser General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* walkGenJrl 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 Lesser Public License for more details.
* You should have received a copy of the GNU Lesser General Public License
* along with walkGenJrl. If not, see <http://www.gnu.org/licenses/>.
*
* Research carried out within the scope of the
* Joint Japanese-French Robotics Laboratory (JRL)
*/
/*! \file TestNaveau2015.cpp
\brief This Example shows you how to use the nmpc_genereator.cpp */
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <jrl/mal/matrixabstractlayer.hh>
#include <jrl/dynamics/dynamicsfactory.hh>
#include <jrl/walkgen/patterngeneratorinterface.hh>
#include <hrp2-dynamics/hrp2OptHumanoidDynamicRobot.h>
#include <ZMPRefTrajectoryGeneration/nmpc_generator.hh>
using namespace std;
using namespace PatternGeneratorJRL;
int main()
{
#ifdef WITH_HRP2DYNAMICS
dynamicsJRLJapan::ObjectFactory aRobotDynamicsObjectConstructor;
Chrp2OptHumanoidDynamicRobot * aHRP2HDR = new Chrp2OptHumanoidDynamicRobot( &aRobotDynamicsObjectConstructor );
Chrp2OptHumanoidDynamicRobot * aHDR = aHRP2HDR ;
#endif
string RobotFileName ("/home/mnaveau/devel/ros_unstable/install/share/hrp2-14/HRP2LAAS_main_small.wrl");
string LinkJointRank ("/home/mnaveau/devel/ros_unstable/install/share/hrp2-14/HRP2LinkJointRankSmall.xml");
string SpecificitiesFileName ("/home/mnaveau/devel/ros_unstable/install/share/hrp2-14/HRP2SpecificitiesSmall.xml");
string InitConfigFileName ("/home/mnaveau/devel/ros_unstable/install/share/hrp2-14/HRP2JRLInitConfigSmall.dat");
dynamicsJRLJapan::parseOpenHRPVRMLFile(*aHRP2HDR,
RobotFileName,
LinkJointRank,
SpecificitiesFileName);
PatternGeneratorInterface * aPGI = patternGeneratorInterfaceFactory(aHRP2HDR);
vector<CjrlJoint *> actuatedJoints = aHDR->getActuatedJoints();
unsigned int lNbActuatedJoints = actuatedJoints.size();
double * dInitPos = new double[lNbActuatedJoints];
ifstream aif;
aif.open(InitConfigFileName.c_str(),ifstream::in);
if (aif.is_open())
{
for(unsigned int i=0;i<lNbActuatedJoints;i++)
aif >> dInitPos[i];
}
aif.close();
MAL_VECTOR_DIM(InitialPosition,double,lNbActuatedJoints);
for(unsigned int i=0;i<MAL_VECTOR_SIZE(InitialPosition);i++)
InitialPosition(i) = dInitPos[i]*M_PI/180.0;
aPGI->SetCurrentJointValues(InitialPosition);
istringstream strm2(":walkmode 0");
aPGI->ParseCmd(strm2);
delete [] dInitPos;
SimplePluginManager * aSPM = new SimplePluginManager();
NMPC_generator nmpc_generator (aSPM,aHRP2HDR) ;
nmpc_generator.initNMPC_generator();
return 0;
}
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment