Commit d6e9fae9 authored by Thomas Moulard's avatar Thomas Moulard

Migrate Python files properly.

parent 10917560
......@@ -45,21 +45,27 @@ ENDFUNCTION()
COMPILE_PLUGIN(dynamic-hrp2_14 dynamic-hrp2_14.cc DynamicHrp2_14)
COMPILE_PLUGIN(dynamic-hrp2_10 dynamic-hrp2_10.cc DynamicHrp2_10)
CONFIG_FILES(dynamic_graph/sot/hrp2_10/robot.py)
CONFIG_FILES(dynamic_graph/sot/hrp2_14/robot.py)
# Install Python files.
SET(PYTHON_MODULE_DIR
${CMAKE_CURRENT_SOURCE_DIR}/dynamic_graph/sot/hrp2)
SET(PYTHON_MODULE_BUILD_DIR
${CMAKE_CURRENT_BINARY_DIR}/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}" "hrp2.py" )
SET(FILES __init__.py robot.py)
# Install dynamic_graph.sot.hrp2_14
SET(PYTHON_MODULE dynamic_graph/sot/hrp2_14)
FOREACH(FILE ${FILES})
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "${FILE}" )
ENDFOREACH()
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" ${PYTHON_SITELIB})
# Install dynamic_graph.sot.hrp2_10
SET(PYTHON_MODULE dynamic_graph/sot/hrp2_10)
FOREACH(FILE ${FILES})
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "${FILE}" )
ENDFOREACH()
PYTHON_INSTALL_ON_SITE("${PYTHON_MODULE}" "__init__.py" )
PYTHON_INSTALL_BUILD("${PYTHON_MODULE}" "robot.py" ${PYTHON_SITELIB})
# -*- 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.hrp2.hrp2 import Hrp2
# -*- 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 FeaturePoint6dRelative, \
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
from dynamic_graph.sot.dynamics.dynamic_hrp2 import DynamicHrp2
from dynamic_graph.sot.dynamics.dynamic_hrp2_10 import DynamicHrp2_10
from dynamic_graph.sot.dynamics.humanoid_robot import AbstractHumanoidRobot
class Hrp2(AbstractHumanoidRobot):
"""
This class instanciates a Hrp2 robot
"""
def smallToFull(self, config):
res = (config + 10*(0.,))
return res
def __init__(self, name, modelDir, xmlDir, device, dynamicType,
tracer = None):
AbstractHumanoidRobot.__init__ (self, name, tracer)
self.OperationalPoints.append('waist')
self.device = device
modelName = 'HRP2JRLmainsmall.wrl'
specificitiesPath = xmlDir + '/HRP2SpecificitiesSmall.xml'
jointRankPath = xmlDir + '/HRP2LinkJointRankSmall.xml'
self.dynamic = self.loadModelFromJrlDynamics(
self.name + '_dynamic',
modelDir,
modelName,
specificitiesPath,
jointRankPath,
dynamicType)
self.dimension = self.dynamic.getDimension()
if self.dimension != len(self.halfSitting):
raise RuntimeError("invalid half-sitting pose")
self.initializeRobot()
__all__ = [Hrp2]
# -*- 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.hrp2 import Hrp2
hrp2_10_pkgdatarootdir = "@HRP2_10_PKGDATAROOTDIR@"
class Robot (Hrp2):
"""
This class instanciates LAAS Hrp2 robot
"""
halfSitting = (
# 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
0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1, 0.261799,
0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1, 0.261799,
)
def __init__(self, name,
modelDir = hrp2_10_pkgdatarootdir,
xmlDir = hrp2_10_pkgdatarootdir,
device = None,
tracer = None):
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2_10,
tracer)
__all__ = [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
from dynamic_graph.sot.hrp2 import Hrp2
hrp2_14_pkgdatarootdir = "@HRP2_14_PKGDATAROOTDIR@"
# Internal helper tool.
def matrixToTuple(M):
tmp = M.tolist()
res = []
for i in tmp:
res.append(tuple(i))
return tuple(res)
class Robot (Hrp2):
"""
This class instanciates LAAS Hrp2 robot
"""
halfSitting = (
# 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
0.261799, -0.17453, 0., -0.523599, 0., 0., 0.1,
0.261799, 0.17453, 0., -0.523599, 0., 0., 0.1,
)
def __init__(self, name,
modelDir = hrp2_14_pkgdatarootdir,
xmlDir = hrp2_14_pkgdatarootdir,
device = None,
tracer = None):
# Define camera positions w.r.t gaze.
# These positions have been copied from hrp2.geom and
# roughly checked. Do not trust them too much.
cameraBottomLeftPosition = np.matrix((
(0.98481, 0.00000, 0.17365, 0.035),
(0., 1., 0., 0.072),
(-0.17365, 0.00000, 0.98481, 0.075 - 0.03),
(0., 0., 0., 1.),
))
cameraBottomRightPosition = np.matrix((
(0.98481, 0.00000, 0.17365, 0.035),
(0., 1., 0., -0.072),
(-0.17365, 0.00000, 0.98481, 0.075 - 0.03),
(0., 0., 0., 1.),
))
cameraTopLeftPosition = np.matrix((
(0.99920, 0.00120, 0.03997, 0.01),
(0.00000, 0.99955,-0.03000, 0.029),
(-0.03999, 0.02997, 0.99875, 0.145 - 0.03),
(0., 0., 0., 1.),
))
cameraTopRightPosition = np.matrix((
(0.99920, 0.00000, 0.03999, 0.01),
(0.00000, 1.00000, 0.00000, -0.029),
(-0.03999, 0.00000, 0.99920, 0.145 - 0.03),
(0., 0., 0., 1.),
))
# Frames re-orientation:
# Z = depth (increase from near to far)
# X = increase from left to right
# Y = increase from top to bottom
c1_M_c = np.matrix(
[[ 0., 0., 1., 0.],
[-1., 0., 0., 0.],
[ 0., -1., 0., 0.],
[ 0., 0., 0., 1.]])
for camera in [cameraBottomLeftPosition, cameraBottomRightPosition,
cameraTopLeftPosition, cameraTopRightPosition]:
camera = camera * c1_M_c
self.AdditionalFrames.append(
("cameraBottomLeft",
matrixToTuple(cameraBottomLeftPosition), "gaze"))
self.AdditionalFrames.append(
("cameraBottomRight",
matrixToTuple(cameraBottomRightPosition), "gaze"))
self.AdditionalFrames.append(
("cameraTopLeft",
matrixToTuple(cameraTopLeftPosition), "gaze"))
self.AdditionalFrames.append(
("cameraTopRight",
matrixToTuple(cameraTopRightPosition), "gaze"))
Hrp2.__init__(self, name, modelDir, xmlDir, device, DynamicHrp2, tracer)
__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