Commit 26e04e00 authored by Rohan Budhiraja's avatar Rohan Budhiraja

compatible with Eigen and pinocchio. adjust specificities requirement

TODO: load specificities with srdf.
parent e9e43aaf
...@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6) ...@@ -17,6 +17,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake) INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake) INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/eigen.cmake)
INCLUDE(cmake/lapack.cmake) INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake) INCLUDE(cmake/cpack.cmake)
INCLUDE(cmake/ros.cmake) INCLUDE(cmake/ros.cmake)
...@@ -38,10 +39,8 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES ...@@ -38,10 +39,8 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
SETUP_PROJECT() SETUP_PROJECT()
# Search for dependencies. # 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-dynamics >= 1.5.0")
ADD_OPTIONAL_DEPENDENCY("hrp2-10-optimized >= 1.0.1") ADD_OPTIONAL_DEPENDENCY("hrp2-10-optimized >= 1.0.1")
...@@ -50,7 +49,7 @@ ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6") ...@@ -50,7 +49,7 @@ ADD_OPTIONAL_DEPENDENCY("hrp2-14 >= 1.8-6")
ADD_REQUIRED_DEPENDENCY("dynamic-graph") ADD_REQUIRED_DEPENDENCY("dynamic-graph")
ADD_REQUIRED_DEPENDENCY("sot-core") 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-python")
ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge_msgs") ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge_msgs")
...@@ -59,6 +58,7 @@ ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge") ...@@ -59,6 +58,7 @@ ADD_REQUIRED_DEPENDENCY("dynamic_graph_bridge")
# Boost # Boost
SET(BOOST_COMPONENTS filesystem system thread) SET(BOOST_COMPONENTS filesystem system thread)
SEARCH_FOR_BOOST() SEARCH_FOR_BOOST()
SEARCH_FOR_EIGEN()
# Handle rpath necessary to handle ROS multiplace packages # Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion # libraries inclusion
......
...@@ -24,10 +24,9 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES) ...@@ -24,10 +24,9 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
SOVERSION ${PROJECT_VERSION}) SOVERSION ${PROJECT_VERSION})
PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph) PKG_CONFIG_USE_DEPENDENCY(${NAME} dynamic-graph)
PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mathtools) PKG_CONFIG_USE_DEPENDENCY(${NAME} eigen3)
PKG_CONFIG_USE_DEPENDENCY(${NAME} jrl-mal)
PKG_CONFIG_USE_DEPENDENCY(${NAME} sot-core) 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) INSTALL(TARGETS ${NAME} DESTINATION ${CMAKE_INSTALL_LIBDIR}/plugin)
...@@ -41,25 +40,25 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES) ...@@ -41,25 +40,25 @@ FUNCTION(COMPILE_PLUGIN NAME SOURCES ENTITIES)
ENDFUNCTION() ENDFUNCTION()
# Compile plug-ins. # Compile plug-ins.
IF (HRP2_DYNAMICS_FOUND) #IF (HRP2_DYNAMICS_FOUND)
MESSAGE(STATUS "Add dynamic-hrp2_14 plugin") # MESSAGE(STATUS "Add dynamic-hrp2_14 plugin")
COMPILE_PLUGIN(dynamic-hrp2_14 dynamic-hrp2_14.cc DynamicHrp2_14) # COMPILE_PLUGIN(dynamic-hrp2_14 dynamic-hrp2_14.cc DynamicHrp2_14)
PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_14 hrp2-dynamics) # PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_14 hrp2-dynamics)
ENDIF() #ENDIF()
IF (HRP2_10_OPTIMIZED_FOUND) #IF (HRP2_10_OPTIMIZED_FOUND)
MESSAGE(STATUS "Add dynamic-hrp2_10 plugin") # MESSAGE(STATUS "Add dynamic-hrp2_10 plugin")
COMPILE_PLUGIN(dynamic-hrp2_10 dynamic-hrp2_10.cc DynamicHrp2_10) # COMPILE_PLUGIN(dynamic-hrp2_10 dynamic-hrp2_10.cc DynamicHrp2_10)
PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_10 hrp2-10-optimized) # PKG_CONFIG_USE_DEPENDENCY(dynamic-hrp2_10 hrp2-10-optimized)
ENDIF() #ENDIF()
IF (HRP2_10_OPTIMIZED_FOUND) #IF (HRP2_10_OPTIMIZED_FOUND)
CONFIG_FILES(dynamic_graph/sot/hrp2_10/robot.py) # CONFIG_FILES(dynamic_graph/sot/hrp2_10/robot.py)
ENDIF() #ENDIF()
IF (HRP2_DYNAMICS_FOUND) #IF (HRP2_DYNAMICS_FOUND)
CONFIG_FILES(dynamic_graph/sot/hrp2_14/robot.py) # CONFIG_FILES(dynamic_graph/sot/hrp2_14/robot.py)
ENDIF() #ENDIF()
# Install Python files. # Install Python files.
SET(PYTHON_MODULE_DIR SET(PYTHON_MODULE_DIR
...@@ -70,23 +69,20 @@ SET(PYTHON_MODULE_BUILD_DIR ...@@ -70,23 +69,20 @@ SET(PYTHON_MODULE_BUILD_DIR
SET(PYTHON_MODULE dynamic_graph/sot/hrp2) SET(PYTHON_MODULE dynamic_graph/sot/hrp2)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" ) PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "hrp2.py" ) PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "hrp2.py" )
SET(FILES __init__.py robot.py) SET(FILES __init__.py robot.py)
# Install dynamic_graph.sot.hrp2_14 # Install dynamic_graph.sot.hrp2_14
IF (HRP2_DYNAMICS_FOUND) SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14)
SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14) PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" ) PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}") PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
ENDIF()
# Install dynamic_graph.sot.hrp2_10 # Install dynamic_graph.sot.hrp2_10
IF (HRP2_10_OPTIMIZED_FOUND) SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10)
SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10) PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" ) PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}") PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py")
ENDIF()
# Add the library to wrap the device of HRP2. # Add the library to wrap the device of HRP2.
MACRO(build_hrp2_device) MACRO(build_hrp2_device)
...@@ -152,11 +148,15 @@ MACRO(build_hrp2_controller robotnumber) ...@@ -152,11 +148,15 @@ MACRO(build_hrp2_controller robotnumber)
ENDMACRO() ENDMACRO()
IF (HRP2_10_OPTIMIZED_FOUND) #TODO: which? 10 or 14?
build_hrp2_controller("10") build_hrp2_controller("14")
ENDIF() build_hrp2_controller("10")
#IF (HRP2_10_OPTIMIZED_FOUND)
# build_hrp2_controller("10")
#ENDIF()
IF (HRP2_DYNAMICS_FOUND) #IF (HRP2_DYNAMICS_FOUND)
build_hrp2_controller("14") # build_hrp2_controller("14")
ENDIF() #ENDIF()
...@@ -32,7 +32,7 @@ namespace dynamicgraph ...@@ -32,7 +32,7 @@ namespace dynamicgraph
namespace hrp2 namespace hrp2
{ {
DynamicHrp2_10::DynamicHrp2_10 (const std::string & name) DynamicHrp2_10::DynamicHrp2_10 (const std::string & name)
: Dynamic (name, false) : Dynamic (name)
{ {
sotDEBUGIN(15); sotDEBUGIN(15);
DynamicHrp2_10::buildModelHrp2 (); DynamicHrp2_10::buildModelHrp2 ();
......
...@@ -17,15 +17,13 @@ ...@@ -17,15 +17,13 @@
from __future__ import print_function from __future__ import print_function
import numpy as np 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.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.ros import RosRobotModel
from rospkg import RosPack
# Internal helper tool. # Internal helper tool.
def matrixToTuple(M): def matrixToTuple(M):
...@@ -67,17 +65,15 @@ class Hrp2(AbstractHumanoidRobot): ...@@ -67,17 +65,15 @@ class Hrp2(AbstractHumanoidRobot):
res = (config + 10*(0.,)) res = (config + 10*(0.,))
return res return res
def __init__(self, name, modelDir, xmlDir, device, dynamicType, def __init__(self, name, robotnumber,
device = None,
tracer = None): tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer) AbstractHumanoidRobot.__init__ (self, name, tracer)
self.OperationalPoints.append('waist') self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest') self.OperationalPoints.append('chest')
self.device = device self.device = device
self.modelDir = modelDir
self.modelName = 'HRP2JRLmainsmall.wrl'
self.specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
self.jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
self.AdditionalFrames.append( self.AdditionalFrames.append(
("accelerometer", ("accelerometer",
...@@ -91,19 +87,25 @@ class Hrp2(AbstractHumanoidRobot): ...@@ -91,19 +87,25 @@ class Hrp2(AbstractHumanoidRobot):
self.AdditionalFrames.append( self.AdditionalFrames.append(
("rightFootForceSensor", ("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.dynamic = RosRobotModel("{0}_dynamic".format(name))
self.name + '_dynamic',
modelDir,
self.modelName,
self.specificitiesPath,
self.jointRankPath,
dynamicType)
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.dynamic.loadUrdf(self.urdfPath)
self.dimension = self.dynamic.getDimension()
self.dynamic.displayModel()
self.plugVelocityFromDevice = True
if self.dimension != len(self.halfSitting): 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)) raise RuntimeError("Dimension of half-sitting: {0} differs from dimension of robot: {1}".format (len(self.halfSitting), self.dimension))
self.initializeRobot() self.initializeRobot()
......
...@@ -17,8 +17,7 @@ ...@@ -17,8 +17,7 @@
from __future__ import print_function from __future__ import print_function
from dynamic_graph.sot.hrp2 import Hrp2 from dynamic_graph.sot.hrp2 import Hrp2
import numpy as np 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. # Internal helper tool.
def matrixToTuple(M): def matrixToTuple(M):
...@@ -49,8 +48,6 @@ class Robot (Hrp2): ...@@ -49,8 +48,6 @@ class Robot (Hrp2):
) )
def __init__(self, name, def __init__(self, name,
modelDir = hrp2_10_pkgdatarootdir,
xmlDir = hrp2_10_pkgdatarootdir,
device = None, device = None,
tracer = None): tracer = None):
...@@ -122,7 +119,6 @@ class Robot (Hrp2): ...@@ -122,7 +119,6 @@ class Robot (Hrp2):
("cameraXtionRGB", ("cameraXtionRGB",
matrixToTuple(cameraXtionRGBPosition), "gaze")) matrixToTuple(cameraXtionRGBPosition), "gaze"))
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10, Hrp2.__init__(self, name, "10", device, tracer)
tracer)
__all__ = ["Robot"] __all__ = ["Robot"]
...@@ -18,10 +18,6 @@ from __future__ import print_function ...@@ -18,10 +18,6 @@ from __future__ import print_function
from dynamic_graph.sot.hrp2 import Hrp2 from dynamic_graph.sot.hrp2 import Hrp2
import numpy as np 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. # Internal helper tool.
def matrixToTuple(M): def matrixToTuple(M):
tmp = M.tolist() tmp = M.tolist()
...@@ -38,21 +34,26 @@ class Robot (Hrp2): ...@@ -38,21 +34,26 @@ class Robot (Hrp2):
# Free flyer # Free flyer
0., 0., 0.648702, 0., 0. , 0., 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 # Chest and head
0., 0., 0., 0., 0., 0., 0., 0.,
# Arms # Left Arm
0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1, 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, 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, def __init__(self, name,
modelDir = hrp2_14_pkgdatarootdir,
xmlDir = hrp2_14_pkgdatarootdir,
device = None, device = None,
tracer = None): tracer = None):
...@@ -112,6 +113,6 @@ class Robot (Hrp2): ...@@ -112,6 +113,6 @@ class Robot (Hrp2):
("cameraTopRight", ("cameraTopRight",
matrixToTuple(cameraTopRightPosition), "gaze")) matrixToTuple(cameraTopRightPosition), "gaze"))
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_14, tracer) Hrp2.__init__(self, name, "14", device, tracer)
__all__ = ["Robot"] __all__ = ["Robot"]
...@@ -50,7 +50,7 @@ SoTHRP2Controller::SoTHRP2Controller(std::string RobotName): ...@@ -50,7 +50,7 @@ SoTHRP2Controller::SoTHRP2Controller(std::string RobotName):
device_(RobotName) device_(RobotName)
{ {
std::cout << "Going through SoTHRP2Controller." << std::endl; std::cout << "Going through SoTHRP2Controller for " <<RobotName<< std::endl;
boost::thread thr(workThread,this); boost::thread thr(workThread,this);
sotDEBUG(25) << __FILE__ << ":" sotDEBUG(25) << __FILE__ << ":"
<< __FUNCTION__ <<"(#" << __FUNCTION__ <<"(#"
......
...@@ -52,7 +52,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName): ...@@ -52,7 +52,7 @@ SoTHRP2Device::SoTHRP2Device(std::string RobotName):
for( int i=0;i<4;++i ) { withForceSignals[i] = true; } for( int i=0;i<4;++i ) { withForceSignals[i] = true; }
signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_ signalRegistration (robotState_ << accelerometerSOUT_ << gyrometerSOUT_
<< currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_); << currentSOUT_ << p_gainsSOUT_ << d_gainsSOUT_);
ml::Vector data (3); data.setZero (); dg::Vector data (3); data.setZero ();
accelerometerSOUT_.setConstant (data); accelerometerSOUT_.setConstant (data);
gyrometerSOUT_.setConstant (data); gyrometerSOUT_.setConstant (data);
baseff_.resize(12); baseff_.resize(12);
...@@ -229,13 +229,13 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut) ...@@ -229,13 +229,13 @@ void SoTHRP2Device::getControl(map<string,dgsot::ControlValues> &controlOut)
int time = controlSIN.getTime (); int time = controlSIN.getTime ();
zmpSIN.recompute (time + 1); zmpSIN.recompute (time + 1);
// Express ZMP in free flyer reference frame // Express ZMP in free flyer reference frame
ml::Vector zmpGlobal (4); dg::Vector zmpGlobal (4);
for (unsigned int i = 0; i < 3; ++i) for (unsigned int i = 0; i < 3; ++i)
zmpGlobal(i) = zmpSIN(time + 1)(i); zmpGlobal(i) = zmpSIN(time + 1)(i);
zmpGlobal(3) = 1.; zmpGlobal(3) = 1.;
dgsot::MatrixHomogeneous inversePose; dgsot::MatrixHomogeneous inversePose;
freeFlyerPose().inverse(inversePose); inversePose = freeFlyerPose().inverse(Eigen::Affine);
ml::Vector localZmp = inversePose * zmpGlobal; dg::Vector localZmp(4); localZmp = inversePose.matrix() * zmpGlobal;
vector<double> ZMPRef(3); vector<double> ZMPRef(3);
for(unsigned int i=0;i<3;++i) for(unsigned int i=0;i<3;++i)
ZMPRef[i] = localZmp(i); ZMPRef[i] = localZmp(i);
......
...@@ -24,9 +24,10 @@ ...@@ -24,9 +24,10 @@
#include <dynamic-graph/linear-algebra.h> #include <dynamic-graph/linear-algebra.h>
#include <sot/core/device.hh> #include <sot/core/device.hh>
#include <sot/core/abstract-sot-external-interface.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 dgsot=dynamicgraph::sot;
namespace dg=dynamicgraph;
class SoTHRP2Device: public class SoTHRP2Device: public
dgsot::Device dgsot::Device
...@@ -60,7 +61,7 @@ protected: ...@@ -60,7 +61,7 @@ protected:
double timestep_; double timestep_;
/// \brief Previous robot configuration. /// \brief Previous robot configuration.
maal::boost::Vector previousState_; dg::Vector previousState_;
/// \brief Robot state provided by OpenHRP. /// \brief Robot state provided by OpenHRP.
/// ///
...@@ -68,29 +69,29 @@ protected: ...@@ -68,29 +69,29 @@ protected:
/// account the stabilization step. Therefore, this usually /// account the stabilization step. Therefore, this usually
/// does *not* match the state control input signal. /// does *not* match the state control input signal.
/// ///
dynamicgraph::Signal<ml::Vector, int> robotState_; dynamicgraph::Signal<dg::Vector, int> robotState_;
/// Accelerations measured by accelerometers /// Accelerations measured by accelerometers
dynamicgraph::Signal <ml::Vector, int> accelerometerSOUT_; dynamicgraph::Signal <dg::Vector, int> accelerometerSOUT_;
/// Rotation velocity measured by gyrometers /// Rotation velocity measured by gyrometers
dynamicgraph::Signal <ml::Vector, int> gyrometerSOUT_; dynamicgraph::Signal <dg::Vector, int> gyrometerSOUT_;
/// motor currents /// motor currents
dynamicgraph::Signal <ml::Vector, int> currentSOUT_; dynamicgraph::Signal <dg::Vector, int> currentSOUT_;
/// proportional and derivative position-control gains /// proportional and derivative position-control gains
dynamicgraph::Signal <ml::Vector, int> p_gainsSOUT_; dynamicgraph::Signal <dg::Vector, int> p_gainsSOUT_;
dynamicgraph::Signal <ml::Vector, int> d_gainsSOUT_; dynamicgraph::Signal <dg::Vector, int> d_gainsSOUT_;
/// Intermediate variables to avoid allocation during control /// Intermediate variables to avoid allocation during control
ml::Vector mlforces; dg::Vector mlforces;
ml::Vector mlRobotState; dg::Vector mlRobotState;
dgsot::MatrixRotation pose; dgsot::MatrixRotation pose;
ml::Vector accelerometer_; dg::Vector accelerometer_;
ml::Vector gyrometer_; dg::Vector gyrometer_;
std::vector<double> baseff_; std::vector<double> baseff_;
ml::Vector torques_; dg::Vector torques_;
ml::Vector currents_; dg::Vector currents_;
ml::Vector p_gains_; dg::Vector p_gains_;
ml::Vector d_gains_; dg::Vector d_gains_;
}; };
#endif /* _SOT_HRP2Device_H_*/ #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