Skip to content
Snippets Groups Projects
Commit 64ec5e3d authored by Nicolas Mansard's avatar Nicolas Mansard
Browse files

Moved dynred2 has the nominal reduced solver.

parent e7135d12
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
python/dynreduced.py
+ 37
− 36
View file @ 64ec5e3d
......@@ -179,8 +179,8 @@ contactRF.support = ((0.11,-0.08,-0.08,0.11),(-0.07,-0.07,0.045,0.045),(-0.105,-
contactRF.feature.frame('desired')
# --- SOT Dyn OpSpaceH --------------------------------------
rs=True # Regular solver
#rs=False # Reduced solver
#rs=True # Regular solver
rs=False # Reduced solver
# SOT controller.
if rs:
......@@ -207,7 +207,7 @@ sot._RF_p.value = contactRF.support
# --- TRACE ----------------------------------------------
'''
from dynamic_graph.tracer import *
tr = Tracer('tr')
tr.open('/tmp/','','.dat')
......@@ -241,41 +241,10 @@ if not rs:
robot.after.addSignal('sot.forcesNormal')
tr.add('sot.activeForces','')
robot.after.addSignal('sot.activeForces')
'''
# --- RUN ------------------------------------------------
taskCom.controlGain.value = 100
#featureComDes.errorIN.value = (0.06, 0.15, 0.8)
#featureComDes.errorIN.value = (0.06, 0.145, 0.8 )
featureComDes.errorIN.value = (0.1, 0.145, 0.8 )
#Nominal: no zmp overshoot featureComDes.errorIN.value = (0.084, 0.144, 0.804 )
sot.push('taskCom')
#@attime(5)
def rm():
featureComDes.errorIN.value = dyn.com.value
attime(500,stop,'stop')
#plug(robot.state,sot.position)
#sot.posture.value = ( 0,0,0,0,0,0, -0.009116303, -0.091409964, -0.471978743, 0.840380193, -0.470232799, 0.089662408, 0.009507818, 0.091110287, -0.469450352, 0.835307995, -0.467686191, -0.093802947, -0.000087565, 0.003264319, -0.000007834, 0.000194743, 0.258370257, -0.175099102, -0.000061173, -0.524953549, 0.000003183, -0.000257600, -0.000003412, 0.258367272, 0.174322098, -0.000088902, -0.524983691, -0.000000346, -0.000265401, 0.3 )
class Chrono:
t0=0
def __init__(self):
self.t0 = time.time()
def tic(self):
print 'elapsed time = ',time.time()-self.t0
chrono=Chrono()
attime(499,chrono.tic)
go()
# --- DEBUG ----------------------------------------------
matlab.prec=9
matlab.fullPrec=0
'''
for i in range(25): inc()
......@@ -320,6 +289,7 @@ else:
'''
'''
if 0: # double check
sotreg = SolverOpSpace('sotreg')
sotreg.setSize(robotDim-6)
......@@ -346,6 +316,37 @@ if 0: # double check
print "ddq1=acceleration; phil1= _LF_f; phir1 = _RF_f; phi1=[ phil1; phir1 ]; fnl1=_LF_fn; fnr1 = _RF_fn; fn1 = [fnl1; fnr1 ]; tau1=control;"
print taskCom.task.m
'''
# --- CHRONO ------------------------------------------------
class Chrono:
t0=0
def __init__(self):
self.t0 = time.time()
def tic(self):
print 'elapsed time = ',time.time()-self.t0
chrono=Chrono()
attime(499,chrono.tic)
# --- RUN ------------------------------------------------
matlab.prec=9
matlab.fullPrec=0
taskCom.controlGain.value = 100
#featureComDes.errorIN.value = (0.06, 0.15, 0.8)
#featureComDes.errorIN.value = (0.06, 0.145, 0.8 )
featureComDes.errorIN.value = (0.1, 0.145, 0.8 )
#Nominal: no zmp overshoot featureComDes.errorIN.value = (0.084, 0.144, 0.804 )
sot.push('taskCom')
#@attime(5)
def rm():
featureComDes.errorIN.value = dyn.com.value
attime(500,stop,'stop')
go()
src/dynamic_graph/sot/dyninv/__init__.py
+ 1
− 3
View file @ 64ec5e3d
......@@ -12,9 +12,7 @@ PseudoRobotDynamic('')
from solver_op_space import SolverOpSpace
SolverOpSpace('')
#from solver_dyn_reduced import SolverDynReduced
from solver_dyn_red2 import SolverDynRed2
SolverDynReduced = SolverDynRed2
from solver_dyn_reduced import SolverDynReduced
SolverDynReduced('')
from solver_kine import SolverKine
......
src/solver-dyn-red2.cpp deleted 100644 → 0
+ 0
− 1174
View file @ e7135d12
This diff is collapsed.
src/solver-dyn-red2.h deleted 100644 → 0
+ 0
− 192
View file @ e7135d12
/*
* Copyright 2011, Nicolas Mansard, LAAS-CNRS
*
* This file is part of sot-dyninv.
* sot-dyninv 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.
* sot-dyninv 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 Lesser General Public License for more details. You should
* have received a copy of the GNU Lesser General Public License along
* with sot-dyninv. If not, see <http://www.gnu.org/licenses/>.
*/
#ifndef __sot_dyninv_SolverDynReduced_H__
#define __sot_dyninv_SolverDynReduced_H__
/* --------------------------------------------------------------------- */
/* --- API ------------------------------------------------------------- */
/* --------------------------------------------------------------------- */
#if defined (WIN32)
# if defined (solver_op_space_EXPORTS)
# define SOTSOLVERDYNREDUCED_EXPORT __declspec(dllexport)
# else
# define SOTSOLVERDYNREDUCED_EXPORT __declspec(dllimport)
# endif
#else
# define SOTSOLVERDYNREDUCED_EXPORT
#endif
/* --------------------------------------------------------------------- */
/* --- INCLUDE --------------------------------------------------------- */
/* --------------------------------------------------------------------- */
/* SOT */
#include <sot-dyninv/signal-helper.h>
#include <sot-dyninv/entity-helper.h>
#include <sot-dyninv/stack-template.h>
#include <sot-dyninv/task-dyn-pd.h>
#include <soth/HCOD.hpp>
#include <Eigen/QR>
#include <Eigen/SVD>
namespace dynamicgraph {
namespace sot {
namespace dyninv {
/* --------------------------------------------------------------------- */
/* --- CLASS ----------------------------------------------------------- */
/* --------------------------------------------------------------------- */
class SOTSOLVERDYNREDUCED_EXPORT SolverDynReduced
:public ::dynamicgraph::Entity
,public ::dynamicgraph::EntityHelper<SolverDynReduced>
,public sot::Stack< TaskDynPD >
{
public: /* --- CONSTRUCTOR ---- */
SolverDynReduced( const std::string & name );
public: /* --- STACK INHERITANCE --- */
typedef sot::Stack<TaskDynPD> stack_t;
using stack_t::TaskDependancyList_t;
using stack_t::StackIterator_t;
using stack_t::StackConstIterator_t;
using stack_t::stack;
virtual TaskDependancyList_t getTaskDependancyList( const TaskDynPD& task );
virtual void addDependancy( const TaskDependancyList_t& depList );
virtual void removeDependancy( const TaskDependancyList_t& depList );
virtual void resetReady( void );
public: /* --- ENTITY INHERITANCE --- */
static const std::string CLASS_NAME;
virtual void display( std::ostream& os ) const;
virtual const std::string& getClassName( void ) const { return CLASS_NAME; }
public: /* --- SIGNALS --- */
DECLARE_SIGNAL_IN(matrixInertia,ml::Matrix);
DECLARE_SIGNAL_IN(inertiaSqroot,ml::Matrix);
DECLARE_SIGNAL_IN(inertiaSqrootInv,ml::Matrix);
DECLARE_SIGNAL_IN(velocity,ml::Vector);
DECLARE_SIGNAL_IN(dyndrift,ml::Vector);
DECLARE_SIGNAL_IN(damping,double);
DECLARE_SIGNAL_IN(breakFactor,double);
DECLARE_SIGNAL_IN(posture,ml::Vector);
DECLARE_SIGNAL_IN(position,ml::Vector);
DECLARE_SIGNAL_OUT(precompute,int);
DECLARE_SIGNAL_OUT(inertiaSqrootOut,ml::Matrix);
DECLARE_SIGNAL_OUT(inertiaSqrootInvOut,ml::Matrix);
DECLARE_SIGNAL_OUT(sizeForcePoint,int);
DECLARE_SIGNAL_OUT(sizeForceSpatial,int);
DECLARE_SIGNAL_OUT(sizeConfiguration,int);
DECLARE_SIGNAL_OUT(Jc,ml::Matrix);
DECLARE_SIGNAL_OUT(forceGenerator,ml::Matrix);
DECLARE_SIGNAL_OUT(freeMotionBase,ml::Matrix);
DECLARE_SIGNAL_OUT(freeForceBase,ml::Matrix);
DECLARE_SIGNAL_OUT(driftContact,ml::Vector);
DECLARE_SIGNAL_OUT(sizeMotion,int);
DECLARE_SIGNAL_OUT(sizeActuation,int);
DECLARE_SIGNAL_OUT(solution,ml::Vector);
DECLARE_SIGNAL_OUT(reducedControl,ml::Vector);
DECLARE_SIGNAL_OUT(reducedForce,ml::Vector);
DECLARE_SIGNAL_OUT(acceleration,ml::Vector);
DECLARE_SIGNAL_OUT(forces,ml::Vector);
DECLARE_SIGNAL_OUT(torque,ml::Vector);
DECLARE_SIGNAL_OUT(forcesNormal,ml::Vector);
DECLARE_SIGNAL_OUT(activeForces,ml::Vector);
/* Temporary time-dependant shared variables. */
DECLARE_SIGNAL(Jcdot,OUT,ml::Matrix);
private: /* --- CONTACT POINTS --- */
typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Matrix,int> > matrixSINPtr;
typedef boost::shared_ptr<dynamicgraph::SignalPtr<ml::Vector,int> > vectorSINPtr;
typedef boost::shared_ptr<dynamicgraph::Signal<ml::Vector,int> > vectorSOUTPtr;
struct Contact
{
matrixSINPtr jacobianSIN;
matrixSINPtr JdotSIN;
matrixSINPtr supportSIN;
vectorSINPtr correctorSIN;
vectorSOUTPtr forceSOUT,fnSOUT;
int position;
std::pair<int,int> range;
};
typedef std::map< std::string,Contact > contacts_t;
contacts_t contactMap;
public:
void addContact( const std::string & name,
dynamicgraph::Signal<ml::Matrix,int> * jacobianSignal,
dynamicgraph::Signal<ml::Matrix,int> * JdotSignal,
dynamicgraph::Signal<ml::Vector,int> * corrSignal,
dynamicgraph::Signal<ml::Matrix,int> * contactPointsSignal );
void addContactFromTask( const std::string & taskName, const std::string & contactName );
void removeContact( const std::string & name );
void dispContacts( std::ostream& os ) const;
public: /* --- COMMANDS --- */
void debugOnce( void );
private: /* --- INTERNAL COMPUTATIONS --- */
void refreshTaskTime( int time );
void resizeSolver( void );
void computeSizesForce( int t );
private:
typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
hcod_ptr_t hsolver;
//Eigen::FullPivHouseholderQR<Eigen::MatrixXd> Gt_qr;
Eigen::JacobiSVD<Eigen::MatrixXd> G_svd;
int G_rank;
Eigen::FullPivHouseholderQR<Eigen::MatrixXd> X_qr;
Eigen::MatrixXd Cforce,Czero;
soth::VectorBound bforce,bzero;
std::vector< Eigen::MatrixXd > Ctasks;
std::vector< soth::VectorBound > btasks;
Eigen::MatrixXd BV;
/* Force drift = xddot^* - Jdot qdot. */
Eigen::VectorXd solution,forceDrift;
}; // class SolverDynReduced
} // namespace dyninv
} // namespace sot
} // namespace dynamicgraph
#endif // #ifndef __sot_dyninv_SolverDynReduced_H__
src/solver-dyn-reduced.cpp
+ 346
− 108
View file @ 64ec5e3d
This diff is collapsed.
src/solver-dyn-reduced.h
+ 18
− 4
View file @ 64ec5e3d
......@@ -93,24 +93,34 @@ namespace dynamicgraph {
DECLARE_SIGNAL_IN(posture,ml::Vector);
DECLARE_SIGNAL_IN(position,ml::Vector);
DECLARE_SIGNAL_OUT(precompute,int);
DECLARE_SIGNAL_OUT(inertiaSqrootOut,ml::Matrix);
DECLARE_SIGNAL_OUT(inertiaSqrootInvOut,ml::Matrix);
DECLARE_SIGNAL_OUT(sizeForce,int);
DECLARE_SIGNAL_OUT(Jc,ml::Matrix);
DECLARE_SIGNAL_OUT(sizeForcePoint,int);
DECLARE_SIGNAL_OUT(sizeForceSpatial,int);
DECLARE_SIGNAL_OUT(sizeConfiguration,int);
DECLARE_SIGNAL_OUT(Jc,ml::Matrix);
DECLARE_SIGNAL_OUT(forceGenerator,ml::Matrix);
DECLARE_SIGNAL_OUT(freeMotionBase,ml::Matrix);
DECLARE_SIGNAL_OUT(freeForceBase,ml::Matrix);
DECLARE_SIGNAL_OUT(driftContact,ml::Vector);
DECLARE_SIGNAL_OUT(sizeMotion,int);
DECLARE_SIGNAL_OUT(sizeActuation,int);
DECLARE_SIGNAL_OUT(solution,ml::Vector);
DECLARE_SIGNAL_OUT(reducedControl,ml::Vector);
DECLARE_SIGNAL_OUT(reducedForce,ml::Vector);
DECLARE_SIGNAL_OUT(acceleration,ml::Vector);
DECLARE_SIGNAL_OUT(forces,ml::Vector);
DECLARE_SIGNAL_OUT(torque,ml::Vector);
DECLARE_SIGNAL_OUT(forcesNormal,ml::Vector);
DECLARE_SIGNAL_OUT(activeForces,ml::Vector);
/* Temporary time-dependant shared variables. */
DECLARE_SIGNAL(Jcdot,OUT,ml::Matrix);
......@@ -126,6 +136,7 @@ namespace dynamicgraph {
matrixSINPtr supportSIN;
vectorSINPtr correctorSIN;
vectorSOUTPtr forceSOUT,fnSOUT;
int position;
std::pair<int,int> range;
};
typedef std::map< std::string,Contact > contacts_t;
......@@ -149,7 +160,8 @@ namespace dynamicgraph {
private: /* --- INTERNAL COMPUTATIONS --- */
void refreshTaskTime( int time );
void resizeSolver( void );
void computeSizesForce( int t );
private:
typedef boost::shared_ptr<soth::HCOD> hcod_ptr_t;
hcod_ptr_t hsolver;
......@@ -157,6 +169,7 @@ namespace dynamicgraph {
//Eigen::FullPivHouseholderQR<Eigen::MatrixXd> Gt_qr;
Eigen::JacobiSVD<Eigen::MatrixXd> G_svd;
int G_rank;
Eigen::FullPivHouseholderQR<Eigen::MatrixXd> X_qr;
Eigen::MatrixXd Cforce,Czero;
soth::VectorBound bforce,bzero;
......@@ -165,6 +178,7 @@ namespace dynamicgraph {
Eigen::MatrixXd BV;
/* Force drift = xddot^* - Jdot qdot. */
Eigen::VectorXd solution,forceDrift;
}; // class SolverDynReduced
......
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