Commit 1c5d9875 authored by Thomas Moulard's avatar Thomas Moulard
Browse files

Add new tests.

parent 1e3d6469
#!/usr/bin/env python
# -*- 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 tools import *
# Move left wrist
reach(robot, 'left-wrist', 0.25, 0.25, 0.5)
reach(robot, 'right-wrist', 0.25, -0.25, 0.5)
# Push tasks
# Operational points tasks
solver.sot.push(robot.name + '.task.right-ankle')
solver.sot.push(robot.name + '.task.left-ankle')
solver.sot.push(robot.name + '.task.left-wrist')
solver.sot.push(robot.name + '.task.right-wrist')
# Center of mass
solver.sot.push(robot.name + '.task.com')
# Main.
# Main loop
for i in xrange(500):
robot.robotSimu.increment(timeStep)
if clt:
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
#!/usr/bin/env python
# -*- 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 tools import *
# Move left wrist
reach(robot, 'left-wrist', 0.25, 0, 0.1)
# Push tasks
# Operational points tasks
solver.sot.push(robot.name + '.task.right-ankle')
solver.sot.push(robot.name + '.task.left-ankle')
solver.sot.push(robot.name + '.task.left-wrist')
solver.sot.push(robot.name + '.task.right-wrist')
# Center of mass
solver.sot.push(robot.name + '.task.com')
# Main.
# Main loop
for i in xrange(500):
robot.robotSimu.increment(timeStep)
if clt:
clt.updateElementConfig(
'hrp', robot.smallToFull(robot.robotSimu.signal('state').value))
print "FINISHED"
......@@ -15,28 +15,16 @@
# received a copy of the GNU Lesser General Public License along with
# dynamic-graph. If not, see <http://www.gnu.org/licenses/>.
from dynamic_graph.sot.core import FeaturePoint6d, FeatureGeneric, SOT
from dynamic_graph.sot.dynamics.humanoid_robot import HumanoidRobot
from dynamic_graph.sot.dynamics.hrp2 import Hrp2
from dynamic_graph import enableTrace, plug
from tools import *
# Move right wrist
sdes = toList(robot.dynamicRobot.signal('right-wrist').value)
sdes[0][3] += 0.25 # Move reference point forward.
sdes[2][3] += 0.1 # Increment reference point altitude.
robot.features['right-wrist'].reference.signal('position').value = toTuple(sdes)
# Select translation only.
robot.features['right-wrist'].feature.signal('selec').value = '000111'
robot.tasks['right-wrist'].signal('controlGain').value = 1.
reach(robot, 'right-wrist', 0.25, 0, 0.1)
# Push tasks
# Operational points tasks
solver.sot.push(robot.name + '.task.right-ankle')
solver.sot.push(robot.name + '.task.left-ankle')
solver.sot.push(robot.name + '.task.left-wrist')
solver.sot.push(robot.name + '.task.right-wrist')
# Center of mass
......
......@@ -82,6 +82,16 @@ def initRobotViewer():
print "failed to connect to robotviewer"
return clt
def reach(robot, op, tx, ty, tz):
sdes = toList(robot.dynamicRobot.signal(op).value)
sdes[0][3] += tx
sdes[1][3] += ty
sdes[2][3] += tz
robot.features[op].reference.signal('position').value = toTuple(sdes)
# Select translation only.
robot.features[op].feature.signal('selec').value = '000111'
robot.tasks[op].signal('controlGain').value = 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