Commit 950fb143 authored by Carlos Mastalli's avatar Carlos Mastalli
Browse files

Merge branch 'topic/icub' into 'devel'

Defined a Python module + installation of it

See merge request !8
parents d29f3c09 fc7d9540
Pipeline #4139 passed with stage
in 34 seconds
......@@ -2,6 +2,7 @@ CMAKE_MINIMUM_REQUIRED(VERSION 2.8)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/test.cmake)
INCLUDE(cmake/python.cmake)
SET(PROJECT_NAMESPACE gepetto)
SET(PROJECT_NAME example-robot-data)
......@@ -11,9 +12,16 @@ SET(PROJECT_URL https://gepgitlab.laas.fr/${PROJECT_NAMESPACE}/${PROJECT_NAME})
SETUP_PROJECT()
ADD_REQUIRED_DEPENDENCY("pinocchio >= 2.1.0")
FINDPYTHON()
INSTALL(DIRECTORY hyq_description DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY icub_description DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY talos_data DESTINATION share/${PROJECT_NAME})
INSTALL(DIRECTORY tiago_description DESTINATION share/${PROJECT_NAME})
ADD_SUBDIRECTORY(python)
ADD_SUBDIRECTORY(unittest)
SETUP_PROJECT_FINALIZE()
......@@ -392,14 +392,14 @@
</link>
<link name="l_wrist_1">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
</link>
<link name="neck_1">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
......@@ -422,7 +422,7 @@
</link>
<link name="neck_2">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
......@@ -768,7 +768,7 @@
</link>
<link name="r_wrist_1">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
......@@ -798,14 +798,14 @@
</link>
<link name="torso">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
</inertial>
</link>
<link name="torso_1">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0.0004544" ixy="-4.263e-05" ixz="-3.889e-08" iyy="0.001141" iyz="0" izz="0.001236" />
</inertial>
......@@ -828,7 +828,7 @@
</link>
<link name="torso_2">
<inertial>
<mass value="0" />
<mass value="0.1" />
<origin xyz="0 0 0" rpy="0 -0 0" />
<inertia ixx="0.0005308" ixy="-1.923e-06" ixz="5.095e-05" iyy="0.002031" iyz="-3.849e-07" izz="0.001803" />
</inertial>
......
This diff is collapsed.
......@@ -8,7 +8,7 @@
<joint name="l_knee" />
<joint name="l_ankle_pitch" />
<joint name="l_ankle_roll" />
<chain base_link="chest" tip_link="l_foot_dh_frame_fixed_joint" />
<chain base_link="chest" tip_link="l_sole" />
</group>
<group name="r_leg">
<joint name="r_hip_pitch" />
......@@ -17,7 +17,7 @@
<joint name="r_knee" />
<joint name="r_ankle_pitch" />
<joint name="r_ankle_roll" />
<chain base_link="chest" tip_link="r_foot_dh_frame_fixed_joint" />
<chain base_link="chest" tip_link="r_sole" />
</group>
<group name="l_arm">
<joint name="l_shoulder_pitch" />
......@@ -27,7 +27,7 @@
<joint name="l_wrist_prosup" />
<joint name="l_wrist_pitch" />
<joint name="l_wrist_yaw" />
<chain base_link="chest" tip_link="l_hand_dh_frame_fixed_joint" />
<chain base_link="chest" tip_link="l_hand" />
</group>
<group name="r_arm">
<joint name="r_shoulder_pitch" />
......@@ -37,7 +37,7 @@
<joint name="r_wrist_prosup" />
<joint name="r_wrist_pitch" />
<joint name="r_wrist_yaw" />
<chain base_link="chest" tip_link="r_hand_dh_frame_fixed_joint" />
<chain base_link="chest" tip_link="r_hand" />
</group>
<group name="head">
<joint name="neck_pitch" />
......
SET(${PROJECT_NAME}_PYTHON_FILES
robots_loader.py
display.py
__init__.py
)
FOREACH(python ${${PROJECT_NAME}_PYTHON_FILES})
PYTHON_BUILD(. ${python})
INSTALL(FILES ${python} DESTINATION ${PYTHON_SITELIB}/example_robot_data)
ENDFOREACH(python ${${PROJECT_NAME}_PYTHON_FILES})
from robots_loader import (getModelPath, loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand,
readParamsFromSrdf)
import sys
sys.path.append('/opt/openrobots/share/example-robot-data/unittest/')
from unittest_utils import loadHyQ, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand, loadICub
from unittest_utils import loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand
sys.path.append('/opt/openrobots/share/example-robot-data/unittest/')
DISPLAY_HYQ = 'hyq' in sys.argv
DISPLAY_TALOS = 'talos' in sys.argv
......@@ -21,7 +22,7 @@ if DISPLAY_TALOS:
talos.display(talos.q0)
if DISPLAY_TALOS_ARM:
talos_arm = loadTalos()
talos_arm = loadTalosArm()
talos_arm.initDisplay(loadModel=True)
talos_arm.display(talos_arm.q0)
......@@ -39,4 +40,3 @@ if DISPLAY_ICUB:
icub = loadICub()
icub.initDisplay(loadModel=True)
icub.display(icub.q0)
from os.path import exists, join
import numpy as np
import pinocchio
from pinocchio.robot_wrapper import RobotWrapper
......@@ -9,7 +8,7 @@ from pinocchio.robot_wrapper import RobotWrapper
def getModelPath(subpath):
for path in ['..', '../..', '/opt/openrobots/share/example-robot-data']:
if exists(join(path, subpath.strip('/'))):
print "using %s as modelPath" % path
print("using %s as modelPath" % path)
return path
raise IOError('%s not found' % (subpath))
......@@ -18,21 +17,9 @@ def readParamsFromSrdf(robot, SRDF_PATH, verbose):
rmodel = robot.model
pinocchio.loadRotorParameters(rmodel, SRDF_PATH, verbose)
rmodel.armature = \
np.multiply(rmodel.rotorInertia.flat,
np.square(rmodel.rotorGearRatio.flat))
try:
pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = \
rmodel.referenceConfigurations["half_sitting"].copy()
except:
print "loadReferenceConfigurations did not work. Please check your \
Pinocchio Version"
try:
pinocchio.getNeutralConfiguration(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = rmodel.neutralConfiguration.copy()
except:
robot.q0.flat[:] = pinocchio.neutral(rmodel)
rmodel.armature = np.multiply(rmodel.rotorInertia.flat, np.square(rmodel.rotorGearRatio.flat))
pinocchio.loadReferenceConfigurations(rmodel, SRDF_PATH, verbose)
robot.q0.flat[:] = rmodel.referenceConfigurations["half_sitting"].copy()
return
......@@ -43,10 +30,10 @@ def loadTalosArm():
SRDF_SUBPATH = "/talos_data/srdf/" + SRDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
return robot
......@@ -57,11 +44,10 @@ def loadTalos():
URDF_SUBPATH = "/talos_data/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
assert((robot.model.armature[:6] == 0.).all())
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
assert ((robot.model.armature[:6] == 0.).all())
return robot
......@@ -72,47 +58,49 @@ def loadHyQ():
URDF_SUBPATH = "/hyq_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
return robot
def loadTiago():
URDF_FILENAME = "tiago.urdf"
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
return robot
def loadTiagoNoHand():
URDF_FILENAME = "tiago_no_hand.urdf"
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
# SRDF_FILENAME = "tiago.srdf"
# SRDF_SUBPATH = "/tiago_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/tiago_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath])
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath])
# Load SRDF file
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
# readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
return robot
def loadICub():
URDF_FILENAME = "icub.urdf"
def loadICub(reduced=True):
if reduced:
URDF_FILENAME = "icub_reduced.urdf"
else:
URDF_FILENAME = "icub.urdf"
SRDF_FILENAME = "icub.srdf"
SRDF_SUBPATH = "/icub_description/srdf/" + SRDF_FILENAME
URDF_SUBPATH = "/icub_description/robots/" + URDF_FILENAME
modelPath = getModelPath(URDF_SUBPATH)
# Load URDF file
robot = RobotWrapper.BuildFromURDF(modelPath+URDF_SUBPATH, [modelPath],
pinocchio.JointModelFreeFlyer())
robot = RobotWrapper.BuildFromURDF(modelPath + URDF_SUBPATH, [modelPath], pinocchio.JointModelFreeFlyer())
# Load SRDF file
readParamsFromSrdf(robot, modelPath+SRDF_SUBPATH, False)
readParamsFromSrdf(robot, modelPath + SRDF_SUBPATH, False)
return robot
[flake8]
max-line-length = 119
exclude = python/__init__.py
[yapf]
column_limit = 119
[isort]
line_length = 119
......@@ -2,7 +2,7 @@
import unittest
from unittest_utils import loadHyQ, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand, loadICub
from example_robot_data import loadHyQ, loadICub, loadTalos, loadTalosArm, loadTiago, loadTiagoNoHand
class RobotTestCase(unittest.TestCase):
......@@ -57,10 +57,12 @@ class TiagoNoHandTest(RobotTestCase):
RobotTestCase.NQ = 14
RobotTestCase.NV = 12
class ICubTest(RobotTestCase):
RobotTestCase.ROBOT = loadICub()
RobotTestCase.ROBOT = loadICub(reduced=False)
RobotTestCase.NQ = 39
RobotTestCase.NV = 38
if __name__ == '__main__':
unittest.main()
Supports Markdown
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