Commit 4389646a authored by Francois Keith's avatar Francois Keith
Browse files

Add python script to easily load romeo.

parent 3a968014
# Copyright 2013, Francois Keith, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
#
# This file is part of sot-romeo.
# sot-romeo is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# sot-romeo is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# sot-romeo. If not, see <http://www.gnu.org/licenses/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/lapack.cmake)
INCLUDE(cmake/cpack.cmake)
INCLUDE(cmake/ros.cmake)
SET(PROJECT_NAME sot-romeo)
SET(PROJECT_DESCRIPTION "dynamic-graph package for Romeo robot")
SET(PROJECT_URL "")
SET(CUSTOM_HEADER_DIR "${PROJECT_NAME}")
SET(CXX_DISABLE_WERROR True)
SET(PKG_CONFIG_ADDITIONAL_VARIABLES
${PKG_CONFIG_ADDITIONAL_VARIABLES}
plugindirname
plugindir
)
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("romeo")
ADD_REQUIRED_DEPENDENCY("dynamic-graph")
ADD_REQUIRED_DEPENDENCY("sot-core")
ADD_REQUIRED_DEPENDENCY("sot-dynamic")
ADD_REQUIRED_DEPENDENCY("dynamic-graph-python")
ADD_ROSPACK_DEPENDENCY("dynamic_graph_bridge")
# Search for dependencies.
# Boost
SET(BOOST_COMPONENTS filesystem system)
SEARCH_FOR_BOOST()
# Handle rpath necessary to handle ROS multiplace packages
# libraries inclusion
SET(CMAKE_SKIP_BUILD_RPATH FALSE)
SET(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
SET(CMAKE_INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib")
SET(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
# Add subdirectories.
ADD_SUBDIRECTORY(src)
SETUP_PROJECT_FINALIZE()
SETUP_PROJECT_CPACK()
# Copyright 2013, Francois Keith, Thomas Moulard, Olivier Stasse, JRL, CNRS/AIST
#
# This file is part of sot-romeo.
# sot-romeo is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# sot-romeo is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# received a copy of the GNU Lesser General Public License along with
# sot-romeo. If not, see <http://www.gnu.org/licenses/>.
INCLUDE(../cmake/python.cmake)
FINDPYTHON()
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_PATH})
CONFIG_FILES(dynamic_graph/sot/romeo/romeo.py)
# Install Python files.
SET(PYTHON_MODULE_DIR
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/romeo)
SET(PYTHON_MODULE_BUILD_DIR
${CMAKE_CURRENT_BINARY_DIR}/dynamic_graph/sot/romeo)
SET(PYTHON_MODULE dynamic_graph/sot/romeo)
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "romeo.py" "${PYTHON_SITELIB}")
SET(FILES __init__.py romeo.py)
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# 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
from dynamic_graph.sot.romeo.romeo import Robot
# -*- coding: utf-8 -*-
# Copyright 2011, Florent Lamiraux, Thomas Moulard, JRL, CNRS/AIST
#
# This file is part of dynamic-graph.
# dynamic-graph is free software: you can redistribute it and/or
# modify it under the terms of the GNU Lesser General Public License
# as published by the Free Software Foundation, either version 3 of
# the License, or (at your option) any later version.
#
# dynamic-graph is distributed in the hope that it will be useful, but
# WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
# General Lesser Public License for more details. You should have
# 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 import SE3, R3, SO3
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.se3 import R3, SO3, SE3
import numpy as np
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
from dynamic_graph.sot.dynamics import Dynamic
romeo_pkgdatarootdir = "@ROMEO_PKGDATAROOTDIR@"
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
# 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)
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