Skip to content
GitLab
Menu
Projects
Groups
Snippets
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Guilhem Saurel
sot-hrp2
Commits
e2b85995
Commit
e2b85995
authored
Dec 12, 2011
by
olivier stasse
Browse files
Implement the one robot one sot-library strategy.
parent
fa3f1f27
Changes
12
Hide whitespace changes
Inline
Side-by-side
src/CMakeLists.txt
View file @
e2b85995
...
...
@@ -58,27 +58,37 @@ SET(PYTHON_MODULE_BUILD_DIR
SET
(
PYTHON_MODULE dynamic_graph/sot/hrp2
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"__init__.py"
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"hrp2.py"
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"prologue.py"
)
SET
(
FILES __init__.py robot.py
)
# Install dynamic_graph.sot.hrp2_14
SET
(
PYTHON_MODULE dynamic_graph/sot/hrp2_14
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"__init__.py"
)
PYTHON_INSTALL_BUILD
(
"
${
PYTHON_MODULE
}
"
"robot.py"
${
PYTHON_SITELIB
}
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"prologue.py"
)
# Install dynamic_graph.sot.hrp2_10
SET
(
PYTHON_MODULE dynamic_graph/sot/hrp2_10
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"__init__.py"
)
PYTHON_INSTALL_BUILD
(
"
${
PYTHON_MODULE
}
"
"robot.py"
${
PYTHON_SITELIB
}
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"prologue.py"
)
# Add the library to wrap the controller of HRP2.
ADD_LIBRARY
(
sot-hrp2-controller
# Add the library to wrap the controller of HRP2-JRL.
MACRO
(
build_hrp2_controller robotnumber
)
ADD_LIBRARY
(
sot-hrp2-
${
robotnumber
}
-controller
SHARED
sot-hrp2-controller.cpp
sot-hrp2-
${
robotnumber
}
-controller.cpp
)
# Link the dynamic library containing the SoT with its dependencies.
PKG_CONFIG_USE_DEPENDENCY
(
sot-hrp2-controller
"dynamic-graph"
)
PKG_CONFIG_USE_DEPENDENCY
(
sot-hrp2-controller
"sot-core"
)
PKG_CONFIG_USE_DEPENDENCY
(
sot-hrp2-controller
"dynamic-graph-corba"
)
PKG_CONFIG_USE_DEPENDENCY
(
sot-hrp2-
${
robotnumber
}
-controller
"dynamic-graph"
)
PKG_CONFIG_USE_DEPENDENCY
(
sot-hrp2-
${
robotnumber
}
-controller
"sot-core"
)
PKG_CONFIG_USE_DEPENDENCY
(
sot-hrp2-
${
robotnumber
}
-controller
"dynamic-graph-corba"
)
INSTALL
(
TARGETS sot-hrp2-
${
robotnumber
}
-controller DESTINATION lib
)
ENDMACRO
()
INSTALL
(
TARGETS sot-hrp2-controller DESTINATION lib/
${
ROBOTNAME
}
)
\ No newline at end of file
build_hrp2_controller
(
"10"
)
build_hrp2_controller
(
"14"
)
\ No newline at end of file
src/dynamic_graph/sot/hrp2/prologue.py
0 → 100644
View file @
e2b85995
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph 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.
#
# dynamic-graph 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
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
print
(
"Prologue HRP-2"
)
from
dynamic_graph
import
plug
from
dynamic_graph.entity
import
PyEntityFactoryClass
from
dynamic_graph.sot.dynamics.solver
import
Solver
def
prologue_hrp2
(
hrp2class
):
# Create the device.
# This entity behaves exactly like robotsimu except:
# 1. it does not provide the increment method
# 2. it forwards the robot control to the sot-abstract
# controller.
Device
=
PyEntityFactoryClass
(
'Device'
)
# Create the robot using the device.
robot
=
hrp2class
(
name
=
'robot'
,
device
=
Device
(
'robot_device'
))
# Initialize the zmp signal to the current com.
_com
=
robot
.
dynamic
.
com
.
value
robot
.
device
.
zmp
.
value
=
(
_com
[
0
],
_com
[
1
],
0.
)
# Create a solver.
solver
=
Solver
(
robot
)
# Make sure com and feet desired positions match the current
# positions.
s
=
[
'left-ankle'
,
'right-ankle'
]
for
i
in
s
:
robot
.
dynamic
.
signal
(
i
).
recompute
(
robot
.
dynamic
.
signal
(
i
).
time
+
1
)
robot
.
features
[
i
].
reference
.
value
=
\
robot
.
dynamic
.
signal
(
i
).
value
robot
.
features
[
i
].
_feature
.
selec
.
value
=
'111111'
robot
.
tasks
[
i
].
controlGain
.
value
=
180.
robot
.
featureComDes
.
errorIN
.
value
=
robot
.
dynamic
.
com
.
value
robot
.
featureComDes
.
selec
.
value
=
'111'
robot
.
comTask
.
controlGain
.
value
=
180.
# Push com and feet tasks.
#
# The robot is currently in half-sitting, so this script freezes com
# and feet position so that the robot will remain stable while the
# user program is starting.
solver
.
push
(
robot
.
comTask
.
name
)
for
i
in
s
:
solver
.
push
(
robot
.
tasks
[
i
].
name
)
print
(
"Prologue ran successfully."
)
# Make sure only robot and solver are visible from the outside.
__all__
=
[
"robot"
,
"solver"
]
####################################
# --- IMPORTANT --- #
# #
# THIS FILE MUST NEVER BE CHANGED. #
# TO RUN YOUR EXPERIMENT, PLEASE #
# WRITE A SEPARATE PYTHON MODULE #
# AND LAUNCH IT USING dg-remote! #
####################################
src/dynamic_graph/sot/hrp2_10/prologue.py
0 → 100644
View file @
e2b85995
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph 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.
#
# dynamic-graph 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
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
print
(
"Prologue HRP2-14"
)
from
dynamic_graph.sot.hrp2_10.robot
import
*
from
dynamic_graph.sot.hrp2.prologue
import
*
robot
,
solver
=
prologue_hrp2
(
"Robot"
)
__all__
=
[
"robot"
,
"solver"
]
####################################
# --- IMPORTANT --- #
# #
# THIS FILE MUST NEVER BE CHANGED. #
# TO RUN YOUR EXPERIMENT, PLEASE #
# WRITE A SEPARATE PYTHON MODULE #
# AND LAUNCH IT USING dg-remote! #
####################################
src/dynamic_graph/sot/hrp2_10/robot.py.in
View file @
e2b85995
...
...
@@ -47,4 +47,4 @@ class Robot (Hrp2):
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
tracer)
__all__ = [Robot]
__all__ = [
"
Robot
"
]
src/dynamic_graph/sot/hrp2_14/prologue.py
0 → 100644
View file @
e2b85995
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph 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.
#
# dynamic-graph 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
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
print
(
"Prologue HRP2-14"
)
from
dynamic_graph.sot.hrp2_14.robot
import
*
from
dynamic_graph.sot.hrp2.prologue
import
*
robot
,
solver
=
prologue_hrp2
(
"Robot"
)
__all__
=
[
"robot"
,
"solver"
]
####################################
# --- IMPORTANT --- #
# #
# THIS FILE MUST NEVER BE CHANGED. #
# TO RUN YOUR EXPERIMENT, PLEASE #
# WRITE A SEPARATE PYTHON MODULE #
# AND LAUNCH IT USING dg-remote! #
####################################
src/dynamic_graph/sot/hrp2_14/robot.py.in
View file @
e2b85995
...
...
@@ -111,4 +111,4 @@ class Robot (Hrp2):
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2, tracer)
__all__ = [Robot]
__all__ = [
"
Robot
"
]
src/sot-hrp2-10-controller.cpp
0 → 100644
View file @
e2b85995
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#include
<sot/core/debug.hh>
#define ROBOTNAME std::string("HRP2JRL")
#include
"sot-hrp2-10-controller.hh"
const
std
::
string
SoTHRP2_10_Controller
::
LOG_PYTHON_10
=
"/tmp/HRP2Controller_10_python.out"
;
SoTHRP2_10_Controller
::
SoTHRP2_10_Controller
()
:
SoTHRP2Controller
(
ROBOTNAME
)
{
std
::
cout
<<
__FILE__
<<
" "
<<
__FUNCTION__
<<
":"
<<
getClassName
()
<<
std
::
endl
;
startupPython
();
}
void
SoTHRP2_10_Controller
::
startupPython
()
{
SoTHRP2Controller
::
startupPython
();
std
::
ofstream
aof
(
LOG_PYTHON_10
.
c_str
());
runPython
(
aof
,
"from dynamic_graph.sot.hrp2_14.prologue import robot, solver"
,
interpreter_
);
interpreter_
.
startCorbaServer
(
"openhrp"
,
""
,
"stackOfTasks"
,
""
);
aof
.
close
();
}
extern
"C"
{
dgsot
::
AbstractSotExternalInterface
*
createSotExternalInterface
()
{
return
new
SoTHRP2_10_Controller
;
}
}
extern
"C"
{
void
destroySotExternalInterface
(
dgsot
::
AbstractSotExternalInterface
*
p
)
{
delete
p
;
}
}
src/sot-hrp2-10-controller.hh
0 → 100644
View file @
e2b85995
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#ifndef _SOT_HRP2_10_Controller_H_
#define _SOT_HRP2_10_Controller_H_
#include
"sot-hrp2-controller.hh"
namespace
dgsot
=
dynamicgraph
::
sot
;
class
SoTHRP2_10_Controller
:
public
SoTHRP2Controller
{
public:
static
const
std
::
string
LOG_PYTHON_10
;
SoTHRP2_10_Controller
();
virtual
~
SoTHRP2_10_Controller
()
{};
protected:
virtual
void
startupPython
();
};
#endif
/* _SOT_HRP2Controller_H_ */
src/sot-hrp2-14-controller.cpp
0 → 100644
View file @
e2b85995
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#include
<sot/core/debug.hh>
#define ROBOTNAME std::string("HRP2JRL")
#include
"sot-hrp2-14-controller.hh"
const
std
::
string
SoTHRP2_14_Controller
::
LOG_PYTHON_14
=
"/tmp/HRP2Controller_14_python.out"
;
SoTHRP2_14_Controller
::
SoTHRP2_14_Controller
()
:
SoTHRP2Controller
(
ROBOTNAME
)
{
startupPython
();
}
void
SoTHRP2_14_Controller
::
startupPython
()
{
SoTHRP2Controller
::
startupPython
();
std
::
ofstream
aof
(
LOG_PYTHON_14
.
c_str
());
runPython
(
aof
,
"from dynamic_graph.sot.hrp2_14.prologue import robot, solver"
,
interpreter_
);
interpreter_
.
startCorbaServer
(
"openhrp"
,
""
,
"stackOfTasks"
,
""
);
}
extern
"C"
{
dgsot
::
AbstractSotExternalInterface
*
createSotExternalInterface
()
{
return
new
SoTHRP2_14_Controller
;
}
}
extern
"C"
{
void
destroySotExternalInterface
(
dgsot
::
AbstractSotExternalInterface
*
p
)
{
delete
p
;
}
}
src/sot-hrp2-14-controller.hh
0 → 100644
View file @
e2b85995
/*
* Copyright 2011,
*
* Olivier Stasse
*
* LAAS, CNRS
*
* This file is part of HRP2Controller.
* HRP2Controller is not a free software,
* it contains information related to HRP-2 which involves
* that you either purchased the proper license to havec access to
* those informations, or that you signed the appropriate
* Non-Disclosure agreement.
*
*
*/
#ifndef _SOT_HRP2_14_Controller_H_
#define _SOT_HRP2_14_Controller_H_
#include
"sot-hrp2-controller.hh"
namespace
dgsot
=
dynamicgraph
::
sot
;
class
SoTHRP2_14_Controller
:
public
SoTHRP2Controller
{
public:
static
const
std
::
string
LOG_PYTHON_14
;
SoTHRP2_14_Controller
();
virtual
~
SoTHRP2_14_Controller
()
{};
protected:
virtual
void
startupPython
();
};
#endif
/* _SOT_HRP2Controller_H_ */
src/sot-hrp2-controller.cpp
View file @
e2b85995
...
...
@@ -20,7 +20,6 @@
#include
"sot-hrp2-controller.hh"
#define ENTITYNAME std::string("DLRBiped")
const
std
::
string
SoTHRP2Controller
::
CLASS_NAME
=
"Device"
;
const
std
::
string
SoTHRP2Controller
::
LOG_PYTHON
=
"/tmp/HRP2Controller_python.out"
;
...
...
@@ -28,17 +27,16 @@ const double SoTHRP2Controller::TIMESTEP_DEFAULT = 0.005;
using
namespace
std
;
SoTHRP2Controller
::
SoTHRP2Controller
()
:
dgsot
::
Device
(
ENTITYNAME
),
SoTHRP2Controller
::
SoTHRP2Controller
(
std
::
string
RobotName
)
:
dgsot
::
Device
(
RobotName
),
timestep_
(
TIMESTEP_DEFAULT
),
previousState_
(),
robotState_
(
"StackOfTasks("
+
ENTITYNAME
+
")::output(vector)::robotState"
),
robotState_
(
"StackOfTasks("
+
RobotName
+
")::output(vector)::robotState"
),
interpreter_
()
{
signalRegistration
(
robotState_
);
std
::
cout
<<
__FILE__
<<
":"
<<
__FUNCTION__
<<
"(#"
<<
__LINE__
<<
" )"
<<
std
::
endl
;
startupPython
();
}
...
...
@@ -212,25 +210,7 @@ void SoTHRP2Controller::startupPython()
" path.append(p)"
,
interpreter_
);
runPython
(
aof
,
"path.extend(sys.path)"
,
interpreter_
);
runPython
(
aof
,
"sys.path = path"
,
interpreter_
);
runPython
(
aof
,
"from dynamic_graph.sot.openhrp.prologue_dlr_biped import robot, solver"
,
interpreter_
);
interpreter_
.
startCorbaServer
(
"openhrp"
,
""
,
"stackOfTasks"
,
""
);
aof
.
close
();
}
extern
"C"
{
dgsot
::
AbstractSotExternalInterface
*
createSotExternalInterface
()
{
return
new
SoTHRP2Controller
;
}
}
extern
"C"
{
void
destroySotExternalInterface
(
dgsot
::
AbstractSotExternalInterface
*
p
)
{
delete
p
;
}
}
src/sot-hrp2-controller.hh
View file @
e2b85995
...
...
@@ -43,8 +43,8 @@ class SoTHRP2Controller: public
return
CLASS_NAME
;
}
SoTHRP2Controller
();
~
SoTHRP2Controller
();
SoTHRP2Controller
(
std
::
string
RobotName
);
virtual
~
SoTHRP2Controller
();
void
setupSetSensors
(
std
::
map
<
std
::
string
,
dgsot
::
SensorValues
>
&
sensorsIn
);
...
...
@@ -54,7 +54,7 @@ class SoTHRP2Controller: public
void
getControl
(
std
::
map
<
std
::
string
,
dgsot
::
ControlValues
>
&
anglesOut
);
pr
ivate
:
pr
otected
:
// Update output port with the control computed from the
// dynamic graph.
void
updateRobotState
(
std
::
vector
<
double
>
&
anglesIn
);
...
...
@@ -63,7 +63,7 @@ class SoTHRP2Controller: public
const
std
::
string
&
command
,
dynamicgraph
::
corba
::
Interpreter
&
interpreter
);
void
startupPython
();
virtual
void
startupPython
();
/// \brief Current integration step.
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment