Skip to content
GitLab
Commit 26e04e00 authored by Rohan Budhiraja's avatar Rohan Budhiraja
Browse files

compatible with Eigen and pinocchio. adjust specificities requirement 

TODO: load specificities with srdf.
parent e9e43aaf
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.,
# Arms
# 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(inversePose);
ml::Vector localZmp = inversePose * zmpGlobal;
inversePose = freeFlyerPose().inverse(Eigen::Affine);
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_*/
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