Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
Stack Of Tasks
sot-hrp2
Commits
26e04e00
Commit
26e04e00
authored
Mar 21, 2016
by
Rohan Budhiraja
Browse files
compatible with Eigen and pinocchio. adjust specificities requirement
TODO: load specificities with srdf.
parent
e9e43aaf
Changes
9
Hide whitespace changes
Inline
Side-by-side
CMakeLists.txt
View file @
26e04e00
...
...
@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE
(
cmake/base.cmake
)
INCLUDE
(
cmake/boost.cmake
)
INCLUDE
(
cmake/eigen.cmake
)
INCLUDE
(
cmake/lapack.cmake
)
INCLUDE
(
cmake/cpack.cmake
)
INCLUDE
(
cmake/ros.cmake
)
...
...
@@ -38,10 +39,8 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
SETUP_PROJECT
()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY
(
"jrl-mathtools"
)
ADD_REQUIRED_DEPENDENCY
(
"jrl-mal >= 1.9.0"
)
ADD_REQUIRED_DEPENDENCY
(
"
jrl-dynamics >= 1.19.0
"
)
ADD_REQUIRED_DEPENDENCY
(
"
pinocchio
"
)
ADD_OPTIONAL_DEPENDENCY
(
"hrp2-dynamics >= 1.5.0"
)
ADD_OPTIONAL_DEPENDENCY
(
"hrp2-10-optimized >= 1.0.1"
)
...
...
@@ -50,7 +49,7 @@ ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6")
ADD_REQUIRED_DEPENDENCY
(
"dynamic-graph"
)
ADD_REQUIRED_DEPENDENCY
(
"sot-core"
)
ADD_REQUIRED_DEPENDENCY
(
"sot-dynamic >=
2.8
"
)
ADD_REQUIRED_DEPENDENCY
(
"sot-dynamic >=
3.1
"
)
ADD_REQUIRED_DEPENDENCY
(
"dynamic-graph-python"
)
ADD_REQUIRED_DEPENDENCY
(
"dynamic_graph_bridge_msgs"
)
...
...
@@ -59,6 +58,7 @@ ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge")
# Boost
SET
(
BOOST_COMPONENTS filesystem system thread
)
SEARCH_FOR_BOOST
()
SEARCH_FOR_EIGEN
()
# Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion
...
...
src/CMakeLists.txt
View file @
26e04e00
...
...
@@ -24,10 +24,9 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
SOVERSION
${
PROJECT_VERSION
}
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
dynamic-graph
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
jrl-mathtools
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
jrl-mal
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
eigen3
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
sot-core
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
jrl-dynamics
)
PKG_CONFIG_USE_DEPENDENCY
(
${
NAME
}
pinocchio
)
INSTALL
(
TARGETS
${
NAME
}
DESTINATION
${
CMAKE_INSTALL_LIBDIR
}
/plugin
)
...
...
@@ -41,25 +40,25 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
ENDFUNCTION
()
# Compile plug-ins.
IF
(
HRP2_DYNAMICS_FOUND
)
MESSAGE
(
STATUS
"Add dynamic-hrp2_14 plugin"
)
COMPILE_PLUGIN
(
dynamic-hrp2_14 dynamic-hrp2_14.cc DynamicHrp2_14
)
PKG_CONFIG_USE_DEPENDENCY
(
dynamic-hrp2_14 hrp2-dynamics
)
ENDIF
()
IF
(
HRP2_10_OPTIMIZED_FOUND
)
MESSAGE
(
STATUS
"Add dynamic-hrp2_10 plugin"
)
COMPILE_PLUGIN
(
dynamic-hrp2_10 dynamic-hrp2_10.cc DynamicHrp2_10
)
PKG_CONFIG_USE_DEPENDENCY
(
dynamic-hrp2_10 hrp2-10-optimized
)
ENDIF
()
IF
(
HRP2_10_OPTIMIZED_FOUND
)
CONFIG_FILES
(
dynamic_graph/sot/hrp2_10/robot.py
)
ENDIF
()
IF
(
HRP2_DYNAMICS_FOUND
)
CONFIG_FILES
(
dynamic_graph/sot/hrp2_14/robot.py
)
ENDIF
()
#
IF (HRP2_DYNAMICS_FOUND)
#
MESSAGE(STATUS "Add dynamic-hrp2_14 plugin")
#
COMPILE_PLUGIN(dynamic-hrp2_14 dynamic-hrp2_14.cc DynamicHrp2_14)
#
PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_14 hrp2-dynamics)
#
ENDIF()
#
IF (HRP2_10_OPTIMIZED_FOUND)
#
MESSAGE(STATUS "Add dynamic-hrp2_10 plugin")
#
COMPILE_PLUGIN(dynamic-hrp2_10 dynamic-hrp2_10.cc DynamicHrp2_10)
#
PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_10 hrp2-10-optimized)
#
ENDIF()
#
IF (HRP2_10_OPTIMIZED_FOUND)
#
CONFIG_FILES(dynamic_graph/sot/hrp2_10/robot.py)
#
ENDIF()
#
IF (HRP2_DYNAMICS_FOUND)
#
CONFIG_FILES(dynamic_graph/sot/hrp2_14/robot.py)
#
ENDIF()
# Install Python files.
SET
(
PYTHON_MODULE_DIR
...
...
@@ -70,23 +69,20 @@ 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"
)
SET
(
FILES __init__.py robot.py
)
# Install dynamic_graph.sot.hrp2_14
IF
(
HRP2_DYNAMICS_FOUND
)
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"
)
ENDIF
()
SET
(
PYTHON_MODULE dynamic_graph/sot/hrp2_14
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"__init__.py"
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"robot.py"
"
${
PYTHON_SITELIB
}
"
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"prologue.py"
)
# Install dynamic_graph.sot.hrp2_10
IF
(
HRP2_10_OPTIMIZED_FOUND
)
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"
)
ENDIF
()
SET
(
PYTHON_MODULE dynamic_graph/sot/hrp2_10
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"__init__.py"
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"robot.py"
"
${
PYTHON_SITELIB
}
"
)
PYTHON_INSTALL_ON_SITE
(
"
${
PYTHON_MODULE
}
"
"prologue.py"
)
# Add the library to wrap the device of HRP2.
MACRO
(
build_hrp2_device
)
...
...
@@ -152,11 +148,15 @@ MACRO(build_hrp2_controller robotnumber)
ENDMACRO
()
IF
(
HRP2_10_OPTIMIZED_FOUND
)
build_hrp2_controller
(
"10"
)
ENDIF
()
#TODO: which? 10 or 14?
build_hrp2_controller
(
"14"
)
build_hrp2_controller
(
"10"
)
#IF (HRP2_10_OPTIMIZED_FOUND)
# build_hrp2_controller("10")
#ENDIF()
IF
(
HRP2_DYNAMICS_FOUND
)
build_hrp2_controller
(
"14"
)
ENDIF
()
#
IF (HRP2_DYNAMICS_FOUND)
#
build_hrp2_controller("14")
#
ENDIF()
src/dynamic-hrp2_10.cc
View file @
26e04e00
...
...
@@ -32,7 +32,7 @@ namespace dynamicgraph
namespace
hrp2
{
DynamicHrp2_10
::
DynamicHrp2_10
(
const
std
::
string
&
name
)
:
Dynamic
(
name
,
false
)
:
Dynamic
(
name
)
{
sotDEBUGIN
(
15
);
DynamicHrp2_10
::
buildModelHrp2
();
...
...
src/dynamic_graph/sot/hrp2/hrp2.py
View file @
26e04e00
...
...
@@ -17,15 +17,13 @@
from
__future__
import
print_function
import
numpy
as
np
from
dynamic_graph.sot.core
import
\
FeatureGeneric
,
FeatureJointLimits
,
Task
,
Constraint
,
GainAdaptive
,
SOT
from
dynamic_graph.sot.dynamics
import
AngleEstimator
from
dynamic_graph
import
enableTrace
,
plug
# from dynamic_graph.sot.dynamics.dynamic_hrp2 import DynamicHrp2
# from dynamic_graph.sot.dynamics.dynamic_hrp2_10 import DynamicHrp2_10
#Don't change this order
from
dynamic_graph.sot.dynamics.humanoid_robot
import
AbstractHumanoidRobot
from
dynamic_graph.ros
import
RosRobotModel
from
rospkg
import
RosPack
# Internal helper tool.
def
matrixToTuple
(
M
):
...
...
@@ -67,18 +65,16 @@ class Hrp2(AbstractHumanoidRobot):
res
=
(
config
+
10
*
(
0.
,))
return
res
def
__init__
(
self
,
name
,
modelDir
,
xmlDir
,
device
,
dynamicType
,
def
__init__
(
self
,
name
,
robotnumber
,
device
=
None
,
tracer
=
None
):
AbstractHumanoidRobot
.
__init__
(
self
,
name
,
tracer
)
self
.
OperationalPoints
.
append
(
'waist'
)
self
.
OperationalPoints
.
append
(
'chest'
)
self
.
device
=
device
self
.
modelDir
=
modelDir
self
.
modelName
=
'HRP2JRLmainsmall.wrl'
self
.
specificitiesPath
=
xmlDir
+
'/HRP2SpecificitiesSmall.xml'
self
.
jointRankPath
=
xmlDir
+
'/HRP2LinkJointRankSmall.xml'
self
.
AdditionalFrames
.
append
(
(
"accelerometer"
,
matrixToTuple
(
self
.
accelerometerPosition
),
"chest"
))
...
...
@@ -90,22 +86,28 @@ class Hrp2(AbstractHumanoidRobot):
self
.
forceSensorInLeftAnkle
,
"left-ankle"
))
self
.
AdditionalFrames
.
append
(
(
"rightFootForceSensor"
,
self
.
forceSensorInRightAnkle
,
"right-ankle"
))
self
.
forceSensorInRightAnkle
,
"right-ankle"
))
self
.
OperationalPointsMap
=
{
'left-wrist'
:
'LARM_JOINT5'
,
'right-wrist'
:
'RARM_JOINT5'
,
'left-ankle'
:
'LLEG_JOINT5'
,
'right-ankle'
:
'RLEG_JOINT5'
,
'gaze'
:
'HEAD_JOINT1'
,
'waist'
:
'WAIST'
,
'chest'
:
'CHEST_JOINT1'
}
self
.
dynamic
=
RosRobotModel
(
"{0}_dynamic"
.
format
(
name
))
self
.
dynamic
=
self
.
loadModelFromJrlDynamics
(
self
.
name
+
'_dynamic'
,
modelDir
,
self
.
modelName
,
self
.
specificitiesPath
,
self
.
jointRankPath
,
dynamicType
)
rospack
=
RosPack
()
self
.
urdfPath
=
rospack
.
get_path
(
'hrp2_{0}_description'
.
format
(
robotnumber
))
+
'/urdf/hrp2_{0}.urdf'
.
format
(
robotnumber
)
self
.
dynamic
.
loadUrdf
(
self
.
urdfPath
)
self
.
dimension
=
self
.
dynamic
.
getDimension
()
self
.
dynamic
.
displayModel
()
self
.
plugVelocityFromDevice
=
True
if
self
.
dimension
!=
len
(
self
.
halfSitting
):
raise
RuntimeError
(
"Dimension of half-sitting: {0} differs from dimension of robot: {1}"
.
format
(
len
(
self
.
halfSitting
),
self
.
dimension
))
self
.
initializeRobot
()
__all__
=
[
Hrp2
]
src/dynamic_graph/sot/hrp2_10/robot.py
.in
→
src/dynamic_graph/sot/hrp2_10/robot.py
View file @
26e04e00
...
...
@@ -17,8 +17,7 @@
from
__future__
import
print_function
from
dynamic_graph.sot.hrp2
import
Hrp2
import
numpy
as
np
from dynamic_graph.sot.hrp2.dynamic_hrp2_10 import DynamicHrp2_10
hrp2_10_pkgdatarootdir = "@HRP2_10_PKGDATAROOTDIR@/hrp2-10"
# Internal helper tool.
def
matrixToTuple
(
M
):
...
...
@@ -49,8 +48,6 @@ class Robot (Hrp2):
)
def
__init__
(
self
,
name
,
modelDir = hrp2_10_pkgdatarootdir,
xmlDir = hrp2_10_pkgdatarootdir,
device
=
None
,
tracer
=
None
):
...
...
@@ -122,7 +119,6 @@ class Robot (Hrp2):
(
"cameraXtionRGB"
,
matrixToTuple
(
cameraXtionRGBPosition
),
"gaze"
))
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
tracer)
Hrp2
.
__init__
(
self
,
name
,
"10"
,
device
,
tracer
)
__all__
=
[
"Robot"
]
src/dynamic_graph/sot/hrp2_14/robot.py
.in
→
src/dynamic_graph/sot/hrp2_14/robot.py
View file @
26e04e00
...
...
@@ -18,10 +18,6 @@ from __future__ import print_function
from
dynamic_graph.sot.hrp2
import
Hrp2
import
numpy
as
np
from dynamic_graph.sot.hrp2.dynamic_hrp2_14 import DynamicHrp2_14
hrp2_14_pkgdatarootdir = "@HRP2_14_PKGDATAROOTDIR@/hrp2-14"
# Internal helper tool.
def
matrixToTuple
(
M
):
tmp
=
M
.
tolist
()
...
...
@@ -38,21 +34,26 @@ class Robot (Hrp2):
# Free flyer
0.
,
0.
,
0.648702
,
0.
,
0.
,
0.
,
# Legs
0., 0., -0.453786, 0.872665, -0.418879, 0.,
0., 0., -0.453786, 0.872665, -0.418879, 0.,
# Chest and head
0.
,
0.
,
0.
,
0.
,
# Arm
s
#
Left
Arm
0.261799
,
-
0.17453
,
0.
,
-
0.523599
,
0.
,
0.
,
0.1
,
# Left hand
#0.,0.,0.,0.,0.,
#Right Arm
0.261799
,
0.17453
,
0.
,
-
0.523599
,
0.
,
0.
,
0.1
,
#Right Hand
#0.,0.,0.,0.,0.,
# Legs
0.
,
0.
,
-
0.453786
,
0.872665
,
-
0.418879
,
0.
,
0.
,
0.
,
-
0.453786
,
0.872665
,
-
0.418879
,
0.
,
)
def
__init__
(
self
,
name
,
modelDir = hrp2_14_pkgdatarootdir,
xmlDir = hrp2_14_pkgdatarootdir,
device
=
None
,
tracer
=
None
):
...
...
@@ -112,6 +113,6 @@ class Robot (Hrp2):
(
"cameraTopRight"
,
matrixToTuple
(
cameraTopRightPosition
),
"gaze"
))
Hrp2.__init__(self, name,
modelDir, xmlDir, device, DynamicHrp2_14
, tracer)
Hrp2
.
__init__
(
self
,
name
,
"14"
,
device
,
tracer
)
__all__
=
[
"Robot"
]
src/sot-hrp2-controller.cpp
View file @
26e04e00
...
...
@@ -50,7 +50,7 @@ SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
device_
(
RobotName
)
{
std
::
cout
<<
"Going through SoTHRP2Controller
."
<<
std
::
endl
;
std
::
cout
<<
"Going through SoTHRP2Controller
for "
<<
RobotName
<<
std
::
endl
;
boost
::
thread
thr
(
workThread
,
this
);
sotDEBUG
(
25
)
<<
__FILE__
<<
":"
<<
__FUNCTION__
<<
"(#"
...
...
src/sot-hrp2-device.cpp
View file @
26e04e00
...
...
@@ -52,7 +52,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
for
(
int
i
=
0
;
i
<
4
;
++
i
)
{
withForceSignals
[
i
]
=
true
;
}
signalRegistration
(
robotState_
<<
accelerometerSOUT_
<<
gyrometerSOUT_
<<
currentSOUT_
<<
p_gainsSOUT_
<<
d_gainsSOUT_
);
ml
::
Vector
data
(
3
);
data
.
setZero
();
dg
::
Vector
data
(
3
);
data
.
setZero
();
accelerometerSOUT_
.
setConstant
(
data
);
gyrometerSOUT_
.
setConstant
(
data
);
baseff_
.
resize
(
12
);
...
...
@@ -229,13 +229,13 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
int
time
=
controlSIN
.
getTime
();
zmpSIN
.
recompute
(
time
+
1
);
// Express ZMP in free flyer reference frame
ml
::
Vector
zmpGlobal
(
4
);
dg
::
Vector
zmpGlobal
(
4
);
for
(
unsigned
int
i
=
0
;
i
<
3
;
++
i
)
zmpGlobal
(
i
)
=
zmpSIN
(
time
+
1
)(
i
);
zmpGlobal
(
3
)
=
1.
;
dgsot
::
MatrixHomogeneous
inversePose
;
freeFlyerPose
().
inverse
(
inversePos
e
);
ml
::
Vector
localZmp
=
inversePose
*
zmpGlobal
;
inversePose
=
freeFlyerPose
().
inverse
(
Eigen
::
Affin
e
);
dg
::
Vector
localZmp
(
4
);
localZmp
=
inversePose
.
matrix
()
*
zmpGlobal
;
vector
<
double
>
ZMPRef
(
3
);
for
(
unsigned
int
i
=
0
;
i
<
3
;
++
i
)
ZMPRef
[
i
]
=
localZmp
(
i
);
...
...
src/sot-hrp2-device.hh
View file @
26e04e00
...
...
@@ -24,9 +24,10 @@
#include
<dynamic-graph/linear-algebra.h>
#include
<sot/core/device.hh>
#include
<sot/core/abstract-sot-external-interface.hh>
#include
<sot/core/matrix-
rotation
.hh>
#include
<sot/core/matrix-
geometry
.hh>
namespace
dgsot
=
dynamicgraph
::
sot
;
namespace
dg
=
dynamicgraph
;
class
SoTHRP2Device
:
public
dgsot
::
Device
...
...
@@ -60,7 +61,7 @@ protected:
double
timestep_
;
/// \brief Previous robot configuration.
maal
::
boost
::
Vector
previousState_
;
dg
::
Vector
previousState_
;
/// \brief Robot state provided by OpenHRP.
///
...
...
@@ -68,29 +69,29 @@ protected:
/// account the stabilization step. Therefore, this usually
/// does *not* match the state control input signal.
///
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
robotState_
;
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
robotState_
;
/// Accelerations measured by accelerometers
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
accelerometerSOUT_
;
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
accelerometerSOUT_
;
/// Rotation velocity measured by gyrometers
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
gyrometerSOUT_
;
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
gyrometerSOUT_
;
/// motor currents
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
currentSOUT_
;
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
currentSOUT_
;
/// proportional and derivative position-control gains
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
p_gainsSOUT_
;
dynamicgraph
::
Signal
<
ml
::
Vector
,
int
>
d_gainsSOUT_
;
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
p_gainsSOUT_
;
dynamicgraph
::
Signal
<
dg
::
Vector
,
int
>
d_gainsSOUT_
;
/// Intermediate variables to avoid allocation during control
ml
::
Vector
mlforces
;
ml
::
Vector
mlRobotState
;
dg
::
Vector
mlforces
;
dg
::
Vector
mlRobotState
;
dgsot
::
MatrixRotation
pose
;
ml
::
Vector
accelerometer_
;
ml
::
Vector
gyrometer_
;
dg
::
Vector
accelerometer_
;
dg
::
Vector
gyrometer_
;
std
::
vector
<
double
>
baseff_
;
ml
::
Vector
torques_
;
ml
::
Vector
currents_
;
ml
::
Vector
p_gains_
;
ml
::
Vector
d_gains_
;
dg
::
Vector
torques_
;
dg
::
Vector
currents_
;
dg
::
Vector
p_gains_
;
dg
::
Vector
d_gains_
;
};
#endif
/* _SOT_HRP2Device_H_*/
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new 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