Commit 1ff37a04 authored by olivier stasse's avatar olivier stasse Committed by GitHub

Merge pull request #13 from proyan/master

Eigen and Pinocchio with changes in python class Robot
parents e9e43aaf cd02e36f
*~
\ No newline at end of file
......@@ -2,3 +2,6 @@
path = cmake
url = git://github.com/jrl-umi3218/jrl-cmakemodules.git
[submodule ".travis"]
path = .travis
url = git://github.com/jrl-umi3218/jrl-travis
Subproject commit 2a03b5bf12ff198b7bc97d45c39a531db54c3a58
language: cpp
sudo: required
compiler: gcc
env:
global:
- APT_DEPENDENCIES="doxygen doxygen-latex libboost-all-dev libeigen3-dev liblapack-dev
libblas-dev gfortran python-dev python-sphinx python-numpy libbullet-dev ros-hydro-angles
ros-hydro-common-msgs
ros-hydro-control-msgs
ros-hydro-realtime-tools
ros-hydro-resource-retriever
ros-hydro-robot-state-publisher
ros-hydro-ros-base
ros-hydro-tf
ros-hydro-urdf
ros-hydro-urdfdom
ros-hydro-urdfdom-py"
- GIT_DEPENDENCIES="stack-of-tasks/pinocchio:devel
proyan/dynamic-graph proyan/dynamic-graph-python proyan/sot-core
proyan/sot-tools proyan/sot-dynamic:topic/sot-pinocchio "
- ROS_DISTRO=hydro
- ROS_GIT_DEPENDENCIES="proyan/dynamic_graph_bridge_msgs proyan/dynamic_graph_bridge"
- ALLOW_CATKINLINT_FAILURE=true
notifications:
email:
- francois.keith@gmail.com
branches:
only:
- master
script: ./.travis/run build
after_success: ./.travis/run after_success
after_failure: ./.travis/run after_failure
before_install: ./.travis/run dependencies/catkin; ./.travis/run before_install
......@@ -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,30 +39,29 @@ 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_OPTIONAL_DEPENDENCY("hrp2-dynamics >= 1.5.0")
ADD_REQUIRED_DEPENDENCY("pinocchio")
ADD_OPTIONAL_DEPENDENCY("hrp2-10-optimized >= 1.0.1")
ADD_OPTIONAL_DEPENDENCY("hrp2-10 >= 1.0.1-10")
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("dynamic-graph-python")
ADD_REQUIRED_DEPENDENCY("dynamic-graph >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-core >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("sot-dynamic >= 3.1")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python >= 3.0.0")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge_msgs")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge >= 3.0")
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS filesystem system thread)
SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
# Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion
SET(CMAKE_INSTALL_LIBDIR lib)
SET(CMAKE_SKIP_BUILD_RPATH FALSE)
SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}")
......
Subproject commit fa2738146fa19d8bbbb1d8c8a97a2b4809ce81af
Subproject commit c8ae5972e825bc27bf978f142b81915df19fef70
......@@ -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)
......@@ -40,27 +39,6 @@ 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()
# Install Python files.
SET(PYTHON_MODULE_DIR
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/hrp2)
......@@ -70,23 +48,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 +127,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()
......@@ -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 ();
......
......@@ -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
import pinocchio as se3
from rospkg import RosPack
# Internal helper tool.
def matrixToTuple(M):
......@@ -67,18 +65,15 @@ class Hrp2(AbstractHumanoidRobot):
res = (config + 10*(0.,))
return res
def __init__(self, name, modelDir, xmlDir, device, dynamicType,
tracer = None):
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 +85,30 @@ 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 = self.loadModelFromJrlDynamics(
self.name + '_dynamic',
modelDir,
self.modelName,
self.specificitiesPath,
self.jointRankPath,
dynamicType)
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.dimension = self.dynamic.getDimension()
self.plugVelocityFromDevice = True
rospack = RosPack()
self.urdfPath = rospack.get_path('hrp2_{0}_description'.format(robotnumber)) + '/urdf/hrp2_{0}.urdf'.format(robotnumber)
self.pinocchioModel = se3.buildModelFromUrdf(self.urdfPath, se3.JointModelFreeFlyer())
self.pinocchioData = self.pinocchioModel.createData()
self.dynamic.setModel(self.pinocchioModel)
self.dynamic.setData(self.pinocchioData)
self.dimension = self.dynamic.getDimension()
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()
self.dynamic.displayModel()
__all__ = [Hrp2]
......@@ -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"]
......@@ -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"]
......@@ -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__ <<"(#"
......
......@@ -52,10 +52,10 @@ 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);
baseff_.resize(7);
using namespace dynamicgraph::command;
std::string docstring;
......@@ -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);
......@@ -244,11 +244,17 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
controlOut["zmp"].setValues(ZMPRef);
// Update position of freeflyer in global frame
for (int i = 0;i < 3; ++i)
baseff_[i*4+3] = freeFlyerPose () (i, 3);
for(unsigned i = 0;i < 3; ++i)
for(unsigned j = 0; j < 3; ++j)
baseff_[i * 4 + j] = freeFlyerPose () (i, j);
Eigen::Vector3d transq_(freeFlyerPose().translation());
dg::sot::VectorQuaternion qt_(freeFlyerPose().linear());
//translation
for(int i=0; i<3; i++) baseff_[i] = transq_(i);
//rotation: quaternion
baseff_[3] = qt_.w();
baseff_[4] = qt_.x();
baseff_[5] = qt_.y();
baseff_[6] = qt_.z();
controlOut["baseff"].setValues(baseff_);
sotDEBUGOUT(25) ;
......
......@@ -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_*/
Markdown is supported
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