Skip to content

GitLab

Commit 1fad49f8 authored by Florent Lamiraux's avatar Florent Lamiraux
Browse files

Update to hpp version 3.

parent ca44ff04
Expand all Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 1fad49f8
# Copyright 2012, 2013, 2014 CNRS-LAAS
#
# Author: Florent Lamiraux, Thomas Moulard
# Author: Florent Lamiraux
#
# This file is part of hpp-model-urdf.
# This file is part of test-hpp
# hpp-model-urdf 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
......@@ -13,48 +13,34 @@
# 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 hpp-model-urdf. If not, see <http://www.gnu.org/licenses/>.
# along with test-hpp If not, see <http://www.gnu.org/licenses/>.
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
include(cmake/base.cmake)
include(cmake/boost.cmake)
include(cmake/eigen.cmake)
SET(PROJECT_NAME hpp-test)
SET(PROJECT_DESCRIPTION "Integration tests for Humanoid Path Planner platform.")
SET(PROJECT_URL "github.com/laas/hpp-test")
SETUP_PROJET()
SETUP_PROJECT()
ADD_REQUIRED_DEPENDENCY("hpp-model-urdf >= 2.0")
ADD_REQUIRED_DEPENDENCY("hrp2_14_decription")
SEARCH_FOR_EIGEN()
# Activate hpp-util logging if requested
SET (HPP_DEBUG FALSE CACHE BOOL "trigger hpp-util debug output")
IF (HPP_DEBUG)
SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DHPP_DEBUG")
ENDIF()
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
# Search for Boost.
# Boost.Test is used by the test suite.
SEARCH_FOR_BOOST()
ADD_REQUIRED_DEPENDENCY("hpp-util")
ADD_REQUIRED_DEPENDENCY("hpp-model-urdf")
ADD_REQUIRED_DEPENDENCY("hrp2_14_description")
# Make Boost.Test generates the main function in test cases.
ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
ADD_SUBDIRECTORY(tests)
# ADD_TESTCASE(NAME)
# ------------------------
#
# Define a test named `NAME'.
#
# This macro will create a binary from `NAME.cc', link it against
# Boost and add it to the test suite.
#
MACRO(ADD_TESTCASE NAME GENERATED)
IF (${GENERATED} STREQUAL TRUE)
ADD_EXECUTABLE(${NAME} ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cc)
ELSE()
ADD_EXECUTABLE(${NAME} ${NAME}.cc)
ENDIF()
ADD_TEST(${NAME} ${RUNTIME_OUTPUT_DIRECTORY}/${NAME})
# Link against Boost and project library.
TARGET_LINK_LIBRARIES(${NAME}
${Boost_LIBRARIES}
${PROJECT_NAME})
PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-model-urdf)
ENDMACRO(ADD_TESTCASE)
SETUP_PROJET_FINALIZE()
SETUP_PROJECT_FINALIZE()
data/configuration.data
View file @ 1fad49f8
0.7832759914634781, -0.6287488328607549, 0.6556417293694411, -4.742634295154079e-10, 4.520655732831794e-10, 2.65740280583136, -0.05964761341484225, -0.07981055048413252, 0.16590322948375874, -0.004144442702387867, 0.26483964517748976, 0.26192191007679017, -0.050295369867256, -0.37034363990486474, 0.02150198818252944, 0.1266227625588889, 0.38, -0.38, 0.38, -0.38, 0.38, -0.38, -0.3899134972725518, -0.5925498643189667, -0.19613681199200853, -1.2401320912584846, 0.2763998757815979, -0.027768398868772835, 0.75, -0.75, 0.75, -0.75, 0.75, -0.75, 0.031489444476891226, -0.012849646226002981, -0.4429796164539539, 0.8176013848056326, -0.3746217737072531, 0.012849649901645105, 0.031489445877715766, -0.0129470969613853, -0.44536550586899665, 0.8087943898553672, -0.3634288825007756, 0.012947096434125274
0.7832759914634781 -0.6287488328607549 0.6556417293694411 0.23973698245374014 0.0 0.0 0.9708378748503661 -0.05964761341484225 -0.07981055048413252 0.16590322948375874 -0.004144442702387867 0.26483964517748976 0.26192191007679017 -0.050295369867256 -0.37034363990486474 0.02150198818252944 0.1266227625588889 0.38 -0.38 0.38 -0.38 0.38 -0.38 -0.3899134972725518 -0.5925498643189667 -0.19613681199200853 -1.2401320912584846 0.2763998757815979 -0.027768398868772835 0.75 -0.75 0.75 -0.75 0.75 -0.75 0.031489444476891226 -0.012849646226002981 -0.4429796164539539 0.8176013848056326 -0.3746217737072531 0.012849649901645105 0.031489445877715766 -0.0129470969613853 -0.44536550586899665 0.8087943898553672 -0.3634288825007756 0.012947096434125274
data/distances-capsule.data
View file @ 1fad49f8
This diff is collapsed.
data/distances-mesh.data
View file @ 1fad49f8
This diff is collapsed.
script/hrp2.py 0 → 100644
View file @ 1fad49f8
#!/usr/bin/env python
# Copyright (c) 2014 CNRS
# Author: Florent Lamiraux
#
# This file is part of hpp-corbaserver.
# hpp-corbaserver 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.
#
# hpp-corbaserver 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
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
from hpp.corbaserver.robot import Robot as Parent
class Robot (Parent):
urdfName = "hrp2_14"
urdfSuffix = ""
srdfSuffix = ""
rcpdfSuffix = ""
halfSitting = \
{"base_joint_x": 0.0,
"base_joint_y": 0.0,
"base_joint_z": 0.648702,
"base_joint_SO3": (1.0, 0.0, 0.0, 0.0),
"CHEST_JOINT0": 0.0,
"CHEST_JOINT1": 0.0,
"HEAD_JOINT0": 0.0,
"HEAD_JOINT1": 0.0,
"LARM_JOINT0": 0.261799,
"LARM_JOINT1": 0.17453,
"LARM_JOINT2": 0.0,
"LARM_JOINT3": -0.523599,
"LARM_JOINT4": 0.0,
"LARM_JOINT5": 0.0,
"LARM_JOINT6": 0.1,
"LHAND_JOINT0": 0.0,
"LHAND_JOINT1": 0.0,
"LHAND_JOINT2": 0.0,
"LHAND_JOINT3": 0.0,
"LHAND_JOINT4": 0.0,
"RARM_JOINT0": 0.261799,
"RARM_JOINT1": -0.17453,
"RARM_JOINT2": 0.0,
"RARM_JOINT3": -0.523599,
"RARM_JOINT4": 0.0,
"RARM_JOINT5": 0.0,
"RARM_JOINT6": 0.1,
"RHAND_JOINT0": 0.0,
"RHAND_JOINT1": 0.0,
"RHAND_JOINT2": 0.0,
"RHAND_JOINT3": 0.0,
"RHAND_JOINT4": 0.0,
"LLEG_JOINT0": 0.0,
"LLEG_JOINT1": 0.0,
"LLEG_JOINT2": -0.453786,
"LLEG_JOINT3": 0.872665,
"LLEG_JOINT4": -0.418879,
"LLEG_JOINT5": 0.0,
"RLEG_JOINT0": 0.0,
"RLEG_JOINT1": 0.0,
"RLEG_JOINT2": -0.453786,
"RLEG_JOINT3": 0.872665,
"RLEG_JOINT4": -0.418879,
"RLEG_JOINT5": 0.0
}
def __init__ (self):
Parent.__init__ (self, self.urdfName, self.urdfSuffix, self.srdfSuffix,
self.rcpdfSuffix)
def getInitialConfig (self):
q = []
for n in self.jointNames:
dof = self.halfSitting [n]
if type (dof) is tuple:
q += dof
else:
q.append (dof)
return q
def leftHandClosed (self) :
dofs = {"LARM_JOINT6": 0.1,
"LHAND_JOINT0": 0.0,
"LHAND_JOINT1": 0.0,
"LHAND_JOINT2": 0.0,
"LHAND_JOINT3": 0.0,
"LHAND_JOINT4": 0.0}
res = []
for name, value in dofs.iteritems ():
res.append ((self.rankInConfiguration [name], value))
return res
def rightHandClosed (self) :
dofs = {"RARM_JOINT6": 0.1,
"RHAND_JOINT0": 0.0,
"RHAND_JOINT1": 0.0,
"RHAND_JOINT2": 0.0,
"RHAND_JOINT3": 0.0,
"RHAND_JOINT4": 0.0}
res = []
for name, value in dofs.iteritems ():
res.append ((self.rankInConfiguration [name], value))
return res
Jcom = ((0.99999999969999998, 0.0, 0.0, 0.0, 0.15958929917090708, -0.0015496712689910483, 0.006457034810302846, 0.0, -0.04114883029389188, -0.017070288099399783, -0.001911897308848219, 4.9860119674112868e-21, -0.006457034810302846, 0.0, -0.04114883029389188, -0.017070288099399783, -0.001911897308848219, -4.9860119674112868e-21, -0.0015496395551697997, 0.023029662476935154, -2.9590114765902141e-05, 0.0018998018895406933, -0.024104545096910745, -0.0012462787861598856, 0.00016866935359163523, -0.0094440717997870197, 3.8088868042355686e-05, -0.0017903568629651118, -2.132722755310925e-05, -0.024090010464800509, 0.0012449800380576245, -0.00016641985705804981, -0.0094289163150136652, -3.5491372847415007e-05, -0.0017903568629651118, -2.132722755310925e-05), (0.0, 0.99999999969999998, 0.0, -0.15958929917090708, 0.0, 0.013695401426293118, 0.005578348463571141, 0.041190213959531319, 0.0, -0.0, 0.0, 0.0018841001489199836, 0.0055783484635711427, 0.041190213959531333, 0.0, -0.0, 0.0, 0.0018841001489199836, -0.0080306604130318347, 0.0, 0.00022111055708152463, 0.0, -0.0, 0.023759807377560616, 0.0047122898941987781, -0.00085741003728384058, 4.2166566902055324e-05, -0.00017392402013282932, 3.1910832047119565e-06, -0.0, 0.023731348485939281, 0.0047615821148691458, 0.00086610143705453523, 9.9084328022212327e-05, 0.00017392402013282932, -3.1910832047119565e-06), (0.0, 0.0, 0.99999999969999998, 0.0015496712689910483, -0.013695401426293118, 0.0, 0.0, -0.0065571272884102517, -0.0058165247579172871, 0.0060679086157445893, -7.5940850137388351e-05, -0.00013512977104984366, 0.0, 0.0065571272884102517, -0.0058165247579172888, 0.0060679086157445893, -7.5940850137388351e-05, 0.00013512977104984366, 0.0, 0.0078121562466710627, 0.0, -0.00022687908475124469, 0.0018498822331865989, -0.0046511829652424027, -0.00090539490661791823, -0.0025037065064851039, 3.8257634520993619e-06, -0.00054146100493501043, 8.064784951399954e-05, 0.001794169430077033, 0.0046463359638198786, 0.00091379015499538864, -0.002558798456728665, 5.868235625132099e-06, -0.00054146100493501043, 8.064784951399954e-05))
script/simple_robot.py 0 → 100644
View file @ 1fad49f8
#/usr/bin/env python
from hpp.corbaserver import Client
from hpp_corbaserver.hpp import Configuration
# Create simple 2D robot
cfg1 = Configuration (rot = (1,0,0,0,1,0,0,0,1), trs = (0,0,0))
cl = Client ()
cl.robot.createRobot ('robot')
bounds = [-5., 5., -5., 5., -.01, .01, 1., -1., 1., -1., 1., -1.]
cl.robot.createJoint ('J1', 'freeflyer', cfg1, bounds)
cl.robot.setRobotRootJoint ('robot', 'J1')
cfg2 = Configuration (rot = (0, -1, 0, 1, 0, 0, 0, 0, 1), trs = (0,0,0))
cl.robot.createSphere ("link", 1.)
cl.robot.addObjectToJoint ("J1", "link", cfg1)
cl.robot.setRobot ('robot')
# Create obstacles
cl.obstacle.createBox ("obst1", 1., 1., 0.2)
cl.obstacle.addObstacle ("obst1", True, True)
init = [-2., 0., 0., 0., 0., 0.]
goal = [2.,0., 0., 0., 0., 0.]
cl.problem.setInitialConfig (init)
cl.problem.addGoalConfig (goal)
cl.problem.solve ()
script/svg.py 0 → 100644
View file @ 1fad49f8
#!/usr/bin/env python
class SvgDrawing (object):
prefix = '<!DOCTYPE html>\n<html>\n<body>\n<svg height="%i" width="%i">\n'
suffix = '</svg>\n</body>\n</html>\n'
def __init__ (self, filename, width = 200, height = 200,
xOffset = 100, yOffset = 100, scale = 40):
self.width = width
self.height = height
self.xOffset = xOffset
self.yOffset = yOffset
self.scale = scale
self.filename = filename
self.file = file (filename, 'w')
self.file.write (self.prefix%(width, height))
self.drawBox ()
def convert (self, xy):
return [xy [0] * self.scale + self.xOffset,
xy [1] * self.scale + self.yOffset]
def __del__ (self):
self.file.write (self.suffix)
self.file.close ()
def drawBox (self):
self.file.write (' <rect x="0" y="0" width="%i" height="%i"'%
(self.width,self.height) +
' style="fill:black;stroke:green;stroke-width:1'+
';fill-opacity:0.1;stroke-opacity:1.0" />\n')
def drawCircle (self, q, color):
x, y = self.convert (q)
self.file.write (' <circle cx="%i" cy="%i" r="3" '%(x+.5,y+.5)+
'stroke="black" stroke-width="1" fill="%s" ;'%color +
'fill-opacity=0.2 />\n')
def drawRectangle (self, xCenter, yCenter, xLength, yLength):
x, y = self.convert ([xCenter - .5*xLength, yCenter - .5*yLength])
w = xLength * self.scale
h = yLength * self.scale
self.file.write (' <rect x="%i" y="%i" width="%i" height="%i" style="fill:black;stroke:pink;stroke-width:0;fill-opacity:0.5;stroke-opacity:0.9"/>\n'%
(x, y, w, h))
def drawLine (self, x1, y1, x2, y2):
X1, Y1 = self.convert ([x1,y1])
X2, Y2 = self.convert ([x2,y2])
self.file.write ('<line x1="%i" y1="%i" x2="%i" y2="%i"'%(X1,Y1,X2,Y2)+
'style="stroke:rgb(0,0,0);stroke-width:1" />\n')
def initAndGoal (self, xi, yi, xg, yg):
self.drawCircle ([xi, yi], "black")
self.drawCircle ([xg, yg], "black")
def rand (self, x, y):
self.drawCircle ([x, y], "red")
def node (self, x, y):
self.drawCircle ([x, y], "blue")
def edge (self, x1, y1, x2, y2):
self.drawLine (x1, y1, x2, y2)
script/test_com.py 0 → 100644
View file @ 1fad49f8
import numpy as np
from hpp.corbaserver import Client
from hrp2 import getHalfSitting
cl = Client ()
cl.robot.loadRobotModel ('hrp2_14', '_capsule', '_capsule', '')
jn = cl.robot.getJointNames ()
q0 = getHalfSitting (jn)
cl.robot.setCurrentConfig (q0)
com0 = np.matrix ([cl.robot.getCenterOfMass ()]).transpose ()
Jcom = np.matrix (cl.robot.getJacobianCenterOfMass ())
delta = 1e-6
for i in range (7, Jcom.shape [1]):
q = q0 [::]; q [i+1] += delta
cl.robot.setCurrentConfig (q)
com = np.matrix ([cl.robot.getCenterOfMass ()]).transpose ()
error = (com - com0)/delta - Jcom [:,i]
print ("i=%i, error=%f"%(i, np.linalg.norm (error)))
script/test_direct_path.py 0 → 100644
View file @ 1fad49f8
#/usr/bin/env python
import time
from hpp.corbaserver import Client
from hpp_corbaserver.hpp import Configuration
from hpp_ros import ScenePublisher
from hpp.tools import PathPlayer
from math import pi
cl = Client ()
cl.robot.loadRobotModel ('hrp2_14', '', '', '')
jn = cl.robot.getJointNames ()
r = ScenePublisher (jn [6:])
q = cl.robot.getCurrentConfig ()
r (q)
q1 = q [::]
q2 = q [::]
q1 [3] = pi/3
q1 [4] = 3*pi/2
q2 [5] = pi - 1e-3
cl.problem.directPath (q1, q2)
pathId = cl.problem.countPaths () - 1
length = cl.problem.pathLength (pathId)
p = PathPlayer (cl, r, pathId)
q1 = [0.0, 0.0, 0.705, 1.0471975511965976, 4.71238898038469, 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
q2 = [0.0, 0.0, 0.705, 0.0, -0.0, 3.1405926535897932, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
script/test_rrt.py 0 → 100644
View file @ 1fad49f8
#/usr/bin/env python
from hpp_corbaserver.hpp import Configuration
from hpp_ros import ScenePublisher
from hpp.tools import PathPlayer
from hrp2 import Robot
from hpp.corbaserver.wholebody_step.client import Client as WsClient
Robot.urdfSuffix = '_capsule'
Robot.srdfSuffix= '_capsule'
robot = Robot ()
robot.setTranslationBounds (-3, 3, -3, 3, 0, 1)
cl = robot.client
r = ScenePublisher (robot.jointNames [4:])
q0 = robot.getInitialConfig ()
r (q0)
# Add constraints
wcl = WsClient ()
wcl.problem.addStaticStabilityConstraints (q0)
# lock hands in closed position
lockedDofs = robot.leftHandClosed ()
for index, value in lockedDofs:
cl.problem.lockDof (index, value)
lockedDofs = robot.rightHandClosed ()
for index, value in lockedDofs:
cl.problem.lockDof (index, value)
q1 = [0.0, 0.0, 0.705, 1.0, 0., 0., 0.0, 0.0, 0.0, 0.0, 0.0, -0.4, 0, -1.2, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
status, q1proj = cl.problem.applyConstraints (q1)
q2 = [0.0, 0.0, 0.705, 1, 0, 0, 0, 0.0, 0.0, 0.0, 0.0, 1.0, 0, -1.4, -1.0, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.261799, -0.17453, 0.0, -0.523599, 0.0, 0.0, 0.174532, -0.174532, 0.174532, -0.174532, 0.174532, -0.174532, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0, 0.0, 0.0, -0.453786, 0.872665, -0.418879, 0.0]
status, q2proj = cl.problem.applyConstraints (q2)
cl.problem.setInitialConfig (q1proj)
cl.problem.addGoalConfig (q2proj)
cl.problem.solve ()
script/tools.py 0 → 100644
View file @ 1fad49f8
#!/usr/bin/env python
import os
from svg import SvgDrawing
class LogParser (object):
prefix = ["INFO:/home/florent/devel/airbus/src/hpp-core/src/roadmap.cc:86: Added node: ", "INFO:/home/florent/devel/airbus/src/hpp-core/src/diffusing-planner.cc:48: q_rand = ", "INFO:/home/florent/devel/airbus/src/hpp-core/src/roadmap.cc:160: Added edge between: ", "INFO:/home/florent/devel/airbus/src/hpp-core/src/roadmap.cc:162: and: "]
def __init__ (self, pid):
self.file = file (os.getenv ("DEVEL_DIR") +
"/install/var/log/hpp/journal." + str (pid) +
".log")
self.svg = SvgDrawing (os.getenv ("DEVEL_DIR") +
"/install/var/log/hpp/journal." + str (pid) +
".html", 400, 400, 200, 200, 40)
self.svg.drawRectangle (0,0,1,.5)
def parse (self, nbLines):
for line, j in zip (self.file, range (nbLines)):
for p,i in zip (self.prefix, xrange (100)):
if line [:len (p)] == p:
q = map (float, line [len (p):].split (',')[:-1])
if i == 0:
# Add node
self.svg.node (q [0], q[1])
if i == 1:
# q_rand
self.svg.rand (q [0], q[1])
if i == 2:
self.nodeFrom = q
if i == 3:
self.svg.edge (self.nodeFrom [0], self.nodeFrom [1],
q [0], q [1])
tests/CMakeLists.txt 0 → 100644
View file @ 1fad49f8
# Copyright 2012, 2013, 2014 CNRS-LAAS
#
# Author: Florent Lamiraux, Thomas Moulard
#
# This file is part of test-hpp
# hpp-model-urdf 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.
#
# hpp-model-urdf 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 test-hpp If not, see <http://www.gnu.org/licenses/>.
INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
# Make Boost.Test generates the main function in test cases.
#ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
# ADD_TESTCASE(NAME)
# ------------------------
#
# Define a test named `NAME'.
#
# This macro will create a binary from `NAME.cc', link it against
# Boost and add it to the test suite.
#
MACRO(ADD_TESTCASE NAME GENERATED)
IF (${GENERATED} STREQUAL TRUE)
ADD_EXECUTABLE(${NAME} ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cc)
ELSE()
ADD_EXECUTABLE(${NAME} ${NAME}.cc)
ENDIF()
ADD_TEST(${NAME} ${RUNTIME_OUTPUT_DIRECTORY}/${NAME})
# Link against Boost and project library.
TARGET_LINK_LIBRARIES(${NAME}
${Boost_LIBRARIES}
)
PKG_CONFIG_USE_DEPENDENCY(${NAME} hpp-model-urdf)
ENDMACRO(ADD_TESTCASE)
CONFIG_FILES (
body-positions-mesh.cc
interbody-distances-mesh.cc
interbody-distances-capsule.cc
)
ADD_TESTCASE (body-positions-mesh TRUE)
ADD_TESTCASE (interbody-distances-mesh TRUE)
ADD_TESTCASE (interbody-distances-capsule TRUE)
tests/body-positions-mesh.cc.in 0 → 100644
View file @ 1fad49f8
// Copyright (C) 2013, 2014 LAAS-CNRS
// Author: Florent Lamiraux
//
// This file is part of the test-hpp.
//
// test-hpp 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.
//
// test-hpp 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 Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public License
// along with test-hpp. If not, see <http://www.gnu.org/licenses/>.
#include <sstream>
#include <ostream>
#include <fstream>
#include <vector>
#include <Eigen/Core>
#include <hpp/util/debug.hh>
#include <hpp/model/device.hh>
#include <hpp/model/body.hh>
#include <hpp/model/collision-object.hh>
#include <hpp/model/joint.hh>
#include <hpp/model/urdf/util.hh>
typedef std::map <std::string, Eigen::Matrix4d, std::less <std::string>,
Eigen::aligned_allocator
<std::pair<const double, Eigen::Matrix4d> > > PositionsMap_t;
void fillExpectedPositionsMap (PositionsMap_t& positions);
#define BOOST_TEST_MODULE forward_kinematics
#include <boost/test/included/unit_test.hpp>
BOOST_AUTO_TEST_SUITE( test_suite )
BOOST_AUTO_TEST_CASE (body_positions_mesh)
{
using hpp::model::Device;
using hpp::model::Joint;
using hpp::model::Body;
typedef hpp::model::ObjectVector_t ObjectVector_t;
typedef hpp::model::JointVector_t JointVector_t;
typedef std::vector<double> vector_t;
std::ifstream fileConfig;
std::ofstream fileHrp2;
vector_t::size_type configSize;
fileConfig.open ("@PROJECT_SOURCE_DIR@/data/configuration.data");
vector_t dofs;
bool eof = false;
double value;
// Read configuration file
while (!eof) {
fileConfig >> value;
if (fileConfig.fail ()) {
eof = true;
} else {
dofs.push_back (value);
}
}
fileConfig.close ();
// Load hrp2
hpp::model::HumanoidRobotShPtr humanoidRobot;
hpp::model::urdf::loadRobotModel (humanoidRobot, "hrp2_14", "", "", "");
configSize = humanoidRobot->configSize ();
// Check size of config
if (dofs.size () != configSize) {
std::ostringstream error;
error << "wrong configuration size: " << dofs.size ()
<< ", expecting " << configSize;
throw std::runtime_error (error.str ());
}
hpp::model::Configuration_t config (configSize);
for (vector_t::size_type i=0; i<configSize; ++i) {
config [i] = dofs [i];
}
humanoidRobot->currentConfiguration (config);
humanoidRobot->computeForwardKinematics ();
hppDout (info, *humanoidRobot);
// Check positions of geometric objects
PositionsMap_t positions;
fillExpectedPositionsMap (positions);
double maxDiff = 0, diff = 0;
std::string maxDiffName;
// Loop over joints to get bodies
const JointVector_t& jv(humanoidRobot->getJointVector ());
for (JointVector_t::const_iterator itJoint = jv.begin ();
itJoint != jv.end (); itJoint++) {
Body* body = (*itJoint)->linkedBody ();
if (body != 0) {
const ObjectVector_t& ov (body->innerObjects (hpp::model::DISTANCE));
for (ObjectVector_t::const_iterator itObj = ov.begin ();
itObj != ov.end (); itObj ++) {
Eigen::Matrix4d expectedPosition = positions [(*itObj)->name ()];
const fcl::Transform3f& position ((*itObj)->fcl ()->getTransform ());
for (std::size_t i=0; i<2; ++i) {
for (std::size_t j=0; i<2; ++i) {
diff = fabs (position.getRotation () (i, j) -
expectedPosition (i, j));
BOOST_CHECK (diff < 1e-5);
if (diff > maxDiff) {
maxDiff = diff;
maxDiffName = (*itObj)->name ();
hppDout (info, "name: " << maxDiffName << std::endl
<< "i=" << i << "\t j=" << j);
}
}
diff = fabs (position.getTranslation ()[i] - expectedPosition (i, 3));
BOOST_CHECK (diff < 1e-5);
if (diff > maxDiff) {
maxDiff = diff;
maxDiffName = (*itObj)->name ();
hppDout (info, "name: " << maxDiffName << std::endl << "i=" << i
<< "\t j=3");