Commit 5866b2dc authored by olivier stasse's avatar olivier stasse
Browse files

Merge pull request #1 from francois-keith/urdf_parsing

Urdf parsing management
parents f1c5a8a4 1523941b
......@@ -36,9 +36,6 @@ SET(PKG_CONFIG_ADDITIONAL_VARIABLES
SETUP_PROJECT()
# Search for dependencies.
ADD_REQUIRED_DEPENDENCY("romeo >= 1.1.1")
# Those packages are not directly linked, but are used in the
# python scripts
ADD_REQUIRED_DEPENDENCY("sot-core >= 2.7")
......
......@@ -17,8 +17,6 @@ INCLUDE(../cmake/python.cmake)
FINDPYTHON()
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
CONFIG_FILES(dynamic_graph/sot/romeo/robot.py)
# Install Python files.
SET(PYTHON_MODULE_DIR
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/romeo)
......@@ -28,6 +26,6 @@ SET(PYTHON_MODULE_BUILD_DIR
SET(PYTHON_MODULE dynamic_graph/sot/romeo)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "prologue.py" )
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "robot.py" "${PYTHON_SITELIB}")
......@@ -14,69 +14,58 @@
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
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.tools.se3 import SE3, R3, SO3
import numpy as np
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.sot.dynamics import Dynamic
romeo_pkgdatarootdir = "@ROMEO_PKGDATAROOTDIR@/romeo"
from dynamic_graph.ros import RosRobotModel
# Sot model for the romeo_small.urdf (with gripper, no fingers)
class Robot (AbstractHumanoidRobot):
"""
This class instanciates Aldebaran Romeo robot
"""
halfSitting = (
0, 0, 0.840252, 0, 0, 0, # Free flyer
0, # chest
0, 0, 0, 0, # head
1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # left arm
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # right arm
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, # left leg
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, ) # right leg
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, # Left leg
0, 0, -0.3490658, 0.6981317, -0.3490658, 0, 0, # Right leg
0, # Trunk
1.5, 0.6, -0.5, -1.05, -0.4, -0.3, -0.2, # Left arm
0, 0, 0, 0, # Head
0, 0 ,0 ,0, # Eyes
1.5, -0.6, 0.5, 1.05, -0.4, -0.3, -0.2, # Right arm
)
# def smallToFull(self, config):
# res = (config + 12*(0.,))
# return res
jointMap = { }
jointMap['BODY'] = 'body'
def __init__(self, name,
modelDir = romeo_pkgdatarootdir,
xmlDir = romeo_pkgdatarootdir,
device = None,
dynamicType = Dynamic,
tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
self.urdfDir = 'package://romeo_description/urdf/'
self.urdfName = 'romeo_small.urdf'
self.OperationalPoints.append('waist')
self.OperationalPoints.append('chest')
self.device = device
self.modelDir = modelDir
self.modelName = 'Romeo.wrl'
self.specificitiesPath = xmlDir + '/RomeoSpecificities.xml'
self.jointRankPath = xmlDir + '/RomeoLinkJointRank.xml'
self.dynamic = self.loadModelFromJrlDynamics(
self.name + '_dynamic',
modelDir,
self.modelName,
self.specificitiesPath,
self.jointRankPath,
dynamicType)
# correct the name of the body link
self.dynamic = RosRobotModel("{0}_dynamic".format(name))
for i in self.jointMap:
self.dynamic.addJointMapping(i, self.jointMap[i])
self.dynamic.loadUrdf(self.urdfDir + self.urdfName)
# complete feet position (TODO: move it into srdf file)
ankle =self.dynamic.getAnklePositionInFootFrame()
self.ankleLength = 0.1935
self.ankleWidth = 0.121
self.dynamic.setFootParameters(True , self.ankleLength, self.ankleWidth, ankle)
self.dynamic.setFootParameters(False, self.ankleLength, self.ankleWidth, ankle)
# check half sitting size
self.dimension = self.dynamic.getDimension()
print (self.dimension)
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose")
self.initializeRobot()
# TODO: ??
# __all__ = [Romeo]
__all__ = ["Robot"]
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