Commit 424a0591 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Check final configurations in examples.

parent 11c239de
......@@ -200,5 +200,18 @@ for i in xrange(int(duration / timeStep)):
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
finalPosition = (
-0.0082169200000000008, -0.0126068, -0.00022860999999999999,
0.019962199999999999, -0.0159528, -0.00037375100000000002,
4.2206800000000002e-07, 0.0032752800000000002, -0.46070899999999998,
0.88794600000000001, -0.41127999999999998, -0.023234500000000002,
1.7543699999999999e-06, 0.00319733, -0.44445400000000002,
0.85594800000000004, -0.39553700000000003, -0.0231565, 0.00126246,
-0.0010669500000000001, -3.2293899999999999e-05, -0.00016289599999999999,
0.26377099999999998, -0.178532, -0.00082797999999999997,
-0.52297099999999996, -9.2273200000000003e-06, 0.000114984, 0.100008,
0.26377400000000001, 0.171155, -0.00065098499999999998,
-0.52324700000000002, -1.23291e-05, 6.0469500000000001e-05, 0.100009)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
print "Exiting."
......@@ -15,6 +15,7 @@
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import sys
from tools import *
# Move left wrist
......@@ -40,4 +41,19 @@ for i in xrange(500):
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
finalPosition = (
-0.0357296, -0.0024092699999999998, 0.033870600000000001,
-0.00120006, 0.075338500000000003, 0.00028531699999999999,
2.9108999999999999e-05, 0.0053813699999999999, -0.41716799999999998,
0.52743700000000004, -0.185608, -0.0041716399999999999,
2.9120099999999999e-05, 0.0053815199999999999, -0.41625699999999999,
0.52549900000000005, -0.184581, -0.0041717899999999999, -0.00306069,
-0.54035500000000003, 0.00036732799999999999, -0.028043100000000001,
-0.69961799999999996, -0.84530499999999997, 0.11480700000000001,
-0.61734999999999995, 0.088004899999999997, 1.0022899999999999,
0.100354, -0.71066200000000002, 0.85328800000000005,
-0.109959, -0.60156299999999996, -0.082422700000000002,
1.0207200000000001, 0.10037500000000001)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
print "Exiting."
......@@ -15,6 +15,7 @@
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import sys
from tools import *
# Move left wrist
......@@ -39,4 +40,18 @@ for i in xrange(500):
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
finalPosition = (
-0.015183500000000001, -0.00037148200000000002, -0.00065935600000000005,
0.0137784, -0.022388600000000002, -0.036052000000000001,
0.036032099999999997, -0.012359200000000001, -0.466526,
0.87994899999999998, -0.39055299999999998, -0.00060408700000000001,
0.036028499999999998, -0.0121996, -0.45257900000000001,
0.86809899999999995, -0.39265, -0.00076379799999999999,
-0.084612699999999999, -0.18716099999999999, 0.00038832500000000002,
-0.0054475900000000004, 0.23852599999999999, -0.19830900000000001,
0.17092299999999999, -0.48823699999999998, -0.014739800000000001,
0.170987, 0.100064, -0.117492, 0.24870200000000001, 0.016264399999999998,
-0.56795700000000005, 0.0040012399999999997, 0.18956200000000001, 0.100089)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
print "Exiting."
......@@ -39,4 +39,19 @@ for i in xrange(500):
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
finalPosition = (
-0.015361, -0.0049075500000000001, -0.00047065200000000001, -0.0172946,
-0.020661800000000001, 0.0374547, -0.037641599999999997,
0.025434399999999999, -0.45398100000000002, 0.86741800000000002,
-0.39213799999999999, -0.0089269499999999995, -0.037646100000000002,
0.025648199999999999, -0.46715499999999999, 0.87717599999999996,
-0.38872200000000001, -0.0091408199999999992, 0.080488199999999996,
-0.18355399999999999, -0.00036695100000000002, -0.0056776600000000002,
-0.12173299999999999, -0.23972599999999999, -0.00637303,
-0.56908000000000003, 0.00296262, 0.19108900000000001, 0.100088,
0.23896800000000001, 0.21485599999999999, -0.18973400000000001,
-0.49457699999999999, 0.040646799999999997, 0.16970299999999999, 0.100067)
checkFinalConfiguration(robot.robotSimu.signal('state').value, finalPosition)
print "Exiting."
......@@ -15,6 +15,7 @@
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
import sys
from optparse import OptionParser
from dynamic_graph import plug
......@@ -72,6 +73,36 @@ def displayHomogeneousMatrix(matrix):
formatStr += '\n'
print formatStr.format(matrix_tuple)
def displayHrp2Configuration(cfg):
if len(cfg) != 36:
raise "bad configuration size"
str = ''
str += 'Free flyer:\n'
str += ' translation {0[0]: <+10f} {0[1]: <+10f} {0[2]: <+10f}\n'
str += ' rotation {0[3]: <+10f} {0[4]: <+10f} {0[5]: <+10f}\n'
str += 'Left leg:\n'
str += ' hip {0[6]: <+10f} {0[7]: <+10f} {0[8]: <+10f}\n'
str += ' knee {0[9]: <+10f}\n'
str += ' ankle {0[10]: <+10f} {0[11]: <+10f}\n'
str += 'Right leg:\n'
str += ' hip {0[12]: <+10f} {0[13]: <+10f} {0[14]: <+10f}\n'
str += ' knee {0[15]: <+10f}\n'
str += ' ankle {0[16]: <+10f} {0[17]: <+10f}\n'
str += 'Chest: {0[18]: <+10f} {0[19]: <+10f}\n'
str += 'Head: {0[20]: <+10f} {0[21]: <+10f}\n'
str += 'Left arm:\n'
str += ' shoulder {0[22]: <+10f} {0[23]: <+10f} {0[24]: <+10f}\n'
str += ' elbow {0[25]: <+10f}\n'
str += ' wrist {0[26]: <+10f} {0[27]: <+10f}\n'
str += ' clench {0[28]: <+10f}\n'
str += 'Left arm:\n'
str += ' shoulder {0[29]: <+10f} {0[30]: <+10f} {0[31]: <+10f}\n'
str += ' elbow {0[32]: <+10f}\n'
str += ' wrist {0[33]: <+10f} {0[34]: <+10f}\n'
str += ' clench {0[35]: <+10f}\n'
print str.format(cfg)
def initRobotViewer():
"""Initialize robotviewer is possible."""
clt = None
......@@ -92,6 +123,22 @@ def reach(robot, op, tx, ty, tz):
robot.features[op].feature.signal('selec').value = '000111'
robot.tasks[op].signal('controlGain').value = 1.
def sqrDist(value, expectedValue):
"""Compute the square of the distance between two configurations."""
return reduce(lambda acc, (a, b): acc + abs(a - b) * abs(a - b),
zip(value, expectedValue), 0.)
def checkFinalConfiguration(position, finalPosition):
if sqrDist(position, finalPosition) >= 1e-3:
print "Wrong final position. Failing."
print "Value:"
displayHrp2Configuration(position)
print "Expected value:"
displayHrp2Configuration(finalPosition)
print "Difference:"
displayHrp2Configuration(map(lambda (x, y): x - y,
zip(position, finalPosition)))
sys.exit(1)
##################
# Helper classes #
......
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