Commit 2139aeb7 authored by Francois Keith's avatar Francois Keith
Browse files

Use the urdf file to build the jrl::dynamic model.

Requires the romeo stack.
parent f1c5a8a4
......@@ -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,45 @@
# 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, # right leg
0, 0 ,0 ,0) # eyes
# def smallToFull(self, config):
# res = (config + 12*(0.,))
# return res
def __init__(self, name,
modelDir = romeo_pkgdatarootdir,
xmlDir = romeo_pkgdatarootdir,
device = None,
dynamicType = Dynamic,
tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
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))
self.dynamic.addJointMapping('BODY', 'body')
self.dynamic.loadFromParameterServer()
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]
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